"""
blur_detect_node.py — D435i motion blur detector (Issue #286).

Subscribes to the RealSense colour stream, evaluates the Laplacian variance
blur metric, and publishes a Bool flag plus a Float32 score.

Intended use: pause or de-weight SLAM keyframe insertion when the robot is
moving fast and the image is visibly blurred.

Subscribes (BEST_EFFORT):
  /camera/color/image_raw     sensor_msgs/Image   BGR8

Publishes:
  /saltybot/image_blurred     std_msgs/Bool       True  = blurred
  /saltybot/blur_score        std_msgs/Float32    Laplacian variance (higher = sharper)

Parameters
----------
threshold   float   100.0   Variance below which image is considered blurred
roi_frac    float   0.0     Fraction of image height to skip from top (0 = full frame)
"""

from __future__ import annotations

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 std_msgs.msg import Bool, Float32

from ._blur_detector import detect_blur


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


class BlurDetectNode(Node):

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

        self.declare_parameter('threshold', 100.0)
        self.declare_parameter('roi_frac',  0.0)

        self._threshold = float(self.get_parameter('threshold').value)
        self._roi_frac  = float(self.get_parameter('roi_frac').value)

        self._bridge = CvBridge()

        self._sub = self.create_subscription(
            Image, '/camera/color/image_raw', self._on_image, _SENSOR_QOS)
        self._pub_flag  = self.create_publisher(Bool,    '/saltybot/image_blurred', 10)
        self._pub_score = self.create_publisher(Float32, '/saltybot/blur_score',    10)

        self.get_logger().info(
            f'blur_detect_node ready — threshold={self._threshold} '
            f'roi_frac={self._roi_frac}'
        )

    # ── Callback ──────────────────────────────────────────────────────────────

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

        roi = None
        if self._roi_frac > 0.0:
            h = bgr.shape[0]
            r0 = int(h * min(self._roi_frac, 0.9))
            roi = (r0, h, 0, bgr.shape[1])

        result = detect_blur(bgr, threshold=self._threshold, roi=roi)

        flag_msg = Bool()
        flag_msg.data = result.is_blurred
        self._pub_flag.publish(flag_msg)

        score_msg = Float32()
        score_msg.data = float(result.variance)
        self._pub_score.publish(score_msg)


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


if __name__ == '__main__':
    main()
