import time

loop_period = 0.25

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

    
def act_on_cmd(cmd, motor, lcm, context_msg, control_msg=None):
    def publish_context_wrap(state):
        publish_context(lcm, state, context_msg, control_msg)

    def report_position_delta(target):
        ready = False
        while not ready:
            time.sleep(loop_period)
            position = int(motor.tell_position())
            delta = abs(target - position)
            publish_context_wrap(f'distance {delta}')
            ready, error = motor.tell_status()

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

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

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

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

    elif cmd[0] == "pr":    # position relative move
        position = int(motor.tell_position())
        publish_context_wrap('moving')
        motor.position_relative(cmd[1])
        report_position_delta(position + int(cmd[1]))
        publish_context_wrap('idling')

    elif cmd[0] == "mo":    # motor off
        publish_context_wrap('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] == "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
        control_msg.run_flag = False
        lcm.publish("APP_CONTROL", control_msg)

    else:
        print("unknown command")

    return run_flag