"""
floor_classifier_node.py — Floor surface type classifier (Issue #249).

Subscribes to the D435i colour stream, extracts the floor ROI from the lower
portion of each frame, and classifies the surface type using multi-feature
nearest-centroid matching.  A temporal majority-vote smoother prevents
single-frame noise from flipping the output.

Subscribes:
  /camera/color/image_raw     sensor_msgs/Image   (D435i colour, BEST_EFFORT)

Publishes:
  /saltybot/floor_type        std_msgs/String     (floor label, 2 Hz)

Parameters
----------
publish_hz        float  2.0   Publication rate (Hz)
roi_frac          float  0.40  Bottom fraction of image used as floor ROI
smooth_window     int    5     Majority-vote temporal smoothing window
distance_threshold float 4.0   Suppress publish if nearest-centroid distance
                                exceeds this value (low confidence; publishes
                                "unknown" instead)
"""

from __future__ import annotations

from typing import Optional

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 String

from ._floor_classifier import classify_floor_patch, LabelSmoother


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


class FloorClassifierNode(Node):

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

        self.declare_parameter('publish_hz',         2.0)
        self.declare_parameter('roi_frac',           0.40)
        self.declare_parameter('smooth_window',      5)
        self.declare_parameter('distance_threshold', 4.0)

        publish_hz        = self.get_parameter('publish_hz').value
        self._roi_frac    = self.get_parameter('roi_frac').value
        smooth_window     = int(self.get_parameter('smooth_window').value)
        self._dist_thresh = self.get_parameter('distance_threshold').value

        self._bridge   = CvBridge()
        self._smoother = LabelSmoother(window=smooth_window)
        self._latest_label: str = 'unknown'

        self._sub = self.create_subscription(
            Image,
            '/camera/color/image_raw',
            self._on_image,
            _SENSOR_QOS,
        )
        self._pub = self.create_publisher(String, '/saltybot/floor_type', 10)
        self.create_timer(1.0 / publish_hz, self._tick)

        self.get_logger().info(
            f'floor_classifier_node ready — '
            f'roi={self._roi_frac:.0%} smooth={smooth_window} hz={publish_hz}'
        )

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

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

        result = classify_floor_patch(bgr, roi_frac=self._roi_frac)

        min_dist = min(result.distances.values())
        raw_label = result.label if min_dist <= self._dist_thresh else 'unknown'
        self._latest_label = self._smoother.update(raw_label)

    # ── 2 Hz publish tick ─────────────────────────────────────────────────────

    def _tick(self) -> None:
        msg = String()
        msg.data = self._latest_label
        self._pub.publish(msg)


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


if __name__ == '__main__':
    main()
