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

Supports two localization modes via the 'nav_mode' argument:

  nav_mode:=slam  (default)
    Uses RTAB-Map for odometry and live map building.
    Requires slam_rtabmap.launch.py to be running.
    Sources: /rtabmap/odom (pose), /map (from RTAB-Map)

  nav_mode:=amcl
    Uses VESC CAN odometry (/odom) + AMCL on a pre-built static map.
    Launches: sensors, VESC odometry bridge, map_server, AMCL, Nav2 stack.
    Sources: /odom (VESC CAN IDs 61/79), /scan (RPLIDAR)
    Map:     saltybot_nav2_slam maps/saltybot_map.yaml (override with map:=...)

  nav_mode:=slam  profile support (Issue #506)
    profile:=indoor  — conservative (0.2 m/s, tight geofence)
    profile:=outdoor — moderate  (0.5 m/s)
    profile:=demo    — agile     (0.3 m/s, tricks enabled)

Run sequence on Orin:
  # SLAM mode (existing behaviour):
  ros2 launch saltybot_bringup nav2.launch.py

  # AMCL mode with saved map:
  ros2 launch saltybot_bringup nav2.launch.py nav_mode:=amcl \
      map:=/mnt/nvme/saltybot/maps/my_map.yaml
"""

import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import (
    DeclareLaunchArgument,
    GroupAction,
    IncludeLaunchDescription,
    LogInfo,
)
from launch.conditions import LaunchConfigurationEquals
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration


def generate_launch_description() -> LaunchDescription:
    nav2_bringup_dir   = get_package_share_directory("nav2_bringup")
    bringup_dir        = get_package_share_directory("saltybot_bringup")
    nav2_slam_dir      = get_package_share_directory("saltybot_nav2_slam")

    # ── Arguments ─────────────────────────────────────────────────────────────

    nav_mode_arg = DeclareLaunchArgument(
        "nav_mode",
        default_value="slam",
        choices=["slam", "amcl"],
        description=(
            "Localization mode. "
            "slam: RTAB-Map live map (requires slam_rtabmap.launch.py running); "
            "amcl: static map + AMCL particle filter + VESC odometry (Issue #655)."
        ),
    )

    profile_arg = DeclareLaunchArgument(
        "profile",
        default_value="indoor",
        choices=["indoor", "outdoor", "demo"],
        description="Nav2 parameter profile for slam mode (Issue #506)",
    )

    map_arg = DeclareLaunchArgument(
        "map",
        default_value=os.path.join(nav2_slam_dir, "maps", "saltybot_map.yaml"),
        description="Map YAML path for AMCL mode",
    )

    use_sim_time_arg = DeclareLaunchArgument(
        "use_sim_time",
        default_value="false",
    )

    # ── Mode: SLAM (original behaviour — RTAB-Map + navigation_launch.py) ────

    slam_nav2_params = os.path.join(bringup_dir, "config", "nav2_params.yaml")
    slam_bt_xml      = os.path.join(
        bringup_dir, "behavior_trees", "navigate_to_pose_with_recovery.xml"
    )

    slam_mode = GroupAction(
        condition=LaunchConfigurationEquals("nav_mode", "slam"),
        actions=[
            LogInfo(msg="[nav2] Mode: SLAM (RTAB-Map odometry + live map)"),
            IncludeLaunchDescription(
                PythonLaunchDescriptionSource(
                    os.path.join(nav2_bringup_dir, "launch", "navigation_launch.py")
                ),
                launch_arguments={
                    "use_sim_time":                "false",
                    "autostart":                   "true",
                    "params_file":                 slam_nav2_params,
                    "default_bt_xml_filename":     slam_bt_xml,
                    "map_subscribe_transient_local": "true",
                }.items(),
            ),
        ],
    )

    # ── Mode: AMCL (static map + AMCL + VESC odometry, Issue #655) ───────────

    amcl_mode = GroupAction(
        condition=LaunchConfigurationEquals("nav_mode", "amcl"),
        actions=[
            LogInfo(msg="[nav2] Mode: AMCL (static map + VESC odometry, Issue #655)"),
            IncludeLaunchDescription(
                PythonLaunchDescriptionSource(
                    os.path.join(
                        nav2_slam_dir, "launch", "nav2_amcl_bringup.launch.py"
                    )
                ),
                launch_arguments={
                    "map":            LaunchConfiguration("map"),
                    "use_sim_time":   LaunchConfiguration("use_sim_time"),
                    # Sensors are already started by saltybot_bringup GROUP A
                    "include_sensors": "false",
                }.items(),
            ),
        ],
    )

    return LaunchDescription([
        nav_mode_arg,
        profile_arg,
        map_arg,
        use_sim_time_arg,
        LogInfo(msg=["[nav2] Loaded nav_mode=", LaunchConfiguration("nav_mode"),
                     "  profile=", LaunchConfiguration("profile")]),
        slam_mode,
        amcl_mode,
    ])
