from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue
from launch_ros.substitutions import FindPackageShare


def generate_launch_description() -> LaunchDescription:
    teleop_config = PathJoinSubstitution([
        FindPackageShare('udp_teleop_bridge'),
        'config',
        'xbox_twist_joy.yaml',
    ])

    teleop_topic = LaunchConfiguration('teleop_topic')

    return LaunchDescription([
        DeclareLaunchArgument('transport', default_value='udp'),
        DeclareLaunchArgument('server_addr', default_value=''),
        DeclareLaunchArgument('relay_via', default_value=''),
        DeclareLaunchArgument('peer_id', default_value='ros-gamepad-ctrl'),
        DeclareLaunchArgument('target_peer', default_value='ros-bridge-ctrl'),
        DeclareLaunchArgument('joy_dev', default_value='/dev/input/js0'),
        DeclareLaunchArgument('deadzone', default_value='0.10'),
        DeclareLaunchArgument('autorepeat_rate', default_value='20.0'),
        DeclareLaunchArgument('frame_id', default_value='pelvis'),
        DeclareLaunchArgument('teleop_topic', default_value='/teleop/cmd_vel'),
        DeclareLaunchArgument('send_rate_hz', default_value='20.0'),
        DeclareLaunchArgument('input_timeout', default_value='0.30'),
        Node(
            package='joy',
            executable='joy_node',
            name='joy_node',
            output='screen',
            parameters=[{
                'dev': LaunchConfiguration('joy_dev'),
                'deadzone': ParameterValue(LaunchConfiguration('deadzone'), value_type=float),
                'autorepeat_rate': ParameterValue(LaunchConfiguration('autorepeat_rate'), value_type=float),
            }],
        ),
        Node(
            package='teleop_twist_joy',
            executable='teleop_node',
            name='teleop_twist_joy',
            output='screen',
            parameters=[
                teleop_config,
                {
                    'publish_stamped_twist': True,
                    'frame': LaunchConfiguration('frame_id'),
                },
            ],
            remappings=[
                ('cmd_vel', teleop_topic),
            ],
        ),
        Node(
            package='udp_teleop_bridge',
            executable='cmd_vel_udp_sender',
            name='cmd_vel_udp_sender',
            output='screen',
            parameters=[{
                'transport': LaunchConfiguration('transport'),
                'server_addr': LaunchConfiguration('server_addr'),
                'relay_via': LaunchConfiguration('relay_via'),
                'peer_id': LaunchConfiguration('peer_id'),
                'target_peer': LaunchConfiguration('target_peer'),
                'input_topic': teleop_topic,
                'send_rate_hz': ParameterValue(LaunchConfiguration('send_rate_hz'), value_type=float),
                'input_timeout': ParameterValue(LaunchConfiguration('input_timeout'), value_type=float),
            }],
        ),
    ])