"""BOOTSTRAP: extract the module meshes out of the Blender GLBs (the only surviving copies while phe108 is
down) back into the canonical STL files the pipeline expects.

From gripper.glb (nodes are ALREADY in servo-frame at x0.01 scale, exported by gen_gripper_urdf):
  servo -> st3215.stl, holder -> holder_approx_drilled.stl, yoke -> yoke_fillet.stl, finger, etc.
From two_servo.glb (arm, parent-chain, GLB_SCALE=10): servo_0 node carries the module mesh (servo+holder+
yoke concatenated, face-colored) — usable for display; splitting it back into parts is lossy, so prefer the
gripper.glb copies for servo/holder/yoke.

Usage: python3 extract_meshes_from_glb.py gripper.glb
"""
import sys
import numpy as np
import trimesh

GRIP_MAP = {
    "servo": "st3215.stl",
    "holder": "holder_approx_drilled.stl",
    "yoke": "yoke_fillet.stl",
    "finger": "finger.stl",
    "servo_to_finger": "gnode_servo_to_finger.stl",
    "finger_adj": "gnode_finger_adj.stl",
    "connector1": "gnode_connector1.stl",
    "yoke_conn": "gnode_yoke_conn.stl",
    "fixed_conn": "gnode_fixed_conn.stl",
}

path = sys.argv[1] if len(sys.argv) > 1 else "gripper.glb"
s = trimesh.load(path)
for node in s.graph.nodes_geometry:
    T, gn = s.graph[node]
    m = s.geometry[gn].copy()
    m.apply_transform(np.array(T, float))
    m.apply_scale(100.0)                       # GLB units (0.01 = 1mm) -> mm
    out = GRIP_MAP.get(node, f"extracted_{node}.stl")
    m.export(out)
    print(f"{node} -> {out}  ({len(m.vertices)} verts, bbox {np.round(m.extents,1).tolist()} mm)")
print("NOTE: gripper.glb node meshes also serve as gnode_*.stl for gen_gripper_print / composer work.")