from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
from launch.substitutions import (
    PathJoinSubstitution,
    LaunchConfiguration,
)
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.substitutions import FindPackageShare
from launch_ros.actions import Node
from launch_ros.descriptions import ParameterFile
from launch.conditions import IfCondition

def generate_launch_description():

    pkg_wave_rover_controller = FindPackageShare(package="").find("wave_rover_controller")
    pkg_unity_robot_controller = FindPackageShare(package="").find("unity_robot_controller")

    # Launch arguments
    launch_args = [
        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"
        ),
        DeclareLaunchArgument(
            "use_joy", default_value="False", description="Use joystick for control"
        ),
    ]


    joy = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            PathJoinSubstitution([pkg_wave_rover_controller, "launch", "joy.launch.py"])
        ),
        launch_arguments={"namespace": LaunchConfiguration("namespace")}.items(),
        condition=IfCondition(LaunchConfiguration('use_joy'))
    )

    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_wave_rover_controller_node = Node(
    #     package="wave_rover_controller",
    #     executable="joy_control_node",
    #     name="joy_control",
    #     # output="screen",
    #     namespace=LaunchConfiguration("namespace"),
    #     parameters=[
    #         ParameterFile(joy_params),
    #         {'serial_port': '/dev/ttyUSB0'}]
    # )

    wave_rover_node = Node(
        package="wave_rover_controller",
        executable="wave_rover_node",
        name="wave_rover_node",
        # output="screen",
        namespace=LaunchConfiguration("namespace"),
        parameters=[
            ParameterFile(joy_params),
            {'serial_port': '/dev/ttyUSB0'}]
    )

    return LaunchDescription(
        launch_args
        + [
            joy,
            wave_rover_controller_node,
            ]
    )