import math


def transform_waypoints(pt_list, vision, surface_height):
    """
    Transform 2D point into 3D.

    Args:
        pt_list: Way points list in Kinect.
        vision: Vision object for points transform.
        surface_height: Height of the table in robot base.
    Returns:
        Transformed way points.
    """
    block = []
    for i, line in enumerate(pt_list):
        if len(line) < 3:  # check empty line (hack).
            break
        b = line.split()
        b = [float(e) for e in b]  # File line in float.
        transformed_point = vision.find3Dfrom2D(b[0:2])
        transformed_point[-1] = b[-1]
        block.append(transformed_point)

    first_point = [float(e) for e in pt_list[0].split()]
    last_point = [float(e) for e in pt_list[-1].split()]
    pixel_dist = math.sqrt((first_point[0] - last_point[0])**2 +
                           (first_point[1] - last_point[1])**2)
    dist = math.sqrt((block[0][0] - block[-1][0])**2 +
                     (block[0][1] - block[-1][1])**2)
    ratio = dist/pixel_dist

    for entry in block:
        entry[2] *= ratio
        entry[2] += surface_height

    return block