from pathlib import Path
from xml.etree import ElementTree

import numpy as np
import pytest

from xr_rm_teleop.placo_ik_solver import (
    PlacoIkSolver,
    _validated_transform,
)


def test_fixed_urdf_has_seven_moving_joints_and_omnipicker_tcp() -> None:
    urdf_path = (
        Path(__file__).resolve().parents[1]
        / "models"
        / "rm75_omnipicker"
        / "urdf"
        / "RM75-B_OmniPicker_fixed.urdf"
    )
    root = ElementTree.parse(urdf_path).getroot()
    moving_joint_names = [
        joint.attrib["name"]
        for joint in root.findall("joint")
        if joint.attrib["type"] != "fixed"
    ]
    tcp_joint = root.find("joint[@name='omnipicker_tcp_joint']")
    mesh_filenames = [
        mesh.attrib["filename"]
        for mesh in root.findall(".//mesh")
    ]

    assert moving_joint_names == [f"joint_{index}" for index in range(1, 8)]
    assert all(
        filename.startswith(
            "package://xr_rm_teleop/models/rm75_omnipicker/meshes/"
        )
        for filename in mesh_filenames
    )
    assert tcp_joint is not None
    assert tcp_joint.attrib["type"] == "fixed"
    assert tcp_joint.find("parent").attrib["link"] == "omnipicker_base_link"
    assert tcp_joint.find("child").attrib["link"] == "omnipicker_tcp"
    assert tcp_joint.find("origin").attrib["xyz"] == "0 0 0.16"
    assert tcp_joint.find("origin").attrib["rpy"] == "0 0 0"


def test_validated_transform_accepts_finite_se3_and_returns_a_copy() -> None:
    transform = np.eye(4)
    transform[:3, 3] = [0.3, -0.1, 0.2]

    actual = _validated_transform(transform)

    assert actual == pytest.approx(transform)
    assert actual is not transform


@pytest.mark.parametrize(
    "transform",
    [
        np.eye(3),
        np.full((4, 4), np.nan),
        np.vstack([np.eye(3, 4), [0.0, 0.0, 0.0, 2.0]]),
        np.diag([2.0, 1.0, 1.0, 1.0]),
    ],
)
def test_validated_transform_rejects_invalid_se3(transform: np.ndarray) -> None:
    with pytest.raises(ValueError):
        _validated_transform(transform)


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))