"""
qr_reader_node.py — QR code reader on CSI surround camera frames (Issue #233).

Subscribes to all four IMX219 CSI camera streams, runs cv2.QRCodeDetector
on each frame, deduplicates detections with a per-data cooldown, and
publishes at up to 10 Hz.

Subscribes (BEST_EFFORT):
  /camera/front/image_raw    sensor_msgs/Image
  /camera/left/image_raw     sensor_msgs/Image
  /camera/rear/image_raw     sensor_msgs/Image
  /camera/right/image_raw    sensor_msgs/Image

Publishes:
  /saltybot/qr_codes         saltybot_scene_msgs/QRDetectionArray  (≤10 Hz)

Algorithm
---------
1. Independent callbacks for each camera decode frames with QRCodeDetector.
2. Any newly decoded QR payload is checked against the dedup cache:
     • If the same payload was published within `dedup_cooldown_s`, skip.
3. A 10 Hz timer assembles all pending (deduplicated) detections and publishes
   them in a single QRDetectionArray message, then clears the pending queue.

Parameters
----------
dedup_cooldown_s  float  2.0  Minimum seconds between re-publishing the same QR data
publish_hz        float  10.0 Publication rate (Hz)
cameras           str    "front,left,rear,right"  Comma-separated camera names to subscribe
"""

from __future__ import annotations

import time
from threading import Lock
from typing import Dict, List

import numpy as np

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

from cv_bridge import CvBridge

from sensor_msgs.msg import Image
from saltybot_scene_msgs.msg import QRDetection, QRDetectionArray

from ._qr_detector import detect_qr, deduplicate


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


class QRReaderNode(Node):

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

        self.declare_parameter('dedup_cooldown_s', 2.0)
        self.declare_parameter('publish_hz',       10.0)
        self.declare_parameter('cameras',          'front,left,rear,right')

        self._cooldown   = self.get_parameter('dedup_cooldown_s').value
        publish_hz       = self.get_parameter('publish_hz').value
        cameras_param    = self.get_parameter('cameras').value

        self._bridge = CvBridge()

        # Dedup cache: {data: last_published_monotonic}
        self._dedup_cache: Dict[str, float] = {}

        # Pending queue (deduplicated results waiting for the next tick)
        self._pending: List[QRDetection] = []
        self._lock = Lock()

        # Subscribe to each requested camera
        camera_names = [c.strip() for c in cameras_param.split(',') if c.strip()]
        for cam in camera_names:
            topic = f'/camera/{cam}/image_raw'
            self.create_subscription(
                Image, topic,
                lambda msg, c=cam: self._on_image(msg, c),
                _SENSOR_QOS,
            )
            self.get_logger().info(f'Subscribed to {topic}')

        # Publisher + tick timer
        self._pub = self.create_publisher(QRDetectionArray, '/saltybot/qr_codes', 10)
        self.create_timer(1.0 / publish_hz, self._tick)

        self.get_logger().info(
            f'qr_reader_node ready — cameras={camera_names} '
            f'cooldown={self._cooldown}s hz={publish_hz}'
        )

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

    def _on_image(self, msg: Image, camera_name: str) -> None:
        try:
            bgr = self._bridge.imgmsg_to_cv2(msg, 'bgr8')
        except Exception as exc:
            self.get_logger().error(
                f'cv_bridge [{camera_name}]: {exc}', throttle_duration_sec=5.0)
            return

        raw_results = detect_qr(bgr)
        if not raw_results:
            return

        now = time.monotonic()
        with self._lock:
            fresh, self._dedup_cache = deduplicate(
                raw_results, self._dedup_cache, self._cooldown, now=now)

            for r in fresh:
                det = QRDetection()
                det.header.stamp    = msg.header.stamp
                det.header.frame_id = msg.header.frame_id
                det.data            = r.data
                det.camera          = camera_name
                det.corners         = [float(v) for v in r.corners]
                det.center.x        = r.cx
                det.center.y        = r.cy
                det.center.z        = 0.0
                self._pending.append(det)

    # ── 10 Hz publish tick ────────────────────────────────────────────────────

    def _tick(self) -> None:
        with self._lock:
            pending, self._pending = self._pending, []

        msg = QRDetectionArray()
        msg.header.stamp    = self.get_clock().now().to_msg()
        msg.header.frame_id = ''
        msg.detections      = pending
        self._pub.publish(msg)


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


if __name__ == '__main__':
    main()
