"""
color_segment_node.py — D435i HSV color object segmenter (Issue #274).

Subscribes to the RealSense colour stream, applies per-color HSV thresholding,
extracts contours, and publishes detected blobs as ColorDetectionArray.

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

Publishes:
  /saltybot/color_objects     saltybot_scene_msgs/ColorDetectionArray

Parameters
----------
active_colors       str    "red,green,blue,yellow,orange"  Comma-separated list
min_area_px         float  200.0   Minimum contour area (pixels²)
max_blobs_per_color int    10      Max detections per color per frame
"""

from __future__ import annotations

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

import numpy as np
from cv_bridge import CvBridge

from sensor_msgs.msg import Image
from std_msgs.msg import Header

from saltybot_scene_msgs.msg import ColorDetection, ColorDetectionArray
from vision_msgs.msg import BoundingBox2D
from geometry_msgs.msg import Pose2D

from ._color_segmenter import find_color_blobs


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

_DEFAULT_COLORS = 'red,green,blue,yellow,orange'


class ColorSegmentNode(Node):

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

        self.declare_parameter('active_colors',       _DEFAULT_COLORS)
        self.declare_parameter('min_area_px',         200.0)
        self.declare_parameter('max_blobs_per_color', 10)

        colors_str = self.get_parameter('active_colors').value
        self._active_colors = [c.strip() for c in colors_str.split(',') if c.strip()]
        self._min_area = float(self.get_parameter('min_area_px').value)
        self._max_blobs = int(self.get_parameter('max_blobs_per_color').value)

        self._bridge = CvBridge()

        self._sub = self.create_subscription(
            Image, '/camera/color/image_raw', self._on_image, _SENSOR_QOS)
        self._pub = self.create_publisher(
            ColorDetectionArray, '/saltybot/color_objects', 10)

        self.get_logger().info(
            f'color_segment_node ready — colors={self._active_colors} '
            f'min_area={self._min_area}px² max_blobs={self._max_blobs}'
        )

    # ── 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

        blobs = find_color_blobs(
            bgr,
            active_colors=self._active_colors,
            min_area_px=self._min_area,
            max_blobs_per_color=self._max_blobs,
        )

        arr = ColorDetectionArray()
        arr.header = msg.header

        for blob in blobs:
            det = ColorDetection()
            det.header = msg.header
            det.color_name  = blob.color_name
            det.confidence  = blob.confidence
            det.area_px     = blob.area_px
            det.contour_id  = blob.contour_id

            bbox = BoundingBox2D()
            center = Pose2D()
            center.x = blob.cx
            center.y = blob.cy
            bbox.center    = center
            bbox.size_x    = blob.w
            bbox.size_y    = blob.h
            det.bbox = bbox

            arr.detections.append(det)

        self._pub.publish(arr)


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


if __name__ == '__main__':
    main()
