import xml.etree.ElementTree as ET

import numpy as np
import pytest
import trimesh

from iiwa_genesis.config import DESCRIPTION


@pytest.mark.parametrize("name", ["base_link"] + [f"link{i}" for i in range(1, 8)])
def test_visual_matches_collision_in_metres(name):
    path = DESCRIPTION / "urdf/iiwa_with_gripper.urdf"
    link = ET.parse(path).find(f"link[@name='{name}']")
    sizes = []
    for tag in ("visual", "collision"):
        node = link.find(f"{tag}/geometry/mesh")
        mesh = trimesh.load(path.parent / node.get("filename"), force="scene")
        scale = np.fromstring(node.get("scale", "1 1 1"), sep=" ")
        sizes.append(mesh.extents * scale)
    np.testing.assert_allclose(sizes[0], sizes[1], rtol=.05, atol=.003)
    assert max(sizes[0]) < .6