"""
night_vision.launch.py — SaltyBot night vision stack.

Starts:
  light_monitor          — histogram ambient-light analysis → /saltybot/vision_mode
  night_vision_ctrl      — D435i emitter + IMX219 gain + GPIO headlight
  ir_slam_bridge         — IR→RGB republish for RTAB-Map (active in dark modes)

Launch args:
  primary_camera_topic   str    '/camera/color/image_raw'
  headlight_pin          int    '33'
  publish_hz             float  '2.0'
  active_ir_modes        str    'night,ir_only'

Verify:
  ros2 topic echo /saltybot/vision_mode
  ros2 topic echo /saltybot/ambient_light
  ros2 topic echo /saltybot/night_vision_status
  ros2 topic hz /saltybot/ir_bridge/image_raw     # should be ~30 Hz in dark
"""

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def generate_launch_description():
    args = [
        DeclareLaunchArgument('primary_camera_topic',
                              default_value='/camera/color/image_raw'),
        DeclareLaunchArgument('headlight_pin',  default_value='33'),
        DeclareLaunchArgument('publish_hz',     default_value='2.0'),
        DeclareLaunchArgument('active_ir_modes', default_value='night,ir_only'),
    ]

    light_monitor = Node(
        package='saltybot_night_vision',
        executable='light_monitor',
        name='light_monitor',
        output='screen',
        parameters=[{
            'primary_camera_topic': LaunchConfiguration('primary_camera_topic'),
            'publish_hz':           LaunchConfiguration('publish_hz'),
        }],
    )

    nv_ctrl = Node(
        package='saltybot_night_vision',
        executable='night_vision_ctrl',
        name='night_vision_controller',
        output='screen',
        parameters=[{
            'headlight_pin': LaunchConfiguration('headlight_pin'),
        }],
    )

    ir_bridge = Node(
        package='saltybot_night_vision',
        executable='ir_slam_bridge',
        name='ir_slam_bridge',
        output='screen',
        parameters=[{
            'active_modes': LaunchConfiguration('active_ir_modes'),
        }],
    )

    return LaunchDescription(args + [light_monitor, nv_ctrl, ir_bridge])
