import time

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


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

# create and open the zmq request socket
req_socket = Request()
req_socket.open()

# create the script out of segments
script = ["hm"]    # start at the home position

forward_segment = ["new_segment", "lg+", "wt 1", "pa 25000", "lg-"]
reverse_segment = ["new_segment", "lg+", "wt 1", "pa 0"    , "lg-"]

for n in range(4):
    script += (forward_segment + reverse_segment)

# run the script
segment_counter = 0       # segment counter

for s in script:
    
    cmd = s.split()
    print(s)

    if cmd[0] == "hm":    # home
        motor.home()
        motor.wait_for_completion()

    elif cmd[0] == "new_segment":  # start a new leg
        segment_counter += 1
        print("segment ", segment_counter)

    elif cmd[0] == "lg+":    # start logging
        req_socket.send("start logging")
        
    elif cmd[0] == "wt":    # wait
        time.sleep(int(cmd[1]))

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

    elif cmd[0] == "lg-":  # stop logging
        req_socket.send("stop logging")
        
    else:
        print("unknown command")

print("done")
motor.close()
req_socket.close()