"""
outdoor_speed.launch.py — Outdoor adaptive speed controller.

Inserts SpeedControllerNode between the person_follower (/cmd_vel_raw)
and cmd_vel_bridge (/cmd_vel), providing adaptive walk/jog/ride profiles
with smooth acceleration curves and emergency deceleration.

IMPORTANT: For ride profile (up to 8 m/s) you must ALSO re-launch
cmd_vel_bridge with matching limits:
  ros2 launch saltybot_bridge cmd_vel_bridge.launch.py max_linear_vel:=8.0

Prerequisite node pipeline:
<<<<<<< HEAD
  person_follower → /cmd_vel_raw → [speed_controller] → /cmd_vel → cmd_vel_bridge → ESP32 BALANCE
=======
  person_follower → /cmd_vel_raw → [speed_controller] → /cmd_vel → cmd_vel_bridge → ESP32-S3
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)

Usage:
  # Defaults (walk profile initially, adapts via UWB + GPS):
  ros2 launch saltybot_speed_controller outdoor_speed.launch.py

  # Force ride profile limits from start:
  ros2 launch saltybot_speed_controller outdoor_speed.launch.py initial_profile:=ride

  # Tune jog profile max speed:
  ros2 launch saltybot_speed_controller outdoor_speed.launch.py jog_max_vel:=4.0

  # Gentle emergency decel (longer stop distance, safer for balance):
  ros2 launch saltybot_speed_controller outdoor_speed.launch.py emergency_decel:=1.0

  # Full outdoor follow-me pipeline (speed controller + person follower):
  ros2 launch saltybot_speed_controller outdoor_speed.launch.py \
      launch_person_follower:=true

Topics:
  /cmd_vel_raw           — Input (from person_follower or Nav2)
  /cmd_vel               — Output (to cmd_vel_bridge)
  /speed_controller/profile — Active profile + status (JSON String)
  /gps/vel               — Ground truth speed input (from gps_driver_node)
  /uwb/ranges            — Target range input (for velocity estimation)
"""

import os

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


def _launch_nodes(context, *args, **kwargs):
    params_file = LaunchConfiguration("params_file").perform(context)

    def _s(k): return LaunchConfiguration(k).perform(context)
    def _f(k): return float(LaunchConfiguration(k).perform(context))
    def _i(k): return int(LaunchConfiguration(k).perform(context))
    def _b(k): return LaunchConfiguration(k).perform(context).lower() == "true"

    inline_params = {
        "control_rate":              _f("control_rate"),
        "input_topic":               _s("input_topic"),
        "initial_profile":           _s("initial_profile"),
        "hysteresis_up_ticks":       _i("hysteresis_up_ticks"),
        "hysteresis_down_ticks":     _i("hysteresis_down_ticks"),
        "emergency_decel_mps2":      _f("emergency_decel"),
        "stop_vel_threshold":        _f("stop_vel_threshold"),
        "hard_stop_decel_threshold": _f("hard_stop_decel_threshold"),
        "gps_runaway_factor":        _f("gps_runaway_factor"),
        "gps_timeout":               _f("gps_timeout"),
        "uwb_timeout":               _f("uwb_timeout"),
        # Per-profile params
        "walk.max_linear_vel":       _f("walk_max_vel"),
        "walk.accel_limit":          _f("walk_accel"),
        "walk.decel_limit":          _f("walk_decel"),
        "jog.max_linear_vel":        _f("jog_max_vel"),
        "jog.accel_limit":           _f("jog_accel"),
        "jog.decel_limit":           _f("jog_decel"),
        "ride.max_linear_vel":       _f("ride_max_vel"),
        "ride.accel_limit":          _f("ride_accel"),
        "ride.decel_limit":          _f("ride_decel"),
    }
    node_params = [params_file, inline_params] if params_file else [inline_params]

    nodes = [
        Node(
            package="saltybot_speed_controller",
            executable="speed_controller_node",
            name="speed_controller",
            output="screen",
            parameters=node_params,
        ),
    ]

    if _b("launch_person_follower"):
        try:
            follower_pkg = get_package_share_directory("saltybot_follower")
            follower_launch = os.path.join(
                follower_pkg, "launch", "person_follower.launch.py")
            nodes.append(IncludeLaunchDescription(
                PythonLaunchDescriptionSource(follower_launch),
                launch_arguments={"cmd_vel_topic": "/cmd_vel_raw"}.items(),
            ))
        except Exception:
            pass   # package not built yet — skip silently

    return nodes


def generate_launch_description():
    pkg_share     = get_package_share_directory("saltybot_speed_controller")
    default_params = os.path.join(pkg_share, "config", "speed_profiles.yaml")

    return LaunchDescription([
        DeclareLaunchArgument(
            "params_file", default_value=default_params,
            description="Full path to speed_profiles.yaml"),

        # General
        DeclareLaunchArgument("control_rate",       default_value="50.0"),
        DeclareLaunchArgument("input_topic",         default_value="/cmd_vel_raw"),
        DeclareLaunchArgument("initial_profile",     default_value="walk"),
        DeclareLaunchArgument("hysteresis_up_ticks",  default_value="5"),
        DeclareLaunchArgument("hysteresis_down_ticks",default_value="15"),
        DeclareLaunchArgument("launch_person_follower", default_value="false",
                              description="Also launch saltybot_follower with /cmd_vel_raw output"),

        # Emergency
        DeclareLaunchArgument("emergency_decel",          default_value="2.0",
                              description="Emergency decel limit (m/s²)"),
        DeclareLaunchArgument("stop_vel_threshold",       default_value="0.4"),
        DeclareLaunchArgument("hard_stop_decel_threshold",default_value="3.0"),

        # GPS runaway
        DeclareLaunchArgument("gps_runaway_factor", default_value="1.5"),
        DeclareLaunchArgument("gps_timeout",        default_value="3.0"),
        DeclareLaunchArgument("uwb_timeout",        default_value="2.0"),

        # Walk profile
        DeclareLaunchArgument("walk_max_vel", default_value="1.5"),
        DeclareLaunchArgument("walk_accel",   default_value="0.6"),
        DeclareLaunchArgument("walk_decel",   default_value="1.0"),

        # Jog profile
        DeclareLaunchArgument("jog_max_vel",  default_value="3.0"),
        DeclareLaunchArgument("jog_accel",    default_value="0.5"),
        DeclareLaunchArgument("jog_decel",    default_value="0.8"),

        # Ride profile
        DeclareLaunchArgument("ride_max_vel", default_value="8.0",
                              description="Max speed for EUC follow-me (m/s)"),
        DeclareLaunchArgument("ride_accel",   default_value="0.3",
                              description="Gentle accel — balance sensitive at 8 m/s"),
        DeclareLaunchArgument("ride_decel",   default_value="0.5"),

        OpaqueFunction(function=_launch_nodes),
    ])
