# calc_angle.py does the calculation for the inverse kinematics
#
# robots definitions are in the file robi_def, which is imported
#
# input parameters:
# height:   is the distance from the hip joint to the ankle (foot) joint
# d_CoM:    is the CoM distance (delta CoM) from the current position
#           in X- direction (moving forward)
# support:  True: calculation for the support leg standing on the floor
#           False: calculation for the moving leg (no support)
#
# return paramaters are the resultinmg angles for hip, knee, ankle and hip_1
# hip_1 is needed for the calculations of the moving leg
#
# !!! all boundary conditions and limitations, such as e.g.
#    servo offsets, servo direction, min and max servo angles
#    need to be taken care of by the calling function!!!
#
# rev 16-Dec-2022 by wolf2018


from math import pi, atan, cos, acos, degrees
import robi_def
import cfg

# we let the python interpreter calculate below constants at load time
# to save time at run time

leg1 = robi_def.leg1
leg2 = robi_def.leg2

leg1_sq = leg1 * leg1
leg2_sq = leg2 * leg2


def target_angle(height, d_CoM, support=True):
    """inverse kinematics calculation for CoM (height), CoM lateral move in walking direction (d_CoM) and if leg is supporting or not"""
    if cfg.debug > 0:
        print(".... in target_angle ....")
        print("CoM= ", height, " cm\n", "delta_x CoM =", d_CoM, "cm \nsupport= ", support)

    if d_CoM == 0:      # save some calc. time
        ang_hip_1 = 0
        H2 = height
        H2_sq = height*height

    else:
        ang_hip_1 = atan(d_CoM/height)
        H2 = height / cos(ang_hip_1)
        H2_sq = H2 * H2

    # apply cosine rules
    if support:
        # leg is standing on the floor
        ang_hip = acos((leg1_sq + H2_sq - leg2_sq) / (2 * leg1 * H2)) - ang_hip_1
        ang_ankle = acos((leg2_sq + H2_sq - leg1_sq) / (2 * leg2 * H2)) + ang_hip_1
    else:
        # leg is moving
        ang_hip = acos((leg1_sq + H2_sq - leg2_sq) / (2 * leg1 * H2)) + ang_hip_1
        ang_ankle = acos((leg2_sq + H2_sq - leg1_sq) / (2 * leg2 * H2)) - ang_hip_1

    ang_knee = pi - acos((leg1_sq + leg2_sq - H2_sq) / (2 * leg1 * leg2))

    # convert radians to degrees

    if support:
        angle_hip = degrees(ang_hip)
    else:
        angle_hip = degrees(ang_hip + ang_hip_1)

    angle_knee = degrees(ang_knee)
    angle_ankle = degrees(ang_ankle)

    if cfg.debug > 0:
        print("height: ", height, "   d_CoM: ", d_CoM, "   hip: ", angle_hip, "hip_1:", degrees(ang_hip_1), "   knee: ", angle_knee, "   ankle: ", angle_ankle)

    return angle_hip, angle_knee, angle_ankle, ang_hip_1