"""
saltybot_bridge launch file.

Two deployment modes:

  1. Full bidirectional (recommended for Nav2):
       ros2 launch saltybot_bridge bridge.launch.py mode:=bidirectional
     Starts saltybot_cmd_node — owns serial port, handles both RX telemetry
     and TX /cmd_vel → ESP32-S3 commands + heartbeat.
  2. RX-only (telemetry monitor, no drive commands):
       ros2 launch saltybot_bridge bridge.launch.py mode:=rx_only
     Starts serial_bridge_node — telemetry RX only. Use when you want to
     observe telemetry without commanding the robot.

Note: never run both nodes simultaneously on the same serial port.
"""

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


def _launch_nodes(context, *args, **kwargs):
    mode       = LaunchConfiguration("mode").perform(context)
    port       = LaunchConfiguration("serial_port").perform(context)
    baud       = LaunchConfiguration("baud_rate").perform(context)
    spd_scale  = LaunchConfiguration("speed_scale").perform(context)
    str_scale  = LaunchConfiguration("steer_scale").perform(context)

    params = {
        "serial_port":      port,
        "baud_rate":        int(baud),
        "timeout":          0.05,
        "reconnect_delay":  2.0,
    }

    if mode == "rx_only":
        return [Node(
            package="saltybot_bridge",
            executable="serial_bridge_node",
            name="esp32_serial_bridge",
            output="screen",
            parameters=[params],
        )]

    # bidirectional (default)
    params.update({
        "heartbeat_period": 0.2,
        "speed_scale":      float(spd_scale),
        "steer_scale":      float(str_scale),
    })
    return [Node(
        package="saltybot_bridge",
        executable="saltybot_cmd_node",
        name="saltybot_cmd",
        output="screen",
        parameters=[params],
    )]


def generate_launch_description():
    return LaunchDescription([
        DeclareLaunchArgument("mode",         default_value="bidirectional",
                              description="bidirectional | rx_only"),
        DeclareLaunchArgument("serial_port",  default_value="/dev/ttyACM0",
                              description="ESP32-S3 USB CDC device node"),        DeclareLaunchArgument("baud_rate",    default_value="921600"),
        DeclareLaunchArgument("speed_scale",  default_value="1000.0",
                              description="m/s → ESC units (linear.x scale)"),
        DeclareLaunchArgument("steer_scale",  default_value="-500.0",
                              description="rad/s → ESC units (angular.z scale, neg=flip)"),
        OpaqueFunction(function=_launch_nodes),
    ])