import sys

from ezsv23.dummy import EZSV23_dummy as EZSV23
from lcm_wrapper.lcm_patterns import Publisher
from config.config import get_config

from ..fdi_channels import app_control_t, context_t


cfg = get_config(validate_present=['motor_com'])
if cfg:
    motor_com = cfg['motor_com']
else:
    print('Problem loading configuration. Exiting.')
    exit()

def publish_context(state):
    global lcm, context_msg
    context_msg.state = state
    lcm.publish("CONTEXT", context_msg) 

with EZSV23(COM=motor_com) as motor, \
    Publisher() as lcm:
    motor.servo_here()

    control_msg = app_control_t()
    context_msg = context_t()  

    # run the user interface - "q" sets run_flag = False
    print("type h for help menu")

    run_flag = True
    while run_flag == True:

        cmd = input(">> ").split()

        if cmd[0] == "tp":    # tell position
            position = motor.tell_position()
            print(position)

        elif cmd[0] == "hm":    # home
            publish_context('homing')
            motor.home()
            motor.wait_for_completion()
            publish_context('idling')

        elif cmd[0] == "hx":    # halt program execution
            publish_context('idling')
            motor.halt_execution()

        elif cmd[0] == "pa":    # position absolute move
            publish_context('moving')
            motor.position_absolute(cmd[1])
            motor.wait_for_completion()
            publish_context('idling')

        elif cmd[0] == "pr":    # position relative move
            publish_context('moving')
            motor.position_relative(cmd[1])
            motor.wait_for_completion()
            publish_context('idling')

        elif cmd[0] == "mo":    # motor off
            publish_context('idling')
            motor.motor_off()

        elif cmd[0] == "sh":    # servo_here
            motor.servo_here()

        elif cmd[0] == "tb":    # tell status
            rc = motor.tell_status()
            if rc[0] is True:
                print('ready (code', rc[1], ')')
            else:
                print('busy  (code', rc[1], ')')
        
        elif cmd[0] == "at":    # emulate how vehicle would presage end
            publish_context('at_target')

        elif cmd[0] == "h":  # help
            print("hm       (home)")
            print("hx       (halt program execution)")
            print("pa nn    (position absolute move to nn)")
            print("pr nn    (position relative move to nn)")
            print("mo       (motor off)")
            print("sh       (servo here)")
            print("tb       (tell status byte)")
            print("tp       (tell position)")
            print("q        (quit)")
            print("at       (at target)")
            

        elif cmd[0] == "q":     # quit
            run_flag = False
            control_msg.run_flag = False
            lcm.publish("APP_CONTROL", control_msg)

        else:
            print("unknown command")

print("done!")