from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import (
    PathJoinSubstitution,
    LaunchConfiguration,
    TextSubstitution,
)
from launch_ros.substitutions import FindPackageShare
from launch_ros.actions import Node
from launch_ros.descriptions import ParameterFile

def generate_launch_description():
    pkg_unity_robot_controller = FindPackageShare(package="").find("unity_robot_controller")


    # Launch arguments
    launch_args = [
        DeclareLaunchArgument(
            "use_sim_time",
            description="Use clock from simulation",
            default_value="True",
        ),
        DeclareLaunchArgument(
            "joy_model",
            description="Used joystick model (default is T-29)",
            default_value="T-29.yaml",
        ),
        DeclareLaunchArgument(
            "namespace", default_value="sirius_bot", description="Top-level namespace"
        ),
    ]

    joy_config = PathJoinSubstitution([pkg_unity_robot_controller, "config", "joy_config.yaml"])

    model = LaunchConfiguration("joy_model")
    joy_params = PathJoinSubstitution([pkg_unity_robot_controller,
                                       "config",
                                       model
                                       ])

    joy_node = Node(
        package="joy",
        executable="joy_node",
        name="joy",
        namespace=LaunchConfiguration("namespace"),
        output="both",
        parameters=[
            ParameterFile(joy_config),
        ],
    )

    joy_control_node = Node(
        package="unity_robot_controller",
        executable="joy_control_node",
        name="joy_control",
        namespace=LaunchConfiguration("namespace"),
            output="both",
            parameters=[
                ParameterFile(joy_params),
            ],
    )

    return LaunchDescription(
        launch_args
        + [
            joy_node,
            joy_control_node,
        ]
    )