import time
from pytrinamic.connections import ConnectionManager
from pytrinamic.modules import TMCM1161
from calibot import Calibot, find_interface


o1 = 0.0029  # offset 1
o2 = 0.0012  # offset 2

id1 = find_interface(159)
id2 = find_interface(160)
assert id1 and id2, "Failed to connect to both axis"

connection_manager1 = ConnectionManager(f"--interface serial_tmcl --port COM{id1} --data-rate 115200")
connection_manager2 = ConnectionManager(f"--interface serial_tmcl --port COM{id2} --data-rate 115200")

with connection_manager1.connect() as my_interface1:
    with connection_manager2.connect() as my_interface2:
        module1 = TMCM1161(my_interface1)  # light axis --- limit travel {0 to 0.75}
        module2 = TMCM1161(my_interface2)  # heavy axis --- limit travel {0 to 0.5}
        calibot = Calibot(o1, o2)

        module1, module2 = calibot.init_config(module1, module2, o1, o2)
        try:
            calibot.go_home(module1, module2)
            time.sleep(2)
            calibot.go_position(module1, module2, 0.5, 0.25)  # pos 1 (load1)
            time.sleep(2)
            calibot.go_position(module1, module2, 0.75, 0.25)  # pos 2
            time.sleep(2)
            calibot.go_position(module1, module2, 0.5, 0)  # pos 3
            time.sleep(2)
            calibot.go_position(module1, module2, 0.25, 0.25)  # pos 4
            time.sleep(2)
            calibot.go_position(module1, module2, 0.25, 0.5)  # pos 5
            time.sleep(2)
            calibot.go_position(module1, module2, 0, 0.25)  # pos 6 (load2)
            time.sleep(2)
        except BaseException as e:
            calibot.e_stop(module1, module2)
            print('safe stop')
            raise e