"""nav2_slam_bringup.launch.py (Issue #422, #696) — SLAM + map persistence."""

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


def generate_launch_description():
    nav2_slam_dir = get_package_share_directory("saltybot_nav2_slam")
    bringup_dir = get_package_share_directory("saltybot_bringup")
    nav2_params = os.path.join(nav2_slam_dir, "config", "nav2_params.yaml")
    slam_params = os.path.join(nav2_slam_dir, "config", "slam_toolbox_params.yaml")

    use_sim_time = LaunchConfiguration("use_sim_time")
    map_dir = LaunchConfiguration("map_dir")
    map_name = LaunchConfiguration("map_name")

    return LaunchDescription([
        DeclareLaunchArgument("use_sim_time", default_value="false"),
        DeclareLaunchArgument(
            "map_dir",
            default_value="/mnt/nvme/saltybot/maps",
            description="Directory for saving/loading map files (Issue #696)",
        ),
        DeclareLaunchArgument(
            "map_name",
            default_value="saltybot_map",
            description="Base filename for saved map (no extension)",
        ),
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(os.path.join(bringup_dir, "launch", "sensors.launch.py")),
            launch_arguments={"use_sim_time": use_sim_time}.items(),
        ),
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(os.path.join(nav2_slam_dir, "launch", "odometry_bridge.launch.py")),
            launch_arguments={"use_sim_time": use_sim_time}.items(),
        ),
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(os.path.join(nav2_slam_dir, "launch", "nav2_slam_integrated.launch.py")),
            launch_arguments={"slam_params_file": slam_params, "use_sim_time": use_sim_time}.items(),
        ),
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(os.path.join(nav2_slam_dir, "launch", "depth_to_costmap.launch.py")),
            launch_arguments={"use_sim_time": use_sim_time}.items(),
        ),
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                os.path.join(get_package_share_directory("nav2_bringup"), "launch", "navigation_launch.py")
            ),
            launch_arguments={
                "use_sim_time": use_sim_time,
                "params_file": nav2_params,
                "autostart": "true",
                "map_subscribe_transient_local": "true",
            }.items(),
        ),
        # Map persistence (Issue #696): map_saver_server + /saltybot/save_map service
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(os.path.join(nav2_slam_dir, "launch", "map_persistence.launch.py")),
            launch_arguments={"map_dir": map_dir, "map_name": map_name, "use_sim_time": use_sim_time}.items(),
        ),
    ])
