from ezsv23.ezsv23 import EZSV23
from zmq_wrapper.zmq_patterns import Request


# create and open the motor + req_socket objects
motor = EZSV23()
motor.servo_here()

req_socket = Request()
req_socket.open()

# 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
        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] == "lg+":    # start logging
        req_socket.send("start logging")
        
    elif cmd[0] == "lg-":  # stop logging
        req_socket.send("stop logging")
        
    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("lg+      (start logging)")
        print("lg-      (stop logging)")
        print("q        (quit)")

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

    else:
        print("unknown command")

motor.close()
req_socket.close()