"""PICO/XR to dual-arm QP IK with a shared MuJoCo backend."""

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
from launch.substitutions import (
    EnvironmentVariable,
    LaunchConfiguration,
    PathJoinSubstitution,
)
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare


def generate_launch_description() -> LaunchDescription:
    config_dir = PathJoinSubstitution(
        [FindPackageShare("xr_rm_bringup"), "config"]
    )
    return LaunchDescription(
        [
            DeclareLaunchArgument("mujoco_gl", default_value="glfw"),
            SetEnvironmentVariable(
                "MUJOCO_GL",
                LaunchConfiguration("mujoco_gl"),
            ),
            SetEnvironmentVariable(
                "PYTHONPATH",
                [
                    PathJoinSubstitution(
                        [
                            EnvironmentVariable("CONDA_PREFIX"),
                            "lib/python3.10/site-packages",
                        ]
                    ),
                    ":",
                    PathJoinSubstitution(
                        [
                            EnvironmentVariable("CONDA_PREFIX"),
                            "lib/python3.10/site-packages/cmeel.prefix/lib/python3.10/site-packages",
                        ]
                    ),
                    ":",
                    EnvironmentVariable("PYTHONPATH", default_value=""),
                ],
            ),
            DeclareLaunchArgument("udp_host", default_value="0.0.0.0"),
            DeclareLaunchArgument("udp_port", default_value="15000"),
            DeclareLaunchArgument("udp_timer_hz", default_value="200.0"),
            DeclareLaunchArgument("control_rate_hz", default_value="90.0"),
            DeclareLaunchArgument("show_viewer", default_value="true"),
            Node(
                package="xr_rm_input",
                executable="udp_controller_receiver",
                name="udp_controller_receiver",
                output="screen",
                parameters=[
                    {
                        "udp_host": LaunchConfiguration("udp_host"),
                        "udp_port": LaunchConfiguration("udp_port"),
                        "timer_hz": LaunchConfiguration("udp_timer_hz"),
                        "left_topic": "/xr/left_controller",
                        "right_topic": "/xr/right_controller",
                    }
                ],
            ),
            Node(
                package="xr_rm_teleop",
                executable="dual_arm_qp_teleop",
                name="dual_arm_qp_teleop",
                output="screen",
                parameters=[
                    {
                        "robot_backend": "mujoco",
                        "control_rate_hz": LaunchConfiguration("control_rate_hz"),
                        "show_viewer": LaunchConfiguration("show_viewer"),
                        "teleop_config_file": PathJoinSubstitution(
                            [config_dir, "dual_arm_rm75.yaml"]
                        ),
                        "peripheral_config_file": PathJoinSubstitution(
                            [config_dir, "peripherals_rm75.yaml"]
                        ),
                    }
                ],
            ),
        ]
    )