"""
uwb_node.py — UWB DW3000 anchor/tag ranging node (Issue #365).

Reads TWR distances from two ESP32-UWB-Pro anchors via USB serial and publishes
a fused bearing + distance estimate for the follow-me controller.

Hardware
--------
  anchor0 (left)  : USB serial, default /dev/ttyUSB0
  anchor1 (right) : USB serial, default /dev/ttyUSB1
  Baseline        : ~25 cm (configurable)

Publishes
---------
  /saltybot/uwb_target   saltybot_scene_msgs/UwbTarget  (10 Hz)

Parameters
----------
  port_anchor0      str    '/dev/ttyUSB0'   Serial device for left anchor
  port_anchor1      str    '/dev/ttyUSB1'   Serial device for right anchor
  baud_rate         int    115200           Serial baud rate
  baseline_m        float  0.25             Anchor separation (m)
  publish_rate_hz   float  10.0             Output publish rate
  stale_timeout_s   float  0.5              Age beyond which a range is discarded
"""

from __future__ import annotations

import threading
import time
from typing import Optional

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy

from std_msgs.msg import Header
from saltybot_scene_msgs.msg import UwbTarget

from ._uwb_tracker import (
    AnchorSerialReader,
    UwbRangingState,
    FIX_NONE, FIX_SINGLE, FIX_DUAL,
)


_PUB_QOS = QoSProfile(
    reliability=ReliabilityPolicy.RELIABLE,
    history=HistoryPolicy.KEEP_LAST,
    depth=10,
)


class UwbNode(Node):

    def __init__(self) -> None:
        super().__init__('uwb_node')

        # ── Parameters ──────────────────────────────────────────────────────
        self.declare_parameter('port_anchor0',    '/dev/ttyUSB0')
        self.declare_parameter('port_anchor1',    '/dev/ttyUSB1')
        self.declare_parameter('baud_rate',       115200)
        self.declare_parameter('baseline_m',      0.25)
        self.declare_parameter('publish_rate_hz', 10.0)
        self.declare_parameter('stale_timeout_s', 0.5)

        p = self.get_parameter

        self._port0       = str(p('port_anchor0').value)
        self._port1       = str(p('port_anchor1').value)
        self._baud        = int(p('baud_rate').value)
        baseline          = float(p('baseline_m').value)
        rate_hz           = float(p('publish_rate_hz').value)
        stale_s           = float(p('stale_timeout_s').value)

        # ── State + readers ──────────────────────────────────────────────────
        self._state = UwbRangingState(
            baseline_m=baseline,
            stale_timeout=stale_s,
        )

        self._readers: list[AnchorSerialReader] = []
        self._open_serial_readers()

        # ── Publisher + timer ────────────────────────────────────────────────
        self._pub = self.create_publisher(UwbTarget, '/saltybot/uwb_target', _PUB_QOS)
        self._timer = self.create_timer(1.0 / max(rate_hz, 1.0), self._publish)

        self.get_logger().info(
            f'uwb_node ready — baseline={baseline:.3f}m, '
            f'rate={rate_hz:.0f}Hz, '
            f'ports=[{self._port0}, {self._port1}]'
        )

    # ── Serial open ───────────────────────────────────────────────────────────

    def _open_serial_readers(self) -> None:
        """
        Attempt to open both serial ports.  Failures are non-fatal — the node
        will publish FIX_NONE until a port becomes available (e.g. anchor
        hardware plugged in after startup).
        """
        for anchor_id, port_name in enumerate([self._port0, self._port1]):
            port = self._try_open_port(anchor_id, port_name)
            if port is not None:
                reader = AnchorSerialReader(
                    anchor_id=anchor_id,
                    port=port,
                    state=self._state,
                    logger=self.get_logger(),
                )
                reader.start()
                self._readers.append(reader)
                self.get_logger().info(f'Anchor {anchor_id} opened on {port_name}')
            else:
                self.get_logger().warn(
                    f'Anchor {anchor_id} port {port_name} unavailable — '
                    f'will publish FIX_NONE until connected'
                )

    def _try_open_port(self, anchor_id: int, port_name: str):
        """Open serial port; return None on failure."""
        try:
            import serial
            return serial.Serial(port_name, baudrate=self._baud, timeout=0.1)
        except Exception as e:
            self.get_logger().warn(
                f'Cannot open anchor {anchor_id} serial port {port_name}: {e}'
            )
            return None

    # ── Publish callback ──────────────────────────────────────────────────────

    def _publish(self) -> None:
        result = self._state.compute()
        msg = UwbTarget()
        msg.header.stamp    = self.get_clock().now().to_msg()
        msg.header.frame_id = 'base_link'

        msg.valid          = result.valid
        msg.bearing_deg    = float(result.bearing_deg)
        msg.distance_m     = float(result.distance_m)
        msg.confidence     = float(result.confidence)
        msg.anchor0_dist_m = float(result.anchor0_dist)
        msg.anchor1_dist_m = float(result.anchor1_dist)
        msg.baseline_m     = float(result.baseline_m)
        msg.fix_quality    = int(result.fix_quality)

        self._pub.publish(msg)

    # ── Cleanup ───────────────────────────────────────────────────────────────

    def destroy_node(self) -> None:
        for r in self._readers:
            r.stop()
        super().destroy_node()


def main(args=None) -> None:
    rclpy.init(args=args)
    node = UwbNode()
    try:
        rclpy.spin(node)
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()
