"""Read a Blender-exported (10x) GLB -> revert scale -> rebuild the URDF + connectors from that layout."""
import sys
import numpy as np
import trimesh
from scipy.spatial.transform import Rotation as R
import gen_two_servo as g

glb = sys.argv[1] if len(sys.argv) > 1 else "two_servo_from_blender.glb"
lay = g.read_layout(glb)                                    # divides out GLB_SCALE, rebases to servo_0

# extract ALL base_cube* pieces (table mount + fiducial-board holder + connector piece etc.) -> servo_0
# frame (mm). Rebase by inv(servo_0) so a servo_0 shift in Blender doesn't offset the base vs the servos
# (the pieces are servo_0's children -> their world pose carries the shift; read_layout rebases servos too).
# Each is a box, so use its exact oriented bounding box as a clean volume (raw GLB box meshes aren't manifold).
scene = trimesh.load(glb)
base_inv = np.linalg.inv(np.array(scene.graph["servo_0"][0], float))
base_pieces = []
for node in sorted(scene.graph.nodes_geometry):
    if node.startswith("base_cube"):
        T, gname = scene.graph[node]
        m = scene.geometry[gname].copy()
        m.apply_transform(base_inv @ np.array(T, float))   # world -> servo_0 frame (matches the servos)
        m.apply_scale(1000.0 / g.GLB_SCALE)                # GLB units -> mm
        base_pieces.append(m.bounding_box_oriented)
        print(f"  {node}: centre(mm)={np.round(m.bounds.mean(0),1).tolist()} size(mm)={np.round(m.extents,1).tolist()}")
if base_pieces:
    holder = trimesh.load("holder_approx_drilled.stl", force="mesh")
    bh = trimesh.boolean.union([holder] + base_pieces, engine="manifold")
    bh.export("base_holder.stl")
    bodies = len(bh.split(only_watertight=False))
    print(f"  base_holder.stl: {len(base_pieces)} base piece(s) unioned  vol={bh.volume/1000:.1f}cm3 "
          f"watertight={bh.is_watertight} bodies={bodies}"
          f"{'  (WARNING: disjoint pieces — not all touching!)' if bodies > 1 else ''}")
print("recovered LAYOUT (metres, servo_0 frame):")
for i, M in enumerate(lay):
    pos = np.round(M[:3, 3] * 1000, 1).tolist()
    rpy = np.round(R.from_matrix(M[:3, :3]).as_euler("xyz", degrees=True), 1).tolist()
    print(f"  servo_{i}: pos_mm={pos}  rpy_deg={rpy}")

g.LAYOUT = lay                                              # gen_urdf/connector_mesh read this global
for i in range(1, len(lay)):
    c = g.connector_mesh(i)
    c.export(f"connector_{i}.stl")
    print(f"  connector_{i}: vol={c.volume/1000:.1f}cm3 watertight={c.is_watertight}")
g.gen_urdf("two_servo.urdf")
g.gen_glb("two_servo.glb", src_glb=glb)                     # refresh GLB: clean servo_0..N names + connectors + base pieces