"""nav2_amcl_bringup.launch.py (Issue #655, #696) — AMCL + auto map detection.

Map selection (Issue #696): if /mnt/nvme/saltybot/maps/saltybot_map.yaml exists
at launch time, it is used automatically. Otherwise falls back to placeholder.

Usage:
  ros2 launch saltybot_nav2_slam nav2_amcl_bringup.launch.py
  ros2 launch saltybot_nav2_slam nav2_amcl_bringup.launch.py map_dir:=/mnt/nvme/saltybot/maps
  ros2 launch saltybot_nav2_slam nav2_amcl_bringup.launch.py map:=/path/to/room.yaml
  ros2 launch saltybot_nav2_slam nav2_amcl_bringup.launch.py include_sensors:=false
"""

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 IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration

# Map auto-detection (Issue #696): evaluated at ros2 launch time on the Jetson.
_NVME_MAP_DIR = "/mnt/nvme/saltybot/maps"
_NVME_MAP_YAML = os.path.join(_NVME_MAP_DIR, "saltybot_map.yaml")


def _default_map() -> str:
    if os.path.isfile(_NVME_MAP_YAML):
        return _NVME_MAP_YAML
    nav2_slam_dir = get_package_share_directory("saltybot_nav2_slam")
    return os.path.join(nav2_slam_dir, "maps", "saltybot_map.yaml")


def generate_launch_description() -> LaunchDescription:
    nav2_slam_dir    = get_package_share_directory("saltybot_nav2_slam")
    bringup_dir      = get_package_share_directory("saltybot_bringup")
    nav2_bringup_dir = get_package_share_directory("nav2_bringup")
    default_map      = _default_map()
    default_params   = os.path.join(nav2_slam_dir, "config", "amcl_nav2_params.yaml")
    map_source = "saved NVMe map" if default_map == _NVME_MAP_YAML else "placeholder map"

    map_file        = LaunchConfiguration("map")
    use_sim_time    = LaunchConfiguration("use_sim_time")
    autostart       = LaunchConfiguration("autostart")
    params_file     = LaunchConfiguration("params_file")
    include_sensors = LaunchConfiguration("include_sensors")

    return LaunchDescription([
        DeclareLaunchArgument("map_dir", default_value=_NVME_MAP_DIR,
                              description="Directory containing saved map files (Issue #696)"),
        DeclareLaunchArgument("map", default_value=default_map,
                              description="Path to map YAML. Auto-detected: " + default_map),
        DeclareLaunchArgument("use_sim_time", default_value="false"),
        DeclareLaunchArgument("autostart", default_value="true"),
        DeclareLaunchArgument("params_file", default_value=default_params),
        DeclareLaunchArgument("include_sensors", default_value="true",
                              description="Launch sensors.launch.py"),

        LogInfo(msg="[nav2_amcl] Nav2 AMCL bringup starting (Issues #655, #696)"),
        LogInfo(msg="[nav2_amcl] Map: " + map_source + " -> " + default_map),

        GroupAction(
            condition=IfCondition(include_sensors),
            actions=[
                LogInfo(msg="[nav2_amcl] Starting sensors (RealSense + RPLIDAR)"),
                IncludeLaunchDescription(
                    PythonLaunchDescriptionSource(os.path.join(bringup_dir, "launch", "sensors.launch.py")),
                ),
            ],
        ),

        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(os.path.join(nav2_slam_dir, "launch", "odometry_bridge.launch.py")),
            launch_arguments={"use_sim_time": use_sim_time}.items(),
        ),

        LogInfo(msg="[nav2_amcl] Starting AMCL localization"),
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(os.path.join(nav2_bringup_dir, "launch", "localization_launch.py")),
            launch_arguments={
                "map": map_file, "use_sim_time": use_sim_time,
                "autostart": autostart, "params_file": params_file,
                "use_lifecycle_mgr": "true",
            }.items(),
        ),

        LogInfo(msg="[nav2_amcl] Starting Nav2 navigation stack"),
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(os.path.join(nav2_bringup_dir, "launch", "navigation_launch.py")),
            launch_arguments={
                "use_sim_time": use_sim_time, "autostart": autostart,
                "params_file": params_file,
                "map_subscribe_transient_local": "true",
                "use_lifecycle_mgr": "true",
            }.items(),
        ),
    ])
