import math
import threading
import time
from types import SimpleNamespace

import numpy as np
import pytest
from builtin_interfaces.msg import Time as TimeMsg

from xr_rm_teleop.realman_adapter import JointStateSnapshot
from xr_rm_teleop.single_arm_velocity_teleop import (
    SingleArmVelocityTeleop,
    _make_transform,
    _so3_exp,
)


class FakeLogger:
    def info(self, *args, **kwargs):
        del args, kwargs

    def warn(self, *args, **kwargs):
        del args, kwargs

    def error(self, *args, **kwargs):
        del args, kwargs


class FakeTime:
    def __sub__(self, other):
        del other
        return SimpleNamespace(nanoseconds=0)

    def to_msg(self):
        return TimeMsg()


class FakePublisher:
    def __init__(self) -> None:
        self.messages = []

    def publish(self, message) -> None:
        self.messages.append(message)


def _tool_state_teleop(*, command_error=None):
    started = threading.Event()
    release = threading.Event()

    class Adapter:
        def set_tool_enabled(self, open_tool):
            del open_tool
            started.set()
            assert release.wait(timeout=1.0)
            if command_error is not None:
                raise command_error

    teleop = object.__new__(SingleArmVelocityTeleop)
    teleop._arm_name = "right_rm75"
    teleop._adapter = Adapter()
    teleop._tool_command_queue = None
    teleop._tool_worker_stop = threading.Event()
    teleop._tool_worker_thread = None
    teleop._tool_state_lock = threading.Lock()
    teleop._tool_target_open = True
    teleop._tool_state_open = True
    teleop._tool_command_pending = False
    teleop._tool_command_failed = False
    teleop.get_logger = lambda: FakeLogger()
    teleop._start_tool_worker()
    return teleop, started, release


def test_tool_state_changes_only_after_command_succeeds() -> None:
    teleop, started, release = _tool_state_teleop()
    try:
        teleop._enqueue_tool_command(False, "test")
        assert started.wait(timeout=1.0)

        assert teleop._tool_state_snapshot() == (False, True, True, False)

        release.set()
        assert teleop._tool_command_queue is not None
        teleop._tool_command_queue.join()

        assert teleop._tool_state_snapshot() == (False, False, False, False)
    finally:
        release.set()
        teleop._shutdown_tool_worker()


def test_tool_failure_keeps_previous_state_and_is_reported() -> None:
    teleop, started, release = _tool_state_teleop(
        command_error=RuntimeError("modbus failed")
    )
    try:
        teleop._enqueue_tool_command(False, "test")
        assert started.wait(timeout=1.0)
        release.set()
        assert teleop._tool_command_queue is not None
        teleop._tool_command_queue.join()

        assert teleop._tool_state_snapshot() == (False, True, False, True)
    finally:
        release.set()
        teleop._shutdown_tool_worker()


def _joint_publishing_teleop() -> SingleArmVelocityTeleop:
    names = [f"omnipic_joint_{index}" for index in range(1, 8)]
    teleop = object.__new__(SingleArmVelocityTeleop)
    teleop._ik_solver = SimpleNamespace(
        joint_names=names,
        update_joint_state=lambda joints: np.eye(4),
    )
    teleop._joint_state_pub = FakePublisher()
    teleop._joint_target_pub = FakePublisher()
    teleop._active = False
    teleop._last_valid_joint_target = None
    teleop.get_clock = lambda: SimpleNamespace(now=lambda: FakeTime())
    return teleop


def test_reset_joint_state_publishes_named_feedback() -> None:
    teleop = _joint_publishing_teleop()
    positions = [0.1 * index for index in range(7)]
    snapshot = JointStateSnapshot(positions, time.monotonic())

    teleop._reset_joint_state(snapshot)

    message = teleop._joint_state_pub.messages[-1]
    assert message.name == teleop._ik_solver.joint_names
    assert message.position == pytest.approx(positions)


def test_sync_joint_feedback_publishes_each_sample() -> None:
    teleop = _joint_publishing_teleop()
    positions = [0.2] * 7

    teleop._sync_joint_feedback(
        JointStateSnapshot(positions, time.monotonic())
    )

    assert len(teleop._joint_state_pub.messages) == 1
    assert teleop._joint_state_pub.messages[0].position == pytest.approx(positions)


def test_send_joint_target_publishes_limited_command() -> None:
    sent = []
    teleop = _joint_publishing_teleop()
    teleop._adapter = SimpleNamespace(
        send_joint_target=lambda joints, follow: sent.append((list(joints), follow))
    )
    teleop._follow = False
    teleop._latest_joint_positions = [0.0] * 7
    teleop._last_joint_command_target = [0.0] * 7
    teleop._last_joint_command_velocity = [0.0] * 7
    teleop._joint_command_max_speed = 1.0
    teleop._joint_command_max_acceleration = 100.0
    teleop._dt = 0.1

    assert teleop._send_joint_target([0.5] * 7)

    assert len(sent) == 1
    assert sent[0][0] == pytest.approx([0.1] * 7)
    assert sent[0][1] is False
    message = teleop._joint_target_pub.messages[-1]
    assert message.name == teleop._ik_solver.joint_names
    assert message.position == pytest.approx(teleop._last_joint_command_target)


def _primary_button_teleop(*, use_mock=False, move_error=None):
    events = []
    errors = []
    snapshot = JointStateSnapshot([0.2] * 7, time.monotonic())

    class Adapter:
        def move_to_initial_pose(self):
            events.append("move")
            if move_error is not None:
                raise move_error

        def read_joint_state(self):
            events.append("read")
            return snapshot

    teleop = object.__new__(SingleArmVelocityTeleop)
    teleop._arm_name = "right_rm75"
    teleop._use_mock = use_mock
    teleop._adapter = Adapter()
    teleop._last_primary_pressed = None
    teleop._grip_rearm_required = False
    teleop._safe_stop = lambda reset_active: events.append(
        ("stop", reset_active)
    )
    teleop._reset_joint_state = lambda value: events.append(("sync", value))
    teleop._handle_trigger_gripper = lambda msg: None
    teleop.get_clock = lambda: SimpleNamespace(now=lambda: FakeTime())
    teleop.get_logger = lambda: SimpleNamespace(
        info=lambda message: None,
        error=lambda message: errors.append(message),
    )
    return teleop, events, errors, snapshot


def test_primary_button_rising_edge_moves_once_and_resyncs() -> None:
    teleop, events, _, snapshot = _primary_button_teleop()
    released = SimpleNamespace(primary=False)
    pressed = SimpleNamespace(primary=True)

    teleop._on_controller(released)
    teleop._on_controller(pressed)
    teleop._on_controller(pressed)
    teleop._on_controller(released)
    teleop._on_controller(pressed)

    expected_once = [
        ("stop", True),
        "move",
        "read",
        ("sync", snapshot),
    ]
    assert events == expected_once * 2
    assert teleop._grip_rearm_required


def test_primary_button_move_failure_logs_and_stays_stopped() -> None:
    failure = RuntimeError("rm_movej failed")
    teleop, events, errors, _ = _primary_button_teleop(
        move_error=failure
    )

    teleop._on_controller(SimpleNamespace(primary=False))
    teleop._on_controller(SimpleNamespace(primary=True))

    assert events == [("stop", True), "move"]
    assert teleop._grip_rearm_required
    assert errors == [
        "right_rm75 回初始位姿失败：rm_movej failed"
    ]


def test_mock_primary_reset_can_reanchor_without_grip_release() -> None:
    teleop, events, _, snapshot = _primary_button_teleop(use_mock=True)

    teleop._on_controller(SimpleNamespace(primary=False))
    teleop._on_controller(SimpleNamespace(primary=True))

    assert events == [
        ("stop", True),
        "move",
        "read",
        ("sync", snapshot),
    ]
    assert not teleop._grip_rearm_required


def test_failed_mock_primary_reset_still_requires_grip_release() -> None:
    failure = RuntimeError("mock reset failed")
    teleop, _, _, _ = _primary_button_teleop(
        use_mock=True,
        move_error=failure,
    )

    teleop._on_controller(SimpleNamespace(primary=False))
    teleop._on_controller(SimpleNamespace(primary=True))

    assert teleop._grip_rearm_required


def test_startup_joint_query_initializes_qp_and_command_history() -> None:
    positions = [0.1] * 7
    pose = np.eye(4)
    teleop = object.__new__(SingleArmVelocityTeleop)
    teleop._arm_name = "right_rm75"
    teleop._adapter = SimpleNamespace(
        read_joint_state=lambda: JointStateSnapshot(
            positions,
            time.monotonic(),
        )
    )
    teleop._ik_solver = SimpleNamespace(
        joint_names=[f"omnipic_joint_{index}" for index in range(1, 8)],
        update_joint_state=lambda joints: pose,
    )
    teleop._joint_state_pub = FakePublisher()
    teleop.get_clock = lambda: SimpleNamespace(now=lambda: FakeTime())
    teleop.get_logger = lambda: FakeLogger()

    teleop._initialize_joint_state()

    assert teleop._latest_joint_positions == positions
    assert teleop._last_valid_joint_target == positions
    assert teleop._last_joint_command_target == positions
    assert teleop._last_joint_command_velocity == [0.0] * 7
    assert teleop._last_current_pose is pose


def test_startup_joint_query_failure_closes_adapter() -> None:
    class FailingAdapter:
        def __init__(self):
            self.close_calls = 0

        def read_joint_state(self):
            raise RuntimeError("rm_get_joint_degree failed with code 7")

        def close(self):
            self.close_calls += 1

    errors = []
    teleop = object.__new__(SingleArmVelocityTeleop)
    teleop._arm_name = "left_rm75"
    teleop._adapter = FailingAdapter()
    teleop.get_logger = lambda: SimpleNamespace(
        error=lambda message: errors.append(message)
    )

    with pytest.raises(RuntimeError, match="code 7"):
        teleop._initialize_joint_state()

    assert teleop._adapter.close_calls == 1
    assert "left_rm75" in errors[0]
    assert "启动关节同步失败" in errors[0]


def _timeout_teleop(adapter) -> SingleArmVelocityTeleop:
    teleop = object.__new__(SingleArmVelocityTeleop)
    teleop._adapter = adapter
    teleop._arm_name = "right_rm75"
    teleop._follow = False
    teleop._active = True
    teleop._joint_feedback_ready = True
    teleop._grip_rearm_required = False
    teleop._feedback_resync_attempted = False
    teleop._control_fault_latched = False
    teleop._last_joint_command_target = [0.1] * 7
    teleop._last_joint_command_velocity = [0.0] * 7
    teleop._latest_joint_positions = [0.1] * 7
    teleop._last_valid_joint_target = [0.1] * 7
    teleop._last_current_pose = np.eye(4)
    teleop._controller_start = None
    teleop._controller_orientation_start = None
    teleop._robot_start_transform = None
    teleop._filtered_target = None
    teleop._filtered_orientation_target = None
    teleop._last_sent_target = None
    teleop._last_sent_orientation = None
    teleop._last_command_time = None
    teleop._ik_solver = SimpleNamespace(
        update_joint_state=lambda joints: np.eye(4)
    )
    teleop._ik_solver.joint_names = [
        f"omnipic_joint_{index}" for index in range(1, 8)
    ]
    teleop._joint_state_pub = FakePublisher()
    teleop._joint_target_pub = FakePublisher()
    teleop.get_clock = lambda: SimpleNamespace(now=lambda: FakeTime())
    teleop._stop_sent = False
    teleop._feedback_resync_timeout_sec = 0.5
    teleop._publish_stop_debug = lambda: None
    teleop.get_logger = lambda: FakeLogger()
    return teleop


def test_missing_or_disabled_joint_snapshot_is_not_motion_ready() -> None:
    assert not SingleArmVelocityTeleop._joint_snapshot_is_motion_ready(None)
    assert not SingleArmVelocityTeleop._joint_snapshot_is_motion_ready(
        JointStateSnapshot(
            [0.0] * 7,
            time.monotonic(),
            motion_ready=False,
        )
    )


def test_short_udp_timeout_repeats_last_limited_target_without_query() -> None:
    sends = []
    adapter = SimpleNamespace(
        send_joint_target=lambda joints, follow: sends.append(
            (list(joints), follow)
        ),
        read_joint_state=lambda: pytest.fail("query must not run"),
        stop=lambda: pytest.fail("stop must not run"),
    )
    teleop = _timeout_teleop(adapter)
    warnings = []
    teleop.get_logger = lambda: SimpleNamespace(
        warn=lambda message: warnings.append(message)
    )

    teleop._handle_stale_joint_feedback(0.2)

    assert sends == [([0.1] * 7, False)]
    assert teleop._last_joint_command_target == [0.1] * 7
    assert teleop._grip_rearm_required
    assert warnings == [
        "right_rm75 UDP关节反馈超时（age=200.0 ms），保持最后安全目标。"
    ]


def test_short_udp_timeout_without_active_target_stays_stopped() -> None:
    stop_calls = []
    adapter = SimpleNamespace(
        send_joint_target=lambda joints, follow: pytest.fail(
            "inactive control must not start CANFD output"
        ),
        read_joint_state=lambda: pytest.fail("query must not run"),
        stop=lambda: stop_calls.append(True),
    )
    teleop = _timeout_teleop(adapter)
    teleop._active = False

    teleop._handle_stale_joint_feedback(0.2)

    assert len(stop_calls) == 1


def test_persistent_udp_timeout_queries_once_and_holds_actual_position() -> None:
    sends = []
    query_calls = []
    adapter = SimpleNamespace(
        send_joint_target=lambda joints, follow: sends.append(list(joints)),
        read_joint_state=lambda: (
            query_calls.append(True)
            or JointStateSnapshot([0.2] * 7, time.monotonic())
        ),
        stop=lambda: None,
    )
    teleop = _timeout_teleop(adapter)

    teleop._handle_stale_joint_feedback(0.5)
    teleop._handle_stale_joint_feedback(0.6)

    assert len(query_calls) == 1
    assert sends == [[0.2] * 7, [0.2] * 7]
    assert teleop._last_valid_joint_target == [0.2] * 7
    assert teleop._last_joint_command_velocity == [0.0] * 7


def test_persistent_udp_timeout_query_failure_latches_control() -> None:
    stop_calls = []
    adapter = SimpleNamespace(
        send_joint_target=lambda joints, follow: pytest.fail(
            "CANFD must stop after query failure"
        ),
        read_joint_state=lambda: (_ for _ in ()).throw(
            RuntimeError("rm_get_joint_degree failed with code 7")
        ),
        stop=lambda: stop_calls.append(True),
    )
    teleop = _timeout_teleop(adapter)

    teleop._handle_stale_joint_feedback(0.5)
    teleop._handle_stale_joint_feedback(0.6)

    assert teleop._control_fault_latched
    assert len(stop_calls) == 1


def test_joint_command_step_limits_acceleration_from_rest() -> None:
    dt = 1.0 / 125.0
    target, velocity = SingleArmVelocityTeleop._limit_joint_command_step(
        target=[0.2] * 7,
        previous_target=[0.0] * 7,
        previous_velocity=[0.0] * 7,
        max_speed=math.radians(180.0),
        max_acceleration=math.radians(300.0),
        dt=dt,
    )

    assert velocity == pytest.approx([math.radians(2.4)] * 7)
    assert target == pytest.approx([math.radians(0.0192)] * 7)


def test_joint_command_step_rejects_non_finite_limits() -> None:
    for max_speed, max_acceleration, dt in (
        (math.inf, 1.0, 0.1),
        (1.0, math.inf, 0.1),
        (1.0, 1.0, math.inf),
    ):
        with pytest.raises(ValueError, match="finite and positive"):
            SingleArmVelocityTeleop._limit_joint_command_step(
                target=[0.5] * 7,
                previous_target=[0.0] * 7,
                previous_velocity=[0.0] * 7,
                max_speed=max_speed,
                max_acceleration=max_acceleration,
                dt=dt,
            )


def test_joint_command_step_arrival_respects_max_speed() -> None:
    target, velocity = SingleArmVelocityTeleop._limit_joint_command_step(
        target=[0.5] * 7,
        previous_target=[0.0] * 7,
        previous_velocity=[0.0] * 7,
        max_speed=1.0,
        max_acceleration=100.0,
        dt=0.1,
    )

    assert velocity == pytest.approx([1.0] * 7)
    assert target == pytest.approx([0.1] * 7)


def test_joint_command_step_reverses_with_acceleration_limit() -> None:
    command = [0.0] * 7
    velocity = [0.0] * 7
    for _ in range(5):
        command, velocity = SingleArmVelocityTeleop._limit_joint_command_step(
            target=[1.0] * 7,
            previous_target=command,
            previous_velocity=velocity,
            max_speed=1.0,
            max_acceleration=1.0,
            dt=0.1,
        )

    previous_command = list(command)
    previous_velocity = list(velocity)
    command, velocity = SingleArmVelocityTeleop._limit_joint_command_step(
        target=[-1.0] * 7,
        previous_target=command,
        previous_velocity=velocity,
        max_speed=1.0,
        max_acceleration=1.0,
        dt=0.1,
    )

    assert previous_velocity == pytest.approx([0.5] * 7)
    assert velocity == pytest.approx([0.4] * 7)
    assert [
        current - previous
        for current, previous in zip(command, previous_command)
    ] == pytest.approx([value * 0.1 for value in velocity])
    assert all(
        current > previous
        for current, previous in zip(command, previous_command)
    )


def test_joint_command_step_brakes_before_fixed_target_without_overshoot() -> None:
    dt = 1.0 / 90.0
    max_speed = math.radians(180.0)
    max_acceleration = math.radians(300.0)
    target = np.radians(
        [10.0, -10.0, 3.0, -3.0, 1.0, -1.0, 0.1]
    ).tolist()
    command = [0.0] * 7
    velocity = [0.0] * 7

    for _ in range(180):
        previous_command = list(command)
        previous_velocity = list(velocity)
        command, velocity = (
            SingleArmVelocityTeleop._limit_joint_command_step(
                target=target,
                previous_target=command,
                previous_velocity=velocity,
                max_speed=max_speed,
                max_acceleration=max_acceleration,
                dt=dt,
            )
        )

        for index in range(7):
            assert min(0.0, target[index]) - 1e-12 <= command[index]
            assert command[index] <= max(0.0, target[index]) + 1e-12
            assert abs(velocity[index]) <= max_speed + 1e-12
            assert (
                abs(velocity[index] - previous_velocity[index])
                <= max_acceleration * dt + 1e-12
            )
            assert command[index] - previous_command[index] == pytest.approx(
                velocity[index] * dt,
                abs=1e-12,
            )

    assert command == pytest.approx(target, abs=1e-12)
    assert velocity == pytest.approx([0.0] * 7, abs=1e-12)


def test_feedback_fault_blocks_grip_until_release() -> None:
    class FakeClock:
        def now(self):
            return FakeTime()

    teleop = object.__new__(SingleArmVelocityTeleop)
    teleop._adapter = SimpleNamespace(
        get_latest_joint_state=lambda: JointStateSnapshot(
            [0.1] * 7,
            time.monotonic(),
        )
    )
    teleop._command_timeout_sec = 0.12
    teleop._joint_feedback_ready = True
    teleop._arm_name = "right_rm75"
    teleop._last_msg = SimpleNamespace(
        grip=True,
        pose=SimpleNamespace(
            position=SimpleNamespace(x=0.0, y=0.0, z=0.0),
            orientation=SimpleNamespace(x=0.0, y=0.0, z=0.0, w=1.0),
        ),
    )
    teleop._last_msg_time = FakeTime()
    teleop._active = False
    teleop._enable_orientation_control = False
    teleop._last_valid_joint_target = None
    teleop._last_current_pose = None
    teleop._ik_solver = SimpleNamespace(
        update_joint_state=lambda joints: np.eye(4)
    )
    teleop._ik_solver.joint_names = [
        f"omnipic_joint_{index}" for index in range(1, 8)
    ]
    teleop._joint_state_pub = FakePublisher()
    teleop._grip_rearm_required = True
    teleop._control_fault_latched = False
    teleop._feedback_resync_attempted = False
    teleop.get_clock = lambda: FakeClock()
    teleop.get_logger = lambda: FakeLogger()
    stopped = []
    entered = []
    teleop._safe_stop = lambda reset_active: stopped.append(reset_active)
    teleop._enter_active_control = lambda *args: entered.append(args)

    teleop._control_tick()
    assert entered == []

    teleop._last_msg.grip = False
    teleop._control_tick()
    assert teleop._grip_rearm_required is False

    teleop._last_msg.grip = True
    teleop._control_tick()
    assert len(entered) == 1


def test_first_feedback_initializes_last_valid_target_without_solving() -> None:
    class FakeSolver:
        def __init__(self) -> None:
            self.solve_calls = 0
            self.joint_names = [
                f"omnipic_joint_{index}" for index in range(1, 8)
            ]

        def update_joint_state(self, joints):
            assert joints == [0.1] * 7
            transform = np.eye(4)
            transform[:3, 3] = [0.3, 0.0, 0.2]
            return transform

        def solve(self, target):
            del target
            self.solve_calls += 1
            return [0.2] * 7

    teleop = object.__new__(SingleArmVelocityTeleop)
    teleop._ik_solver = FakeSolver()
    teleop._active = False
    teleop._last_valid_joint_target = None
    teleop._last_current_pose = None
    teleop._joint_state_pub = FakePublisher()
    teleop.get_clock = lambda: SimpleNamespace(now=lambda: FakeTime())

    pose = teleop._sync_joint_feedback(
        JointStateSnapshot([0.1] * 7, time.monotonic())
    )

    assert pose == pytest.approx(
        _make_transform([0.3, 0.0, 0.2], np.eye(3))
    )
    assert teleop._last_valid_joint_target == [0.1] * 7
    assert teleop._ik_solver.solve_calls == 0


def test_qp_failure_returns_last_known_good_target() -> None:
    class FailingSolver:
        def solve(self, target):
            del target
            raise RuntimeError("NaN in QP solution")

    teleop = object.__new__(SingleArmVelocityTeleop)
    teleop._ik_solver = FailingSolver()
    teleop._last_valid_joint_target = [0.1] * 7
    teleop._arm_name = "right_rm75"
    teleop.get_logger = lambda: FakeLogger()

    target, qp_success = teleop._solve_joint_target(np.eye(4))

    assert target == pytest.approx([0.1] * 7)
    assert not qp_success
    assert teleop._last_valid_joint_target == pytest.approx([0.1] * 7)


def test_qp_success_updates_last_known_good_target() -> None:
    class SuccessfulSolver:
        def solve(self, target):
            del target
            return [0.2] * 7

    teleop = object.__new__(SingleArmVelocityTeleop)
    teleop._ik_solver = SuccessfulSolver()
    teleop._last_valid_joint_target = [0.1] * 7
    teleop._arm_name = "left_rm75"
    teleop.get_logger = lambda: FakeLogger()

    target, qp_success = teleop._solve_joint_target(np.eye(4))

    assert target == pytest.approx([0.2] * 7)
    assert qp_success
    assert teleop._last_valid_joint_target == pytest.approx([0.2] * 7)


def test_enter_active_control_initializes_se3_orientation_state() -> None:
    teleop = object.__new__(SingleArmVelocityTeleop)
    transform = _make_transform(
        [0.3, -0.1, 0.2],
        _so3_exp(np.asarray([0.1, -0.2, 0.3])),
    )
    published = []
    teleop._arm_name = "right_rm75"
    teleop.get_logger = lambda: FakeLogger()
    teleop._publish_debug = lambda *args: published.append(args)

    teleop._enter_active_control(
        [0.0, 0.0, 0.0],
        (0.0, 0.0, 0.0, 1.0),
        transform,
        FakeTime(),
    )

    assert teleop._robot_start_transform == pytest.approx(transform)
    assert teleop._filtered_target == pytest.approx(transform[:3, 3])
    assert teleop._filtered_orientation_target == pytest.approx(transform[:3, :3])
    assert teleop._last_sent_orientation == pytest.approx(transform[:3, :3])
    assert len(published) == 1


def test_command_angular_velocity_uses_so3_rotation_vector() -> None:
    teleop = object.__new__(SingleArmVelocityTeleop)
    teleop._dt = 0.1
    teleop._last_sent_target = [0.0, 0.0, 0.0]
    teleop._last_sent_orientation = np.eye(3)
    teleop._last_command_time = None

    velocity = teleop._estimate_command_velocity(
        [0.0, 0.0, 0.0],
        _so3_exp(np.asarray([0.0, 0.0, 0.1])),
        FakeTime(),
    )

    assert velocity == pytest.approx([0.0, 0.0, 0.0, 0.0, 0.0, 1.0])


def test_timing_stats_logs_summary_and_clears_window() -> None:
    messages = []
    teleop = object.__new__(SingleArmVelocityTeleop)
    teleop._arm_name = "right_rm75"
    teleop._dt = 0.008
    teleop._timing_stats_window = 3
    teleop._timing_samples = {
        name: []
        for name in (
            "period",
            "total",
            "qp",
            "send",
            "feedback_age",
            "feedback_read",
            "feedback_interval",
        )
    }
    teleop._last_timing_feedback_received_at = None
    teleop.get_logger = lambda: SimpleNamespace(
        info=lambda message: messages.append(message)
    )
    first_feedback = JointStateSnapshot([0.0] * 7, 10.0, 2.0, None)
    second_feedback = JointStateSnapshot([0.0] * 7, 10.011, 3.0, 11.0)

    teleop._record_timing_sample(7.0, 6.0, 1.0, 0.5, 3.0, first_feedback)
    teleop._record_timing_sample(8.0, 8.0, 1.5, 0.6, 3.5, first_feedback)
    assert messages == []

    teleop._record_timing_sample(9.0, 10.0, 2.0, 0.7, 4.0, second_feedback)

    assert len(messages) == 1
    assert "right_rm75 timing n=3 deadline=8.000 ms" in messages[0]
    assert (
        "period[n=3 mean=8.000 p95=8.900 p99=8.980 "
        "max=9.000 ms overruns=1]"
    ) in messages[0]
    assert (
        "total[n=3 mean=8.000 p95=9.800 p99=9.960 "
        "max=10.000 ms overruns=1]"
    ) in messages[0]
    assert "qp[n=3" in messages[0]
    assert "send[n=3" in messages[0]
    assert "feedback_age[n=3" in messages[0]
    assert "feedback_read[n=2" in messages[0]
    assert "feedback_interval[n=1" in messages[0]
    assert all(not samples for samples in teleop._timing_samples.values())


def test_canfd_error_stops_queries_and_requires_grip_rearm() -> None:
    class RecoveringAdapter:
        def __init__(self):
            self.stop_calls = 0
            self.read_calls = 0

        def send_joint_target(self, joints, follow):
            del joints, follow
            raise RuntimeError("rm_movej_canfd failed with code 9")

        def stop(self):
            self.stop_calls += 1

        def read_joint_state(self):
            self.read_calls += 1
            return JointStateSnapshot([0.2] * 7, time.monotonic())

    teleop = _timeout_teleop(RecoveringAdapter())
    teleop._joint_command_max_speed = math.radians(180.0)
    teleop._joint_command_max_acceleration = math.radians(300.0)
    teleop._dt = 1.0 / 90.0

    sent = teleop._send_joint_target([0.3] * 7)

    assert not sent
    assert teleop._adapter.stop_calls == 1
    assert teleop._adapter.read_calls == 1
    assert not teleop._control_fault_latched
    assert teleop._grip_rearm_required
    assert teleop._last_joint_command_target == [0.2] * 7


def test_canfd_error_latches_when_joint_query_also_fails() -> None:
    class FailingAdapter:
        def __init__(self):
            self.stop_calls = 0

        def send_joint_target(self, joints, follow):
            del joints, follow
            raise RuntimeError("rm_movej_canfd failed with code 9")

        def stop(self):
            self.stop_calls += 1

        def read_joint_state(self):
            raise RuntimeError("rm_get_joint_degree failed with code 7")

    teleop = _timeout_teleop(FailingAdapter())
    teleop._joint_command_max_speed = math.radians(180.0)
    teleop._joint_command_max_acceleration = math.radians(300.0)
    teleop._dt = 1.0 / 90.0

    assert not teleop._send_joint_target([0.3] * 7)
    assert teleop._control_fault_latched
    assert teleop._adapter.stop_calls == 1