# 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.
#
#
# rev 13-Dec-2022 by wolf2018
# rev 14-Feb-2023 refactored code for core2 to this module only, second core not yet fully tested, by default the robot controller runs on one core
# rev 19-Feb-2023 release with robi_web
#
version = "19-Feb-2023"

from calc_angle import target_angle
import robi_def
from walk import walk
import cfg
import _thread
import gc
import micropython


# 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)

    cfg.status_dict["message"] = "robot controller initialized"
    cfg.status_dict["action"] = "stand"


# function to move com up or down in dual support mode
def move_CoM(_action, 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 = []
    if _action == "stand":
        CoM_height = robi_def.height_max
    else:    # we have a ready action
        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)

        if _action == "stand":
            cfg.status_dict["action"] = "stand"
            cfg.status_dict["message"] = "CoM moved to max position"
        else:
            cfg.status_dict["action"] = "ready"
            cfg.status_dict["message"] = "CoM moved to requested position"

    else:
        # CoM did not change, do nothing
        cfg.status_dict["message"] = "warning: CoM not changed, already there"

        if _action == "stand":
            cfg.status_dict["action"] = "stand"
        else:
            cfg.status_dict["action"] = "ready"


def calc_positions(action, w_parameters):
    """function to determine and calculate all movement actions
       parameters are handed over by a dictionary"""

    if cfg.debug > 1:
        print("entering ... calc_positions")
    micro_steps = []    # clear micro steps buffer

    if cfg.debug > 1:
        print("action: ", action,"    calculation uses: ", 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! Set to min height.")
        cfg.parameters["CoM_height"] = robi_def.height_min

    #
    # we have a valid CoM, now check what action to take
    #
    #
    # 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)

    # action "stand" moves servos of both legs to max CoM
    if action == "stand":
        if cfg.debug > 0:
            print("\n ... caution initializing Robi  ...")
            if cfg.debug > 1:
                input("press <ENTER> to initialize Robi....")
        move_CoM("stand", w_parameters)

    # action "ready" moves all servos to achieve the set CoM position
    elif action == "ready":    # move CoM to ready position
        move_CoM("ready", w_parameters)   # CoM has to be set in parameters

    # action "walk" creates a walking pattern
    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")
            cfg.status_dict["w_status"] = "invalid target: 0"
            return

            # 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
            cfg.status_dict["message"] = "step = 0 using default step"
            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

            if cfg.core2:
                if cfg.debug > 0:
                    print("starting thread, free memory is: ", gc.mem_free())
                    print(micropython.mem_info())
                _thread.start_new_thread((walk), (w_parameters, number_of_steps))
            else:
                walk(w_parameters, number_of_steps)

            cfg.status_dict["message"] = "walking execution completed"
            cfg.status_dict["action"] = "ready"

        elif leading == "R":

            # right leg leading
            cfg.status_dict["message"] = "Warning : R leading - not yet implemented"
            cfg.status_dict["action"] = "ready"
            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 ...")