import xml.etree.ElementTree as ET

import pytest
import yaml

from iiwa_genesis.config import ROOT, DESCRIPTION, load_config


def config_file(tmp_path, mode="without_gripper"):
    data = yaml.safe_load((ROOT / "configs/scene.yaml").read_text())
    data["scene"]["mode"] = mode
    file = tmp_path / "scene.yaml"
    file.write_text(yaml.safe_dump(data))
    return file


@pytest.mark.parametrize("mode,count", [("without_gripper", 1), ("with_gripper", 2)])
def test_mode_selects_urdf_and_cameras(tmp_path, mode, count):
    cfg = load_config(config_file(tmp_path, mode))
    assert len(cfg["cameras"]) == count
    assert cfg["robot"]["urdf"].name == f"iiwa_{mode}.urdf"


@pytest.mark.parametrize("mode", ["without_gripper", "with_gripper"])
def test_urdf_tree_and_assets(mode):
    path = DESCRIPTION / f"urdf/iiwa_{mode}.urdf"
    root = ET.parse(path).getroot()
    links = {l.get("name") for l in root.findall("link")}
    joints = root.findall("joint")
    assert sum(j.get("type") == "revolute" for j in joints) == 7
    children = [j.find("child").get("link") for j in joints]
    assert len(children) == len(set(children))
    assert links - set(children) == {"world"}
    assert all(j.find("parent").get("link") in links for j in joints)
    assert ("camera_optical_frame" in links) == (mode == "with_gripper")
    for mesh in root.iter("mesh"):
        assert (path.parent / mesh.get("filename")).is_file()
    text = path.read_text()
    for prohibited in ("package://", "xacro", "ros2_control", "<gazebo", "<webots", "$(find"):
        assert prohibited not in text


def test_tool_transforms_are_preserved():
    original = ET.parse(ROOT / "iiwa_description/urdf/tools/patron.xacro").getroot()
    exported = ET.parse(DESCRIPTION / "urdf/iiwa_with_gripper.urdf").getroot()
    for joint in original.findall("joint"):
        actual = exported.find(f"joint[@name='{joint.get('name')}']")
        assert actual is not None
        assert actual.attrib == joint.attrib
        for tag in ("origin", "parent", "child"):
            assert actual.find(tag).attrib == joint.find(tag).attrib


@pytest.mark.parametrize("bad", ["mode", "joints", "up", "fps", "range"])
def test_invalid_config(tmp_path, bad):
    file = config_file(tmp_path)
    data = yaml.safe_load(file.read_text())
    if bad == "mode":
        data["scene"]["mode"] = "invalid"
    elif bad == "joints":
        data["robot"]["joints"]["joint1"] = 100
    elif bad == "up":
        data["cameras"]["overhead"]["up"] = [0, 0, 1]
    elif bad == "fps":
        data["cameras"]["overhead"]["fps"] = 0
    else:
        data["cameras"]["overhead"]["min_range"] = 10
    file.write_text(yaml.safe_dump(data))
    with pytest.raises(ValueError):
        load_config(file)


def test_table_mesh_paths():
    path = DESCRIPTION / "table/table.urdf"
    for mesh in ET.parse(path).iter("mesh"):
        assert (path.parent / mesh.get("filename")).is_file()