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("planner")

    params_file = LaunchConfiguration(
        "params_file",
        default=os.path.join(pkg_dir, "config", "topology_pure_pursuit.yaml"),
    )
    graph_file = LaunchConfiguration(
        "graph_file",
        default=os.path.join(pkg_dir, "config", "topology_graph.yaml"),
    )
    use_sim_time = LaunchConfiguration("use_sim_time", default="false")

    topology_node = Node(
        package="planner",
        executable="topology_pure_pursuit_node.py",
        name="topology_pure_pursuit",
        output="screen",
        parameters=[
            params_file,
            {
                "graph_file": graph_file,
                "use_sim_time": use_sim_time,
            },
        ],
    )

    return LaunchDescription(
        [
            DeclareLaunchArgument(
                "params_file",
                default_value=os.path.join(
                    pkg_dir, "config", "topology_pure_pursuit.yaml"
                ),
                description="Parameters for topology Pure Pursuit.",
            ),
            DeclareLaunchArgument(
                "graph_file",
                default_value=os.path.join(pkg_dir, "config", "topology_graph.yaml"),
                description="Topology graph YAML.",
            ),
            DeclareLaunchArgument(
                "use_sim_time",
                default_value="false",
                description="Use simulation clock.",
            ),
            topology_node,
        ]
    )
