from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from pathlib import Path


def generate_launch_description():
    pkg_share = FindPackageShare("saltybot_object_detection")
    config_dir = Path(str(pkg_share)) / "config"
    config_file = str(config_dir / "object_detection_params.yaml")

    # Declare launch arguments
    confidence_threshold_arg = DeclareLaunchArgument(
        "confidence_threshold",
        default_value="0.5",
        description="Detection confidence threshold (0-1)"
    )

    publish_debug_arg = DeclareLaunchArgument(
        "publish_debug_image",
        default_value="false",
        description="Publish annotated debug images"
    )

    # Object detection node
    object_detection_node = Node(
        package="saltybot_object_detection",
        executable="object_detection",
        name="object_detection",
        parameters=[
            config_file,
            {"confidence_threshold": LaunchConfiguration("confidence_threshold")},
            {"publish_debug_image": LaunchConfiguration("publish_debug_arg")},
        ],
        remappings=[
            ("color_image", "/camera/color/image_raw"),
            ("depth_image", "/camera/depth/image_rect_raw"),
            ("camera_info", "/camera/color/camera_info"),
        ],
        output="screen",
    )

    return LaunchDescription([
        confidence_threshold_arg,
        publish_debug_arg,
        object_detection_node,
    ])
