"""
adaptive_pid.launch.py — Launch the adaptive balance PID controller.

Usage:
    ros2 launch saltybot_adaptive_pid adaptive_pid.launch.py
    ros2 launch saltybot_adaptive_pid adaptive_pid.launch.py balance_setpoint_rad:=0.02

Dynamic reconfigure (no restart needed):
    ros2 param set /adaptive_pid gain_blend_alpha 0.1
    ros2 param set /adaptive_pid override_enabled true
    ros2 param set /adaptive_pid override_kp 12.0
    ros2 param set /adaptive_pid override_ki 0.4
    ros2 param set /adaptive_pid override_kd 1.2
    ros2 param set /adaptive_pid override_enabled false

Calibration:
    ros2 service call /saltybot/calibrate_balance std_srvs/srv/Trigger
"""

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_adaptive_pid")
    params_file = os.path.join(pkg_share, "config", "adaptive_pid_params.yaml")

    setpoint_arg = DeclareLaunchArgument(
        "balance_setpoint_rad",
        default_value="0.0",
        description="Target tilt angle in rad (0 = upright)",
    )

    adaptive_pid = Node(
        package="saltybot_adaptive_pid",
        executable="adaptive_pid_node",
        name="adaptive_pid",
        parameters=[
            params_file,
            {"balance_setpoint_rad": LaunchConfiguration("balance_setpoint_rad")},
        ],
        output="screen",
        emulate_tty=True,
    )

    return LaunchDescription([
        setpoint_arg,
        adaptive_pid,
    ])
