"""
night_vision_controller_node.py — Hardware adaptation on vision mode changes.

Subscribes to /saltybot/vision_mode and drives:
  • D435i IR emitter (via realsense2_camera parameter service)
  • IMX219 gain / exposure (via v4l2-ctl subprocess)
  • GPIO headlight brightness

Mode → hardware mapping:
  'day'      → emitter=auto, IMX219 AE, headlight off
  'twilight' → emitter=on,   IMX219 boosted, headlight dim
  'night'    → emitter=on,   IMX219 max gain, headlight medium
  'ir_only'  → emitter=on,   IMX219 max gain, headlight full

Publishes:
  /saltybot/visual_odom_status  std_msgs/String  (JSON — reuses VO status topic)

Parameters:
  headlight_pin    int    33     (physical 40-pin board pin)
  day_headlight    float  0.0
  twilight_headlight float 0.25
  night_headlight  float  0.50
  ir_only_headlight float 1.0
"""

import json
import time

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

from std_msgs.msg import String

from .camera_controller import CameraController
from .gpio_headlight import GPIOHeadlight


_RELIABLE_QOS = QoSProfile(
    reliability=ReliabilityPolicy.RELIABLE,
    history=HistoryPolicy.KEEP_LAST,
    depth=10,
)


class NightVisionControllerNode(Node):

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

        self.declare_parameter('headlight_pin',       33)
        self.declare_parameter('day_headlight',        0.0)
        self.declare_parameter('twilight_headlight',   0.25)
        self.declare_parameter('night_headlight',      0.50)
        self.declare_parameter('ir_only_headlight',    1.0)

        pin = self.get_parameter('headlight_pin').value
        self._brightness_map = {
            'day':      self.get_parameter('day_headlight').value,
            'twilight': self.get_parameter('twilight_headlight').value,
            'night':    self.get_parameter('night_headlight').value,
            'ir_only':  self.get_parameter('ir_only_headlight').value,
        }

        self._cam_ctrl   = CameraController(self)
        self._headlight  = GPIOHeadlight(pin=pin)
        self._cur_mode: str | None = None
        self._mode_change_t: float = 0.0

        self.create_subscription(
            String, '/saltybot/vision_mode', self._on_mode, _RELIABLE_QOS
        )

        self._status_pub = self.create_publisher(
            String, '/saltybot/night_vision_status', 10
        )
        self.create_timer(5.0, self._status_tick)

        self.get_logger().info(
            f'night_vision_controller ready — headlight pin={pin}'
        )

    # ── Subscription ──────────────────────────────────────────────────────────

    def _on_mode(self, msg: String) -> None:
        mode = msg.data
        if mode == self._cur_mode:
            return

        prev = self._cur_mode
        self._cur_mode = mode
        self._mode_change_t = time.monotonic()

        # Apply camera profile (D435i emitter + IMX219 gain/exposure)
        self._cam_ctrl.set_profile(mode)

        # Apply headlight
        brightness = self._brightness_map.get(mode, 0.0)
        self._headlight.set_brightness(brightness)

        self.get_logger().info(
            f'[nv_ctrl] {prev} → {mode} | '
            f'headlight={brightness:.2f}'
        )

    # ── Status heartbeat ──────────────────────────────────────────────────────

    def _status_tick(self) -> None:
        status = {
            'mode':              self._cur_mode or 'unknown',
            'headlight':         round(self._headlight._brightness, 2),
            'mode_age_s':        round(time.monotonic() - self._mode_change_t, 1)
                                 if self._cur_mode else None,
        }
        msg      = String()
        msg.data = json.dumps(status)
        self._status_pub.publish(msg)

    # ── Cleanup ───────────────────────────────────────────────────────────────

    def destroy_node(self) -> None:
        self._headlight.cleanup()
        super().destroy_node()


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


if __name__ == '__main__':
    main()
