from pytrinamic.connections import ConnectionManager
from pytrinamic.modules import TMCM1140
from pytrinamic.tmcl import TMCLCommand
import serial.tools.list_ports
import time
from typing import Optional

RIGHT_EYE_POS = 12000  # 18000 for Jack's prototype VAP fixture
LEFT_EYE_POS = 55700  # 61700 for Jack's prototype VAP fixture
TMCL_VENDOR_ID = 0x2A3C

def find_com_port_by_vendor_id(vendor_id):
    ports = serial.tools.list_ports.comports()
    for port in ports:
        if port.vid == vendor_id:
            return port.device[3:]
    return None

def find_interface() -> Optional[int]:

    com_port = find_com_port_by_vendor_id(TMCL_VENDOR_ID)
    if com_port is not None:
        try:
            connection_manager = ConnectionManager(f"--interface serial_tmcl --port COM{com_port} --data-rate 115200")
            with connection_manager.connect() as module:
                mod_val = TMCM1140(module).connection.send(TMCLCommand.GGP, 1, 2, 0).value
                return com_port
        except ConnectionError:
            pass
    return None


class MotorController:
    def __init__(self):
        self.connection_manager = None
        self.mrs = 4
        index = find_interface()
        self.connection_manager = ConnectionManager(f"--interface serial_tmcl --port COM{index} --data-rate 115200")
        self.my_interface = self.connection_manager.connect()
        self.module = TMCM1140(self.my_interface)
        self.motor = self.module.motors[0]

        self.module.connection.send(TMCLCommand.SAP, 5, 0, 50)  # Acceleration
        self.module.connection.send(TMCLCommand.SAP, 6, 0, 80)  # Current
        self.module.connection.send(TMCLCommand.SAP, 4, 0, 300)  # Speed max
        self.module.connection.send(TMCLCommand.SAP, 140, 0, 4)  # Micro-step resolution
        self.module.connection.send(TMCLCommand.SAP, 153, 0, 7)  # Ramp divisor
        self.module.connection.send(TMCLCommand.SAP, 154, 0, 3)  # Pulse divisor

    def go_home(self):
        self.module.connection.send(TMCLCommand.SAP, 13, 0, 1)  # Left limit activate
        self.module.connection.send(TMCLCommand.SAP, 193, 0, 1)  # Set left search mode
        self.module.connection.send(TMCLCommand.SAP, 194, 0, 500)  # Set left search speed
        self.module.connection.send(TMCLCommand.SAP, 195, 0, 100)  # Set left search switch speed
        self.module.connection.send(TMCLCommand.RFS, 0, 0, 0)  # Start left reference search

        timeout = 60  # seconds
        sec0 = time.time()
        sec1 = time.time()

        while self.module.connection.send(TMCLCommand.RFS, 2, 0, 0).value != 0:
            sec1 = time.time()
            if sec1 > sec0 + timeout:
                self.module.connection.send(TMCLCommand.MST, 0, 0, 0)  # stop motor when timed out
                break
            time.sleep(0.5)

    def go_right_eye(self):
        self.module.connection.send(TMCLCommand.MVP, 0, 0, RIGHT_EYE_POS)

        while self.module.connection.send(TMCLCommand.GAP, 1, 0, 0).value != RIGHT_EYE_POS:
            time.sleep(0.5)


    def go_left_eye(self):
        self.module.connection.send(TMCLCommand.MVP, 0, 0, LEFT_EYE_POS)

        while self.module.connection.send(TMCLCommand.GAP, 1, 0, 0).value != LEFT_EYE_POS:
            time.sleep(0.5)

    def stop(self):
        self.module.connection.send(TMCLCommand.MST, 0, 0, 0)