import math

import numpy as np
import pytest

from xr_rm_teleop.placo_ik_solver import (
    PlacoIkSolver,
    _arm_pose_to_transform,
    _tool_pose_to_transform,
    _transform_to_arm_pose,
)
from xr_rm_teleop.realman_adapter import ArmPose


def test_tool_offset_rotates_with_flange_and_roundtrips() -> None:
    flange_pose = ArmPose(0.30, -0.10, 0.20, 0.0, math.pi / 2.0, 0.0)
    tool_pose = [0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0]

    base_to_flange = _arm_pose_to_transform(flange_pose)
    flange_to_tool = _tool_pose_to_transform(tool_pose)
    base_to_tool = base_to_flange @ flange_to_tool
    recovered_flange = base_to_tool @ np.linalg.inv(flange_to_tool)

    assert base_to_tool[:3, 3] == pytest.approx([0.49, -0.10, 0.20])
    assert recovered_flange == pytest.approx(base_to_flange)


def test_transform_to_arm_pose_roundtrip() -> None:
    expected = ArmPose(0.25, -0.30, 0.40, 0.20, -0.30, 0.40)

    actual = _transform_to_arm_pose(_arm_pose_to_transform(expected))

    assert actual.xyz() == pytest.approx(expected.xyz())
    assert actual.rpy() == pytest.approx(expected.rpy())


def test_qp_result_rejects_nan_position_and_velocity_violations() -> None:
    solver = object.__new__(PlacoIkSolver)
    solver._joint_limits = np.asarray([[-1.0, 1.0]] * 7)
    solver._velocity_limits = np.ones(7)
    solver._dt = 0.1
    solver._actual_joints = np.zeros(7)

    with pytest.raises(ValueError, match="finite"):
        solver._validate_result(np.full(7, np.nan))
    with pytest.raises(ValueError, match="position"):
        solver._validate_result(np.full(7, 2.0))
    with pytest.raises(ValueError, match="velocity"):
        solver._validate_result(np.full(7, 0.2))