from geometry_msgs.msg import Point, Pose, Quaternion
from hexapod_python.ik import leg_offsets

standing_pose = {
    'body': Pose(
        position = Point(x = 0.0, y = 0.0, z = 105.0 ),
        orientation = Quaternion(x = 0.0, y = 0.0, z = 0.0, w = 1.0)
    ),
    'legs': {
        'e1': Point(x = leg_offsets['e1'].x + -100.0, y = leg_offsets['e1'].y, z = 0.0),
        'e2': Point(x = leg_offsets['e2'].x + -100.0, y = leg_offsets['e2'].y, z = 0.0),
        'e3': Point(x = leg_offsets['e3'].x + -100.0, y = leg_offsets['e3'].y, z = 0.0),
        'e4': Point(x = leg_offsets['e4'].x +  100.0, y = leg_offsets['e4'].y, z = 0.0),
        'e5': Point(x = leg_offsets['e5'].x +  100.0, y = leg_offsets['e5'].y, z = 0.0),
        'e6': Point(x = leg_offsets['e6'].x +  100.0, y = leg_offsets['e6'].y, z = 0.0)
    }
}

stand_up_pose = {
    'body': Pose(
        position = Point(x = 0.0, y = 0.0, z = 105.0 ),
        orientation = Quaternion(x = 0.0, y = 0.0, z = 0.0, w = 1.0)
    ),
    'legs': {
        'e1': Point(x = leg_offsets['e1'].x + -140.0, y = leg_offsets['e1'].y, z = 0.0),
        'e2': Point(x = leg_offsets['e2'].x + -140.0, y = leg_offsets['e2'].y, z = 0.0),
        'e3': Point(x = leg_offsets['e3'].x + -140.0, y = leg_offsets['e3'].y, z = 0.0),
        'e4': Point(x = leg_offsets['e4'].x +  140.0, y = leg_offsets['e4'].y, z = 0.0),
        'e5': Point(x = leg_offsets['e5'].x +  140.0, y = leg_offsets['e5'].y, z = 0.0),
        'e6': Point(x = leg_offsets['e6'].x +  140.0, y = leg_offsets['e6'].y, z = 0.0)
    }
}



shutdown_pose = {
    'body': Pose(
        position = Point(x = 0.0, y = 0.0, z = -5.0 ),
        orientation = Quaternion(x = 0.0, y = 0.0, z = 0.0, w = 1.0)
    ),
    'legs': {
        'e1': Point(x = leg_offsets['e1'].x + -140.0, y = leg_offsets['e1'].y, z = 0.0),
        'e2': Point(x = leg_offsets['e2'].x + -140.0, y = leg_offsets['e2'].y, z = 0.0),
        'e3': Point(x = leg_offsets['e3'].x + -140.0, y = leg_offsets['e3'].y, z = 0.0),
        'e4': Point(x = leg_offsets['e4'].x +  140.0, y = leg_offsets['e4'].y, z = 0.0),
        'e5': Point(x = leg_offsets['e5'].x +  140.0, y = leg_offsets['e5'].y, z = 0.0),
        'e6': Point(x = leg_offsets['e6'].x +  140.0, y = leg_offsets['e6'].y, z = 0.0),
    }
}

calibration_pose = {
    'body': Pose(
        position = Point(x = 0.0, y = 0.0, z = 0.0 ),
        orientation = Quaternion(x = 0.0, y = 0.0, z = 0.0, w = 1.0)
    ),
    'legs': {
        'e1': Point(x = -200.0, y =  150.0, z = 0.0),
        'e2': Point(x = -200.0, y =    0.0, z = 0.0),
        'e3': Point(x = -200.0, y = -150.0, z = 0.0),         

        'e4': Point(x =  200.0, y = -150.0, z = 0.0),
        'e5': Point(x =  200.0, y =    0.0, z = 0.0),
        'e6': Point(x =  200.0, y =  150.0, z = 0.0),

    }

}


def interpolate_pose(start, end, steps):

    body_step_x = (end['body'].position.x - start['body'].position.x) / (steps-1)
    body_step_y = (end['body'].position.y - start['body'].position.y) / (steps-1)
    body_step_z = (end['body'].position.z - start['body'].position.z) / (steps-1)

    leg_step_x = {}
    leg_step_y = {}
    leg_step_z = {}

    for leg in start['legs']:
        leg_step_x[leg] = (end['legs'][leg].x - start['legs'][leg].x) / (steps-1)
        leg_step_y[leg] = (end['legs'][leg].y - start['legs'][leg].y) / (steps-1)
        leg_step_z[leg] = (end['legs'][leg].z - start['legs'][leg].z) / (steps-1)

    poses = []
    for i in range(0, steps):
        pose = {
            'body': Pose(
                position = Point(
                    x = start['body'].position.x + body_step_x * i, 
                    y = start['body'].position.y + body_step_y * i, 
                    z = start['body'].position.z + body_step_z * i
                ),
                orientation = Quaternion(x = 0.0, y = 0.0, z = 0.0, w = 1.0)
            ),
            'legs': {}            
        }

        for leg in start['legs']:
            pose['legs'][leg] = Point(
                x = start['legs'][leg].x + leg_step_x[leg] * i,
                y = start['legs'][leg].y + leg_step_y[leg] * i,
                z = start['legs'][leg].z + leg_step_z[leg] * i
            )

        poses.append(pose)

    return poses