import time
from types import SimpleNamespace

import pytest

from xr_rm_teleop.realman_adapter import ArmPose, JointStateSnapshot
from xr_rm_teleop.single_arm_velocity_teleop import SingleArmVelocityTeleop


class FakeLogger:
    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 test_missing_or_stale_feedback_does_not_enable_qp() -> None:
    teleop = object.__new__(SingleArmVelocityTeleop)
    teleop._command_timeout_sec = 0.12
    teleop._adapter = SimpleNamespace(get_latest_joint_state=lambda: None)

    assert teleop._fresh_joint_state() is None

    teleop._adapter = SimpleNamespace(
        get_latest_joint_state=lambda: JointStateSnapshot(
            [0.0] * 7,
            time.monotonic() - 1.0,
        )
    )
    assert teleop._fresh_joint_state() is None


def test_stale_feedback_stops_before_active_control() -> None:
    stopped = []
    entered = []
    teleop = object.__new__(SingleArmVelocityTeleop)
    teleop._adapter = SimpleNamespace(
        get_latest_joint_state=lambda: JointStateSnapshot(
            [0.0] * 7,
            time.monotonic() - 1.0,
        )
    )
    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.get_clock = lambda: SimpleNamespace(now=lambda: FakeTime())
    teleop.get_logger = lambda: FakeLogger()
    teleop._safe_stop = lambda reset_active: stopped.append(reset_active)
    teleop._enter_active_control = lambda *args: entered.append(args)

    teleop._control_tick()

    assert stopped == [True]
    assert entered == []


def test_first_feedback_initializes_last_valid_target_without_solving() -> None:
    class FakeSolver:
        def __init__(self) -> None:
            self.solve_calls = 0

        def update_joint_state(self, joints):
            assert joints == [0.1] * 7
            return ArmPose(0.3, 0.0, 0.2)

        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

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

    assert pose == ArmPose(0.3, 0.0, 0.2)
    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 = teleop._solve_joint_target(ArmPose(0.3, 0.0, 0.2))

    assert target == pytest.approx([0.1] * 7)
    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 = teleop._solve_joint_target(ArmPose(0.3, 0.0, 0.2))

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


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

        def send_joint_target(self, joints, follow):
            del joints, follow
            raise RuntimeError("send failed")

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

    reset_calls = []
    teleop = object.__new__(SingleArmVelocityTeleop)
    teleop._adapter = FailingAdapter()
    teleop._follow = False
    teleop._arm_name = "left_rm75"
    teleop._stop_sent = False
    teleop.get_logger = lambda: FakeLogger()
    teleop._safe_stop = lambda reset_active: reset_calls.append(reset_active)

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

    assert not sent
    assert teleop._adapter.stop_calls == 1
    assert reset_calls == [True]