import sys

from ezsv23.ezsv23 import EZSV23
from lcm_wrapper.lcm_patterns import Publisher
from config.config import get_config

from .fdi_channels import app_control_t, context_t
from .vehicle_cmds import act_on_cmd, publish_context


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

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()
        run_flag = act_on_cmd(cmd, motor, lcm, context_msg, control_msg)
        
print("done!")