"""单臂/双臂通用调试入口。

该 launch 文件用于现场调试阶段按需启动左臂、右臂或双臂，并可通过
`use_mock` 在 mock 模式和 RM75 真机模式之间切换。它会固定启动 UDP
手柄接收节点，再根据 `arm:=left|right|both` 选择对应的遥操作节点。
"""

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


def _as_bool(value: str) -> bool:
    """把 launch 字符串参数转换成 Python bool，便于在 OpaqueFunction 中分支。"""
    return value.strip().lower() in ("1", "true", "yes", "on")


def _config_file(name: str) -> PathJoinSubstitution:
    """生成 xr_rm_bringup/config 下配置文件的可安装路径。"""
    return PathJoinSubstitution([
        FindPackageShare("xr_rm_bringup"),
        "config",
        name,
    ])


def _udp_receiver_node() -> Node:
    """接收 PICO/XR UDP 数据，并发布左右手柄 ROS2 话题。"""
    return 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",
        }],
    )


def _single_arm_node(
    arm: str,
    use_mock: bool,
    move_to_initial_pose: bool,
    avoid_singularity: int,
    frame_type: int,
    control_rate_hz: float,
    follow: bool,
    configure_safety_limits: bool,
    enable_tool_control: bool,
    enable_trigger_gripper_control: bool,
    trigger_close_threshold: float,
    configure_peripheral_on_connect: bool,
) -> Node:
    """创建单臂调试节点；左/右臂分别使用独立 YAML，节点名保持单臂默认名。"""
    config_name = "left_arm_rm75.yaml" if arm == "left" else "right_arm_rm75.yaml"
    robot_ip = LaunchConfiguration("left_robot_ip" if arm == "left" else "right_robot_ip")
    arm_name = _arm_name(arm)
    return Node(
        package="xr_rm_teleop",
        executable="single_arm_velocity_teleop",
        name="single_arm_velocity_teleop",
        output="screen",
        parameters=[
            _config_file(config_name),
            {
                "use_mock": use_mock,
                "robot_ip": robot_ip,
                "robot_port": LaunchConfiguration("robot_port"),
                "avoid_singularity": avoid_singularity,
                "frame_type": frame_type,
                "control_rate_hz": control_rate_hz,
                "follow": follow,
                "configure_safety_limits": configure_safety_limits,
                "move_to_initial_pose_on_connect": move_to_initial_pose,
                "enable_tool_control": enable_tool_control,
                "enable_trigger_gripper_control": enable_trigger_gripper_control,
                "trigger_close_threshold": trigger_close_threshold,
                "configure_peripheral_on_connect": configure_peripheral_on_connect,
                "peripheral_config_file": _config_file("peripherals_rm75.yaml"),
                "peripheral_arm": arm,
                "tool_command_topic": f"/xr_rm/{arm_name}/tool_enable",
            },
        ],
    )


def _arm_name(arm: str) -> str:
    return "left_rm75" if arm == "left" else "right_rm75"


def _dual_arm_nodes(
    use_mock: bool,
    move_to_initial_pose: bool,
    left_avoid_singularity: int,
    right_avoid_singularity: int,
    frame_type: int,
    control_rate_hz: float,
    follow: bool,
    configure_safety_limits: bool,
    enable_tool_control: bool,
    enable_trigger_gripper_control: bool,
    trigger_close_threshold: float,
    configure_peripheral_on_connect: bool,
) -> list[Node]:
    """创建双臂节点；两个节点共用双臂 YAML，但节点名区分左右臂参数命名空间。"""
    config_file = _config_file("dual_arm_rm75.yaml")
    return [
        Node(
            package="xr_rm_teleop",
            executable="single_arm_velocity_teleop",
            name="left_arm_teleop",
            output="screen",
            parameters=[
                config_file,
                {
                    "use_mock": use_mock,
                    "robot_ip": LaunchConfiguration("left_robot_ip"),
                    "robot_port": LaunchConfiguration("robot_port"),
                    "avoid_singularity": left_avoid_singularity,
                    "frame_type": frame_type,
                    "control_rate_hz": control_rate_hz,
                    "follow": follow,
                    "configure_safety_limits": configure_safety_limits,
                    "move_to_initial_pose_on_connect": move_to_initial_pose,
                    "enable_tool_control": enable_tool_control,
                    "enable_trigger_gripper_control": enable_trigger_gripper_control,
                    "trigger_close_threshold": trigger_close_threshold,
                    "configure_peripheral_on_connect": configure_peripheral_on_connect,
                    "peripheral_config_file": _config_file("peripherals_rm75.yaml"),
                    "peripheral_arm": "left",
                    "tool_command_topic": "/xr_rm/left_rm75/tool_enable",
                },
            ],
        ),
        Node(
            package="xr_rm_teleop",
            executable="single_arm_velocity_teleop",
            name="right_arm_teleop",
            output="screen",
            parameters=[
                config_file,
                {
                    "use_mock": use_mock,
                    "robot_ip": LaunchConfiguration("right_robot_ip"),
                    "robot_port": LaunchConfiguration("robot_port"),
                    "avoid_singularity": right_avoid_singularity,
                    "frame_type": frame_type,
                    "control_rate_hz": control_rate_hz,
                    "follow": follow,
                    "configure_safety_limits": configure_safety_limits,
                    "move_to_initial_pose_on_connect": move_to_initial_pose,
                    "enable_tool_control": enable_tool_control,
                    "enable_trigger_gripper_control": enable_trigger_gripper_control,
                    "trigger_close_threshold": trigger_close_threshold,
                    "configure_peripheral_on_connect": configure_peripheral_on_connect,
                    "peripheral_config_file": _config_file("peripherals_rm75.yaml"),
                    "peripheral_arm": "right",
                    "tool_command_topic": "/xr_rm/right_rm75/tool_enable",
                },
            ],
        ),
    ]


def _launch_setup(context, *args, **kwargs):
    """运行时读取 launch 参数，决定启动单臂还是双臂。"""
    del args, kwargs
    arm = LaunchConfiguration("arm").perform(context).strip().lower()
    use_mock = _as_bool(LaunchConfiguration("use_mock").perform(context))
    move_to_initial_pose = _as_bool(
        LaunchConfiguration("move_to_initial_pose_on_connect").perform(context)
    )
    avoid_override = LaunchConfiguration("avoid_singularity").perform(context).strip()
    left_avoid_singularity = int(
        avoid_override or LaunchConfiguration("left_avoid_singularity").perform(context)
    )
    right_avoid_singularity = int(
        avoid_override or LaunchConfiguration("right_avoid_singularity").perform(context)
    )
    frame_type = int(LaunchConfiguration("frame_type").perform(context))
    control_rate_hz = float(LaunchConfiguration("control_rate_hz").perform(context))
    follow = _as_bool(LaunchConfiguration("follow").perform(context))
    configure_safety_limits = _as_bool(
        LaunchConfiguration("configure_safety_limits").perform(context)
    )
    configure_peripheral_on_connect = _as_bool(
        LaunchConfiguration("configure_peripheral_on_connect").perform(context)
    )
    enable_tool_control = _as_bool(
        LaunchConfiguration("enable_tool_control").perform(context)
    )
    enable_trigger_gripper_control = _as_bool(
        LaunchConfiguration("enable_trigger_gripper_control").perform(context)
    )
    trigger_close_threshold = float(
        LaunchConfiguration("trigger_close_threshold").perform(context)
    )

    if arm not in ("left", "right", "both"):
        raise ValueError("arm must be one of: left, right, both")

    nodes = [_udp_receiver_node()]
    if arm == "both":
        nodes.extend(
            _dual_arm_nodes(
                use_mock,
                move_to_initial_pose,
                left_avoid_singularity,
                right_avoid_singularity,
                frame_type,
                control_rate_hz,
                follow,
                configure_safety_limits,
                enable_tool_control,
                enable_trigger_gripper_control,
                trigger_close_threshold,
                configure_peripheral_on_connect,
            )
        )
    else:
        avoid_singularity = left_avoid_singularity if arm == "left" else right_avoid_singularity
        nodes.append(
            _single_arm_node(
                arm,
                use_mock,
                move_to_initial_pose,
                avoid_singularity,
                frame_type,
                control_rate_hz,
                follow,
                configure_safety_limits,
                enable_tool_control,
                enable_trigger_gripper_control,
                trigger_close_threshold,
                configure_peripheral_on_connect,
            )
        )
    return nodes


def generate_launch_description() -> LaunchDescription:
    return LaunchDescription([
        # 调试目标：left/right/both；默认右臂方便单臂逐步上真机。
        DeclareLaunchArgument("arm", default_value="right"),
        # true 时只跑 mock，不连接 RM75；false 时通过 RealMan SDK 连接真机。
        DeclareLaunchArgument("use_mock", default_value="true"),
        # UDP 监听参数，需要与 PICO 端或 sample_udp_sender 保持一致。
        DeclareLaunchArgument("udp_host", default_value="0.0.0.0"),
        DeclareLaunchArgument("udp_port", default_value="15000"),
        # UDP receiver 轮询频率高于 PICO 发送频率，减少 socket 中等待时间。
        DeclareLaunchArgument("udp_timer_hz", default_value="200.0"),
        # 左右 RM75 默认 IP，可在命令行中按现场网络覆盖。
        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("left_avoid_singularity", default_value="0"),
        DeclareLaunchArgument("right_avoid_singularity", default_value="1"),
        # 非空时作为左右臂全局覆盖，例如 avoid_singularity:=0。
        DeclareLaunchArgument("avoid_singularity", default_value=""),
        DeclareLaunchArgument("frame_type", default_value="1"),
        # 现场调参入口：默认按 PICO 90Hz 输入节奏发送 rm_movep_canfd。
        DeclareLaunchArgument("control_rate_hz", default_value="90.0"),
        # 默认低跟随；高跟随请确认控制器和网络能稳定满足厂商周期要求后再打开。
        DeclareLaunchArgument("follow", default_value="false"),
        DeclareLaunchArgument("configure_safety_limits", default_value="true"),
        # 工具控制通过遥操作节点复用同一个 RealMan 连接，避免两个进程抢同一机械臂连接。
        DeclareLaunchArgument("enable_tool_control", default_value="true"),
        # trigger 上升沿切换夹爪开/关；grip 仍只控制机械臂运动。
        DeclareLaunchArgument("enable_trigger_gripper_control", default_value="true"),
        DeclareLaunchArgument("trigger_close_threshold", default_value="0.95"),
        # 连接成功后是否配置外设；关闭后仅订阅开合话题，但开合前需要另行完成外设配置。
        DeclareLaunchArgument("configure_peripheral_on_connect", default_value="true"),
        # 默认不自动移动到初始点，确认安全区后再显式打开。
        DeclareLaunchArgument("move_to_initial_pose_on_connect", default_value="false"),
        # OpaqueFunction 允许根据 arm/use_mock 等运行时参数动态生成节点。
        OpaqueFunction(function=_launch_setup),
    ])
