from config.config import get_config

from .ezsv23 import EZSV23


cfg = get_config(validate_present=['motor_com'])
if cfg:
    COM=cfg['motor_com']
else:
    COM='COM4'
    
with EZSV23(COM=COM) as motor:
    motor.servo_here()

    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
            motor.home()
            motor.wait_for_completion()

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

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

        elif cmd[0] == "pr":    # position absolute move
            motor.position_relative(cmd[1])

        elif cmd[0] == "mo":    # motor off
            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] == "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)")

        elif cmd[0] == "q":     # quit
            run_flag = False

        else:
            print("unknown command")