"""One-time, ROS-free export of this repository's known description macros.

This is deliberately not a general Xacro interpreter. Runtime uses the emitted URDFs.
"""
from copy import deepcopy
from pathlib import Path
import re
import shutil
import xml.etree.ElementTree as ET

ROOT = Path(__file__).resolve().parents[1]
SOURCE = ROOT / "iiwa_description"
DEST = ROOT / "iiwa_genesis_description"
NS = "{http://www.ros.org/wiki/xacro}"


def export():
    props = {}
    for name in ("params", "meshes"):
        for node in ET.parse(SOURCE / f"urdf/{name}.xacro").getroot():
            if node.tag == NS + "property":
                props[node.get("name")] = node.get("value")

    def substitute(value, variables):
        for _ in range(8):
            updated = re.sub(r"\$\{([^}]+)\}", lambda m: variables[m[1]], value)
            if updated == value:
                return value
            value = updated
        raise ValueError("Unresolved variable")

    macros = {n.get("name"): n for n in ET.parse(SOURCE / "urdf/macros.xacro").getroot()}

    def expand(node):
        if not node.tag.startswith(NS):
            return [deepcopy(node)]
        name = node.tag[len(NS):]
        if name == "include":
            return []
        result = []
        for template in macros[name]:
            item = deepcopy(template)
            for child in item.iter():
                child.attrib = {k: substitute(v, props | node.attrib) for k, v in child.attrib.items()}
            result.append(item)
        return result

    def write(robot, path):
        path.parent.mkdir(parents=True, exist_ok=True)
        for mesh in robot.iter("mesh"):
            source = mesh.get("filename")
            prefix = "package://iiwa_description/resource/meshes/"
            if not source.startswith(prefix):
                raise ValueError(source)
            relative = source[len(prefix):]
            target = DEST / "urdf/meshes" / relative
            target.parent.mkdir(parents=True, exist_ok=True)
            shutil.copy2(SOURCE / "resource/meshes" / relative, target)
            mesh.set("filename", ("meshes/" if path.parent.name == "urdf" else "../urdf/meshes/") + relative)
        ET.indent(robot)
        ET.ElementTree(robot).write(path, encoding="utf-8", xml_declaration=True)

    base = ET.Element("robot", name="iiwa7")
    for name in ("links", "joints"):
        for node in ET.parse(SOURCE / f"urdf/{name}.xacro").getroot():
            base.extend(expand(node))
    write(deepcopy(base), DEST / "urdf/iiwa_without_gripper.urdf")
    for node in ET.parse(SOURCE / "urdf/tools/patron.xacro").getroot():
        if node.tag in ("link", "joint"):
            base.append(deepcopy(node))
    write(base, DEST / "urdf/iiwa_with_gripper.urdf")
    write(ET.parse(SOURCE / "urdf/table/table.urdf").getroot(), DEST / "table/table.urdf")
    shutil.copy2(SOURCE / "LICENSE", DEST / "LICENSE")


if __name__ == "__main__":
    export()