import math
import time
from types import SimpleNamespace

import numpy as np
import pytest

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 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_disabled_joint_feedback_does_not_enable_qp() -> None:
    teleop = object.__new__(SingleArmVelocityTeleop)
    teleop._command_timeout_sec = 0.12
    teleop._adapter = SimpleNamespace(
        get_latest_joint_state=lambda: JointStateSnapshot(
            [0.0] * 7,
            time.monotonic(),
            motion_ready=False,
        )
    )

    assert teleop._fresh_joint_state() is None


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_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._grip_rearm_required = True
    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_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
            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

    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 = teleop._solve_joint_target(np.eye(4))

    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(np.eye(4))

    assert target == pytest.approx([0.2] * 7)
    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_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._last_joint_command_target = [0.0] * 7
    teleop._last_joint_command_velocity = [0.0] * 7
    teleop._joint_command_max_speed = math.radians(180.0)
    teleop._joint_command_max_acceleration = math.radians(300.0)
    teleop._dt = 1.0 / 125.0
    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]