# robi_test.py
# program to control a humanoid robot
#
# the hardware is based on the instructable by Technovation, see:
# https://www.instructables.com/Arduino-Controlled-Robotic-Biped/
#
#
# the software is written in Micropython and requires
# a Raspberri Pi Pico board with MicroPython v1.19 (or later) installed.
#
# by wolf2018, initial version 3-Apr-2022
# rev 3-Feb-2023: adjusted for testing Robi controller without web interface
# current version:
version_date = "3-Feb-2023"

import gc
import cfg
from walking_positions import calc_positions

# to use both cores set in cfg.py line 34: core2=True
# see exceptions below and uncomment further lines if you use both cores
# uncomment import lines below if two cores should be used
# import sys
# import _thread

# assign the pointers to the global objects
status_dict = cfg.status_dict
w_parameter0 = cfg.w_parameter0
w_parameter1 = cfg.w_parameter1

# instructions to be displayed when program starts
instructions = """
\n+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+
+  to do a test walk enter following commands:
+    i    Initialize Robi
+    p    enter walking Parameters, !entries are not checked for validity!
+    r    move the CoM down to Ready position
+    w    execute a test Walk
+    .    repeat walk a number of times
+
+    d    Set Debug level
+
+     stop program with CTRL-C
+
"""


def set_debug_level():
    """user can change debug level interactively"""
    dlevel = int(input("set debug level [0,1,2]: "))
    if dlevel == 0:
        cfg.debug = 0
    elif dlevel == 1:
        cfg.debug = 1
    elif dlevel == 2:
        cfg.debug = 2
    else:
        print("invalid debug level entry, debug level set to 0")
        cfg.debug = 0

def continuous_steps():
    """simple user interface to do basic walk testing using the USB interface"""
    # initialize default walking parameters
    cfg.parameters["CoM_height"] = 10.2
    w_parameter0["target"] = 4
    w_parameter0["leading"] = "L"
    cfg.parameters["mstep_width"] = 0.2
    w_parameter0["step"] = 2
    w_parameter0["leading"] = "L"    # leading leg R is not yet implemented

    while True:
        print("-------------starting test--------------\n")
        action = input("i : init    r : ready    p : enter parameters    w : walk     . : repeat walk\nd : set debug level  ---> ")

        if action == "i":
            # move Robi to stand upright
            calc_positions("init", w_parameter0)

        elif action == "r":
            # get Robi to ready position by moving CoM down
            calc_positions("ready", w_parameter0)

        elif action == "p":
            # enter walking parameter values
            # walking parameter values entered are NOT checked!!!
            cfg.parameters["CoM_height"] = float(input("CoM height [9.5...10.5]: "))
            w_parameter0["target"] = float(input("target distance to walk: "))
            cfg.parameters["mstep_width"] = float(input("micro step width [0.2...0.5]: "))
            w_parameter0["step"] = float(input("step size [1...4]: "))

        elif action == "w":
            # execute a walk action using pre-defined walking parameters.
            calc_positions("walk", w_parameter0)

        elif action == ".":
            # repeat walking action number of times
            repeat = int(input("number of repeats?: "))
            for _ in range(repeat):
                calc_positions("walk", w_parameter0)

        elif action == "d":
            # set debug level
            set_debug_level()

        else:
            print("\nunknown command entered\n")


###################################
# main program starts here

def main():
    try:
        print("============= starting Robi Test program ==============")

        # ask user to set debug level
        set_debug_level()

        # notify user of settings to use one or two cores
        if cfg.debug > 0:
            if cfg.core2:
                print("+++ using both cores +++")
            else:
                print("--- using core 0 only ---")

        # initialize servo statemachines
        if cfg.debug > 0:
            print("... init state machines ...")
        cfg.init_sm()
        gc.collect()
        if cfg.debug > 1:
            print("mem: before USB if is started: " + str(gc.mem_free()))
        if cfg.debug > 0:
            print("... starting USB interface ...")

        # start test loop
        print(instructions)
        continuous_steps()

    except KeyboardInterrupt:
        print('\nGot ctrl-c ....... stopping Robi ')
        cfg.de_init_sm()
        # uncomment line below if both cores are used
        #_thread.exit()
        gc.collect()
        #soft_reset()

    except Exception as e:
        print('mainloop crashed: ', e)

    finally:
        print('finally: cleaning up')
        cfg.de_init_sm()
        # uncomment lines below if both cores are used
        #_thread.exit()
        # sys.exit()

# end of main program
###################################

main()