"""Export the OpenSCAD robot as meshes for the Unity simulation.

    python export_unity.py            (from hardware/openscad, needs OpenSCAD)

Writes Hexapod_Simulation_Unity/Assets/HexapodSim/Resources/OpenScadModel/*.bytes,
loaded by Assets/HexapodSim/OpenScadModel.cs. Every file is one rig segment
in the robot frame (mm, +X forward, +Y left, +Z up), relative to the
segment's pivot, in the rest pose that matches the rig's zero rotations
(coxa 0, femur 0, tibia -90):
  body           Z = 0 on the joint plane (13.5 mm above the bottom plate)
  leg_<side>_*   left = as leg 4 (mount +90), right = its mirror (mount -90)

File format (little endian): b"HXM1", int32 groups, then per group
float32 r, g, b, a, int32 triangles, triangles * 9 float32 (x, y, z).
Triangles wind counter-clockwise seen from outside (right-handed).
"""
import os, re, struct, subprocess, sys, tempfile

HERE = os.path.dirname(os.path.abspath(__file__)).replace("\\", "/")
OUT = os.path.normpath(os.path.join(HERE, "..", "..", "Hexapod_Simulation_Unity", "Assets",
                                    "HexapodSim", "Resources", "OpenScadModel"))
OPENSCAD = os.environ.get("OPENSCAD", r"C:/Program Files/OpenSCAD/openscad.exe")
HEADER = f"use <{HERE}/body.scad>\nuse <{HERE}/leg.scad>\n"
JOINT_PLANE_Z = 13.5


def hexcol(h, a=1.0):
    h = h.lstrip("#")
    return [int(h[i:i + 2], 16) / 255 for i in (0, 2, 4)] + [a]


# segment -> list of (colour, OpenSCAD geometry in the segment's frame)
BODY = [
    ("#3a3a3a", f"translate([0, 0, -{JOINT_PLANE_Z}]) fusion_to_robot() bottom_half();"),
    ("#555555", f"translate([0, 0, -{JOINT_PLANE_Z}]) fusion_to_robot() top_half();"),
    ("#2a2a2a", f"translate([0, 0, -{JOINT_PLANE_Z}]) fusion_to_robot() body_servos_uv();"),
    ("#2b6b3a", f"translate([0, 0, -{JOINT_PLANE_Z}]) pcb_in_body();"),
]
LEFT = "rotate([0, 0, 90]) "   # leg frame -> robot frame for the left legs (mount +90)
LEG = {
    "coxa": [
        ("#d9822b", LEFT + "{ coxa_bracket_upper(); coxa_bracket_lower(); }"),
        ("#bdbdbd", LEFT + "translate([55, 0, 0]) along_y(28.7) horn_adapter();"),
        ("#5a3a1e", LEFT + "coxa_cables();"),
    ],
    "femur": [
        ("#4a7fbf", LEFT + "{ link_channel(); link_lid(); }"),
        ("#2a2a2a", LEFT + "link_servos();"),
        ("#5a3a1e", LEFT + "femur_cables();"),
    ],
    "tibia": [
        ("#5fa35f", LEFT + "{ tibia_bearing_half(); tibia_horn_half(); }"),
        ("#bdbdbd", LEFT + "along_y(28.7) horn_adapter();"),
        ("#1c1c1c", LEFT + "foot_sock();"),
    ],
}


def render(geometry, tmp):
    scad = os.path.join(tmp, "part.scad"); stl = os.path.join(tmp, "part.stl")
    with open(scad, "w") as f:
        f.write(HEADER + geometry + "\n")
    r = subprocess.run([OPENSCAD, "-o", stl, scad], capture_output=True, text=True)
    if r.returncode != 0 or not os.path.exists(stl):
        sys.exit(f"OpenSCAD failed on: {geometry}\n{r.stderr}")
    tris = []
    num = r"([-+0-9.eE]+)"
    for m in re.finditer(r"outer loop\s+vertex\s+%s\s+%s\s+%s\s+vertex\s+%s\s+%s\s+%s\s+vertex\s+%s\s+%s\s+%s" % ((num,) * 9),
                         open(stl).read()):
        tris.append([float(v) for v in m.groups()])
    os.remove(stl)
    return tris


def mirror_y(tris):
    # (x, y, z) -> (x, -y, z) and swap two vertices to keep outward winding
    out = []
    for t in tris:
        a, b, c = t[0:3], t[3:6], t[6:9]
        a, b, c = [a[0], -a[1], a[2]], [b[0], -b[1], b[2]], [c[0], -c[1], c[2]]
        out.append(a + c + b)
    return out


def write(name, groups):
    path = os.path.join(OUT, name + ".bytes")
    with open(path, "wb") as f:
        f.write(b"HXM1" + struct.pack("<i", len(groups)))
        for col, tris in groups:
            f.write(struct.pack("<4f", *col) + struct.pack("<i", len(tris)))
            for t in tris:
                f.write(struct.pack("<9f", *t))
    print(f"  {name}: {sum(len(t) for _, t in groups)} triangles")


def main():
    os.makedirs(OUT, exist_ok=True)
    with tempfile.TemporaryDirectory() as tmp:
        print("body")
        write("body", [(hexcol(c), render(g, tmp)) for c, g in BODY])
        for seg, parts in LEG.items():
            print("leg", seg)
            groups = [(hexcol(c), render(g, tmp)) for c, g in parts]
            write(f"leg_left_{seg}", groups)
            write(f"leg_right_{seg}", [(c, mirror_y(t)) for c, t in groups])


if __name__ == "__main__":
    main()