import numpy as np
import pinocchio as pin
import pytest

from rm75_ik import (
    RM75Kinematics,
    physical_joint_limits,
    pose_errors,
    teleop_joint_limits,
)


def test_limit_profiles_match_stage_one_contract():
    physical = physical_joint_limits()
    teleop = teleop_joint_limits()

    np.testing.assert_allclose(
        np.rad2deg(physical.upper), [178, 130, 178, 135, 178, 128, 360]
    )
    np.testing.assert_allclose(
        np.rad2deg(teleop.lower), [-150, -30, -170, -130, -175, -125, -179]
    )
    np.testing.assert_allclose(
        np.rad2deg(teleop.upper), [150, 110, 170, 130, 175, 125, 179]
    )


def test_zero_configuration_reaches_documented_flange_height():
    kinematics = RM75Kinematics()
    pose = kinematics.forward(np.zeros(7))

    np.testing.assert_allclose(pose.translation, [0.0, 0.0, 0.8505], atol=3e-6)
    np.testing.assert_allclose(pose.rotation, np.eye(3), atol=1e-7)


def test_tool_pose_is_composed_after_flange():
    kinematics = RM75Kinematics()
    q = np.deg2rad([30, -20, 40, 60, -50, 25, 90])
    tool = pin.SE3(np.eye(3), np.array([0.0, 0.0, 0.16]))

    expected = kinematics.forward(q) * tool
    actual = kinematics.forward(q, tool)

    assert pose_errors(expected, actual) == pytest.approx((0.0, 0.0), abs=1e-12)


@pytest.mark.parametrize(
    "configuration",
    [
        np.zeros(6),
        np.full(7, np.nan),
        np.deg2rad([179, 0, 0, 0, 0, 0, 0]),
    ],
)
def test_invalid_configuration_is_rejected(configuration):
    with pytest.raises(ValueError):
        RM75Kinematics().forward(configuration)


def test_jacobian_has_expected_shape_and_is_finite():
    jacobian = RM75Kinematics().jacobian(np.deg2rad([10, 20, -30, 40, 50, -60, 70]))
    assert jacobian.shape == (6, 7)
    assert np.all(np.isfinite(jacobian))