#!/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,
            "/oak/stereo/image_raw",
            self.callback,
            qos_profile_sensor_data,
#            10,
        )
        self.publisher = self.create_publisher(
            Image,
            "/oak/stereo/image_mono8",
            10,
        )

    def callback(self, msg):
        depth = self.bridge.imgmsg_to_cv2(
            msg, desired_encoding="passthrough"
        ).astype(np.float32)

        # Ignore invalid depth samples for determining display limits.
        valid = np.isfinite(depth) & (depth > 0)

        mono8 = np.zeros(depth.shape, dtype=np.uint8)

        if np.any(valid):
            # Robust automatic scaling: ignore extreme near/far outliers.
            low, high = np.percentile(depth[valid], [2, 98])

            if high > low:
                mono8[valid] = np.clip(
                    (depth[valid] - low) * 255.0 / (high - low),
                    0,
                    255,
                ).astype(np.uint8)

        output = self.bridge.cv2_to_imgmsg(mono8, 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()