"""
csi_cameras.launch.py — 4× IMX219 CSI surround camera launch

Hardware: Jetson Orin Nano Super
  CSI-A (connector 0): Camera 0 (front) + Camera 1 (left) via multi-camera adapter
  CSI-B (connector 1): Camera 2 (rear)  + Camera 3 (right) via multi-camera adapter

V4L2 device assignment (with ArduCam multi-camera adapter or equivalent):
  /dev/video0 → front   (sensor_id=0, CSI-A, cam-select=0)
  /dev/video2 → left    (sensor_id=1, CSI-A, cam-select=1)
  /dev/video4 → rear    (sensor_id=2, CSI-B, cam-select=0)
  /dev/video6 → right   (sensor_id=3, CSI-B, cam-select=1)

Topics published per camera:
  /camera/<name>/image_raw          sensor_msgs/Image      30Hz
  /camera/<name>/camera_info        sensor_msgs/CameraInfo 30Hz

TF frames:
  camera_front_link, camera_left_link, camera_rear_link, camera_right_link
  (all children of sensor_head_link — see camera_tf.launch.py)

Usage:
  ros2 launch saltybot_cameras csi_cameras.launch.py
  ros2 launch saltybot_cameras csi_cameras.launch.py width:=1280 height:=720 fps:=15
"""

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, GroupAction
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node, PushRosNamespace


_CAMERAS = [
    {"name": "front", "device": "/dev/video0", "frame_id": "camera_front_link"},
    {"name": "left",  "device": "/dev/video2", "frame_id": "camera_left_link"},
    {"name": "rear",  "device": "/dev/video4", "frame_id": "camera_rear_link"},
    {"name": "right", "device": "/dev/video6", "frame_id": "camera_right_link"},
]


def generate_launch_description():
    width_arg  = DeclareLaunchArgument("width",  default_value="640")
    height_arg = DeclareLaunchArgument("height", default_value="480")
    fps_arg    = DeclareLaunchArgument("fps",    default_value="30")
    # Pixel format for IMX219 via V4L2: YUYV or MJPG
    # On Orin with Argus ISP path: use 'RG10' (raw Bayer) or let V4L2 negotiate
    fmt_arg    = DeclareLaunchArgument(
        "pixel_format", default_value="YUYV",
        description="V4L2 pixel format: YUYV | MJPG | RG10"
    )

    nodes = [width_arg, height_arg, fps_arg, fmt_arg]

    for cam in _CAMERAS:
        group = GroupAction([
            PushRosNamespace(f"camera/{cam['name']}"),
            Node(
                package="v4l2_camera",
                executable="v4l2_camera_node",
                name=f"cam_{cam['name']}",
                output="screen",
                parameters=[{
                    "video_device":   cam["device"],
                    "image_size":     [
                        LaunchConfiguration("width"),
                        LaunchConfiguration("height"),
                    ],
                    "time_per_frame": [1, LaunchConfiguration("fps")],
                    "pixel_format":   LaunchConfiguration("pixel_format"),
                    "camera_frame_id": cam["frame_id"],
                    "camera_name":    cam["name"],
                    # Disable auto-exposure for consistent surround-view balance
                    "brightness":     0,
                    "auto_exposure":  1,   # 1 = manual on most V4L2 drivers
                }],
                # Remap within namespace so topic is /camera/<name>/image_raw
                remappings=[("image_raw", "image_raw")],
            ),
        ])
        nodes.append(group)

    # Include static TF for all cameras
    from launch.actions import IncludeLaunchDescription
    from launch.launch_description_sources import PythonLaunchDescriptionSource
    import os
    tf_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            os.path.join(
                os.path.dirname(os.path.realpath(__file__)),
                "camera_tf.launch.py",
            )
        ])
    )
    nodes.append(tf_launch)

    return LaunchDescription(nodes)
