import os
from pathlib import Path

import numpy as np
import pinocchio as pin
import pytest

from rm75_ik import (
    DualArmAssembly,
    RM75Kinematics,
    RealManFkReference,
    pose_errors,
)


@pytest.fixture(scope="module")
def reference():
    sdk_root = os.environ.get("REALMAN_SDK_ROOT")
    if not sdk_root:
        pytest.skip("REALMAN_SDK_ROOT is not set")
    return RealManFkReference(Path(sdk_root))


@pytest.mark.parametrize(
    "q_deg",
    [
        [0, 0, 0, 0, 0, 0, 0],
        [30, -20, 40, 60, -50, 25, 90],
        [-100, 80, -90, -70, 120, -60, -180],
    ],
)
def test_pinocchio_fk_matches_realman_algo(reference, q_deg):
    q = np.deg2rad(q_deg)
    position_error, orientation_error = pose_errors(
        RM75Kinematics().forward(q), reference.forward(q)
    )

    assert position_error < 1e-4
    assert orientation_error < np.deg2rad(0.01)


def test_tool_fk_matches_realman_algo(reference):
    q = np.deg2rad([30, -20, 40, 60, -50, 25, 90])
    tool = pin.SE3(np.eye(3), np.array([0.0, 0.0, 0.19]))
    position_error, orientation_error = pose_errors(
        RM75Kinematics().forward(q, tool), reference.forward(q, tool)
    )

    assert position_error < 1e-4
    assert orientation_error < np.deg2rad(0.01)


def test_dual_arm_mounts_reuse_single_arm_geometry(reference):
    assembly = DualArmAssembly.from_source_urdf()
    q = np.deg2rad([20, -10, 30, 40, -20, 15, 80])

    assert assembly.dof == 14
    assert assembly.mounts.right_visual_origin_delta_m == pytest.approx(0.001, abs=1e-8)
    for arm, mount in (
        ("left", assembly.mounts.left_base),
        ("right", assembly.mounts.right_base),
    ):
        local = mount.actInv(assembly.forward(arm, q))
        errors = pose_errors(local, reference.forward(q))
        assert errors[0] < 1e-4
        assert errors[1] < np.deg2rad(0.01)