import math
from types import SimpleNamespace

import pytest

from xr_rm_teleop.realman_adapter import ArmPose, MockRealManAdapter
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
    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])


def test_mock_adapter_uses_shortest_angular_velocity() -> None:
    adapter = MockRealManAdapter([0.0, 0.0, 0.0, 3.13, 0.0, -3.13], 0.1)

    adapter.send_cartesian_target(ArmPose(0.0, 0.0, 0.0, -3.13, 0.0, 3.13), False)

    assert abs(adapter.last_velocity[3]) < 1.0
    assert abs(adapter.last_velocity[5]) < 1.0