"""
camera_health_node.py — Camera watchdog for all SaltyBot image streams.

Monitors frame arrival timestamps for each configured camera topic.
On silence:
  > warn_timeout_s  (default 2s)  → WARNING log
  > error_timeout_s (default 10s) → ERROR log + v4l2 reset attempt (CSI only)

Publishes /saltybot/camera_health as std_msgs/String JSON at 1 Hz:
  {
    "ts":  "2026-03-02T15:00:00Z",
    "cameras": {
      "d435i_color": {
        "topic":      "/camera/color/image_raw",
        "status":     "ok" | "warn" | "error" | "never",
        "age_s":      1.23,
        "fps":        29.8,
        "total_frames": 1234
      },
      ...
    },
    "healthy": 5,
    "degraded": 1,
    "total": 6
  }

CSI camera map (topic → /dev/videoN) is used for v4l2 reset.
D435i streams are USB — no v4l2 reset (RealSense SDK manages recovery).

Parameters:
  warn_timeout_s   float  2.0
  error_timeout_s  float  10.0
  publish_hz       float  1.0
  fps_window_s     float  5.0   rolling window for FPS estimate
"""

from __future__ import annotations

import json
import subprocess
from datetime import datetime, timezone
from typing import Dict

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

from sensor_msgs.msg import Image
from std_msgs.msg import String

from ._camera_state import CameraState


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

# Default cameras: name → (topic, csi_device_or_None)
_DEFAULT_CAMERAS: Dict[str, tuple] = {
    'd435i_color': ('/camera/color/image_raw',      None),
    'd435i_depth': ('/camera/depth/image_rect_raw', None),
    'csi_front':   ('/camera/front/image_raw',      '/dev/video0'),
    'csi_right':   ('/camera/right/image_raw',      '/dev/video2'),
    'csi_rear':    ('/camera/rear/image_raw',        '/dev/video4'),
    'csi_left':    ('/camera/left/image_raw',        '/dev/video6'),
}


class CameraHealthNode(Node):

    def __init__(self):
        super().__init__('camera_health_monitor')

        self.declare_parameter('warn_timeout_s',  2.0)
        self.declare_parameter('error_timeout_s', 10.0)
        self.declare_parameter('publish_hz',       1.0)
        self.declare_parameter('fps_window_s',     5.0)

        self._warn_thr  = self.get_parameter('warn_timeout_s').value
        self._error_thr = self.get_parameter('error_timeout_s').value
        self._fps_win   = self.get_parameter('fps_window_s').value

        # Build camera states
        self._cameras: Dict[str, CameraState] = {
            name: CameraState(topic=topic, v4l2_dev=dev)
            for name, (topic, dev) in _DEFAULT_CAMERAS.items()
        }

        # Subscribe to each topic
        for name, cam in self._cameras.items():
            self.create_subscription(
                Image, cam.topic,
                self._make_cb(name),
                _SENSOR_QOS,
            )

        self._pub = self.create_publisher(String, '/saltybot/camera_health', 10)
        self.create_timer(
            1.0 / self.get_parameter('publish_hz').value, self._tick
        )

        self.get_logger().info(
            f'camera_health_monitor ready — watching {len(self._cameras)} cameras '
            f'warn={self._warn_thr}s error={self._error_thr}s'
        )

    # ── Callbacks ─────────────────────────────────────────────────────────────

    def _make_cb(self, name: str):
        def _cb(msg: Image) -> None:
            self._cameras[name].on_frame()
        return _cb

    # ── 1 Hz watchdog tick ────────────────────────────────────────────────────

    def _tick(self) -> None:
        healthy = degraded = 0

        for name, cam in self._cameras.items():
            status = cam.status(self._warn_thr, self._error_thr)

            if status == 'ok':
                healthy += 1
            elif status in ('warn', 'error', 'never'):
                degraded += 1

            age = cam.age_s
            age_str = f'{age:.1f}' if age != float('inf') else '∞'

            if status == 'warn':
                self.get_logger().warning(
                    f'[cam_health] {name} ({cam.topic}): '
                    f'no frame for {age_str}s'
                )
            elif status in ('error', 'never'):
                self.get_logger().error(
                    f'[cam_health] {name} ({cam.topic}): '
                    f'no frame for {age_str}s — camera may be offline'
                )
                if status == 'error' and cam.should_reset():
                    self._v4l2_reset(name, cam)

        payload = self._build_json(healthy, degraded)
        msg = String()
        msg.data = payload
        self._pub.publish(msg)

    # ── v4l2 reset (CSI cameras only) ─────────────────────────────────────────

    def _v4l2_reset(self, name: str, cam: CameraState) -> None:
        dev = cam.v4l2_dev
        self.get_logger().warning(
            f'[cam_health] attempting v4l2 reset for {name} on {dev}'
        )
        cam.mark_reset()
        try:
            # Toggle streaming off then on to nudge the driver
            subprocess.run(
                ['v4l2-ctl', '-d', dev, '--stream-off'],
                capture_output=True, timeout=2.0
            )
            subprocess.run(
                ['v4l2-ctl', '-d', dev, '--stream-on'],
                capture_output=True, timeout=2.0
            )
            self.get_logger().info(
                f'[cam_health] v4l2 reset sent to {dev} for {name}'
            )
        except (subprocess.TimeoutExpired, FileNotFoundError) as exc:
            self.get_logger().warning(
                f'[cam_health] v4l2 reset failed for {name}: {exc}'
            )

    # ── JSON builder ──────────────────────────────────────────────────────────

    def _build_json(self, healthy: int, degraded: int) -> str:
        cameras_out = {}
        for name, cam in self._cameras.items():
            age = cam.age_s
            cameras_out[name] = {
                'topic':        cam.topic,
                'status':       cam.status(self._warn_thr, self._error_thr),
                'age_s':        round(age, 2) if age != float('inf') else None,
                'fps':          round(cam.fps(self._fps_win), 1),
                'total_frames': cam.total_frames,
            }

        return json.dumps({
            'ts':       datetime.now(timezone.utc).strftime('%Y-%m-%dT%H:%M:%SZ'),
            'cameras':  cameras_out,
            'healthy':  healthy,
            'degraded': degraded,
            'total':    len(self._cameras),
        })


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


if __name__ == '__main__':
    main()
