import math

import pytest

from xr_rm_teleop.realman_adapter import RealManAdapter
from xr_rm_teleop.realman_adapter import MockRealManAdapter
from xr_rm_teleop.fun_peripheral import PeripheralConfig


def test_initial_pose_uses_joint_move_only() -> None:
    class FakeArm:
        def __init__(self) -> None:
            self.calls = []

        def rm_movej(self, *args):
            self.calls.append(args)
            return 0

    joints = [-167.21, 28.48, 28.21, 61.35, -14.40, 84.49, -124.51]
    adapter = RealManAdapter("127.0.0.1", 8080, 0, 1, initial_joint_pose=joints)
    adapter._arm = FakeArm()

    adapter._move_to_initial_pose()

    assert adapter._arm.calls == [(joints, 20, 0, 0, 1)]


def test_peripheral_config_exposes_selected_tool() -> None:
    config = PeripheralConfig(
        scissorgripper=1,
        tools_in_ee={
            "first": [[0.0] * 7, [0.0] * 7],
            "second": [[0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0], [0.0] * 7],
        },
    )

    assert config.tool_name == "second"
    assert config.tool_pose == [0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0]


def test_joint_feedback_is_cached_in_radians() -> None:
    class FakeArm:
        def rm_get_joint_degree(self):
            return 0, [0.0, 10.0, -20.0, 30.0, -40.0, 50.0, -60.0]

    adapter = RealManAdapter("127.0.0.1", 8080, 0, 0.01)
    adapter._arm = FakeArm()

    adapter._read_joint_state_once()
    snapshot = adapter.get_latest_joint_state()

    assert snapshot is not None
    assert snapshot.positions == pytest.approx(
        [math.radians(value) for value in [0, 10, -20, 30, -40, 50, -60]]
    )


def test_joint_target_uses_movej_canfd_in_degrees() -> None:
    class FakeArm:
        def __init__(self) -> None:
            self.calls = []

        def rm_movej_canfd(self, *args):
            self.calls.append(args)
            return 0

    adapter = RealManAdapter("127.0.0.1", 8080, 0, 0.01)
    adapter._arm = FakeArm()
    target = [math.radians(value) for value in [1, 2, 3, 4, 5, 6, 7]]

    adapter.send_joint_target(target, follow=False)

    assert len(adapter._arm.calls) == 1
    degrees, follow, expand, trajectory_mode, radio = adapter._arm.calls[0]
    assert degrees == pytest.approx([1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0])
    assert (follow, expand, trajectory_mode, radio) == (False, 0, 2, 0)


def test_mock_joint_feedback_is_available_without_vendor_sdk() -> None:
    adapter = MockRealManAdapter([1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0])

    adapter.connect()
    snapshot = adapter.get_latest_joint_state()

    assert snapshot is not None
    assert snapshot.positions == pytest.approx(
        [math.radians(value) for value in [1, 2, 3, 4, 5, 6, 7]]
    )