"""
fleet_coordinator.launch.py — Fleet-wide coordinator nodes (run on ONE robot or dedicated node).

Starts:
  fleet_manager       — robot registry, map merging, frontier task allocator
  frontier_detector   — detect frontiers on the merged map

Typically run on the robot that has the most compute headroom, or on the Orin.
Only one instance should run per fleet.  Other robots run fleet_robot.launch.py only.

Launch args:
  robot_id            str    "saltybot_1"   (which robot's pose to use for scoring)
  map_resolution      float  "0.05"
  heartbeat_timeout   float  "10.0"
  merge_hz            float  "2.0"
  min_frontier_cells  int    "10"

Example:
  ros2 launch saltybot_fleet fleet_coordinator.launch.py robot_id:=saltybot_1
"""

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def generate_launch_description():
    robot_id_arg = DeclareLaunchArgument(
        'robot_id', default_value='saltybot_1',
    )
    res_arg = DeclareLaunchArgument(
        'map_resolution', default_value='0.05',
    )
    hb_arg = DeclareLaunchArgument(
        'heartbeat_timeout', default_value='10.0',
    )
    hz_arg = DeclareLaunchArgument(
        'merge_hz', default_value='2.0',
    )
    frontier_arg = DeclareLaunchArgument(
        'min_frontier_cells', default_value='10',
    )

    fleet_manager = Node(
        package='saltybot_fleet',
        executable='fleet_manager',
        name='fleet_manager',
        output='screen',
        parameters=[{
            'heartbeat_timeout_sec': LaunchConfiguration('heartbeat_timeout'),
            'merge_hz':              LaunchConfiguration('merge_hz'),
            'frontier_timeout_sec':  60.0,
            'map_resolution':        LaunchConfiguration('map_resolution'),
        }],
    )

    frontier_detector = Node(
        package='saltybot_fleet',
        executable='frontier_detector',
        name='frontier_detector',
        output='screen',
        parameters=[{
            'robot_id':           LaunchConfiguration('robot_id'),
            'detection_hz':       1.0,
            'min_frontier_cells': LaunchConfiguration('min_frontier_cells'),
        }],
    )

    return LaunchDescription([
        robot_id_arg,
        res_arg,
        hb_arg,
        hz_arg,
        frontier_arg,
        fleet_manager,
        frontier_detector,
    ])
