"""
person_detection.launch.py — Launch person detection node with config.

Usage:
    ros2 launch saltybot_perception person_detection.launch.py

    # Override engine path:
    ros2 launch saltybot_perception person_detection.launch.py \\
        engine_path:=/mnt/nvme/saltybot/models/yolov8n.engine

    # Use ONNX fallback (dev/CI):
    ros2 launch saltybot_perception person_detection.launch.py \\
        onnx_path:=/mnt/nvme/saltybot/models/yolov8n.onnx

    # Enable debug image stream:
    ros2 launch saltybot_perception person_detection.launch.py \\
        publish_debug_image:=true

Prerequisites:
  - RealSense D435i node running and publishing:
      /camera/color/image_raw
      /camera/depth/image_rect_raw
      /camera/color/camera_info
  - TF tree containing base_link ← camera_color_optical_frame
  - YOLOv8n TensorRT engine (build with scripts/build_trt_engine.py)
"""

import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory


def generate_launch_description():
    pkg_dir = get_package_share_directory('saltybot_perception')
    default_config = os.path.join(pkg_dir, 'config', 'person_detection_params.yaml')
    default_engine = '/mnt/nvme/saltybot/models/yolov8n.engine'
    default_onnx = '/mnt/nvme/saltybot/models/yolov8n.onnx'

    return LaunchDescription([
        # ── Launch arguments ───────────────────────────────────────────────
        DeclareLaunchArgument(
            'engine_path',
            default_value=default_engine,
            description='Path to TensorRT .engine file (built by build_trt_engine.py)',
        ),
        DeclareLaunchArgument(
            'onnx_path',
            default_value=default_onnx,
            description='Path to ONNX model (fallback when engine_path not found)',
        ),
        DeclareLaunchArgument(
            'publish_debug_image',
            default_value='false',
            description='Publish annotated debug image on /person/debug_image',
        ),
        DeclareLaunchArgument(
            'target_frame',
            default_value='base_link',
            description='TF frame for /person/target PoseStamped output',
        ),
        DeclareLaunchArgument(
            'confidence_threshold',
            default_value='0.4',
            description='Minimum YOLO detection confidence',
        ),
        DeclareLaunchArgument(
            'max_depth',
            default_value='5.0',
            description='Maximum person tracking distance in metres',
        ),

        # ── Person detector node ───────────────────────────────────────────
        Node(
            package='saltybot_perception',
            executable='person_detector',
            name='person_detector',
            output='screen',
            parameters=[
                default_config,
                {
                    'engine_path': LaunchConfiguration('engine_path'),
                    'onnx_path': LaunchConfiguration('onnx_path'),
                    'publish_debug_image': LaunchConfiguration('publish_debug_image'),
                    'target_frame': LaunchConfiguration('target_frame'),
                    'confidence_threshold': LaunchConfiguration('confidence_threshold'),
                    'max_depth': LaunchConfiguration('max_depth'),
                },
            ],
            remappings=[
                # Standard RealSense topic names — no remapping needed by default
                # ('/camera/color/image_raw', '/camera/color/image_raw'),
                # ('/camera/depth/image_rect_raw', '/camera/depth/image_rect_raw'),
            ],
        ),
    ])
