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


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

script_file = sys.argv[1]

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

    control_msg = app_control_t()
    context_msg = context_t()  

    with open(script_file, 'r') as script:
        for line in script:
            cmd = line.strip().split()
            print(cmd)
            act_on_cmd(cmd, motor, lcm, context_msg, control_msg)