"""
depth_hole_fill_node.py — D435i depth image hole filler (Issue #268).

Subscribes to the raw D435i depth stream, fills zero/NaN pixels using
multi-pass spatial-Gaussian bilateral interpolation, and republishes the
filled image at camera rate.

Subscribes (BEST_EFFORT):
  /camera/depth/image_rect_raw     sensor_msgs/Image  float32 depth (m)

Publishes:
  /camera/depth/filled             sensor_msgs/Image  float32 depth (m), holes filled

The filled image preserves all original valid pixels exactly and only
modifies pixels that had no return (0 or NaN).  The output is suitable
for all downstream consumers that expect a dense depth map (VO, RTAB-Map,
collision avoidance, floor classifier).

Parameters
----------
input_topic    str    /camera/depth/image_rect_raw   Input depth topic
output_topic   str    /camera/depth/filled           Output depth topic
kernel_size    int    5      Initial Gaussian kernel side length (pixels)
d_min          float  0.1    Minimum valid depth (m)
d_max          float  10.0   Maximum valid depth (m)
max_passes     int    3      Fill passes (growing kernel per pass)
"""

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 ._depth_hole_fill import fill_holes


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


class DepthHoleFillNode(Node):

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

        self.declare_parameter('input_topic',  '/camera/depth/image_rect_raw')
        self.declare_parameter('output_topic', '/camera/depth/filled')
        self.declare_parameter('kernel_size',  5)
        self.declare_parameter('d_min',        0.1)
        self.declare_parameter('d_max',        10.0)
        self.declare_parameter('max_passes',   3)

        input_topic   = self.get_parameter('input_topic').value
        output_topic  = self.get_parameter('output_topic').value
        self._ks      = int(self.get_parameter('kernel_size').value)
        self._d_min   = self.get_parameter('d_min').value
        self._d_max   = self.get_parameter('d_max').value
        self._passes  = int(self.get_parameter('max_passes').value)

        self._bridge = CvBridge()

        self._sub = self.create_subscription(
            Image, input_topic, self._on_depth, _SENSOR_QOS)
        self._pub = self.create_publisher(Image, output_topic, 10)

        self.get_logger().info(
            f'depth_hole_fill_node ready — '
            f'{input_topic} → {output_topic} '
            f'kernel={self._ks} passes={self._passes} '
            f'd=[{self._d_min},{self._d_max}]m'
        )

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

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

        depth = depth.astype(np.float32)

        # Handle uint16 mm → float32 m conversion (D435i raw stream)
        if depth.max() > 100.0:
            depth /= 1000.0

        filled = fill_holes(
            depth,
            kernel_size=self._ks,
            d_min=self._d_min,
            d_max=self._d_max,
            max_passes=self._passes,
        )

        try:
            out_msg = self._bridge.cv2_to_imgmsg(filled, encoding='32FC1')
        except Exception as exc:
            self.get_logger().error(
                f'cv2_to_imgmsg: {exc}', throttle_duration_sec=5.0)
            return

        out_msg.header = msg.header
        self._pub.publish(out_msg)


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


if __name__ == '__main__':
    main()
