import queue
import threading
import time

import serial
from serial.tools import list_ports

from config import BAUD_RATE, HOME

_JUNK = ("bluetooth-incoming-port", "debug-console", "wlan-debug")
_BT_HINTS = ("hc-05", "hc-06", "bluetooth", "rfcomm", "bt")


def available_ports():
    """Paired Bluetooth serial ports first, then whatever else is plugged in."""
    ports = []
    for p in list_ports.comports():
        name = p.device.lower()
        if any(j in name for j in _JUNK):
            continue
        label = p.description if p.description and p.description != "n/a" else p.device.split("/")[-1]
        is_bt = any(h in (name + label.lower()) for h in _BT_HINTS)
        ports.append((not is_bt, label, p.device))
    return [(label, dev) for _, label, dev in sorted(ports)]


class ArmError(Exception):
    pass


class Arm:
    def __init__(self, port, on_lost=None):
        self.port = port
        self.pose = HOME
        self._on_lost = on_lost
        self._ser = None
        self._replies = queue.Queue()
        self._write_lock = threading.Lock()
        self._stopping = threading.Event()
        self._closed = False
        self._live = False

    def open(self):
        try:
            self._ser = serial.Serial(self.port, BAUD_RATE, timeout=0.2, write_timeout=2)
        except (serial.SerialException, OSError) as e:
            raise ArmError(str(e)) from None

        threading.Thread(target=self._read_loop, daemon=True).start()

        # USB boards reboot when the port opens, so keep pinging for a bit.
        deadline = time.monotonic() + 4
        while time.monotonic() < deadline:
            self._send("P")
            try:
                if self._wait(("PONG",), 0.4):
                    self._live = True
                    return
            except ArmError:
                pass
        self.close()
        raise ArmError("no answer from the arm. Is robot_arm.ino uploaded?")

    def close(self):
        self._closed = True
        if self._ser:
            try:
                self._ser.close()
            except Exception:
                pass

    def move(self, pose, ms):
        """Blocks until the arm gets there. Returns False if it was stopped midway."""
        self._drain()
        self._send("M " + " ".join(map(str, pose)) + f" {ms}")
        reply = self._wait(("OK", "STOPPED"), ms / 1000 + 3)
        if reply is None:
            raise ArmError("arm stopped responding")
        return reply == "OK"

    def run(self, steps, on_step=None):
        self._stopping.clear()
        for i, (pose, ms) in enumerate(steps):
            if self._stopping.is_set():
                return False
            if on_step:
                on_step(i, pose)
            if not self.move(pose, ms):
                return False
        return True

    def stop(self):
        self._stopping.set()
        self._send("S")

    def _send(self, line):
        with self._write_lock:
            try:
                self._ser.write((line + "\n").encode())
            except (serial.SerialException, OSError) as e:
                self._lost(e)
                raise ArmError("send failed") from None

    def _drain(self):
        while not self._replies.empty():
            self._replies.get_nowait()

    def _wait(self, kinds, timeout):
        end = time.monotonic() + timeout
        while (left := end - time.monotonic()) > 0:
            try:
                kind = self._replies.get(timeout=left)
            except queue.Empty:
                break
            if kind in kinds:
                return kind
        return None

    def _read_loop(self):
        buf = b""
        while not self._closed:
            try:
                buf += self._ser.read(64)
            except (serial.SerialException, OSError, TypeError) as e:
                self._lost(e)
                return
            while b"\n" in buf:
                raw, buf = buf.split(b"\n", 1)
                self._handle(raw.decode(errors="ignore").strip())

    def _handle(self, line):
        if not line:
            return
        kind, *rest = line.split()
        if kind in ("OK", "STOPPED") and len(rest) == 4:
            try:
                self.pose = tuple(int(x) for x in rest)
            except ValueError:
                pass
        self._replies.put(kind)

    def _lost(self, err):
        if self._closed:
            return
        self.close()
        if self._live and self._on_lost:
            self._on_lost(str(err))