#!/usr/bin/env python3

## from AI Chat.
import numpy as np

import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from sensor_msgs.msg import Image
from cv_bridge import CvBridge


class DepthToMono8(Node):
    def __init__(self):
        super().__init__("depth_to_mono8")

        self.bridge = CvBridge()
        self.min_depth_mm = 200
        self.max_depth_mm = 5000

        self.subscription = self.create_subscription(
            Image,
            "/obstacle_avoidance/debug/depth_image",
            self.callback,
            qos_profile_sensor_data,
        )
        self.publisher = self.create_publisher(
            Image,
            "/obstacle_avoidance/debug/depth_image_mono8",
            10,
        )

    def callback(self, msg):
        depth_mm = self.bridge.imgmsg_to_cv2(
            msg, desired_encoding="passthrough"
        )

        scaled = np.clip(
            (depth_mm.astype(np.float32) - self.min_depth_mm)
            * 255.0
            / (self.max_depth_mm - self.min_depth_mm),
            0,
            255,
        ).astype(np.uint8)

        # Mark invalid / zero-depth pixels as black.
        scaled[depth_mm == 0] = 0

        output = self.bridge.cv2_to_imgmsg(scaled, encoding="mono8")
        output.header = msg.header
        self.publisher.publish(output)


def main():
    rclpy.init()
    node = DepthToMono8()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()


if __name__ == "__main__":
    main()