"""
outdoor_nav.launch.py — Full outdoor navigation stack for SaltyBot

Services started:
  osm_router_node         — Overpass API OSM routing → /outdoor/waypoints
  gps_waypoint_follower   — /outdoor/waypoints → Nav2 navigate_through_poses
  geofence_node           — GPS polygon safety boundary
  navsat_transform_node   — robot_localization GPS→map projection (EKF)
  ekf_filter_node_odom    — local EKF (odom frame)
  ekf_filter_node_map     — global EKF (map frame, GPS-fused)

Nav2 is NOT started here — it must already be running (saltybot-nav2 container).

Outdoor mode: uses GPS for global localisation instead of RTAB-Map.
Indoor mode:  use slam_rtabmap.launch.py instead (RTAB-Map provides map→odom).

Arguments:
  use_rtk         (default false) — set true when ZED-F9P RTK module installed
  fence_active    (default true)  — enable geofence safety boundary
  goal_lat        (default 0.0)   — navigation goal latitude
  goal_lon        (default 0.0)   — navigation goal longitude
"""

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_outdoor')
    params_file    = os.path.join(pkg_dir, 'config', 'outdoor_nav_params.yaml')
    ekf_file       = os.path.join(pkg_dir, 'config', 'ekf_outdoor.yaml')
    geofence_file  = os.path.join(pkg_dir, 'config', 'geofence_vertices.yaml')

    args = [
        DeclareLaunchArgument('use_rtk',      default_value='false'),
        DeclareLaunchArgument('fence_active', default_value='true'),
        DeclareLaunchArgument('goal_lat',     default_value='0.0'),
        DeclareLaunchArgument('goal_lon',     default_value='0.0'),
    ]

    # ── robot_localization: local EKF (odom frame) ───────────────────────────
    ekf_local = Node(
        package='robot_localization',
        executable='ekf_node',
        name='ekf_filter_node_odom',
        output='screen',
        parameters=[ekf_file, {'use_sim_time': False}],
        remappings=[('odometry/filtered', '/odometry/local')],
    )

    # ── robot_localization: global EKF (map frame, GPS-fused) ────────────────
    ekf_global = Node(
        package='robot_localization',
        executable='ekf_node',
        name='ekf_filter_node_map',
        output='screen',
        parameters=[ekf_file, {'use_sim_time': False}],
        remappings=[('odometry/filtered', '/odometry/global')],
    )

    # ── navsat_transform_node — GPS (NavSatFix) → map-frame Odometry ─────────
    navsat = Node(
        package='robot_localization',
        executable='navsat_transform_node',
        name='navsat_transform',
        output='screen',
        parameters=[ekf_file, {'use_sim_time': False}],
        remappings=[
            ('imu/data',       '/imu/data'),
            ('gps/fix',        '/gps/fix'),
            ('odometry/filtered', '/odometry/global'),
            ('odometry/gps',   '/odometry/gps'),
        ],
    )

    # ── OSM router ───────────────────────────────────────────────────────────
    osm_router = Node(
        package='saltybot_outdoor',
        executable='osm_router_node',
        name='osm_router',
        output='screen',
        parameters=[
            params_file,
            {
                'goal_lat': LaunchConfiguration('goal_lat'),
                'goal_lon': LaunchConfiguration('goal_lon'),
            },
        ],
    )

    # ── GPS waypoint follower ─────────────────────────────────────────────────
    gps_follower = Node(
        package='saltybot_outdoor',
        executable='gps_waypoint_follower',
        name='gps_waypoint_follower',
        output='screen',
        parameters=[params_file],
    )

    # ── Geofence ──────────────────────────────────────────────────────────────
    geofence = Node(
        package='saltybot_outdoor',
        executable='geofence_node',
        name='geofence',
        output='screen',
        parameters=[geofence_file],
    )

    return LaunchDescription([
        *args,
        ekf_local,
        ekf_global,
        navsat,
        osm_router,
        gps_follower,
        geofence,
    ])
