import time
from pytrinamic.connections import ConnectionManager
from pytrinamic.modules import TMCM1161
from pytrinamic.tmcl import TMCLCommand


connection_manager = ConnectionManager("--interface serial_tmcl --port COM9 --data-rate 115200")

with connection_manager.connect() as my_interface:
    module = TMCM1161(my_interface)
    motor = module.motors[0]
    module.connection.send(TMCLCommand.SAP, 5, 0, 100)  # acceleration
    module.connection.send(TMCLCommand.SAP, 6, 0, 80)  # current
    module.connection.send(TMCLCommand.SAP, 4, 0, 1600)  # speed max
    mrs = 5
    module.connection.send(TMCLCommand.SAP, 140, 0, mrs)  # micro step res 5 (128x)
    module.connection.send(TMCLCommand.SAP, 153, 0, 7)  # ramp divisor
    module.connection.send(TMCLCommand.SAP, 154, 0, 3)  # pulse divisor
    module.connection.send(TMCLCommand.SIO, 0, 0, 1)  # pull-up resistor
    wormratio = 180
    steps = 1 * wormratio * (2 ** mrs) * 200

    module.connection.send(TMCLCommand.SAP, 0, 0, steps)
    timeout = 60  # seconds
    sec0 = time.time()
    sec1 = time.time()

    while module.connection.send(TMCLCommand.GAP, 8, 0, 0).value == 0:
        print("Position = {}".format(motor.actual_position), "target = ", steps)
        sec1 = time.time()
        if sec1 > sec0 + timeout:
            print("TIMED OUT")
            module.connection.send(TMCLCommand.MST, 0, 0, 0)  # stop motor when timed out
            break
        time.sleep(0.2)
    time.sleep(0.5)
    print("Position = {}".format(motor.actual_position))
    print("MOVE COMPLETE")
    print("MOVE ELAPSED TIME =", sec1 - sec0)