import math
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,
    _euler_to_quaternion,
    _normalize_quaternion,
    _quaternion_to_euler,
)


def _make_teleop_for_orientation() -> SingleArmVelocityTeleop:
    teleop = object.__new__(SingleArmVelocityTeleop)
    teleop._enable_orientation_control = True
    teleop._enable_orientation_axes = [True, True, True]
    teleop._controller_orientation_start = (0.0, 0.0, 0.0, 1.0)
    teleop._robot_start_pose = ArmPose(0.3, 0.0, 0.2, 0.1, -0.2, 0.3)
    teleop._xr_to_robot_matrix = [
        0.0, 1.0, 0.0,
        0.0, 0.0, 1.0,
        1.0, 0.0, 0.0,
    ]
    return teleop


def assert_angles_close(actual: list[float] | tuple[float, ...], expected: list[float]) -> None:
    assert len(actual) == len(expected)
    for actual_value, expected_value in zip(actual, expected):
        assert math.atan2(math.sin(actual_value - expected_value), math.cos(actual_value - expected_value)) == pytest.approx(0.0)


def test_identity_controller_orientation_keeps_tcp_orientation() -> None:
    teleop = _make_teleop_for_orientation()

    target = teleop._raw_orientation_from_controller((0.0, 0.0, 0.0, 1.0))

    assert_angles_close(target, teleop._robot_start_pose.rpy())


def test_xr_relative_rotation_maps_through_xr_to_robot_matrix() -> None:
    teleop = _make_teleop_for_orientation()
    teleop._robot_start_pose = ArmPose(0.3, 0.0, 0.2, 0.0, 0.0, 0.0)
    xr_roll = _euler_to_quaternion(0.2, 0.0, 0.0)

    target = teleop._raw_orientation_from_controller(xr_roll)

    assert_angles_close(target, [0.0, 0.0, 0.2])


def test_orientation_deadband_filter_and_speed_limit() -> None:
    teleop = object.__new__(SingleArmVelocityTeleop)
    teleop._orientation_deadband_rad = 0.01
    teleop._orientation_filter_alpha = 0.5
    teleop._max_orientation_speed = 0.5
    teleop._dt = 0.1
    teleop._last_sent_orientation = [0.0, 0.0, 0.0]
    teleop._filtered_orientation_target = [0.0, 0.0, 0.0]

    assert teleop._apply_orientation_deadband([0.001, 0.0, 0.0]) == [0.0, 0.0, 0.0]

    filtered = teleop._filter_orientation_target([0.2, 0.0, 0.0])
    assert_angles_close(filtered, [0.1, 0.0, 0.0])

    limited, was_limited = teleop._limit_orientation_step([0.2, 0.0, 0.0])
    assert was_limited
    assert_angles_close(limited, [0.05, 0.0, 0.0])


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

    class FakeClock:
        def now(self):
            return FakeTime()

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

    teleop = object.__new__(SingleArmVelocityTeleop)
    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=0.0),
        ),
    )
    teleop._last_msg_time = FakeTime()
    teleop._arm_name = "test_rm75"
    teleop._command_timeout_sec = 0.12
    teleop._enable_orientation_control = True
    teleop._adapter = SimpleNamespace(
        get_latest_joint_state=lambda: JointStateSnapshot(
            [0.1] * 7,
            time.monotonic(),
        )
    )
    teleop._ik_solver = SimpleNamespace(
        update_joint_state=lambda joints: ArmPose(0.3, 0.0, 0.2)
    )
    teleop._active = False
    teleop._last_valid_joint_target = None
    teleop._last_current_pose = None
    teleop._joint_feedback_ready = True
    stopped = []
    teleop.get_clock = lambda: FakeClock()
    teleop.get_logger = lambda: FakeLogger()
    teleop._safe_stop = lambda reset_active: stopped.append(reset_active)

    teleop._control_tick()

    assert stopped == [True]


def test_quaternion_roundtrip_for_small_rpy() -> None:
    quat = _normalize_quaternion(_euler_to_quaternion(0.2, -0.1, 0.3))
    assert_angles_close(_quaternion_to_euler(quat), [0.2, -0.1, 0.3])


def test_zero_quaternion_is_invalid() -> None:
    with pytest.raises(ValueError):
        _normalize_quaternion([0.0, 0.0, 0.0, 0.0])