"""
route_system.launch.py — Route recording and replay system for SaltyBot

Starts:
  route_recorder_node  — records GPS + odom path during follow-me rides
  route_replayer_node  — replays recorded routes autonomously via Nav2
  route_manager_node   — list/delete/info for saved routes

Depends on:
  saltybot-nav2 container (Nav2 action server /navigate_through_poses)
  saltybot_cellular (/gps/fix from SIM7600X GPS — PR #65)
  saltybot_uwb (/uwb/target — PR #66, used for context during recording)
  STM32 bridge (/odom from wheel encoders)
  D435i (/imu/data for heading)

Usage — record a route:
  # Set name, start recording, ride with Tee, stop and save:
  ros2 param set /route_recorder route_name "friday_loop"
  ros2 service call /route/start_recording std_srvs/srv/Trigger '{}'
  # ... ride the route ...
  ros2 service call /route/stop_recording std_srvs/srv/Trigger '{}'
  ros2 service call /route/save std_srvs/srv/Trigger '{}'

Usage — replay a route:
  ros2 param set /route_replayer route_name "friday_loop"
  ros2 service call /route/load std_srvs/srv/Trigger '{}'
  ros2 service call /route/start_replay std_srvs/srv/Trigger '{}'

Arguments:
  save_dir      (default /data/routes) — NVMe route storage
  route_name    (default route)        — name for record/replay
"""

import os
from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def generate_launch_description():
    pkg_dir    = get_package_share_directory('saltybot_routes')
    params_file = os.path.join(pkg_dir, 'config', 'route_params.yaml')

    args = [
        DeclareLaunchArgument('save_dir',   default_value='/data/routes'),
        DeclareLaunchArgument('route_name', default_value='route'),
    ]

    recorder = Node(
        package='saltybot_routes',
        executable='route_recorder_node',
        name='route_recorder',
        output='screen',
        parameters=[
            params_file,
            {
                'save_dir':   LaunchConfiguration('save_dir'),
                'route_name': LaunchConfiguration('route_name'),
            },
        ],
    )

    replayer = Node(
        package='saltybot_routes',
        executable='route_replayer_node',
        name='route_replayer',
        output='screen',
        parameters=[
            params_file,
            {
                'save_dir':   LaunchConfiguration('save_dir'),
                'route_name': LaunchConfiguration('route_name'),
            },
        ],
    )

    manager = Node(
        package='saltybot_routes',
        executable='route_manager_node',
        name='route_manager',
        output='screen',
        parameters=[
            params_file,
            {
                'save_dir':   LaunchConfiguration('save_dir'),
                'route_name': LaunchConfiguration('route_name'),
            },
        ],
    )

    return LaunchDescription([
        *args,
        recorder,
        replayer,
        manager,
    ])