"""
sky_detect_node.py — D435i sky detector for outdoor navigation (Issue #307).

Classifies the top portion of the D435i colour image as sky vs non-sky using
HSV blue/grey thresholding, then estimates the horizon line.

Useful for:
  - Outdoor/indoor scene detection (sky_fraction > 0.1 → likely outdoor)
  - Camera tilt correction (horizon_y deviation from expected row → tilt estimate)
  - Disabling outdoor-only nodes (colour correction, sun glare filter) indoors

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

Publishes:
  /saltybot/sky_fraction      std_msgs/Float32    sky fraction in [0, 1]  (per frame)
  /saltybot/horizon_y         std_msgs/Int32      horizon pixel row; -1 = no sky

Parameters
----------
scan_frac       float   0.60    Top fraction of image analysed for sky_fraction
row_threshold   float   0.30    Per-row sky fraction required to count a row as sky
"""

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, Int32

from ._sky_detector import detect_sky


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


class SkyDetectNode(Node):

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

        self.declare_parameter('scan_frac',     0.60)
        self.declare_parameter('row_threshold', 0.30)

        self._scan_frac     = float(self.get_parameter('scan_frac').value)
        self._row_threshold = float(self.get_parameter('row_threshold').value)

        self._bridge = CvBridge()

        self._sub = self.create_subscription(
            Image, '/camera/color/image_raw', self._on_image, _SENSOR_QOS)

        self._pub_frac    = self.create_publisher(Float32, '/saltybot/sky_fraction', 10)
        self._pub_horizon = self.create_publisher(Int32,   '/saltybot/horizon_y',    10)

        self.get_logger().info(
            f'sky_detect_node ready — scan_frac={self._scan_frac} '
            f'row_threshold={self._row_threshold}'
        )

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

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

        result = detect_sky(
            bgr,
            scan_frac=self._scan_frac,
            row_threshold=self._row_threshold,
        )

        frac_msg = Float32()
        frac_msg.data = result.sky_fraction
        self._pub_frac.publish(frac_msg)

        hz_msg = Int32()
        hz_msg.data = result.horizon_y
        self._pub_horizon.publish(hz_msg)


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


if __name__ == '__main__':
    main()
