"""
light_monitor_node.py — Ambient light monitor for SaltyBot night vision.

Uses the front IMX219 RGB stream (or D435i infra1 as fallback) to estimate
ambient brightness via histogram analysis.  Publishes the current vision mode
and raw intensity on a 2 Hz digest timer.

Subscribes (any ONE active at a time):
  /camera/color/image_raw          sensor_msgs/Image   RGB from IMX219 front (primary)
  /camera/infra1/image_rect_raw    sensor_msgs/Image   IR fallback (mono8)

Publishes:
  /saltybot/ambient_light          std_msgs/Float32    mean grey intensity 0–255
  /saltybot/vision_mode            std_msgs/String     'day'|'twilight'|'night'|'ir_only'

Parameters:
  primary_camera_topic    str   '/camera/color/image_raw'
  fallback_ir_topic       str   '/camera/infra1/image_rect_raw'
  publish_hz              float  2.0
  initial_mode            str   'day'
  history_len             int    6
"""

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy

import numpy as np
import cv2
from cv_bridge import CvBridge

from sensor_msgs.msg import Image
from std_msgs.msg import Float32, String

from .light_analyser import LightAnalyser


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


class LightMonitorNode(Node):

    def __init__(self):
        super().__init__('light_monitor')

        self.declare_parameter('primary_camera_topic',  '/camera/color/image_raw')
        self.declare_parameter('fallback_ir_topic',     '/camera/infra1/image_rect_raw')
        self.declare_parameter('publish_hz',             2.0)
        self.declare_parameter('initial_mode',           'day')
        self.declare_parameter('history_len',             6)

        primary_topic  = self.get_parameter('primary_camera_topic').value
        fallback_topic = self.get_parameter('fallback_ir_topic').value
        pub_hz         = self.get_parameter('publish_hz').value
        initial_mode   = self.get_parameter('initial_mode').value
        history_len    = self.get_parameter('history_len').value

        self._bridge   = CvBridge()
        self._analyser = LightAnalyser(initial_mode=initial_mode, history_len=history_len)

        # Latest decoded frame (thread-safe in single-threaded executor)
        self._latest_frame: np.ndarray | None = None
        self._using_fallback = False

        # Subscriptions — primary preferred; fallback used when primary silent
        self.create_subscription(Image, primary_topic,  self._on_primary,  _SENSOR_QOS)
        self.create_subscription(Image, fallback_topic, self._on_fallback,  _SENSOR_QOS)

        self._mode_pub  = self.create_publisher(String,  '/saltybot/vision_mode',   10)
        self._light_pub = self.create_publisher(Float32, '/saltybot/ambient_light', 10)

        self.create_timer(1.0 / pub_hz, self._tick)

        self.get_logger().info(
            f'light_monitor ready — primary={primary_topic} fallback={fallback_topic}'
        )

    # ── Subscriptions ─────────────────────────────────────────────────────────

    def _on_primary(self, msg: Image) -> None:
        try:
            frame = self._bridge.imgmsg_to_cv2(msg, desired_encoding='passthrough')
            self._latest_frame  = frame
            self._using_fallback = False
        except Exception as exc:
            self.get_logger().warning(f'[light_monitor] primary decode: {exc}')

    def _on_fallback(self, msg: Image) -> None:
        # Only use fallback if no primary frame recently arrived
        if self._latest_frame is not None and not self._using_fallback:
            return   # primary is active
        try:
            frame = self._bridge.imgmsg_to_cv2(msg, desired_encoding='mono8')
            self._latest_frame  = frame
            self._using_fallback = True
        except Exception as exc:
            self.get_logger().warning(f'[light_monitor] fallback decode: {exc}')

    # ── Publish timer ─────────────────────────────────────────────────────────

    def _tick(self) -> None:
        if self._latest_frame is None:
            return   # no data yet

        # Subsample for speed (analyse quarter-resolution)
        frame = self._latest_frame
        if frame.shape[0] > 120:
            frame = frame[::4, ::4]

        mode, intensity = self._analyser.update(frame)

        msg_mode          = String()
        msg_mode.data     = mode
        self._mode_pub.publish(msg_mode)

        msg_light         = Float32()
        msg_light.data    = float(intensity)
        self._light_pub.publish(msg_light)

        if self._using_fallback:
            self.get_logger().debug(
                f'[light_monitor] IR fallback — mode={mode} intensity={intensity:.1f}'
            )


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


if __name__ == '__main__':
    main()
