"""
uwb.launch.py — Launch UWB driver node for MaUWB ESP32-S3 DW3000 (Issue #90).

Topics:
  /uwb/ranges   100 Hz raw TWR ranges
  /uwb/bearing   10 Hz Kalman-fused bearing
  /uwb/target    10 Hz triangulated PoseStamped (backwards compat)

Usage:
  ros2 launch saltybot_uwb uwb.launch.py
  ros2 launch saltybot_uwb uwb.launch.py enrolled_tag_ids:="['0xDEADBEEF']"
"""

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 = get_package_share_directory("saltybot_uwb")
    cfg = os.path.join(pkg, "config", "uwb_config.yaml")

    return LaunchDescription([
        DeclareLaunchArgument("port_a",            default_value="/dev/uwb-anchor0"),
        DeclareLaunchArgument("port_b",            default_value="/dev/uwb-anchor1"),
        DeclareLaunchArgument("baudrate",           default_value="115200"),
        DeclareLaunchArgument("anchor_separation",  default_value="0.25"),
        DeclareLaunchArgument("anchor_height",      default_value="0.80"),
        DeclareLaunchArgument("tag_height",         default_value="0.90"),
        DeclareLaunchArgument("range_timeout_s",    default_value="1.0"),
        DeclareLaunchArgument("max_range_m",        default_value="8.0"),
        DeclareLaunchArgument("min_range_m",        default_value="0.05"),
        DeclareLaunchArgument("kf_process_noise",   default_value="0.1"),
        DeclareLaunchArgument("kf_meas_noise",      default_value="0.3"),
        DeclareLaunchArgument("range_rate",         default_value="100.0"),
        DeclareLaunchArgument("bearing_rate",       default_value="10.0"),
        DeclareLaunchArgument("enrolled_tag_ids",   default_value="['']"),

        Node(
            package="saltybot_uwb",
            executable="uwb_driver",
            name="uwb_driver",
            output="screen",
            parameters=[
                cfg,
                {
                    "port_a":            LaunchConfiguration("port_a"),
                    "port_b":            LaunchConfiguration("port_b"),
                    "baudrate":          LaunchConfiguration("baudrate"),
                    "anchor_separation": LaunchConfiguration("anchor_separation"),
                    "anchor_height":     LaunchConfiguration("anchor_height"),
                    "tag_height":        LaunchConfiguration("tag_height"),
                    "range_timeout_s":   LaunchConfiguration("range_timeout_s"),
                    "max_range_m":       LaunchConfiguration("max_range_m"),
                    "min_range_m":       LaunchConfiguration("min_range_m"),
                    "kf_process_noise":  LaunchConfiguration("kf_process_noise"),
                    "kf_meas_noise":     LaunchConfiguration("kf_meas_noise"),
                    "range_rate":        LaunchConfiguration("range_rate"),
                    "bearing_rate":      LaunchConfiguration("bearing_rate"),
                    "enrolled_tag_ids":  LaunchConfiguration("enrolled_tag_ids"),
                },
            ],
        ),
    ])
