# cfg.py
# holds all "global" objects and functions needed by multiple modules
# if needed objects are instantiated here, e.g. state machines for servos
#
# import cfg.py in all modules which need one or more global objects
# objects can be accessed by cfg.<object name>
#
# initial version date: 25-Jun-2022 by wolf2018
#
# updates:
# 6-Jul-2022: added initialization of state machines and servos
# 19-Jul-2022: removed "stop_core_2" flag. Core 2 is stopped by a disconnect
# 2-Aug - 2022: add ADC2 (Pin28) to read battery voltage and transfer to UI
# 4-Aug-2022: add "2cores" switch to make debug easier

import gc
# print("cfg before imports" + str(gc.mem_free()))
import robi_def
import pio_pwm
import cfg
from machine import ADC, Pin

version_date = "13-Dec-2022"

# global variables and switches
# host: hostname, will be configuerd by wifiConnect
# debug: debug level 0=no info, 1=high level info, 2=detailed info
# core2: if False running whole program on one core only, e.g. for debug
#        if True, calc and execution of walking patterns on core 2

host = None
debug = 1
# core2 = True
core2 = False

batt_adc = ADC(Pin(28))

# status_dict contains all status variables needed to control the program flow
# part of the variables will be sent back to the UI
# state:        !!! to be defined, if needed
# w_status:     holds the walking engine states
#               stop: servos de-init
#               stand: servos moved to resting, upright position
#               ready: CoM moves at walking posistion, still double leg support
#               walking: Robi executes walking pattern
# active_p0:    True if parameterset 0 is used for calculation,
#               False: it can be updated
# active_p1:    True if parameterset 0 is used for calculation,
#               False: it can be updated
# messaage:     status message of Robi
# memory:       free memory, will be reported back to Robi UI
#


status_dict = {"state": "off",
                "w_status": "stop",
                "action": "stop",
                "active_p0": False,
                "active_p1": False,
                "message": "none",
                "memory": "0",
                "last_CoM": robi_def.height_max,
               }


# parameters:    hold the static parameters for the walking engine's
#                calculations. Cannot be loaded while Robi is walking
#
#   CoM_height: height of center of mass (CoM)
#   mstep_width:    micro step lenghth, smallest increment of movement [cm]
#               step/mstep_width = number of micro movements to make a full step
#   friction:   a percentage of loss of distance, if not given friction = 0
#               if given, target is recalculated to:
#               target += target * (1 + friction)
#   delay:      delay in ms between two micro movements.
#               This will slow down the walking speed

parameters = {"mstep_width": 0.2,
                "friction": 0,
                "delay": robi_def.delay_default,
                "CoM_height": robi_def.height_max,
              }

# w_parameter_n:    hold the dynamic parameters for the walking engine's
#                calculations
#               there are two dictionaries, one is used for the calculations
#               while the other can be loaded with new parameters
#
#   step:       distance of a single step [cm]
#   leading:    leading foot to start with [left, right]
#   target:     target distance to walk [cm]
#               if target < step -> step = target
#               if target = step -> a sigle step will calculated
#               if target > step -> a number of steps will be calculated to meet the target

w_parameter0 = {"target": 0,
                "leading": L,
                "step": 0,
                "w_pos": [],
                }

w_parameter1 = {"target": 0,
                "leading": None,
                "step": 0,
                "w_pos": [],
                }


def init_sm():
    """ instantiate and start state machines (sm) for all servos
    state machines are initiated with init_value -1, which turns off the PWM"""
    if cfg.debug > 1:
        print("mem: init_sm invoked: " + str(gc.mem_free()))
    gc.collect()
    if cfg.debug > 1:
        print("mem: init_sm after gc collect: " + str(gc.mem_free()))
    global S_L_ankle, S_L_knee, S_L_hip, S_R_hip, S_R_knee, S_R_ankle

    init_value = -1
    if cfg.debug > 1:
        print("init left hip")
    S_L_hip = pio_pwm.PIOPWM(robi_def.L_hip["sm"], robi_def.L_hip["pin"], robi_def.pwm_count, robi_def.pio_freq, init_value)
    if cfg.debug > 1:
        print("init right hip")
    S_R_hip = pio_pwm.PIOPWM(robi_def.R_hip["sm"], robi_def.R_hip["pin"], robi_def.pwm_count, robi_def.pio_freq, init_value)

    if cfg.debug > 1:
        print("init left knee")
    S_L_knee = pio_pwm.PIOPWM(robi_def.L_knee["sm"], robi_def.L_knee["pin"], robi_def.pwm_count, robi_def.pio_freq, init_value)
    if cfg.debug > 1:
        print("init right knee")
    S_R_knee = pio_pwm.PIOPWM(robi_def.R_knee["sm"], robi_def.R_knee["pin"], robi_def.pwm_count, robi_def.pio_freq, init_value)

    if cfg.debug > 1:
        print("init left ankle")
    S_L_ankle = pio_pwm.PIOPWM(robi_def.L_ankle["sm"], robi_def.L_ankle["pin"], robi_def.pwm_count, robi_def.pio_freq, init_value)
    if cfg.debug > 1:
        print("init right ankle")
    S_R_ankle = pio_pwm.PIOPWM(robi_def.R_ankle["sm"], robi_def.R_ankle["pin"], robi_def.pwm_count, robi_def.pio_freq, init_value)

    status_dict["state"] = "stop"


def de_init_sm():
    """function to stop all state machines"""
    S_L_hip.de_init()
    S_R_hip.de_init()
    S_L_knee.de_init()
    S_R_knee.de_init()
    S_L_ankle.de_init()
    S_R_ankle.de_init()

def read_batt():
    """read ADC and calculate battery voltage"""
    batt_voltage = 5.4 * batt_adc.read_u16() / 65635
    return batt_voltage