"""v2 UMI-ization: (a) TPU inserts posed per Cameron's finger placements (mirror-aware);
(b) transplant v1 camera bracket + marker cube onto v2 (cube re-anchored to the riser east face)."""
import numpy as np, trimesh

def L(f): return trimesh.load(f, force="mesh")
BASE = "enpire_gripper/Gripper+Finger+for+Robot+Arm/"
hard_src = L(BASE + "enpire_gripper_finger_hard.stl")
soft_src = L(BASE + "enpire_gripper_finger_soft.stl")

# solve each finger's source->world map from vertex correspondence (round-trip preserves order)
def solve_map(world_mesh):
    A, B = hard_src.vertices, world_mesh.vertices
    assert len(A) == len(B), f"vert count changed {len(A)} vs {len(B)}"
    ca, cb = A.mean(0), B.mean(0)
    H = (A - ca).T @ (B - cb)
    U_, S, Vt = np.linalg.svd(H)
    R = Vt.T @ U_.T                      # allows det=-1 (reflection) naturally
    res = np.abs((A - ca) @ R.T + cb - B).max()
    T = np.eye(4); T[:3, :3] = R; T[:3, 3] = cb - R @ ca
    return T, res

for nm in ("fixed", "moving"):
    Tm, res = solve_map(L(f"gnode_v2_enpire_{nm}.stl"))
    det = np.linalg.det(Tm[:3, :3])
    print(f"{nm}: residual {res:.3f}mm det={det:+.2f} {'(mirrored)' if det < 0 else ''}")
    soft = soft_src.copy(); soft.apply_transform(Tm)
    if det < 0: soft.invert()
    soft.export(f"gripper_v2_soft_{nm}.stl")
    print(f"  soft insert vol {soft.volume/1000:.1f}cm3 wt={soft.is_watertight}")

# --- transplant camera cluster (verbatim) + cube (re-anchored) ---
riser = L("gnode_v2_fixed_riser.stl"); plate = L("gnode_v2_fixed_plate.stl")
riser_e = riser.bounds[1][0]
cube = L("gnode_aruco_cube.stl")
dx = (riser_e + 0.05) - cube.bounds[0][0]
cube.apply_translation([dx, 0, 0])
cube.export("gnode_v2_aruco_cube.stl")
print(f"cube shifted {dx:+.1f}mm; now x {cube.bounds[0][0]:.1f}..{cube.bounds[1][0]:.1f}")
# clearance audit: v1 camera cluster vs v2 structures (riser/plate/cradle)
targets = {"riser": riser, "plate": plate, "finger_cradle": L("gnode_v2_finger_cradle.stl")}
for part in ("cam_conn", "camera_mount", "camera_body", "camera_cover"):
    m = L(f"gnode_{part}.stl")
    pts, _ = trimesh.sample.sample_surface(m, 6000)
    for tn, tm in targets.items():
        pen = float(trimesh.proximity.ProximityQuery(tm).signed_distance(pts).max())
        if pen > -2.0:
            print(f"  {part} vs {tn}: {'PENETRATES %.2f' % pen if pen > 0.05 else 'kiss/near %.2f' % pen}mm")
print("camera cluster audit done (silent = >2mm clear)")