"""
calibration_capture.launch.py — Calibration workflow launcher for saltybot IMX219 cameras.

Full calibration workflow:
  Step 1: Start cameras (requires saltybot_cameras package to be running)
  Step 2: Run capture_calibration_images.py for each camera (30 images each)
  Step 3: Run calibrate_camera.py for each camera
  Step 4: Run calibrate_extrinsics.py
  Step 5: Run generate_static_transforms.py

This launch file starts the capture script for one camera via ExecuteProcess.
For full workflow, run each step manually (see jetson/calibration/README.md).

Usage:
  ros2 launch saltybot_calibration calibration_capture.launch.py camera:=front
  ros2 launch saltybot_calibration calibration_capture.launch.py camera:=all

Arguments:
  camera        Camera to capture (front/right/rear/left/all, default: front)
  n_images      Images to capture per camera (default: 30)
  output_dir    Output directory (default: /mnt/nvme/saltybot/calibration/raw)
  board_size    Checkerboard inner corners WxH (default: 8x6)
  square_size   Physical square size in metres (default: 0.025)
"""

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, ExecuteProcess, LogInfo
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare


def generate_launch_description():
    camera_arg = DeclareLaunchArgument(
        'camera', default_value='front',
        description='Camera to capture from: front/right/rear/left/all',
    )
    n_images_arg = DeclareLaunchArgument(
        'n_images', default_value='30',
        description='Number of calibration images to capture per camera',
    )
    output_dir_arg = DeclareLaunchArgument(
        'output_dir', default_value='/mnt/nvme/saltybot/calibration/raw',
        description='Root output directory for captured images',
    )
    board_size_arg = DeclareLaunchArgument(
        'board_size', default_value='8x6',
        description='Checkerboard inner corners as WxH',
    )
    square_size_arg = DeclareLaunchArgument(
        'square_size', default_value='0.025',
        description='Physical checkerboard square size in metres',
    )

    pkg_share = FindPackageShare('saltybot_calibration')
    capture_script = PathJoinSubstitution([
        pkg_share, '..', '..', '..', 'lib', 'saltybot_calibration',
        'capture_calibration_images.py',
    ])

    capture_process = ExecuteProcess(
        cmd=[
            'python3', capture_script,
            '--ros',
            '--camera', LaunchConfiguration('camera'),
            '--n-images', LaunchConfiguration('n_images'),
            '--output-dir', LaunchConfiguration('output_dir'),
            '--board-size', LaunchConfiguration('board_size'),
            '--square-size', LaunchConfiguration('square_size'),
        ],
        output='screen',
    )

    workflow_info = LogInfo(
        msg=[
            '\n',
            '=== IMX219 Calibration Workflow ===\n',
            'Step 1 (this launch): Capture checkerboard images\n',
            '  Controls: SPACE=capture  r=reset  q=done\n',
            '\n',
            'Step 2: Calibrate intrinsics per camera:\n',
            '  ros2 run saltybot_calibration calibrate_camera.py \\\n',
            '    --images-dir /mnt/nvme/saltybot/calibration/raw/cam_front \\\n',
            '    --output /mnt/nvme/saltybot/calibration/front/camera_info.yaml \\\n',
            '    --camera-name camera_front --image-size 640x480\n',
            '\n',
            'Step 3: Compute extrinsics:\n',
            '  ros2 run saltybot_calibration calibrate_extrinsics.py \\\n',
            '    --mode mechanical \\\n',
            '    --output /mnt/nvme/saltybot/calibration/extrinsics.yaml\n',
            '\n',
            'Step 4: Generate static transforms launch file:\n',
            '  ros2 run saltybot_calibration generate_static_transforms.py \\\n',
            '    --extrinsics /mnt/nvme/saltybot/calibration/extrinsics.yaml \\\n',
            '    --output ~/ros2_ws/src/saltybot_bringup/launch/static_transforms.launch.py\n',
            '\n',
            'See jetson/calibration/README.md for full instructions.\n',
        ]
    )

    return LaunchDescription([
        camera_arg,
        n_images_arg,
        output_dir_arg,
        board_size_arg,
        square_size_arg,
        workflow_info,
        capture_process,
    ])