"""SEPARATE test variant (2026-07-13, NOT default): marker CUBE instead of the slab.
Cube pose = Cameron-placed (Blender GLB round-trip); joined AS-IS, no keepout carve (his call).
Marker faces: back(+x), top, left, right — all flat, IDs assigned later.
Outputs: gripper_v2_print_fixed_umicube.stl + umi_gripper_v2_cube.urdf"""
import numpy as np, trimesh, trimesh.repair
import xml.etree.ElementTree as ET
from gen_curvy import _rrect, _stitch

def LR(f):
    m = trimesh.load(f, force="mesh")
    if not m.is_volume:
        m.merge_vertices(merge_tex=True, merge_norm=True)
        m.update_faces(m.nondegenerate_faces()); m.update_faces(m.unique_faces())
        trimesh.repair.fill_holes(m); m.fix_normals()
    return m
def U(parts):
    m = trimesh.boolean.union(parts, engine="manifold")
    keep = [b for b in m.split(only_watertight=False) if b.volume > 100.0]
    return keep[0] if len(keep) == 1 else trimesh.util.concatenate(keep)
def D(a, b): return trimesh.boolean.difference([a, b], engine="manifold")
def prism(lo, hi, r=4.0):
    L = hi - lo; k = int(np.argmax(L)); lat = [i for i in range(3) if i != k]
    e1, e2 = np.eye(3)[lat[0]], np.eye(3)[lat[1]]
    c = (lo + hi) / 2; rings = []
    for i in range(12):
        t = i / 11.0
        q = _rrect(L[lat[0]] / 2, L[lat[1]] / 2, min(r, L[lat[0]] / 2 - 0.4, L[lat[1]] / 2 - 0.4))
        p = c.copy(); p[k] = lo[k] + L[k] * t
        rings.append(p + np.outer(q[:, 0], e1) + np.outer(q[:, 1], e2))
    return _stitch(rings)

cube = LR("gnode_v2_aruco_cubevar.stl")
CUBE_LO, CUBE_HI = np.array(cube.bounds[0]), np.array(cube.bounds[1])
riser = LR("gnode_v2_fixed_riser.stl")
re = riser.bounds[1][0]; rb = riser.bounds
yo = (max(rb[0][1], CUBE_LO[1]), min(rb[1][1], CUBE_HI[1]))
zo = (max(rb[0][2], CUBE_LO[2]), min(rb[1][2], CUBE_HI[2]))
if yo[1] - yo[0] < 8.0:
    yc = (yo[0] + yo[1]) / 2
    yo = (max(CUBE_LO[1], yc - 7.0), min(rb[1][1], yc + 7.0))
if zo[1] - zo[0] < 8.0:
    zc = (zo[0] + zo[1]) / 2
    zo = (zc - 7.0, zc + 7.0)
bridge = prism(np.array([re - 5.0, yo[0], zo[0] + 1]), np.array([min(CUBE_LO[0] + 6.0, CUBE_HI[0] - 2.0), yo[1], zo[1] - 1]))
piece = U([LR("gripper_v2_print_fixed.stl"), cube, bridge,
           LR("gnode_v2_cam_conn.stl"), LR("gnode_v2_camera_mount.stl")])
T = np.load("/tmp/T_auth_to_v2.npy")
cor = trimesh.creation.box(extents=[16.6, 12.8, 50.0])
cor.apply_translation([-7.0, -13.7, -28.0]); cor.apply_transform(T)
piece = D(piece, cor)
body = LR("gnode_v2_camera_body.stl")
blob = trimesh.boolean.intersection([body, piece], engine="manifold")
if blob.volume > 1.0:
    V = blob.convex_hull.vertices
    offs = [d * s for d in np.eye(3) for s in (1.5, -1.5)]
    piece = D(piece, trimesh.PointCloud(np.vstack([V + o for o in offs])).convex_hull)
keep = [b for b in piece.split(only_watertight=False) if b.volume > 100.0]
piece = keep[0] if len(keep) == 1 else trimesh.util.concatenate(keep)
piece.export("gripper_v2_print_fixed_umicube.stl")
nb = len(piece.split(only_watertight=False))
chk = trimesh.boolean.intersection([body, piece], engine="manifold").volume
print(f"umicube piece: wt={piece.is_watertight} bodies={nb} vol={piece.volume/1000:.1f}cm3 body-clash={chk:.1f}mm3")
for nm in ("handle_column", "thumb_cradle"):
    iv = trimesh.boolean.intersection([LR(f"gnode_v2_{nm}.stl"), cube], engine="manifold").volume
    if iv > 1: print(f"  note: cube overlaps {nm}: {iv/1000:.2f}cm3")
t = ET.parse("umi_gripper_v2.urdf"); r = t.getroot(); r.set("name", "umi_v2_cube")
for link in r.findall("link"):
    for v in link.findall("visual"):
        mesh = v.find("geometry/mesh")
        if mesh is not None and "aruco_cube" in mesh.get("filename"):
            mesh.set("filename", "gnode_v2_aruco_cubevar.stl")
t.write("umi_gripper_v2_cube.urdf", encoding="unicode", xml_declaration=True)
print("wrote umi_gripper_v2_cube.urdf")