"""nav2_uwb.launch.py — Full Nav2 stack with UWB localization (Issue #599).

Launch sequence
───────────────
  1. uwb_pose_bridge_node    — broadcasts map→odom TF from UWB-IMU EKF pose
  2. nav2_bringup navigation — controller, planner, BT navigator, costmaps
  3. lifecycle_manager_navigation — autostart all Nav2 nodes
  4. waypoint_follower_node  — sequential waypoint execution action server

Localization is handled by the UWB bridge; AMCL is NOT launched.

Arguments
─────────
  use_sim_time   (bool)   default: false
  params_file    (str)    default: <package>/config/nav2_uwb_params.yaml
  uwb_pose_topic (str)    default: /saltybot/pose/fused_cov
  odom_topic     (str)    default: /odom
  map_frame      (str)    default: map
  odom_frame     (str)    default: odom
  base_frame     (str)    default: base_link
  max_vel_x      (float)  default: 1.0  — DWB hard cap (m/s)
"""

import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import (
    DeclareLaunchArgument,
    GroupAction,
    IncludeLaunchDescription,
    LogInfo,
    TimerAction,
)
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import (
    LaunchConfiguration,
    PathJoinSubstitution,
    PythonExpression,
)
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare


def generate_launch_description():
    pkg_nav2_uwb  = get_package_share_directory("saltybot_nav2_uwb")
    pkg_nav2_bringup = get_package_share_directory("nav2_bringup")

    default_params = os.path.join(pkg_nav2_uwb, "config", "nav2_uwb_params.yaml")

    # ── Declare arguments ──────────────────────────────────────────────────
    args = [
        DeclareLaunchArgument(
            "use_sim_time", default_value="false",
            description="Use simulation clock"),
        DeclareLaunchArgument(
            "params_file", default_value=default_params,
            description="Full path to nav2_uwb_params.yaml"),
        DeclareLaunchArgument(
            "uwb_pose_topic",  default_value="/saltybot/pose/fused_cov",
            description="UWB-IMU EKF fused pose topic (PoseWithCovarianceStamped)"),
        DeclareLaunchArgument(
            "odom_topic",  default_value="/odom",
            description="Wheel odometry topic (Odometry)"),
        DeclareLaunchArgument(
            "map_frame",   default_value="map",
            description="Global map frame id"),
        DeclareLaunchArgument(
            "odom_frame",  default_value="odom",
            description="Odometry frame id"),
        DeclareLaunchArgument(
            "base_frame",  default_value="base_link",
            description="Robot base frame id"),
        DeclareLaunchArgument(
            "autostart",   default_value="true",
            description="Automatically start Nav2 lifecycle nodes"),
    ]

    # ── UWB pose bridge — starts immediately ──────────────────────────────
    uwb_bridge = Node(
        package="saltybot_nav2_uwb",
        executable="uwb_pose_bridge_node",
        name="uwb_pose_bridge",
        output="screen",
        parameters=[{
            "use_sim_time":    LaunchConfiguration("use_sim_time"),
            "uwb_pose_topic":  LaunchConfiguration("uwb_pose_topic"),
            "odom_topic":      LaunchConfiguration("odom_topic"),
            "map_frame":       LaunchConfiguration("map_frame"),
            "odom_frame":      LaunchConfiguration("odom_frame"),
            "base_frame":      LaunchConfiguration("base_frame"),
            "tf_publish_hz":   20.0,
            "tf_timeout_s":    0.5,
        }],
    )

    uwb_bridge_log = LogInfo(
        msg="[nav2_uwb] UWB pose bridge launched — map→odom TF will be "
            "broadcast once UWB + odom data arrive."
    )

    # ── Nav2 navigation stack (t=2s: allow bridge to initialise first) ─────
    nav2_navigation = TimerAction(
        period=2.0,
        actions=[
            LogInfo(msg="[nav2_uwb] Starting Nav2 navigation stack..."),
            IncludeLaunchDescription(
                PythonLaunchDescriptionSource(
                    os.path.join(pkg_nav2_bringup, "launch", "navigation_launch.py")
                ),
                launch_arguments={
                    "use_sim_time":                  LaunchConfiguration("use_sim_time"),
                    "params_file":                   LaunchConfiguration("params_file"),
                    "autostart":                     LaunchConfiguration("autostart"),
                    # No map_subscribe_transient_local — no static map
                    "use_lifecycle_mgr":             "true",
                    "use_sim_time":                  LaunchConfiguration("use_sim_time"),
                }.items(),
            ),
        ],
    )

    # ── Waypoint follower (t=4s: Nav2 needs time to come up) ──────────────
    waypoint_follower = TimerAction(
        period=4.0,
        actions=[
            LogInfo(msg="[nav2_uwb] Starting waypoint follower action server..."),
            Node(
                package="saltybot_nav2_uwb",
                executable="waypoint_follower_node",
                name="waypoint_follower",
                output="screen",
                parameters=[{
                    "use_sim_time":       LaunchConfiguration("use_sim_time"),
                    "map_frame":          LaunchConfiguration("map_frame"),
                    "goal_xy_tolerance":  0.20,
                    "goal_yaw_tolerance": 0.15,
                    "nav_timeout_s":      60.0,
                    "status_hz":          2.0,
                }],
            ),
        ],
    )

    ready_log = TimerAction(
        period=4.5,
        actions=[
            LogInfo(
                msg="[nav2_uwb] Stack ready.\n"
                    "  Localization : UWB-IMU EKF bridge (no AMCL)\n"
                    "  Controller   : DWB ≤ 1.0 m/s  (differential drive)\n"
                    "  Waypoints    : /saltybot/nav/follow_waypoints  (action)\n"
                    "  Status       : /saltybot/nav/waypoint_status   (JSON)\n"
                    "  Cancel       : /saltybot/nav/cancel            (topic)"
            ),
        ],
    )

    return LaunchDescription(
        args + [
            uwb_bridge_log,
            uwb_bridge,
            nav2_navigation,
            waypoint_follower,
            ready_log,
        ]
    )
