#!/usr/bin/env python3
"""Launch the racing_control orchestration 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_dir = get_package_share_directory("racing_control")
    default_params = os.path.join(pkg_dir, "config", "racing_control.yaml")

    params_file = LaunchConfiguration("params_file")
    auto_start = LaunchConfiguration("auto_start")
    split_qr_to_vlm_segment = LaunchConfiguration("split_qr_to_vlm_segment")
    enable_vlm_image_relay = LaunchConfiguration("enable_vlm_image_relay")
    vlm_image_input_topic = LaunchConfiguration("vlm_image_input_topic")
    vlm_image_output_topic = LaunchConfiguration("vlm_image_output_topic")

    racing_control = Node(
        package="racing_control",
        executable="racing_control",
        name="racing_control",
        output="screen",
        parameters=[
            params_file,
            {
                "auto_start": auto_start,
                "split_qr_to_vlm_segment": split_qr_to_vlm_segment,
                "enable_vlm_image_relay": enable_vlm_image_relay,
                "vlm_image_input_topic": vlm_image_input_topic,
                "vlm_image_output_topic": vlm_image_output_topic,
            },
        ],
    )

    return LaunchDescription(
        [
            DeclareLaunchArgument(
                "params_file",
                default_value=default_params,
                description="YAML parameter file for the racing_control node",
            ),
            DeclareLaunchArgument(
                "auto_start",
                default_value="false",
                description="Start the race immediately instead of waiting for SPACE",
            ),
            DeclareLaunchArgument(
                "split_qr_to_vlm_segment",
                default_value="true",
                description="Stop at the VLM waypoint before planning the final home segment",
            ),
            DeclareLaunchArgument(
                "enable_vlm_image_relay",
                default_value="false",
                description="Publish one cached image frame to /vlm_image before VLM trigger",
            ),
            DeclareLaunchArgument(
                "vlm_image_input_topic",
                default_value="/image",
                description="CompressedImage input topic used for one-frame VLM relay",
            ),
            DeclareLaunchArgument(
                "vlm_image_output_topic",
                default_value="/vlm_image",
                description="CompressedImage output topic for one-frame VLM relay",
            ),
            racing_control,
        ]
    )