"""
fleet_robot.launch.py — Per-robot fleet nodes (run on every robot in the fleet).

Starts:
  map_broadcaster    — publish this robot's compressed map to /fleet/maps
  fleet_explorer     — cooperative frontier exploration state machine

This launch file is namespace-aware: set robot_id and robot_namespace to
isolate each robot's internal topics while sharing /fleet/* topics.

Launch args:
  robot_id          str  "saltybot_1"
  robot_namespace   str  "/saltybot_1"
  battery_topic     str  ""   (optional Float32 battery percentage topic)
  exploration       bool "true"  — set false to broadcast map only, no exploration

Example (robot 1):
  ros2 launch saltybot_fleet fleet_robot.launch.py robot_id:=saltybot_1 robot_namespace:=/saltybot_1

Example (robot 2):
  ros2 launch saltybot_fleet fleet_robot.launch.py robot_id:=saltybot_2 robot_namespace:=/saltybot_2
"""

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition
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',
        description='Unique robot identifier (no slashes)',
    )
    ns_arg = DeclareLaunchArgument(
        'robot_namespace', default_value='/saltybot_1',
        description='ROS2 namespace for this robot',
    )
    battery_arg = DeclareLaunchArgument(
        'battery_topic', default_value='',
        description='Optional Float32 battery percentage topic',
    )
    exploration_arg = DeclareLaunchArgument(
        'exploration', default_value='true',
        description='Enable cooperative exploration state machine',
    )

    map_broadcaster = Node(
        package='saltybot_fleet',
        executable='map_broadcaster',
        name='map_broadcaster',
        output='screen',
        parameters=[{
            'robot_id':        LaunchConfiguration('robot_id'),
            'robot_namespace': LaunchConfiguration('robot_namespace'),
            'broadcast_hz':    1.0,
            'battery_topic':   LaunchConfiguration('battery_topic'),
        }],
    )

    fleet_explorer = Node(
        package='saltybot_fleet',
        executable='fleet_explorer',
        name='fleet_explorer',
        output='screen',
        condition=IfCondition(LaunchConfiguration('exploration')),
        parameters=[{
            'robot_id':         LaunchConfiguration('robot_id'),
            'robot_namespace':  LaunchConfiguration('robot_namespace'),
            'exploration_hz':   0.5,
            'goal_tolerance_m': 0.5,
            'nav_timeout_sec':  120.0,
            'backoff_sec':      5.0,
            'battery_min_pct':  20.0,
        }],
    )

    return LaunchDescription([
        robot_id_arg,
        ns_arg,
        battery_arg,
        exploration_arg,
        map_broadcaster,
        fleet_explorer,
    ])
