# walk.py executes a series of movements by programming the PWMs for the servos
#
# for the walking engine we might use the second core later on, thus all
# variables, data, status is handed over using the dictionaries status_dict, parameter0 and parameter1
#
# input:    w_pos: a list of tuples of micro steps = servo movements,
#               containing the foot, hip_angle, knee_angle, ankle_angle
#               e.g. [("L", hip_angle, knee_angle, ankle_angle),
#                   ("R", hip_angle, knee_angle, ankle_angle),
#                   (....)]
#           m_delay: delay in ms between each micro step, default set in robi_def.py
#           repeat: number of times the list of microsteps is executed, default= 1
#
# with the input data the PWMs are programmed to control the servos
# output:
#   status_dict:    variable to show the walking status [ready, walking, standing]
# input and output variables are stored in a dictionary
#
#
# as robi_def is static, all constants will import correctly by import *
import robi_def
import cfg
import time


def calc_pwm_count(s_angle):
    """ convert degree to pwm_count; input is servo angle"""

    p_count = int(robi_def.servo_range * (s_angle/90) + robi_def.servo_max)
    return p_count


def walk(parameters, repeat=1):
    """ execute micro steps in parameters number of <repeat> times  """

    cfg.status_dict["w_status"] = "walking"
    if cfg.debug > 1:
        print("....in walk....")
        print("\nnumber of micro steps = ", len(parameters["w_pos"]))
        print("parameters : ", parameters)

    for s in range(repeat):
        # repeat micro steps for as many times as steps needed to reach target)

        for i in range(len(parameters["w_pos"])):       # determine number  micro step tuples to be processed

            # a micro step tuple contains (0:foot, 1:hip angle, 2:knee angle, 3:ankle angle)
            micro_step = parameters["w_pos"][i]
            if cfg.debug > 1:
                print("micro steps is ", micro_step)
            if micro_step[0] == "L":
                cfg.S_L_hip.set(calc_pwm_count(micro_step[1]))
                cfg.S_L_knee.set(calc_pwm_count(micro_step[2]))
                cfg.S_L_ankle.set(calc_pwm_count(micro_step[3]))

            else:       # it's the right foot
                cfg.S_R_hip.set(calc_pwm_count(micro_step[1]))
                cfg.S_R_knee.set(calc_pwm_count(micro_step[2]))
                cfg.S_R_ankle.set(calc_pwm_count(micro_step[3]))

            if cfg.debug > 1:
                print("delay is: ", cfg.parameters["delay"])

            # small delay to let servos move to new position
            time.sleep_ms(cfg.parameters["delay"])

    # set walk engine status to ready and return
    cfg.status_dict["w_status"] = "ready"