"""
surround_vision.launch.py — 360° surround vision pipeline

Launches:
  1. saltybot_cameras/csi_cameras.launch.py  — IMX219 V4L2 camera drivers
     (skip if cameras already running from a separate container)
  2. surround_costmap_node  — obstacle PointCloud2 for Nav2
  3. surround_vision_node   — 360° bird's-eye image for visualisation

Arguments:
  start_cameras  (default true)  — include csi_cameras.launch.py
                                    set false if cameras run in separate process
  camera_height  (default 0.30)  — sensor head height above floor (m)
  camera_pitch   (default 0.0)   — camera tilt-down angle in degrees
  max_range      (default 3.0)   — max obstacle detection range (m)
  publish_rate   (default 5.0)   — costmap + vision publish frequency (Hz)
"""

import os
from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import (
    DeclareLaunchArgument,
    IncludeLaunchDescription,
    OpaqueFunction,
)
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def generate_launch_description():
    bringup_dir = get_package_share_directory('saltybot_surround')
    cameras_dir = get_package_share_directory('saltybot_cameras')

    params_file = os.path.join(bringup_dir, 'config', 'surround_vision_params.yaml')

    # ── Launch arguments ─────────────────────────────────────────────────────
    args = [
        DeclareLaunchArgument('start_cameras',  default_value='true'),
        DeclareLaunchArgument('camera_height',  default_value='0.30'),
        DeclareLaunchArgument('camera_pitch',   default_value='0.0'),
        DeclareLaunchArgument('max_range',      default_value='3.0'),
        DeclareLaunchArgument('publish_rate',   default_value='5.0'),
    ]

    # ── Camera drivers (optional — skip if already running) ──────────────────
    csi_cameras = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(cameras_dir, 'launch', 'csi_cameras.launch.py')
        ),
        launch_arguments={'width': '640', 'height': '480', 'fps': '30'}.items(),
        condition=IfCondition(LaunchConfiguration('start_cameras')),
    )

    # ── surround_costmap_node ─────────────────────────────────────────────────
    costmap_node = Node(
        package='saltybot_surround',
        executable='surround_costmap_node',
        name='surround_costmap',
        output='screen',
        parameters=[
            params_file,
            {
                'camera_height':      LaunchConfiguration('camera_height'),
                'camera_pitch_deg':   LaunchConfiguration('camera_pitch'),
                'max_obstacle_range': LaunchConfiguration('max_range'),
                'publish_rate':       LaunchConfiguration('publish_rate'),
            },
        ],
    )

    # ── surround_vision_node ──────────────────────────────────────────────────
    vision_node = Node(
        package='saltybot_surround',
        executable='surround_vision_node',
        name='surround_vision',
        output='screen',
        parameters=[
            params_file,
            {
                'camera_height':   LaunchConfiguration('camera_height'),
                'camera_pitch_deg': LaunchConfiguration('camera_pitch'),
                'ipm_range_m':     LaunchConfiguration('max_range'),
                'publish_rate':    LaunchConfiguration('publish_rate'),
            },
        ],
    )

    return LaunchDescription([*args, csi_cameras, costmap_node, vision_node])
