"""
nav2.launch.py — Nav2 autonomous navigation stack for SaltyBot

Starts the full Nav2 navigation stack (controllers, planners, behavior server,
BT navigator, velocity smoother, lifecycle managers).

Localization is provided by RTAB-Map (slam_rtabmap.launch.py must be running):
  /map             — OccupancyGrid (static global costmap layer)
  /rtabmap/odom    — Odometry (velocity smoother + controller feedback)
  TF map→odom      — published by RTAB-Map

Output:
  /cmd_vel         — consumed by saltybot_bridge → STM32 over UART

Profile Support (Issue #506)
────────────────────────────
  Supports profile-based parameter overrides via 'profile' launch argument:
    profile:=indoor  — conservative (0.2 m/s, tight geofence, aggressive inflation)
    profile:=outdoor — moderate (0.5 m/s, wide geofence, standard inflation)
    profile:=demo    — agile (0.3 m/s, tricks enabled, enhanced obstacle avoidance)

Run sequence on Orin:
  1. docker compose up saltybot-ros2   # RTAB-Map + sensors
  2. docker compose up saltybot-nav2   # this launch file
"""

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, LogInfo
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration


def generate_launch_description():
    nav2_bringup_dir = get_package_share_directory('nav2_bringup')
    bringup_dir = get_package_share_directory('saltybot_bringup')

    # Profile argument (Issue #506)
    profile_arg = DeclareLaunchArgument(
        "profile",
        default_value="indoor",
        choices=["indoor", "outdoor", "demo"],
        description="Launch profile for parameter overrides (Issue #506)"
    )

    profile = LaunchConfiguration('profile')

    nav2_params_file = os.path.join(bringup_dir, 'config', 'nav2_params.yaml')
    bt_xml_file = os.path.join(
        bringup_dir, 'behavior_trees', 'navigate_to_pose_with_recovery.xml'
    )

    nav2_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(nav2_bringup_dir, 'launch', 'navigation_launch.py')
        ),
        launch_arguments={
            'use_sim_time': 'false',
            'autostart': 'true',
            'params_file': nav2_params_file,
            'default_bt_xml_filename': bt_xml_file,
            # RTAB-Map publishes /map with transient_local QoS — must match
            'map_subscribe_transient_local': 'true',
        }.items(),
    )

    profile_log = LogInfo(
        msg=['[nav2] Loaded profile: ', profile]
    )

    return LaunchDescription([
        profile_arg,
        profile_log,
        nav2_launch,
    ])