"""
slam_rtabmap.launch.py — RTAB-Map RGB-D + LIDAR SLAM (Orin primary entry point)

Replaces slam.launch.py (slam_toolbox) as the docker-compose default on Orin.
RTAB-Map chosen over slam_toolbox for Orin because:
  - Native D435i depth + RPLIDAR sensor fusion
  - 3D point cloud output (Ampere GPU handles it)
  - Better loop closure with visual features
  - Outputs occupancy map directly consumable by Nav2

Stack:
  sensors.launch.py             (RPLIDAR + RealSense D435i + static TF)
  rtabmap_ros/rtabmap           (RTAB-Map SLAM node)
  saltybot_mapping/map_manager  (auto-save every 5 min + export services)

RTAB-Map input topics:
  /camera/color/image_raw          30 Hz  (RGB from D435i)
  /camera/depth/image_rect_raw     30 Hz  (depth from D435i)
  /camera/color/camera_info        30 Hz
  /scan                            ~5.5 Hz (RPLIDAR A1M8)

RTAB-Map output topics:
  /rtabmap/map             OccupancyGrid   (2D — Nav2 input)
  /rtabmap/cloud_map       PointCloud2     (3D — visualization)
  /rtabmap/odom            Odometry        (visual-inertial)

Persistence (Issue #123):
  database_path = /mnt/nvme/saltybot/maps/current.db  (default)
  - Normal boot: loads existing map, relocates, continues mapping (multi-session)
  - fresh_start:=true  → deletes current.db before launch → clean new map
  - database_path:=<path>  → load a specific named map (from map_manager)

Map management commands:
  ros2 service call /mapping/maps/list        saltybot_mapping/srv/ListMaps
  ros2 service call /mapping/maps/save_as     saltybot_mapping/srv/SaveMap    '{map_name: home, overwrite: false}'
  ros2 service call /mapping/maps/load        saltybot_mapping/srv/LoadMap    '{map_name: home}'
  ros2 service call /mapping/maps/delete      saltybot_mapping/srv/DeleteMap  '{map_name: old_map}'
  ros2 service call /mapping/export/occupancy saltybot_mapping/srv/ExportOccupancy '{output_dir: /tmp, map_name: home}'
  ros2 service call /mapping/export/pointcloud saltybot_mapping/srv/ExportPointCloud '{output_dir: /tmp, map_name: home, format: ply}'

Verify:
  ros2 topic hz /rtabmap/map          # ~1Hz map updates
  ros2 topic hz /rtabmap/cloud_map    # ~1Hz 3D cloud updates
  ros2 topic hz /rtabmap/odom         # ~10Hz odometry
  ros2 topic echo /mapping/status     # map_manager JSON status (1Hz)
"""

import os
from launch import LaunchDescription
from launch.actions import (
    DeclareLaunchArgument,
    ExecuteProcess,
    IncludeLaunchDescription,
)
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory


RTABMAP_PARAMS_FILE = '/config/rtabmap_params.yaml'
DEFAULT_DB_PATH     = '/mnt/nvme/saltybot/maps/current.db'


def generate_launch_description():
    bringup_share = get_package_share_directory('saltybot_bringup')

    # ── Launch arguments ──────────────────────────────────────────────────
    use_sim_time_arg = DeclareLaunchArgument(
        'use_sim_time',
        default_value='false',
        description='Use simulation clock (set true for rosbag playback)',
    )
    fresh_start_arg = DeclareLaunchArgument(
        'fresh_start',
        default_value='false',
        description=(
            'true = delete current.db before launch (fresh map). '
            'false = reload existing map and relocalize (default).'
        ),
    )
    database_path_arg = DeclareLaunchArgument(
        'database_path',
        default_value=DEFAULT_DB_PATH,
        description=(
            'RTAB-Map database path. Override to load a specific saved map, '
            'e.g. /mnt/nvme/saltybot/maps/home_garden.db'
        ),
    )

    # ── Optional: wipe DB when fresh_start:=true ──────────────────────────
    delete_db_action = ExecuteProcess(
        cmd=['bash', '-c',
             f'DB="{DEFAULT_DB_PATH}"; '
             '[ -f "$DB" ] && rm "$DB" '
             '&& echo "[slam_rtabmap] Deleted $DB — starting fresh map" '
             '|| echo "[slam_rtabmap] No existing DB at $DB"'],
        output='screen',
        condition=IfCondition(LaunchConfiguration('fresh_start')),
    )

    # ── Sensors ───────────────────────────────────────────────────────────
    sensors_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(bringup_share, 'launch', 'sensors.launch.py')
        ),
    )

    # ── RTAB-Map node ─────────────────────────────────────────────────────
    # --delete_db_on_start intentionally removed: persistence is the default.
    # Use fresh_start:=true launch arg to start a new map instead.
    rtabmap_node = Node(
        package='rtabmap_ros',
        executable='rtabmap',
        name='rtabmap',
        output='screen',
        parameters=[
            RTABMAP_PARAMS_FILE,
            {
                'use_sim_time':  LaunchConfiguration('use_sim_time'),
                'frame_id':      'base_link',
                'odom_frame_id': 'odom',
                'map_frame_id':  'map',
                # Override database_path from YAML with launch arg
                'database_path': LaunchConfiguration('database_path'),
            },
        ],
        remappings=[
            ('rgb/image',       '/camera/color/image_raw'),
            ('rgb/camera_info', '/camera/color/camera_info'),
            ('depth/image',     '/camera/depth/image_rect_raw'),
            ('scan',            '/scan'),
        ],
    )

    # ── Map manager node (persistence + auto-save + export) ───────────────
    map_manager_node = Node(
        package='saltybot_mapping',
        executable='map_manager',
        name='map_manager',
        output='screen',
        parameters=[{
            'maps_dir':          '/mnt/nvme/saltybot/maps',
            'exports_dir':       '/mnt/nvme/saltybot/maps/exports',
            'autosave_interval': 300.0,   # every 5 minutes
            'keep_autosaves_n':  5,
            'max_db_size_mb':    2048,
        }],
    )

    # ── Optional: RTAB-Map visualiser (comment in for debug sessions) ─────
    # rtabmap_viz_node = Node(
    #     package='rtabmap_ros',
    #     executable='rtabmapviz',
    #     name='rtabmapviz',
    #     output='screen',
    #     parameters=[{
    #         'use_sim_time': LaunchConfiguration('use_sim_time'),
    #         'frame_id':     'base_link',
    #     }],
    #     remappings=[
    #         ('rgb/image',       '/camera/color/image_raw'),
    #         ('rgb/camera_info', '/camera/color/camera_info'),
    #         ('depth/image',     '/camera/depth/image_rect_raw'),
    #         ('scan',            '/scan'),
    #     ],
    # )

    return LaunchDescription([
        use_sim_time_arg,
        fresh_start_arg,
        database_path_arg,
        delete_db_action,     # no-op unless fresh_start:=true
        sensors_launch,
        rtabmap_node,
        map_manager_node,
        # rtabmap_viz_node,
    ])
