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")
    planner_params = LaunchConfiguration(
        "planner_params",
        default=os.path.join(pkg_dir, "config", "grid_astar_theta.yaml"),
    )
    tracker_params = LaunchConfiguration(
        "tracker_params",
        default=os.path.join(pkg_dir, "config", "pure_pursuit_tracker.yaml"),
    )
    use_sim_time = LaunchConfiguration("use_sim_time", default="false")

    planner_node = Node(
        package="planner",
        executable="grid_astar_theta_node",
        name="grid_astar_theta_planner",
        output="screen",
        parameters=[planner_params, {"use_sim_time": use_sim_time}],
    )
    tracker_node = Node(
        package="planner",
        executable="topology_pure_pursuit_node.py",
        name="topology_pure_pursuit",
        output="screen",
        parameters=[tracker_params, {"use_sim_time": use_sim_time}],
    )

    return LaunchDescription(
        [
            DeclareLaunchArgument(
                "planner_params",
                default_value=os.path.join(pkg_dir, "config", "grid_astar_theta.yaml"),
                description="Parameters for lightweight Ackermann Hybrid A* planner.",
            ),
            DeclareLaunchArgument(
                "tracker_params",
                default_value=os.path.join(
                    pkg_dir, "config", "pure_pursuit_tracker.yaml"
                ),
                description="Parameters for yaw-aware tracker-only mode.",
            ),
            DeclareLaunchArgument(
                "use_sim_time",
                default_value="false",
                description="Use simulation clock.",
            ),
            planner_node,
            tracker_node,
        ]
    )
