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]
    # initialization config
    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
    module.connection.send(TMCLCommand.SAP, 140, 0, 5)  # 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

    # ref search config
    module.connection.send(TMCLCommand.SAP, 12, 0, 1)  # right limit activate
    module.connection.send(TMCLCommand.SAP, 13, 0, 1)  # left limit activate
    module.connection.send(TMCLCommand.SAP, 149, 0, 1)  # soft stop
    module.connection.send(TMCLCommand.SAP, 193, 0, 4)  # search mode
    module.connection.send(TMCLCommand.SAP, 194, 0, 900)  # search speed
    module.connection.send(TMCLCommand.SAP, 195, 0, 100)  # search switch speed
    module.connection.send(TMCLCommand.SIO, 0, 2, 1)  # OUT_0 HIGH

    # start ref search
    module.connection.send(TMCLCommand.RFS, 0, 0, 0) # start reference search
    timeout = 60 # seconds
    sec0 = time.time()


    while module.connection.send(TMCLCommand.RFS, 2, 0, 0).value != 0:
        print("Position = {}".format(motor.actual_position))
        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)
    module.connection.send(TMCLCommand.SAP, 1, 0, 0)  # reset current position to 0
    module.connection.send(TMCLCommand.SAP, 0, 0, 0)  # reset target position to 0
    time.sleep(0.5)
    print("POSITION = {}".format(module.connection.send(TMCLCommand.GAP, 1, 0, 0).value))
    print("HOMING COMPLETE")
    print("HOMING ELAPSED TIME =", sec1-sec0)