# walking_positions.py, module to generate walking positions
# servos = [L_ankle, L_knee, L_hip, R_hip, R_knee, R_ankle]
#
# output:       a list of tuples of individual servo movements
#               e.g. [("L", hip_angle, knee_angle, ankle_angle),
#                       ("R", hip_angle, knee_angle, ankle_angle),
#                       (....)]
# input:    w_parameters dict as argument, holding dynamic parameters:
#               step, leading leg, target
#            cfg.parameters, holding static parameters:
#               CoM_height, mstep_width, friction, delay
#
#   Remark: the calling function needs to take care of the validity of the input parameters.
#           examples:   height of COM determines the maximum step size
#                       a large delay may cause the robot to tilt over in single support phases
#
# rev 13-Dec-2022 by wolf2018
from calc_angle import target_angle
import robi_def
from walk import walk
import cfg


# init and store parameters for next calculation

cfg.status_dict["last_CoM"] = robi_def.height_max
last_step = 0
micro_steps = []

last_left = {"hip": 0,
            "knee": 0,
            "ankle": 0,
            "hip_1": 0}

last_right = {"hip": 0,
            "knee": 0,
            "ankle": 0,
            "hip_1": 0}


def calc_servo(s_angle, servo):
    """adjust servo position: apply limitations, correct for rotation direction, neutral position and servo offset"""
    if servo["rot"]:    # positive rotation
        #print(servo["name"], "positive rotation")
        _angle = s_angle + servo["offset"]
    else:               # inverse rotation
        #print(servo["name"], "inverted rotation")
        _angle = 90 - s_angle - servo["offset"]
    #print("angle corrected= ", str(_angle))

    #protect servos by limiting angle
    if _angle > servo["max_angle"]:
        _angle = servo["max_angle"]
        if cfg.debug > 1:
            print("max angle: ", _angle)

    if _angle < servo["min_angle"]:
        _angle = servo["min_angle"]
        if cfg.debug > 1:
            print("min angle: ", _angle)

    return _angle


def initialize_CoM(w_parameters):
    """ straight move of all servos to init position, Robi stands upright on both legs. Care should be taken if init is called not to block any leg or let Robi drop"""
    support = True
    step = 0

    micro_steps = []

    hip_ang, knee_ang, ankle_ang, hip_1_ang = target_angle(robi_def.height_max, step, support)

    micro_steps.append(("L", calc_servo(hip_ang, robi_def.L_hip), calc_servo(knee_ang, robi_def.L_knee), calc_servo(ankle_ang, robi_def.L_ankle)))
    micro_steps.append(("R", calc_servo(hip_ang, robi_def.R_hip), calc_servo(knee_ang, robi_def.R_knee), calc_servo(ankle_ang, robi_def.R_ankle)))

    cfg.status_dict["last_CoM"] = robi_def.height_max
    if cfg.debug > 0:
        print("CoM init position: ", cfg.status_dict["last_CoM"])
        print("microsteps: ", micro_steps)
    # execute position
    w_parameters["w_pos"] = micro_steps
    walk(w_parameters)

# function to move com up or down in dual support mode
def move_CoM(w_parameters):
    """ moves CoM up or down, both feet go to new CoM position, both feet are in support (standing on the floor)"""
    #global last_CoM
    support = True
    step = 0
    micro_steps = []
    CoM_height = cfg.parameters["CoM_height"]
    m_step_width = cfg.parameters["mstep_width"]

    if CoM_height != cfg.status_dict["last_CoM"]:

        delta_CoM = CoM_height - cfg.status_dict["last_CoM"]
        micro_CoM = delta_CoM / m_step_width
        if cfg.debug > 1:
            print("delta CoM= ", delta_CoM, "micro_CoM= ", micro_CoM)

        if delta_CoM < 0:   # move CoM down

            i_CoM = cfg.status_dict["last_CoM"]

            while i_CoM > CoM_height:

                i_CoM -= m_step_width
                hip_ang, knee_ang, ankle_ang, hip_1_ang = target_angle(i_CoM, step, support)

                micro_steps.append(("L", calc_servo(hip_ang, robi_def.L_hip), calc_servo(knee_ang, robi_def.L_knee), calc_servo(ankle_ang, robi_def.L_ankle)))
                micro_steps.append(("R", calc_servo(hip_ang, robi_def.R_hip), calc_servo(knee_ang, robi_def.R_knee), calc_servo(ankle_ang, robi_def.R_ankle)))

        else:       # move CoM up

            i_CoM = cfg.status_dict["last_CoM"]

            while i_CoM < CoM_height:

                i_CoM += m_step_width
                if i_CoM > robi_def.height_max:
                    i_CoM = robi_def.height_max

                hip_ang, knee_ang, ankle_ang, hip_1_ang = target_angle(i_CoM, step, support)

                micro_steps.append(("L", calc_servo(hip_ang, robi_def.L_hip), calc_servo(knee_ang, robi_def.L_knee), calc_servo(ankle_ang, robi_def.L_ankle)))
                micro_steps.append(("R", calc_servo(hip_ang, robi_def.R_hip), calc_servo(knee_ang, robi_def.R_knee), calc_servo(ankle_ang, robi_def.R_ankle)))

        cfg.status_dict["last_CoM"] = CoM_height
        if cfg.debug > 1:
            print("new CoM position: ", cfg.status_dict["last_CoM"])
            print("microsteps: ", micro_steps)
        # execute position
        w_parameters["w_pos"] = micro_steps
        walk(w_parameters)

    else:
        # decide later what to do in case move_CoM is called, but CoM did not change
        # for now do nothing
        pass


def calc_positions(action, w_parameters):
    """function to determine and calculate all movement actions
       parameters are handed over by a dictionary"""
    if cfg.debug > 0:
        print("calc_positions using:", w_parameters)

    CoM_height = cfg.parameters["CoM_height"]
    delay = cfg.parameters["delay"]
    # friction = cfg.parameters["friction"]    # friction not implemented yet
    mstep_width = cfg.parameters["mstep_width"]

    target = w_parameters["target"]
    leading = w_parameters["leading"]
    step = w_parameters["step"]

    # check for invalid CoM_height
    if cfg.debug > 1:
        print("last_CoM= ", cfg.status_dict["last_CoM"])

    if CoM_height > robi_def.height_max:

        if cfg.debug > 0:
            print("CoM_height is higher than max possible CoM_height! Set to max height.")
        cfg.parameters["CoM_height"] = robi_def.height_max

    if CoM_height < robi_def.height_min:

        # implement exception here
        if cfg.debug > 0:
            print("CoM_height is lower than minimal possible CoM_height!")
        pass    # for now do nothing

    #
    # we have a valid CoM, now check what action to take
    #
    #
    micro_steps = []    # clear micro steps buffer

    # action "init" moves servos of both legs straight to init position
    if action == "init":
        if cfg.debug > 0:
            print("\n ... caution initializing Robi  ...")
            if cfg.debug > 1:
                input("press <ENTER> to initialize Robi....")
        initialize_CoM(w_parameters)

    elif action == "ready":    # move CoM to ready position
        move_CoM(w_parameters)   # CoM has to be set in parameters

    elif action == "walk":
        # now calculate steps and micro steps needed to meet target

        if target == 0:  # no valid target distance

            if cfg.debug > 0:
                print("invalid target=0 ==> do nothing")
            pass

            # Define an exception here?

        if step == 0:
            if cfg.debug > 0:
                print("invalid step=0 ==> set to default step value!")

            # add exception here??????
            # set step to default value for now
            step = robi_def.step_default

        if target < step:
            step = target

        number_of_steps = int(target / step)
        last_step_width = target % step         # remaining distance to meet target
        step_width = (target-last_step_width) / number_of_steps

        if cfg.debug > 0:
            print("need ", number_of_steps, " steps plus ", last_step_width, "cm to meet target of ", target, " cm")

        # now calculate steps

        # leading leg calculation
        if leading == "L":
            # 2: lift leading leg in one movement (no microsteps)
            hip_ang, knee_ang, ankle_ang, hip_1_ang = target_angle(CoM_height-robi_def.safety_height, 0, support=False)

            micro_steps.append(("L", calc_servo(hip_ang, robi_def.L_hip), calc_servo(knee_ang, robi_def.L_knee), calc_servo(ankle_ang, robi_def.L_ankle)))

            # 3: move CoM forward using micro steps
            if cfg.debug > 1:
                print("step_width= "+str(step_width))
                print("mstep_width= "+str(mstep_width))

            number_msteps = step_width / mstep_width

            for i in range(1, number_msteps):
                d_CoM_x = i * mstep_width

                # for now this calculates only a straight move of the leading leg in x direction, no additional lifting of the foot above the safety height

                # calculate support leg to move CoM
                hip_ang, knee_ang, ankle_ang, hip_1_ang = target_angle(CoM_height, d_CoM_x, support=True)

                micro_steps.append(("R", calc_servo(hip_ang, robi_def.R_hip), calc_servo(knee_ang, robi_def.R_knee), calc_servo(ankle_ang, robi_def.R_ankle)))

            # 4: lower leading leg to support position
            hip_ang, knee_ang, ankle_ang, hip_1_ang = target_angle(CoM_height, 0, support=False)

            micro_steps.append(("L", calc_servo(hip_ang, robi_def.L_hip), calc_servo(knee_ang, robi_def.L_knee), calc_servo(ankle_ang, robi_def.L_ankle)))

            # 5: lift trailing leg in one movement (here no microsteps)
            hip_ang, knee_ang, ankle_ang, hip_1_ang = target_angle(CoM_height-robi_def.safety_height, 0, support=False)

            micro_steps.append(("R", calc_servo(hip_ang, robi_def.R_hip), calc_servo(knee_ang, robi_def.R_knee), calc_servo(ankle_ang, robi_def.R_ankle)))

            # 6: move move CoM forward using micro steps

            number_msteps = step_width / mstep_width

            for i in range(1, number_msteps):
                d_CoM_x = i * mstep_width

                # for now this calculates only a straight move of the leading leg in x direction, no additional lifting of the foot above the safety height
                # calculate support leg to move CoM
                hip_ang, knee_ang, ankle_ang, hip_1_ang = target_angle(CoM_height, d_CoM_x, support=True)

                micro_steps.append(("L", calc_servo(hip_ang, robi_def.L_hip), calc_servo(knee_ang, robi_def.L_knee), calc_servo(ankle_ang, robi_def.L_ankle)))

            # 7: lower trailing leg to support position
            hip_ang, knee_ang, ankle_ang, hip_1_ang = target_angle(CoM_height, 0, support=True)

            micro_steps.append(("R", calc_servo(hip_ang, robi_def.R_hip), calc_servo(knee_ang, robi_def.R_knee), calc_servo(ankle_ang, robi_def.R_ankle)))
            micro_steps.append(("L", calc_servo(hip_ang, robi_def.L_hip), calc_servo(knee_ang, robi_def.L_knee), calc_servo(ankle_ang, robi_def.L_ankle)))

            # now execute the calculated micro steps
            if cfg.debug > 1:
                print("walking: ", micro_steps)
            w_parameters["w_pos"] = micro_steps
            cfg.parameters["w_delay"] = delay
            walk(w_parameters, number_of_steps)

        elif leading == "R":

            # right leg leading
            pass

        else:       # leading is not L or R

            # raise error here????????? tbd.
            if cfg.debug > 0:
                print("no leading leg given! do nothing :-) ")
            pass


def test_walk():
    """ execute walking pattern for debug only.
    uncomment input statements to enter parameters """

    cfg.parameters["CoM_height"] = 10.5
    # cfg.parameters["CoM_height"] = float(input("CoM height: "))
    cfg.w_parameter0["target"] = 30
    # cfg.w_parameter0["target"] = float(input("target: "))
    cfg.w_parameter0["leading"] = "L"
    # cfg.w_parameter0["leading"] = input("leading L or R: ")
    cfg.w_parameter0["step"] = 2
    # cfg.w_parameter0["step"] = float(input("step: "))

    calc_positions("walk", cfg.w_parameter0)
    if cfg.debug > 0:
        print("... done test walk ...")