from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition
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:
    config_file = PathJoinSubstitution([
        FindPackageShare("xr_rm_bringup"),
        "config",
        LaunchConfiguration("robot_config"),
    ])

    return LaunchDescription([
        DeclareLaunchArgument("udp_host", default_value="0.0.0.0"),
        DeclareLaunchArgument("udp_port", default_value="15000"),
        DeclareLaunchArgument("left_robot_ip", default_value="192.168.192.18"),
        DeclareLaunchArgument("right_robot_ip", default_value="192.168.192.19"),
        DeclareLaunchArgument("robot_port", default_value="8080"),
        DeclareLaunchArgument("robot_config", default_value="dual_arm_rm75.yaml"),
        DeclareLaunchArgument("run_udp", default_value="true"),
        DeclareLaunchArgument("run_left_arm", default_value="true"),
        DeclareLaunchArgument("run_right_arm", default_value="true"),
        DeclareLaunchArgument("move_to_initial_pose_on_connect", default_value="false"),
        Node(
            package="xr_rm_input",
            executable="udp_controller_receiver",
            name="udp_controller_receiver",
            output="screen",
            condition=IfCondition(LaunchConfiguration("run_udp")),
            parameters=[{
                "udp_host": LaunchConfiguration("udp_host"),
                "udp_port": LaunchConfiguration("udp_port"),
                "left_topic": "/xr/left_controller",
                "right_topic": "/xr/right_controller",
            }],
        ),
        Node(
            package="xr_rm_teleop",
            executable="single_arm_velocity_teleop",
            name="left_arm_teleop",
            output="screen",
            condition=IfCondition(LaunchConfiguration("run_left_arm")),
            parameters=[
                config_file,
                {
                    "use_mock": False,
                    "robot_ip": LaunchConfiguration("left_robot_ip"),
                    "robot_port": LaunchConfiguration("robot_port"),
                    "move_to_initial_pose_on_connect": ParameterValue(
                        LaunchConfiguration("move_to_initial_pose_on_connect"),
                        value_type=bool,
                    ),
                },
            ],
        ),
        Node(
            package="xr_rm_teleop",
            executable="single_arm_velocity_teleop",
            name="right_arm_teleop",
            output="screen",
            condition=IfCondition(LaunchConfiguration("run_right_arm")),
            parameters=[
                config_file,
                {
                    "use_mock": False,
                    "robot_ip": LaunchConfiguration("right_robot_ip"),
                    "robot_port": LaunchConfiguration("robot_port"),
                    "move_to_initial_pose_on_connect": ParameterValue(
                        LaunchConfiguration("move_to_initial_pose_on_connect"),
                        value_type=bool,
                    ),
                },
            ],
        ),
    ])