"""mode_switch.launch.py — launch mode_switch_node + cmd_vel_mux_node."""

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_share    = get_package_share_directory("saltybot_mode_switch")
    default_cfg  = os.path.join(pkg_share, "config", "mode_switch_params.yaml")

    return LaunchDescription([
        DeclareLaunchArgument(
            "params_file",
            default_value=default_cfg,
            description="Path to mode_switch_params.yaml",
        ),
        DeclareLaunchArgument(
            "ramp_duration_s",
            default_value="0.5",
            description="RC↔AUTO blend ramp duration (seconds)",
        ),
        DeclareLaunchArgument(
            "return_delay_s",
            default_value="2.0",
            description="Sticks-neutral duration before auto re-engage (seconds)",
        ),
        DeclareLaunchArgument(
            "slam_fix_topic",
            default_value="/slam_toolbox/pose_with_covariance_stamped",
            description="SLAM fix pose topic",
        ),

        Node(
            package="saltybot_mode_switch",
            executable="mode_switch_node",
            name="mode_switch",
            output="screen",
            parameters=[
                LaunchConfiguration("params_file"),
                {
                    "ramp_duration_s": LaunchConfiguration("ramp_duration_s"),
                    "return_delay_s":  LaunchConfiguration("return_delay_s"),
                    "slam_fix_topic":  LaunchConfiguration("slam_fix_topic"),
                },
            ],
        ),

        Node(
            package="saltybot_mode_switch",
            executable="cmd_vel_mux_node",
            name="cmd_vel_mux",
            output="screen",
            parameters=[LaunchConfiguration("params_file")],
        ),
    ])
