"""camera_hotplug_node.py — USB camera hot-plug monitor.
Issue #320

Polls ``/dev/video*`` at a configurable rate to detect USB camera
connect / disconnect events and publishes a status string on
/saltybot/camera_status.

Status values
─────────────
  "connected"     — one or more video devices present and stable
  "disconnected"  — no /dev/video* devices found
  "restarting"    — device set changed (hot-swap / reconnect); downstream
                    capture pipelines should restart.  Held for
                    ``restart_hold_s`` seconds then resolved to
                    "connected" or "disconnected" based on re-scan.

State machine
─────────────
  DISCONNECTED ──(devices appear, within grace window)──► RESTARTING
  DISCONNECTED ──(devices appear, outside grace window)─► CONNECTED
  CONNECTED    ──(all devices lost)──────────────────────► DISCONNECTED
  CONNECTED    ──(device set changed)────────────────────► RESTARTING
  RESTARTING   ──(hold_s elapsed, devices present)───────► CONNECTED
  RESTARTING   ──(hold_s elapsed, no devices)────────────► DISCONNECTED

  Initial state is set by the first scan at startup (no "restarting" on boot).

Publications
────────────
  /saltybot/camera_status   std_msgs/String  — "connected" | "disconnected"
                                               | "restarting"

Parameters
──────────
  video_glob       (str,   "/dev/video*")  glob for device discovery
  poll_rate        (float, 2.0)            scan frequency (Hz)
  restart_grace_s  (float, 5.0)            reconnect window; if a camera
                                           reappears within this many seconds
                                           of being lost → "restarting"
  restart_hold_s   (float, 2.0)            how long to hold "restarting"
  publish_always   (bool,  False)          publish on every tick, not only
                                           on state change
  output_topic     (str,   "/saltybot/camera_status")
"""

from __future__ import annotations

import glob as _glob_mod
import threading
import time
from typing import FrozenSet, Tuple

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile
from std_msgs.msg import String


# ── Status constants ───────────────────────────────────────────────────────────

STATUS_CONNECTED    = "connected"
STATUS_DISCONNECTED = "disconnected"
STATUS_RESTARTING   = "restarting"


# ── Pure helpers ───────────────────────────────────────────────────────────────

def scan_video_devices(video_glob: str) -> FrozenSet[str]:
    """Return the frozenset of /dev/video* paths currently present.

    Catches permission / OS errors and returns an empty frozenset.
    """
    try:
        return frozenset(_glob_mod.glob(video_glob))
    except Exception:
        return frozenset()


def compute_transition(
    state: str,
    prev_devices: FrozenSet[str],
    curr_devices: FrozenSet[str],
    now: float,
    disconnect_time: float,
    restart_until: float,
    restart_grace_s: float,
    restart_hold_s: float,
) -> Tuple[str, float, float]:
    """Pure state-machine step — no I/O, no ROS.

    Parameters
    ----------
    state            : current status string
    prev_devices     : device set from the previous scan
    curr_devices     : device set from the current scan
    now              : current monotonic time (s)
    disconnect_time  : monotonic time of the last CONNECTED → DISCONNECTED
                       transition (0.0 = never disconnected)
    restart_until    : monotonic time at which RESTARTING resolves
    restart_grace_s  : reconnect window that triggers RESTARTING
    restart_hold_s   : duration to hold RESTARTING before resolving

    Returns
    -------
    (new_state, new_disconnect_time, new_restart_until)
    """
    if state == STATUS_RESTARTING:
        if now >= restart_until:
            if curr_devices:
                return STATUS_CONNECTED, disconnect_time, restart_until
            return STATUS_DISCONNECTED, now, restart_until
        return STATUS_RESTARTING, disconnect_time, restart_until

    if state == STATUS_CONNECTED:
        if not curr_devices:
            # All cameras lost
            return STATUS_DISCONNECTED, now, restart_until
        if curr_devices != prev_devices:
            # Hot-swap or device renumbering
            return STATUS_RESTARTING, disconnect_time, now + restart_hold_s
        return STATUS_CONNECTED, disconnect_time, restart_until

    # STATUS_DISCONNECTED
    if curr_devices:
        # Reconnect: check if we're inside the grace window
        if disconnect_time > 0.0 and (now - disconnect_time) <= restart_grace_s:
            return STATUS_RESTARTING, disconnect_time, now + restart_hold_s
        return STATUS_CONNECTED, disconnect_time, restart_until
    return STATUS_DISCONNECTED, disconnect_time, restart_until


# ── ROS2 node ──────────────────────────────────────────────────────────────────

class CameraHotplugNode(Node):
    """Monitors /dev/video* and publishes camera connection status."""

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

        self.declare_parameter("video_glob",      "/dev/video*")
        self.declare_parameter("poll_rate",        2.0)
        self.declare_parameter("restart_grace_s",  5.0)
        self.declare_parameter("restart_hold_s",   2.0)
        self.declare_parameter("publish_always",   False)
        self.declare_parameter("output_topic",     "/saltybot/camera_status")

        self._glob         = self.get_parameter("video_glob").value
        poll_rate          = float(self.get_parameter("poll_rate").value)
        self._grace_s      = float(self.get_parameter("restart_grace_s").value)
        self._hold_s       = float(self.get_parameter("restart_hold_s").value)
        self._pub_always   = bool(self.get_parameter("publish_always").value)
        output_topic       = self.get_parameter("output_topic").value

        # Injected scan function — allows unit-test mocking
        self._scan_fn = scan_video_devices

        # Perform initial scan (before any lock or timer)
        initial = self._scan_fn(self._glob)
        self._state          = STATUS_CONNECTED if initial else STATUS_DISCONNECTED
        self._prev_devices   = initial
        self._disconnect_time: float = 0.0
        self._restart_until:  float  = 0.0
        self._lock = threading.Lock()

        qos = QoSProfile(depth=10)
        self._pub   = self.create_publisher(String, output_topic, qos)
        self._timer = self.create_timer(1.0 / poll_rate, self._scan_cb)

        # Publish initial state
        self._publish(self._state)

        n_dev = len(initial)
        self.get_logger().info(
            f"CameraHotplugNode ready — initial state: {self._state} "
            f"({n_dev} device(s): {sorted(initial)})"
        )

    # ── Scan callback ───────────────────────────────────────────────────

    def _scan_cb(self) -> None:
        now  = time.monotonic()
        curr = self._scan_fn(self._glob)

        with self._lock:
            prev   = self._prev_devices
            state  = self._state
            disc_t = self._disconnect_time
            rup    = self._restart_until

        new_state, new_disc_t, new_rup = compute_transition(
            state, prev, curr, now, disc_t, rup,
            self._grace_s, self._hold_s,
        )

        changed = new_state != state

        with self._lock:
            self._prev_devices    = curr
            self._state           = new_state
            self._disconnect_time = new_disc_t
            self._restart_until   = new_rup

        if changed:
            self.get_logger().info(
                f"CameraHotplug: {state!r} → {new_state!r} "
                f"(devices: {sorted(curr) or 'none'})"
            )

        if changed or self._pub_always:
            self._publish(new_state)

    # ── Publish ─────────────────────────────────────────────────────────

    def _publish(self, status: str) -> None:
        msg = String()
        msg.data = status
        self._pub.publish(msg)

    # ── Public accessors (for tests / introspection) ─────────────────────

    @property
    def state(self) -> str:
        with self._lock:
            return self._state

    @property
    def devices(self) -> FrozenSet[str]:
        with self._lock:
            return self._prev_devices


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