#!/usr/bin/env python3
"""Launch file for terrain classification node."""

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


def generate_launch_description():
    """Generate launch description."""
    depth_topic = LaunchConfiguration("depth_topic", default="/camera/depth/image_rect_raw")
    lidar_topic = LaunchConfiguration("lidar_topic", default="/scan")
    control_rate = LaunchConfiguration("control_rate", default="10.0")
    confidence_threshold = LaunchConfiguration("confidence_threshold", default="0.5")

    return LaunchDescription([
        DeclareLaunchArgument("depth_topic", default_value="/camera/depth/image_rect_raw",
                            description="RealSense depth topic"),
        DeclareLaunchArgument("lidar_topic", default_value="/scan",
                            description="RPLIDAR scan topic"),
        DeclareLaunchArgument("control_rate", default_value="10.0",
                            description="Control loop rate (Hz)"),
        DeclareLaunchArgument("confidence_threshold", default_value="0.5",
                            description="Minimum confidence to publish"),

        Node(
            package="saltybot_terrain_classification",
            executable="terrain_classification_node",
            name="terrain_classification",
            output="screen",
            parameters=[
                {"depth_topic": depth_topic},
                {"lidar_topic": lidar_topic},
                {"control_rate": control_rate},
                {"confidence_threshold": confidence_threshold},
                {"depth_std_threshold": 0.02},
                {"min_depth_m": 0.1},
                {"max_depth_m": 3.0},
                {"hysteresis_samples": 5},
            ],
        ),
    ])
