"""
relay_node.py — ESP-NOW → ROS2 serial relay (Issue #618)

Hardware
────────
  An ESP32 (e.g. a spare UWB-Pro board in WiFi-only mode) acts as an
  ESP-NOW broadcast receiver.  It receives 20-byte EspNowPacket frames
  from nearby UWB tags and forwards each one verbatim over USB serial
  with a lightweight framing wrapper:

      STX(0x02) LEN(0x14) DATA[20] XOR-CRC(1)   = 23 bytes per frame

Subscriptions
─────────────
  (none — passive relay)

Publishes
─────────
  /uwb/espnow/ranges     saltybot_uwb_msgs/UwbRange       — MSG_RANGE frames
  /uwb/espnow/estop      std_msgs/Bool                    — MSG_ESTOP frames
                           True  = e-stop active
                           False = e-stop cleared
  /uwb/espnow/heartbeat  saltybot_uwb_espnow_relay/msg/   — MSG_HEARTBEAT
                         EspNowHeartbeat (see below)
  /saltybot/estop        std_msgs/Bool                    — mirrors /uwb/espnow/estop
                           (shared e-stop bus, latched True until explicit clear)

Parameters
──────────
  serial_port          /dev/espnow-relay  (udev symlink for receiver ESP32)
  baudrate             115200
  read_timeout_s       0.1               (serial read timeout)
  min_range_m          0.1               (discard implausibly short ranges)
  max_range_m          120.0             (DW3000 rated max)
  estop_latch_s        2.0               (hold /saltybot/estop True for N s
                                          after last ESTOP packet before clearing)
  publish_heartbeat    true

udev rule for receiver ESP32 (add to /etc/udev/rules.d/99-uwb-anchors.rules):
  SUBSYSTEM=="tty", ATTRS{idVendor}=="303a", ATTRS{idProduct}=="1001",
  ATTRS{serial}=="<RECEIVER_SERIAL>", SYMLINK+="espnow-relay"
"""

from __future__ import annotations

import threading
import time

import serial

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

from std_msgs.msg import Bool, Header
from saltybot_uwb_msgs.msg import UwbRange, EspNowHeartbeat
from saltybot_uwb_espnow_relay.packet import (
    FrameReader, MSG_RANGE, MSG_ESTOP, MSG_HEARTBEAT,
)


_SENSOR_QOS = QoSProfile(
    reliability=ReliabilityPolicy.BEST_EFFORT,
    history=HistoryPolicy.KEEP_LAST,
    depth=10,
)


class EspNowRelayNode(Node):

    def __init__(self) -> None:
        super().__init__("espnow_relay")

        # ── Parameters ────────────────────────────────────────────────────
        self.declare_parameter("serial_port",       "/dev/espnow-relay")
        self.declare_parameter("baudrate",          115200)
        self.declare_parameter("read_timeout_s",    0.1)
        self.declare_parameter("min_range_m",       0.1)
        self.declare_parameter("max_range_m",       120.0)
        self.declare_parameter("estop_latch_s",     2.0)
        self.declare_parameter("publish_heartbeat", True)

        self._port_path    = self.get_parameter("serial_port").value
        self._baudrate     = self.get_parameter("baudrate").value
        self._read_timeout = self.get_parameter("read_timeout_s").value
        self._min_range    = self.get_parameter("min_range_m").value
        self._max_range    = self.get_parameter("max_range_m").value
        self._estop_latch  = self.get_parameter("estop_latch_s").value
        self._pub_hb       = self.get_parameter("publish_heartbeat").value

        # ── Publishers ────────────────────────────────────────────────────
        self._range_pub = self.create_publisher(
            UwbRange, "/uwb/espnow/ranges", _SENSOR_QOS
        )
        self._estop_espnow_pub = self.create_publisher(
            Bool, "/uwb/espnow/estop", _SENSOR_QOS
        )
        self._estop_shared_pub = self.create_publisher(
            Bool, "/saltybot/estop", _SENSOR_QOS
        )
        if self._pub_hb:
            self._hb_pub = self.create_publisher(
                EspNowHeartbeat, "/uwb/espnow/heartbeat", _SENSOR_QOS
            )
        else:
            self._hb_pub = None

        # ── E-stop latch ──────────────────────────────────────────────────
        self._estop_last_t: float = 0.0
        self._estop_active: bool = False
        self._estop_lock = threading.Lock()
        self.create_timer(0.1, self._estop_latch_check)

        # ── Serial reader thread ──────────────────────────────────────────
        self._reader   = FrameReader()
        self._running  = True
        self._ser: serial.Serial | None = None
        self._serial_thread = threading.Thread(
            target=self._serial_loop, daemon=True, name="espnow-serial"
        )
        self._serial_thread.start()

        self.get_logger().info(
            f"ESP-NOW relay ready — port={self._port_path} "
            f"baud={self._baudrate} "
            f"range=[{self._min_range:.1f}, {self._max_range:.1f}] m"
        )

    def destroy_node(self) -> None:
        self._running = False
        if self._ser and self._ser.is_open:
            self._ser.close()
        super().destroy_node()

    # ── Serial read loop ───────────────────────────────────────────────────

    def _serial_loop(self) -> None:
        while self._running:
            try:
                self._ser = serial.Serial(
                    self._port_path,
                    baudrate=self._baudrate,
                    timeout=self._read_timeout,
                )
                self.get_logger().info(f"Serial opened: {self._port_path}")
                self._read_loop()
            except serial.SerialException as exc:
                if self._running:
                    self.get_logger().warn(
                        f"Serial error: {exc} — retrying in 2 s",
                        throttle_duration_sec=10.0,
                    )
                    time.sleep(2.0)
            except Exception as exc:
                if self._running:
                    self.get_logger().error(f"Unexpected serial error: {exc}")
                    time.sleep(2.0)
            finally:
                if self._ser and self._ser.is_open:
                    self._ser.close()

    def _read_loop(self) -> None:
        while self._running and self._ser and self._ser.is_open:
            raw = self._ser.read(64)
            if not raw:
                continue
            packets = self._reader.feed(raw)
            for pkt in packets:
                self._dispatch(pkt)

    # ── Packet dispatch ────────────────────────────────────────────────────

    def _dispatch(self, pkt) -> None:
        now = self.get_clock().now().to_msg()
        hdr = Header()
        hdr.stamp = now
        hdr.frame_id = "espnow"

        if pkt.msg_type == MSG_RANGE:
            range_m = pkt.range_mm / 1000.0
            if not (self._min_range <= range_m <= self._max_range):
                return
            msg = UwbRange()
            msg.header   = hdr
            msg.anchor_id = pkt.anchor_id
            msg.range_m  = float(range_m)
            msg.raw_mm   = max(0, pkt.range_mm)
            msg.rssi     = float(pkt.rssi_dbm)
            msg.tag_id   = str(pkt.tag_id)
            self._range_pub.publish(msg)

        elif pkt.msg_type == MSG_ESTOP:
            active = pkt.estop_active
            with self._estop_lock:
                if active:
                    self._estop_last_t = time.monotonic()
                    self._estop_active = True
                else:
                    # Explicit clear from tag (3× clear packets on release)
                    self._estop_active = False
            b = Bool()
            b.data = active
            self._estop_espnow_pub.publish(b)
            b2 = Bool()
            b2.data = self._estop_active
            self._estop_shared_pub.publish(b2)

        elif pkt.msg_type == MSG_HEARTBEAT and self._hb_pub:
            msg = EspNowHeartbeat()
            msg.header       = hdr
            msg.tag_id       = pkt.tag_id
            msg.battery_pct  = pkt.battery_pct
            msg.seq_num      = pkt.seq_num
            msg.timestamp_ms = pkt.timestamp_ms
            self._hb_pub.publish(msg)

    # ── E-stop latch timer ─────────────────────────────────────────────────

    def _estop_latch_check(self) -> None:
        """Clear /saltybot/estop if no ESTOP packet received within latch window."""
        with self._estop_lock:
            if self._estop_active:
                age = time.monotonic() - self._estop_last_t
                if age > self._estop_latch:
                    self._estop_active = False
                    b = Bool()
                    b.data = False
                    self._estop_shared_pub.publish(b)


def main(args=None) -> None:
    rclpy.init(args=args)
    node = EspNowRelayNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.try_shutdown()


if __name__ == "__main__":
    main()
