"""
Serial communication with Arduino Nano motor controller.
"""

import serial
import serial.tools.list_ports
import threading
import time


class MotorSerial:
    def __init__(self, baud=9600, timeout=1.0):
        self.baud = baud
        self.timeout = timeout
        self.ser = None
        self.lock = threading.Lock()
        self.last_distance = 999.0
        self._running = False
        self._distance_thread = None

    def connect(self):
        port = self._find_arduino()
        if port is None:
            raise ConnectionError(
                "Arduino not found. Check USB cable and run: "
                "sudo usermod -a -G dialout $USER"
            )
        try:
            self.ser = serial.Serial(port, self.baud, timeout=self.timeout)
            time.sleep(2)
            self.ser.reset_input_buffer()
            print(f"[MotorSerial] Connected to Arduino on {port}")
            return True
        except serial.SerialException as e:
            print(f"[MotorSerial] Connection failed: {e}")
            return False

    def _find_arduino(self):
        ports = serial.tools.list_ports.comports()
        for p in ports:
            if "ttyUSB" in p.device or "ttyACM" in p.device:
                return p.device
            if p.vid in (0x2341, 0x1A86, 0x0403):
                return p.device
        return None

    def send(self, command: str):
        if self.ser is None or not self.ser.is_open:
            return False
        with self.lock:
            try:
                self.ser.write(f"{command}\n".encode())
                return True
            except serial.SerialException:
                print("[MotorSerial] Write failed, connection lost")
                return False

    def forward(self, speed=180):
        self.send(f"F,{speed}")

    def backward(self, speed=150):
        self.send(f"B,{speed}")

    def spin_left(self, speed=160):
        self.send(f"L,{speed}")

    def spin_right(self, speed=160):
        self.send(f"R,{speed}")

    def forward_left(self, left_speed, right_speed):
        self.send(f"FL,{left_speed},{right_speed}")

    def forward_right(self, left_speed, right_speed):
        self.send(f"FR,{left_speed},{right_speed}")

    def stop(self):
        self.send("S")

    def request_distance(self):
        if self.ser is None or not self.ser.is_open:
            return 999.0
        with self.lock:
            try:
                self.ser.reset_input_buffer()
                self.ser.write(b"D\n")
                response = self.ser.readline().decode().strip()
                if response.startswith("DIST,"):
                    return float(response.split(",")[1])
            except (serial.SerialException, ValueError, IndexError):
                pass
        return 999.0

    def start_distance_polling(self, interval=0.15):
        self._running = True
        self._distance_thread = threading.Thread(
            target=self._poll_distance, args=(interval,), daemon=True
        )
        self._distance_thread.start()

    def _poll_distance(self, interval):
        while self._running:
            self.last_distance = self.request_distance()
            time.sleep(interval)

    def get_distance(self):
        return self.last_distance

    def close(self):
        self._running = False
        if self._distance_thread:
            self._distance_thread.join(timeout=2)
        self.stop()
        if self.ser and self.ser.is_open:
            self.ser.close()
        print("[MotorSerial] Closed")