"""
scene_understanding.launch.py — Semantic scene understanding stack.

Starts:
  scene_detector      — YOLOv8-nano TRT + room classifier + hazard detection
  behavior_adapter    — speed/personality hints from room + hazards
  costmap_publisher   — obstacle PointCloud2 for Nav2

Launch args:
  engine_path       str   '/config/yolov8n_scene.engine'
  onnx_path         str   '/config/yolov8n_scene.onnx'
  conf_threshold    float '0.35'
  publish_debug     bool  'false'
  publish_markers   bool  'true'

Verify:
  ros2 topic hz /social/scene/objects          # ~15 Hz
  ros2 topic hz /social/scene/room_type        # ~2 Hz
  ros2 topic hz /social/scene/behavior_hint    # ~1 Hz + on-change
  ros2 topic hz /social/scene/obstacle_cloud   # ~15 Hz (when objects detected)
  ros2 topic echo /social/scene/room_type
  ros2 topic echo /social/scene/behavior_hint
"""

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('engine_path',    default_value='/config/yolov8n_scene.engine'),
        DeclareLaunchArgument('onnx_path',      default_value='/config/yolov8n_scene.onnx'),
        DeclareLaunchArgument('conf_threshold', default_value='0.35'),
        DeclareLaunchArgument('publish_debug',  default_value='false'),
        DeclareLaunchArgument('publish_markers',default_value='true'),
    ]

    scene_detector = Node(
        package='saltybot_scene',
        executable='scene_detector',
        name='scene_detector',
        output='screen',
        parameters=[{
            'engine_path':      LaunchConfiguration('engine_path'),
            'onnx_path':        LaunchConfiguration('onnx_path'),
            'conf_threshold':   LaunchConfiguration('conf_threshold'),
            'iou_threshold':    0.45,
            'publish_debug':    LaunchConfiguration('publish_debug'),
            'room_update_hz':   2.0,
            'max_depth_m':      6.0,
        }],
    )

    behavior_adapter = Node(
        package='saltybot_scene',
        executable='behavior_adapter',
        name='behavior_adapter',
        output='screen',
    )

    costmap_publisher = Node(
        package='saltybot_scene',
        executable='costmap_publisher',
        name='costmap_publisher',
        output='screen',
        parameters=[{
            'max_obstacle_distance_m': 4.0,
            'min_confidence':          0.40,
            'obstacle_frame':         'base_link',
            'publish_markers':         LaunchConfiguration('publish_markers'),
        }],
    )

    return LaunchDescription(args + [
        scene_detector,
        behavior_adapter,
        costmap_publisher,
    ])
