"""
terrain_rough_node.py — D435i terrain roughness estimator (Issue #296).

Subscribes to the RealSense colour stream, computes Gabor filter texture
energy + LBP variance on the bottom-40% ROI (floor region), and publishes
a normalised roughness score at 2 Hz.

Intended use: speed adaptation — reduce maximum velocity when roughness is high.

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

Publishes:
  /saltybot/terrain_roughness   std_msgs/Float32   roughness in [0,1] (0=smooth, 1=rough)

Parameters
----------
roi_frac    float   0.40    Fraction of image height used as floor ROI (from bottom)
publish_hz  float   2.0     Publication rate (Hz)
"""

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 Float32

from ._terrain_roughness import estimate_roughness


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


class TerrainRoughNode(Node):

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

        self.declare_parameter('roi_frac',   0.40)
        self.declare_parameter('publish_hz', 2.0)

        self._roi_frac   = float(self.get_parameter('roi_frac').value)
        publish_hz       = float(self.get_parameter('publish_hz').value)

        self._bridge      = CvBridge()
        self._latest_bgr  = None   # updated by subscription callback

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

        period = 1.0 / max(publish_hz, 0.1)
        self._timer = self.create_timer(period, self._on_timer)

        self.get_logger().info(
            f'terrain_rough_node ready — roi_frac={self._roi_frac} '
            f'publish_hz={publish_hz}'
        )

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

    def _on_image(self, msg: Image) -> None:
        try:
            self._latest_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)

    def _on_timer(self) -> None:
        if self._latest_bgr is None:
            return

        result = estimate_roughness(self._latest_bgr, roi_frac=self._roi_frac)

        msg = Float32()
        msg.data = result.roughness
        self._pub.publish(msg)


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


if __name__ == '__main__':
    main()
