"""
<<<<<<< HEAD
cmd_vel_bridge.launch.py — Nav2 cmd_vel → ESP32 BALANCE autonomous drive bridge.
=======
cmd_vel_bridge.launch.py — Nav2 cmd_vel → ESP32-S3 autonomous drive bridge.
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)

Starts cmd_vel_bridge_node, which owns the serial port exclusively and provides:
  - /cmd_vel subscription with velocity limits + smooth ramp
  - Deadman switch (zero speed if /cmd_vel silent > cmd_vel_timeout)
<<<<<<< HEAD
  - Mode gate (drives only when ESP32 BALANCE is in AUTONOMOUS mode, md=2)
=======
  - Mode gate (drives only when ESP32-S3 is in AUTONOMOUS mode, md=2)
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
  - Telemetry RX → /saltybot/imu, /saltybot/balance_state, /diagnostics
  - /saltybot/cmd publisher (observability)

Do NOT run alongside serial_bridge_node or saltybot_cmd_node on the same port.

Usage:
  # Defaults (max 0.5 m/s, 2.0 rad/s, 1s ramp, 500ms deadman):
  ros2 launch saltybot_bridge cmd_vel_bridge.launch.py

  # Override speed limit for cautious tuning:
  ros2 launch saltybot_bridge cmd_vel_bridge.launch.py max_linear_vel:=0.3

  # Load all params from YAML:
  ros2 launch saltybot_bridge cmd_vel_bridge.launch.py \\
      params_file:=/path/to/cmd_vel_bridge_params.yaml
"""

import os

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


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

    # Build parameter dict from individual launch args (allow YAML override)
    inline_params = {
        "serial_port":     LaunchConfiguration("serial_port").perform(context),
        "baud_rate":       int(LaunchConfiguration("baud_rate").perform(context)),
        "timeout":         0.05,
        "reconnect_delay": 2.0,
        "heartbeat_period":float(LaunchConfiguration("heartbeat_period").perform(context)),
        "max_linear_vel":  float(LaunchConfiguration("max_linear_vel").perform(context)),
        "max_angular_vel": float(LaunchConfiguration("max_angular_vel").perform(context)),
        "speed_scale":     float(LaunchConfiguration("speed_scale").perform(context)),
        "steer_scale":     float(LaunchConfiguration("steer_scale").perform(context)),
        "ramp_rate":       int(LaunchConfiguration("ramp_rate").perform(context)),
        "cmd_vel_timeout": float(LaunchConfiguration("cmd_vel_timeout").perform(context)),
    }

    # If a params file is provided, it takes precedence (ROS2 merges inline last)
    node_params = [params_file, inline_params] if params_file else [inline_params]

    return [Node(
        package="saltybot_bridge",
        executable="cmd_vel_bridge_node",
        name="cmd_vel_bridge",
        output="screen",
        parameters=node_params,
    )]


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

    return LaunchDescription([
        DeclareLaunchArgument(
            "params_file",
            default_value=default_params,
            description="Full path to cmd_vel_bridge_params.yaml (overrides inline args)"),
        DeclareLaunchArgument(
            "serial_port",     default_value="/dev/ttyACM0",
<<<<<<< HEAD
            description="ESP32 USB CDC device node"),
=======
            description="ESP32-S3 USB CDC device node"),
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
        DeclareLaunchArgument(
            "baud_rate",       default_value="921600"),
        DeclareLaunchArgument(
            "heartbeat_period",default_value="0.2",
<<<<<<< HEAD
            description="Heartbeat interval (s); must be < ESP32 BALANCE HB timeout (0.5s)"),
=======
            description="Heartbeat interval (s); must be < ESP32-S3 HB timeout (0.5s)"),
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
        DeclareLaunchArgument(
            "max_linear_vel",  default_value="0.5",
            description="Hard speed cap before scaling (m/s)"),
        DeclareLaunchArgument(
            "max_angular_vel", default_value="2.0",
            description="Hard angular rate cap before scaling (rad/s)"),
        DeclareLaunchArgument(
            "speed_scale",     default_value="1000.0",
            description="m/s → ESC units (linear.x)"),
        DeclareLaunchArgument(
            "steer_scale",     default_value="-500.0",
            description="rad/s → ESC units (angular.z, neg=flip CCW)"),
        DeclareLaunchArgument(
            "ramp_rate",       default_value="500",
            description="Max ESC-unit change per second (acceleration limit)"),
        DeclareLaunchArgument(
            "cmd_vel_timeout", default_value="0.5",
            description="Deadman: zero targets if /cmd_vel silent this long (s)"),
        OpaqueFunction(function=_launch_bridge),
    ])
