"""RM75 单臂 XR 相对位姿透传遥操作节点。

节点订阅左/右手柄位姿，在 grip 按下时锁定手柄和 TCP 起点，把手柄相对位姿
映射成机器人坐标系中的目标 TCP，并通过 rm_movep_canfd 持续下发目标位姿。
"""

from __future__ import annotations

import math
import queue
import threading
from typing import Iterable

import rclpy
from geometry_msgs.msg import PoseStamped, TwistStamped
from rclpy.node import Node
from rclpy.time import Time
from std_msgs.msg import Bool

from xr_rm_interfaces.msg import XrController

from .realman_adapter import ArmPose, MockRealManAdapter, RealManAdapter


def _norm(values: Iterable[float]) -> float:
    return math.sqrt(sum(value * value for value in values))


def _clamp(value: float, low: float, high: float) -> float:
    return min(max(value, low), high)


def _wrap_angle(angle: float) -> float:
    return math.atan2(math.sin(angle), math.cos(angle))


def _angle_delta(target: float, current: float) -> float:
    return _wrap_angle(target - current)


def _normalize_quaternion(values: Iterable[float]) -> tuple[float, float, float, float]:
    x, y, z, w = [float(value) for value in values]
    norm = math.sqrt(x * x + y * y + z * z + w * w)
    if norm <= 1e-9 or not math.isfinite(norm):
        raise ValueError("quaternion norm is zero or non-finite")
    return x / norm, y / norm, z / norm, w / norm


def _quaternion_conjugate(
    quat: tuple[float, float, float, float],
) -> tuple[float, float, float, float]:
    x, y, z, w = quat
    return -x, -y, -z, w


def _quaternion_multiply(
    left: tuple[float, float, float, float],
    right: tuple[float, float, float, float],
) -> tuple[float, float, float, float]:
    lx, ly, lz, lw = left
    rx, ry, rz, rw = right
    return _normalize_quaternion(
        (
            lw * rx + lx * rw + ly * rz - lz * ry,
            lw * ry - lx * rz + ly * rw + lz * rx,
            lw * rz + lx * ry - ly * rx + lz * rw,
            lw * rw - lx * rx - ly * ry - lz * rz,
        )
    )


def _quaternion_to_matrix(quat: tuple[float, float, float, float]) -> list[float]:
    x, y, z, w = quat
    xx = x * x
    yy = y * y
    zz = z * z
    xy = x * y
    xz = x * z
    yz = y * z
    wx = w * x
    wy = w * y
    wz = w * z
    return [
        1.0 - 2.0 * (yy + zz),
        2.0 * (xy - wz),
        2.0 * (xz + wy),
        2.0 * (xy + wz),
        1.0 - 2.0 * (xx + zz),
        2.0 * (yz - wx),
        2.0 * (xz - wy),
        2.0 * (yz + wx),
        1.0 - 2.0 * (xx + yy),
    ]


def _matrix_multiply(left: list[float], right: list[float]) -> list[float]:
    return [
        sum(left[row * 3 + k] * right[k * 3 + col] for k in range(3))
        for row in range(3)
        for col in range(3)
    ]


def _matrix_transpose(matrix: list[float]) -> list[float]:
    return [
        matrix[0], matrix[3], matrix[6],
        matrix[1], matrix[4], matrix[7],
        matrix[2], matrix[5], matrix[8],
    ]


def _euler_to_quaternion(roll: float, pitch: float, yaw: float) -> tuple[float, float, float, float]:
    cy = math.cos(yaw * 0.5)
    sy = math.sin(yaw * 0.5)
    cp = math.cos(pitch * 0.5)
    sp = math.sin(pitch * 0.5)
    cr = math.cos(roll * 0.5)
    sr = math.sin(roll * 0.5)

    qw = cr * cp * cy + sr * sp * sy
    qx = sr * cp * cy - cr * sp * sy
    qy = cr * sp * cy + sr * cp * sy
    qz = cr * cp * sy - sr * sp * cy
    return _normalize_quaternion((qx, qy, qz, qw))


def _euler_to_matrix(roll: float, pitch: float, yaw: float) -> list[float]:
    return _quaternion_to_matrix(_euler_to_quaternion(roll, pitch, yaw))


def _matrix_to_euler(matrix: list[float]) -> tuple[float, float, float]:
    sy = -_clamp(matrix[6], -1.0, 1.0)
    pitch = math.asin(sy)
    cp = math.cos(pitch)
    if abs(cp) > 1e-9:
        roll = math.atan2(matrix[7], matrix[8])
        yaw = math.atan2(matrix[3], matrix[0])
    else:
        roll = math.atan2(-matrix[5], matrix[4])
        yaw = 0.0
    return _wrap_angle(roll), _wrap_angle(pitch), _wrap_angle(yaw)


def _quaternion_to_euler(quat: tuple[float, float, float, float]) -> tuple[float, float, float]:
    return _matrix_to_euler(_quaternion_to_matrix(quat))


class SingleArmVelocityTeleop(Node):
    """基于 XR 手柄相对位姿的 RM75 单臂位姿透传遥操节点。

    类名和可执行入口沿用旧名称，避免已有 launch、UI 和命令失效。
    """

    def __init__(self) -> None:
        super().__init__("single_arm_velocity_teleop")

        self.declare_parameter("arm_name", "rm75")
        self.declare_parameter("controller_topic", "/xr/right_controller")
        self.declare_parameter("control_rate_hz", 90.0)
        self.declare_parameter("command_timeout_sec", 0.12)
        self.declare_parameter("scale", 1.0)
        self.declare_parameter("deadband_m", 0.001)
        self.declare_parameter("target_filter_alpha", 0.65)
        self.declare_parameter("target_filter_alpha_fast", 0.9)
        self.declare_parameter("target_filter_fast_threshold_m", 0.03)
        self.declare_parameter("max_linear_speed", 0.25)
        self.declare_parameter("enable_position_axes", [True, True, True])
        self.declare_parameter("enable_orientation_control", False)
        self.declare_parameter("enable_orientation_axes", [True, True, True])
        self.declare_parameter("orientation_deadband_rad", 0.005)
        self.declare_parameter("orientation_filter_alpha", 0.65)
        self.declare_parameter("max_orientation_speed", 0.6)
        self.declare_parameter("workspace_min", [0.20, -0.35, 0.10])
        self.declare_parameter("workspace_max", [0.65, 0.35, 0.60])
        self.declare_parameter("cyl_radius_limit", [0.20, 0.60])
        self.declare_parameter("low_z_threshold", 0.20)
        self.declare_parameter("low_z_min_radius", 0.21)
        self.declare_parameter("xr_to_robot_matrix", [0.0, 0.0, -1.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0])
        self.declare_parameter("current_pose_poll_hz", 10.0)
        self.declare_parameter("use_mock", True)
        self.declare_parameter("mock_initial_pose", [0.35, 0.0, 0.30, 0.0, 0.0, 0.0])
        self.declare_parameter("robot_ip", "192.168.1.18")
        self.declare_parameter("robot_port", 8080)
        self.declare_parameter("avoid_singularity", 1)
        self.declare_parameter("frame_type", 1)
        self.declare_parameter("follow", False)
        self.declare_parameter("configure_safety_limits", True)
        self.declare_parameter("max_line_speed", 1.0)
        self.declare_parameter("max_angular_speed", 1.5)
        self.declare_parameter("max_line_acc", 1.0)
        self.declare_parameter("max_angular_acc", 2.0)
        self.declare_parameter("joint_max_speed", 180.0)
        self.declare_parameter("joint_max_acc", 180.0)
        self.declare_parameter("move_to_initial_pose_on_connect", False)
        self.declare_parameter("initial_joint_pose", [0.0] * 7)
        self.declare_parameter("initial_tcp_pose", [0.35, 0.0, 0.30, 0.0, 0.0, 0.0])
        self.declare_parameter("init_move_speed", 20)
        self.declare_parameter("canfd_trajectory_mode", 2)
        self.declare_parameter("canfd_radio", 0)
        self.declare_parameter("enable_tool_control", True)
        self.declare_parameter("enable_trigger_gripper_control", True)
        self.declare_parameter("trigger_close_threshold", 0.95)
        self.declare_parameter("configure_peripheral_on_connect", True)
        self.declare_parameter("peripheral_config_file", "")
        self.declare_parameter("peripheral_arm", "")
        self.declare_parameter("tool_command_topic", "")
        self.declare_parameter("debug_topic_prefix", "/xr_rm")

        self._arm_name = str(self.get_parameter("arm_name").value)
        topic = self.get_parameter("controller_topic").value
        control_rate_hz = float(self.get_parameter("control_rate_hz").value)
        if control_rate_hz <= 0.0:
            raise ValueError("control_rate_hz must be > 0")
        self._dt = 1.0 / control_rate_hz
        self._command_timeout_sec = float(self.get_parameter("command_timeout_sec").value)
        self._scale = float(self.get_parameter("scale").value)
        self._deadband_m = float(self.get_parameter("deadband_m").value)
        self._target_filter_alpha = float(self.get_parameter("target_filter_alpha").value)
        self._target_filter_alpha_fast = float(self.get_parameter("target_filter_alpha_fast").value)
        self._target_filter_fast_threshold_m = float(
            self.get_parameter("target_filter_fast_threshold_m").value
        )
        self._max_linear_speed = float(self.get_parameter("max_linear_speed").value)
        self._enable_position_axes = self._bool_list_parameter("enable_position_axes", 3)
        self._enable_orientation_control = self._bool_parameter("enable_orientation_control")
        self._enable_orientation_axes = self._bool_list_parameter("enable_orientation_axes", 3)
        self._orientation_deadband_rad = float(self.get_parameter("orientation_deadband_rad").value)
        self._orientation_filter_alpha = float(self.get_parameter("orientation_filter_alpha").value)
        self._max_orientation_speed = float(self.get_parameter("max_orientation_speed").value)
        self._workspace_min = self._float_list_parameter("workspace_min", 3)
        self._workspace_max = self._float_list_parameter("workspace_max", 3)
        self._cyl_radius_limit = self._float_list_parameter("cyl_radius_limit", 2)
        self._low_z_threshold = float(self.get_parameter("low_z_threshold").value)
        self._low_z_min_radius = float(self.get_parameter("low_z_min_radius").value)
        self._xr_to_robot_matrix = self._float_list_parameter("xr_to_robot_matrix", 9)
        self._current_pose_poll_hz = float(self.get_parameter("current_pose_poll_hz").value)
        self._follow = self._bool_parameter("follow")
        self._enable_tool_control = self._bool_parameter("enable_tool_control")
        self._enable_trigger_gripper_control = self._bool_parameter("enable_trigger_gripper_control")
        self._trigger_close_threshold = float(self.get_parameter("trigger_close_threshold").value)
        self._debug_topic_prefix = str(self.get_parameter("debug_topic_prefix").value).rstrip("/")
        if not self._debug_topic_prefix:
            self._debug_topic_prefix = "/xr_rm"
        self._validate_parameters()

        self._last_msg: XrController | None = None
        self._last_msg_time: Time | None = None
        self._active = False
        self._controller_start: list[float] | None = None
        self._controller_orientation_start: tuple[float, float, float, float] | None = None
        self._robot_start_pose: ArmPose | None = None
        self._filtered_target: list[float] | None = None
        self._filtered_orientation_target: list[float] | None = None
        self._last_sent_target: list[float] | None = None
        self._last_sent_orientation: list[float] | None = None
        self._last_command_time: Time | None = None
        self._last_current_pose: ArmPose | None = None
        self._last_current_pose_time: Time | None = None
        self._stop_sent = True
        self._trigger_tool_open = True
        self._last_trigger_pressed: bool | None = None
        self._tool_command_queue: queue.Queue[tuple[bool, str] | None] | None = None
        self._tool_worker_stop = threading.Event()
        self._tool_worker_thread: threading.Thread | None = None

        self._adapter = self._make_adapter()
        self._adapter.connect()
        self._setup_tool_control()

        debug_ns = f"{self._debug_topic_prefix}/{self._arm_name}"
        self._current_pose_pub = self.create_publisher(PoseStamped, f"{debug_ns}/current_pose", 10)
        self._raw_target_pose_pub = self.create_publisher(PoseStamped, f"{debug_ns}/raw_target_pose", 10)
        self._target_pose_pub = self.create_publisher(PoseStamped, f"{debug_ns}/target_pose", 10)
        self._cmd_vel_pub = self.create_publisher(TwistStamped, f"{debug_ns}/cmd_vel", 10)
        self._target_clamped_pub = self.create_publisher(Bool, f"{debug_ns}/target_clamped", 10)

        self.create_subscription(XrController, topic, self._on_controller, 10)
        self.create_timer(self._dt, self._control_tick)
        self.get_logger().info(
            f"{self._arm_name} 位姿透传遥操节点已启动，监听话题：{topic}, "
            f"dt={self._dt:.4f}s, follow={self._follow}, "
            f"orientation_control={self._enable_orientation_control}"
        )

    def _make_adapter(self):
        # mock 和真机共享同一位姿目标链路，只在适配层切换执行方式。
        if self._bool_parameter("use_mock"):
            return MockRealManAdapter(
                [float(v) for v in self.get_parameter("mock_initial_pose").value],
                self._dt,
            )

        return RealManAdapter(
            robot_ip=self.get_parameter("robot_ip").value,
            robot_port=int(self.get_parameter("robot_port").value),
            avoid_singularity=int(self.get_parameter("avoid_singularity").value),
            frame_type=int(self.get_parameter("frame_type").value),
            logger=self.get_logger(),
            configure_safety_limits=self._bool_parameter("configure_safety_limits"),
            max_line_speed=float(self.get_parameter("max_line_speed").value),
            max_angular_speed=float(self.get_parameter("max_angular_speed").value),
            max_line_acc=float(self.get_parameter("max_line_acc").value),
            max_angular_acc=float(self.get_parameter("max_angular_acc").value),
            joint_max_speed=float(self.get_parameter("joint_max_speed").value),
            joint_max_acc=float(self.get_parameter("joint_max_acc").value),
            move_to_initial_pose_on_connect=self._bool_parameter("move_to_initial_pose_on_connect"),
            initial_joint_pose=self._float_list_parameter("initial_joint_pose", 7),
            initial_tcp_pose=self._float_list_parameter("initial_tcp_pose", 6),
            init_move_speed=int(self.get_parameter("init_move_speed").value),
            canfd_trajectory_mode=int(self.get_parameter("canfd_trajectory_mode").value),
            canfd_radio=int(self.get_parameter("canfd_radio").value),
        )

    def _setup_tool_control(self) -> None:
        if not self._enable_tool_control:
            if self._enable_trigger_gripper_control:
                self.get_logger().warn(
                    f"{self._arm_name} trigger 夹爪控制已请求，但 enable_tool_control=false，已禁用。"
                )
            return

        peripheral_arm = self._peripheral_arm_name()
        config_file = str(self.get_parameter("peripheral_config_file").value)
        if self._bool_parameter("configure_peripheral_on_connect"):
            self._adapter.configure_peripheral(config_file, peripheral_arm)

        self._start_tool_worker()

        topic = str(self.get_parameter("tool_command_topic").value).strip()
        if not topic:
            topic = f"{self._debug_topic_prefix}/{self._arm_name}/tool_enable"
        self.create_subscription(Bool, topic, self._on_tool_command, 10)
        trigger_text = (
            f", trigger >= {self._trigger_close_threshold:.2f} 上升沿切换开合"
            if self._enable_trigger_gripper_control
            else ""
        )
        self.get_logger().info(
            f"{self._arm_name} 工具控制已启用，监听：{topic} "
            f"(true=open, false=close{trigger_text})"
        )

    def _on_tool_command(self, msg: Bool) -> None:
        open_tool = bool(msg.data)
        self._trigger_tool_open = open_tool
        self._enqueue_tool_command(open_tool, "topic")

    def _start_tool_worker(self) -> None:
        if self._tool_worker_thread is not None:
            return
        self._tool_command_queue = queue.Queue(maxsize=1)
        self._tool_worker_stop.clear()
        self._tool_worker_thread = threading.Thread(
            target=self._tool_worker_loop,
            name=f"{self._arm_name}_tool_worker",
            daemon=True,
        )
        self._tool_worker_thread.start()

    def _enqueue_tool_command(self, open_tool: bool, source: str) -> None:
        if self._tool_command_queue is None:
            self.get_logger().warn(
                f"{self._arm_name} 工具控制未启用，忽略 {source} 夹爪命令。",
                throttle_duration_sec=1.0,
            )
            return

        item = (open_tool, source)
        while True:
            try:
                self._tool_command_queue.put_nowait(item)
                return
            except queue.Full:
                try:
                    self._tool_command_queue.get_nowait()
                    self._tool_command_queue.task_done()
                except queue.Empty:
                    continue

    def _tool_worker_loop(self) -> None:
        assert self._tool_command_queue is not None
        while not self._tool_worker_stop.is_set():
            try:
                item = self._tool_command_queue.get(timeout=0.1)
            except queue.Empty:
                continue

            try:
                if item is None:
                    return
                open_tool, source = item
                action = "open" if open_tool else "close"
                try:
                    self._adapter.set_tool_enabled(open_tool)
                except Exception as exc:
                    self.get_logger().error(
                        f"{self._arm_name} tool {action} failed from {source}: {exc}"
                    )
                    continue
                self.get_logger().info(
                    f"{self._arm_name} tool {action} command sent from {source}"
                )
            finally:
                self._tool_command_queue.task_done()

    def _peripheral_arm_name(self) -> str:
        configured = str(self.get_parameter("peripheral_arm").value).strip().lower()
        if configured:
            return configured
        arm_name = self._arm_name.lower()
        if arm_name.startswith("left"):
            return "left"
        if arm_name.startswith("right"):
            return "right"
        return arm_name

    def _on_controller(self, msg: XrController) -> None:
        self._last_msg = msg
        self._last_msg_time = self.get_clock().now()
        self._handle_trigger_gripper(msg)

    def _handle_trigger_gripper(self, msg: XrController) -> None:
        if not self._enable_tool_control or not self._enable_trigger_gripper_control:
            return

        trigger_pressed = msg.trigger >= self._trigger_close_threshold
        if self._last_trigger_pressed is None:
            self._last_trigger_pressed = trigger_pressed
            return

        rising_edge = trigger_pressed and not self._last_trigger_pressed
        self._last_trigger_pressed = trigger_pressed
        if not rising_edge:
            return

        self._trigger_tool_open = not self._trigger_tool_open
        self._enqueue_tool_command(self._trigger_tool_open, "trigger")

    def _control_tick(self) -> None:
        now = self.get_clock().now()
        if self._last_msg is None or self._last_msg_time is None:
            self._safe_stop(reset_active=True)
            return

        age = (now - self._last_msg_time).nanoseconds * 1e-9
        if age > self._command_timeout_sec:
            if self._active or not self._stop_sent:
                self.get_logger().warn(
                    f"{self._arm_name} XR 手柄数据超时，机械臂停止。",
                    throttle_duration_sec=1.0,
                )
            self._safe_stop(reset_active=True)
            self._last_msg = None
            self._last_msg_time = None
            return

        if not self._last_msg.grip:
            if self._active:
                self.get_logger().info(f"{self._arm_name} Grip 松开，退出相对位姿遥操。")
            self._safe_stop(reset_active=True)
            return

        controller_now = self._controller_xyz(self._last_msg)
        try:
            controller_quat = self._controller_quaternion(self._last_msg)
        except ValueError as exc:
            self.get_logger().warn(
                f"{self._arm_name} XR 手柄姿态无效，停止输出：{exc}",
                throttle_duration_sec=1.0,
            )
            self._safe_stop(reset_active=True)
            return
        if not self._active:
            self._enter_active_control(controller_now, controller_quat, now)
            return

        assert self._controller_start is not None
        assert self._robot_start_pose is not None

        self._maybe_refresh_current_pose(now)
        raw_target_xyz = self._raw_target_from_controller(controller_now)
        raw_target_rpy = self._raw_orientation_from_controller(controller_quat)
        workspace_target, workspace_clamped = self._clamp_workspace_with_flag(raw_target_xyz)
        desired_target = self._apply_deadband(workspace_target)
        filtered_target = self._filter_target(desired_target)
        sent_target, step_limited = self._limit_target_step(filtered_target)
        sent_target, final_clamped = self._clamp_workspace_with_flag(sent_target)
        desired_orientation = self._apply_orientation_deadband(raw_target_rpy)
        filtered_orientation = self._filter_orientation_target(desired_orientation)
        sent_orientation, orientation_step_limited = self._limit_orientation_step(filtered_orientation)

        velocity = self._estimate_command_velocity(sent_target, sent_orientation, now)
        target_clamped = workspace_clamped or step_limited or final_clamped or orientation_step_limited
        raw_target_pose = ArmPose(
            x=raw_target_xyz[0],
            y=raw_target_xyz[1],
            z=raw_target_xyz[2],
            rx=raw_target_rpy[0],
            ry=raw_target_rpy[1],
            rz=raw_target_rpy[2],
        )
        target_pose = ArmPose(
            x=sent_target[0],
            y=sent_target[1],
            z=sent_target[2],
            rx=sent_orientation[0],
            ry=sent_orientation[1],
            rz=sent_orientation[2],
        )

        self._publish_debug(raw_target_pose, target_pose, velocity, target_clamped)
        if self._send_cartesian_target(target_pose):
            self._last_sent_target = sent_target
            self._last_sent_orientation = sent_orientation
            self._last_command_time = now
            self._stop_sent = False

    def _enter_active_control(
        self,
        controller_now: list[float],
        controller_quat: tuple[float, float, float, float],
        now: Time,
    ) -> None:
        try:
            robot_pose = self._read_current_pose_for_control(now)
        except Exception as exc:
            self.get_logger().error(
                f"{self._arm_name} 读取 TCP 位姿失败，停止输出：{exc}",
                throttle_duration_sec=1.0,
            )
            self._safe_stop(reset_active=True)
            return

        robot_xyz = robot_pose.xyz()
        self._active = True
        self._controller_start = controller_now
        self._controller_orientation_start = controller_quat
        self._robot_start_pose = robot_pose
        self._filtered_target = robot_xyz
        self._filtered_orientation_target = robot_pose.rpy()
        self._last_sent_target = robot_xyz
        self._last_sent_orientation = robot_pose.rpy()
        self._last_command_time = now
        self._stop_sent = True
        self.get_logger().info(f"{self._arm_name} Grip 按下，已锁定手柄和机械臂初始位姿。")
        self._publish_debug(robot_pose, robot_pose, [0.0] * 6, False)

    @staticmethod
    def _controller_xyz(msg: XrController) -> list[float]:
        return [msg.pose.position.x, msg.pose.position.y, msg.pose.position.z]

    def _controller_quaternion(self, msg: XrController) -> tuple[float, float, float, float]:
        if not self._enable_orientation_control:
            return 0.0, 0.0, 0.0, 1.0
        return _normalize_quaternion(
            (
                msg.pose.orientation.x,
                msg.pose.orientation.y,
                msg.pose.orientation.z,
                msg.pose.orientation.w,
            )
        )

    def _raw_target_from_controller(self, controller_now: list[float]) -> list[float]:
        assert self._controller_start is not None
        assert self._robot_start_pose is not None
        controller_delta = [controller_now[i] - self._controller_start[i] for i in range(3)]
        robot_delta = self._map_xr_delta_to_robot(controller_delta)
        target = [
            self._robot_start_pose.x + self._scale * robot_delta[0],
            self._robot_start_pose.y + self._scale * robot_delta[1],
            self._robot_start_pose.z + self._scale * robot_delta[2],
        ]
        return [target[i] if self._enable_position_axes[i] else self._robot_start_pose.xyz()[i] for i in range(3)]

    def _map_xr_delta_to_robot(self, delta: list[float]) -> list[float]:
        matrix = self._xr_to_robot_matrix
        return [
            matrix[0] * delta[0] + matrix[1] * delta[1] + matrix[2] * delta[2],
            matrix[3] * delta[0] + matrix[4] * delta[1] + matrix[5] * delta[2],
            matrix[6] * delta[0] + matrix[7] * delta[1] + matrix[8] * delta[2],
        ]

    def _raw_orientation_from_controller(
        self,
        controller_quat: tuple[float, float, float, float],
    ) -> list[float]:
        assert self._robot_start_pose is not None
        if not self._enable_orientation_control:
            return self._robot_start_pose.rpy()

        assert self._controller_orientation_start is not None
        xr_delta_quat = _quaternion_multiply(
            controller_quat,
            _quaternion_conjugate(self._controller_orientation_start),
        )
        xr_delta_matrix = _quaternion_to_matrix(xr_delta_quat)
        matrix = self._xr_to_robot_matrix
        robot_delta_matrix = _matrix_multiply(
            _matrix_multiply(matrix, xr_delta_matrix),
            _matrix_transpose(matrix),
        )
        robot_start_matrix = _euler_to_matrix(
            self._robot_start_pose.rx,
            self._robot_start_pose.ry,
            self._robot_start_pose.rz,
        )
        target_matrix = _matrix_multiply(robot_delta_matrix, robot_start_matrix)
        target_rpy = list(_matrix_to_euler(target_matrix))
        start_rpy = self._robot_start_pose.rpy()
        return [
            target_rpy[i] if self._enable_orientation_axes[i] else start_rpy[i]
            for i in range(3)
        ]

    def _apply_deadband(self, target: list[float]) -> list[float]:
        if self._deadband_m <= 0.0 or self._last_sent_target is None:
            return target
        delta = [target[i] - self._last_sent_target[i] for i in range(3)]
        if _norm(delta) < self._deadband_m:
            return list(self._last_sent_target)
        return target

    def _filter_target(self, target: list[float]) -> list[float]:
        if self._filtered_target is None:
            self._filtered_target = list(target)
            return list(target)

        delta = [target[i] - self._filtered_target[i] for i in range(3)]
        distance = _norm(delta)
        alpha = self._adaptive_filter_alpha(distance)
        self._filtered_target = [
            alpha * target[i] + (1.0 - alpha) * self._filtered_target[i]
            for i in range(3)
        ]
        return list(self._filtered_target)

    def _adaptive_filter_alpha(self, distance: float) -> float:
        if self._target_filter_fast_threshold_m <= 1e-9:
            return self._target_filter_alpha_fast
        ratio = _clamp(distance / self._target_filter_fast_threshold_m, 0.0, 1.0)
        return (
            self._target_filter_alpha
            + (self._target_filter_alpha_fast - self._target_filter_alpha) * ratio
        )

    def _limit_target_step(self, target: list[float]) -> tuple[list[float], bool]:
        if self._last_sent_target is None:
            return target, False

        delta = [target[i] - self._last_sent_target[i] for i in range(3)]
        distance = _norm(delta)
        max_step = self._max_linear_speed * self._dt
        if distance <= max_step or distance <= 1e-9:
            return target, False

        scale = max_step / distance
        return [
            self._last_sent_target[i] + delta[i] * scale
            for i in range(3)
        ], True

    def _apply_orientation_deadband(self, target_rpy: list[float]) -> list[float]:
        if self._orientation_deadband_rad <= 0.0 or self._last_sent_orientation is None:
            return [_wrap_angle(value) for value in target_rpy]
        delta = [
            _angle_delta(target_rpy[i], self._last_sent_orientation[i])
            for i in range(3)
        ]
        if _norm(delta) < self._orientation_deadband_rad:
            return list(self._last_sent_orientation)
        return [_wrap_angle(value) for value in target_rpy]

    def _filter_orientation_target(self, target_rpy: list[float]) -> list[float]:
        if self._filtered_orientation_target is None:
            self._filtered_orientation_target = [_wrap_angle(value) for value in target_rpy]
            return list(self._filtered_orientation_target)

        self._filtered_orientation_target = [
            _wrap_angle(
                self._filtered_orientation_target[i]
                + self._orientation_filter_alpha
                * _angle_delta(target_rpy[i], self._filtered_orientation_target[i])
            )
            for i in range(3)
        ]
        return list(self._filtered_orientation_target)

    def _limit_orientation_step(self, target_rpy: list[float]) -> tuple[list[float], bool]:
        if self._last_sent_orientation is None:
            return [_wrap_angle(value) for value in target_rpy], False

        delta = [
            _angle_delta(target_rpy[i], self._last_sent_orientation[i])
            for i in range(3)
        ]
        distance = _norm(delta)
        max_step = self._max_orientation_speed * self._dt
        if distance <= max_step or distance <= 1e-9:
            return [_wrap_angle(value) for value in target_rpy], False

        scale = max_step / distance
        return [
            _wrap_angle(self._last_sent_orientation[i] + delta[i] * scale)
            for i in range(3)
        ], True

    def _clamp_workspace_with_flag(self, target: list[float]) -> tuple[list[float], bool]:
        clamped = [
            _clamp(target[i], self._workspace_min[i], self._workspace_max[i])
            for i in range(3)
        ]
        clamped = self._clamp_cylinder_radius(clamped)
        was_clamped = any(abs(clamped[i] - target[i]) > 1e-9 for i in range(3))
        return clamped, was_clamped

    def _clamp_cylinder_radius(self, target: list[float]) -> list[float]:
        min_radius, max_radius = self._cyl_radius_limit
        if target[2] < self._low_z_threshold:
            min_radius = max(min_radius, self._low_z_min_radius)

        radius = math.hypot(target[0], target[1])
        if max_radius > 0.0 and radius > max_radius:
            scale = max_radius / radius
            target[0] *= scale
            target[1] *= scale
        elif min_radius > 0.0 and radius < min_radius:
            if radius > 1e-9:
                scale = min_radius / radius
                target[0] *= scale
                target[1] *= scale
            else:
                target[0] = min_radius
                target[1] = 0.0

        return target

    def _estimate_command_velocity(
        self,
        target_xyz: list[float],
        target_rpy: list[float],
        now: Time,
    ) -> list[float]:
        if self._last_sent_target is None or self._last_sent_orientation is None:
            return [0.0] * 6

        dt = self._dt
        if self._last_command_time is not None:
            measured_dt = (now - self._last_command_time).nanoseconds * 1e-9
            if measured_dt > 1e-6:
                dt = measured_dt
        return [
            (target_xyz[i] - self._last_sent_target[i]) / dt
            for i in range(3)
        ] + [
            _angle_delta(target_rpy[i], self._last_sent_orientation[i]) / dt
            for i in range(3)
        ]

    def _read_current_pose_for_control(self, now: Time) -> ArmPose:
        pose = self._adapter.get_current_pose()
        self._last_current_pose = pose
        self._last_current_pose_time = now
        return pose

    def _maybe_refresh_current_pose(self, now: Time) -> None:
        if self._current_pose_poll_hz <= 0.0:
            return
        if self._last_current_pose_time is not None:
            age = (now - self._last_current_pose_time).nanoseconds * 1e-9
            if age < 1.0 / self._current_pose_poll_hz:
                return

        try:
            self._read_current_pose_for_control(now)
        except Exception as exc:
            self.get_logger().warn(
                f"{self._arm_name} 低频读取 TCP 位姿失败，继续透传目标：{exc}",
                throttle_duration_sec=1.0,
            )

    def _safe_stop(self, reset_active: bool) -> None:
        if not self._stop_sent:
            self._send_stop_once()
        if reset_active:
            self._active = False
            self._controller_start = None
            self._controller_orientation_start = None
            self._robot_start_pose = None
            self._filtered_target = None
            self._filtered_orientation_target = None
            self._last_sent_target = None
            self._last_sent_orientation = None
            self._last_command_time = None
        self._publish_stop_debug()

    def _send_stop_once(self) -> None:
        try:
            self._adapter.stop()
        except Exception as exc:
            self.get_logger().error(
                f"{self._arm_name} 停止机械臂失败：{exc}",
                throttle_duration_sec=1.0,
            )
        self._stop_sent = True

    def _publish_stop_debug(self) -> None:
        pose = self._debug_pose_fallback()
        if pose is None:
            return
        self._publish_debug(pose, pose, [0.0] * 6, False)

    def _debug_pose_fallback(self) -> ArmPose | None:
        if self._last_current_pose is not None:
            return self._last_current_pose
        if self._robot_start_pose is not None:
            return self._robot_start_pose
        if self._last_sent_target is not None:
            rpy = self._last_sent_orientation or [0.0, 0.0, 0.0]
            return ArmPose(*self._last_sent_target, *rpy)
        return None

    def _send_cartesian_target(self, pose: ArmPose) -> bool:
        try:
            self._adapter.send_cartesian_target(pose, self._follow)
        except Exception as exc:
            self.get_logger().error(
                f"{self._arm_name} 发送位姿透传命令失败：{exc}",
                throttle_duration_sec=1.0,
            )
            self._active = False
            self._send_stop_once()
            return False
        return True

    def _publish_debug(
        self,
        raw_target_pose: ArmPose,
        target_pose: ArmPose,
        command_velocity: list[float],
        target_clamped: bool,
    ) -> None:
        stamp = self.get_clock().now().to_msg()
        current_pose = self._debug_pose_fallback()
        if current_pose is None:
            current_pose = target_pose

        raw_pose = self._pose_msg(stamp, raw_target_pose)
        target_msg = self._pose_msg(stamp, target_pose)
        velocity_msg = TwistStamped()
        velocity_msg.header.stamp = stamp
        velocity_msg.header.frame_id = "rm_base"
        velocity_msg.twist.linear.x = float(command_velocity[0])
        velocity_msg.twist.linear.y = float(command_velocity[1])
        velocity_msg.twist.linear.z = float(command_velocity[2])
        velocity_msg.twist.angular.x = float(command_velocity[3])
        velocity_msg.twist.angular.y = float(command_velocity[4])
        velocity_msg.twist.angular.z = float(command_velocity[5])

        clamped_msg = Bool()
        clamped_msg.data = bool(target_clamped)

        self._current_pose_pub.publish(self._pose_msg(stamp, current_pose))
        self._raw_target_pose_pub.publish(raw_pose)
        self._target_pose_pub.publish(target_msg)
        self._cmd_vel_pub.publish(velocity_msg)
        self._target_clamped_pub.publish(clamped_msg)

    @staticmethod
    def _pose_msg(stamp, pose: ArmPose) -> PoseStamped:
        qx, qy, qz, qw = _euler_to_quaternion(pose.rx, pose.ry, pose.rz)
        msg = PoseStamped()
        msg.header.stamp = stamp
        msg.header.frame_id = "rm_base"
        msg.pose.position.x = float(pose.x)
        msg.pose.position.y = float(pose.y)
        msg.pose.position.z = float(pose.z)
        msg.pose.orientation.x = qx
        msg.pose.orientation.y = qy
        msg.pose.orientation.z = qz
        msg.pose.orientation.w = qw
        return msg

    def _float_list_parameter(self, name: str, expected_length: int) -> list[float]:
        values = [float(value) for value in self.get_parameter(name).value]
        if len(values) != expected_length:
            raise ValueError(f"{name} must contain {expected_length} values")
        return values

    def _bool_list_parameter(self, name: str, expected_length: int) -> list[bool]:
        values = [self._as_bool(value) for value in self.get_parameter(name).value]
        if len(values) != expected_length:
            raise ValueError(f"{name} must contain {expected_length} values")
        return values

    def _bool_parameter(self, name: str) -> bool:
        return self._as_bool(self.get_parameter(name).value)

    @staticmethod
    def _as_bool(value) -> bool:
        if isinstance(value, bool):
            return value
        if isinstance(value, str):
            return value.strip().lower() in ("1", "true", "yes", "on")
        return bool(value)

    def _validate_parameters(self) -> None:
        if self._command_timeout_sec <= 0.0:
            raise ValueError("command_timeout_sec must be > 0")
        if self._deadband_m < 0.0:
            raise ValueError("deadband_m must be >= 0")
        if not 0.0 <= self._target_filter_alpha <= 1.0:
            raise ValueError("target_filter_alpha must be in [0, 1]")
        if not 0.0 <= self._target_filter_alpha_fast <= 1.0:
            raise ValueError("target_filter_alpha_fast must be in [0, 1]")
        if self._target_filter_alpha_fast < self._target_filter_alpha:
            raise ValueError("target_filter_alpha_fast must be >= target_filter_alpha")
        if self._target_filter_fast_threshold_m < 0.0:
            raise ValueError("target_filter_fast_threshold_m must be >= 0")
        if self._max_linear_speed <= 0.0:
            raise ValueError("max_linear_speed must be > 0")
        if self._current_pose_poll_hz < 0.0:
            raise ValueError("current_pose_poll_hz must be >= 0")
        if self._orientation_deadband_rad < 0.0:
            raise ValueError("orientation_deadband_rad must be >= 0")
        if not 0.0 <= self._orientation_filter_alpha <= 1.0:
            raise ValueError("orientation_filter_alpha must be in [0, 1]")
        if self._max_orientation_speed <= 0.0:
            raise ValueError("max_orientation_speed must be > 0")
        if not 0.0 <= self._trigger_close_threshold <= 1.0:
            raise ValueError("trigger_close_threshold must be in [0, 1]")
        for axis, (low, high) in enumerate(zip(self._workspace_min, self._workspace_max)):
            if low >= high:
                raise ValueError(f"workspace_min[{axis}] must be smaller than workspace_max[{axis}]")
        min_radius, max_radius = self._cyl_radius_limit
        if min_radius < 0.0:
            raise ValueError("cyl_radius_limit[0] must be >= 0")
        if max_radius <= min_radius:
            raise ValueError("cyl_radius_limit[1] must be > cyl_radius_limit[0]")
        if self._low_z_min_radius < 0.0:
            raise ValueError("low_z_min_radius must be >= 0")

    def _shutdown_tool_worker(self) -> None:
        if self._tool_worker_thread is None or self._tool_command_queue is None:
            return
        self._tool_worker_stop.set()
        while True:
            try:
                self._tool_command_queue.put_nowait(None)
                break
            except queue.Full:
                try:
                    self._tool_command_queue.get_nowait()
                    self._tool_command_queue.task_done()
                except queue.Empty:
                    continue
        self._tool_worker_thread.join(timeout=3.0)
        if self._tool_worker_thread.is_alive():
            self.get_logger().warn(f"{self._arm_name} 工具控制后台线程未在超时时间内退出。")
        self._tool_worker_thread = None
        self._tool_command_queue = None

    def destroy_node(self) -> bool:
        self._shutdown_tool_worker()
        self._adapter.close()
        return super().destroy_node()


def main(args: list[str] | None = None) -> None:
    rclpy.init(args=args)
    node = SingleArmVelocityTeleop()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == "__main__":
    main()
