"""Arm-variant gripper URDF (no slab, shaved camera) + one-piece arm print."""
import numpy as np, trimesh, trimesh.repair
import xml.etree.ElementTree as ET
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
br = trimesh.load("gnode_v2_cam_conn_arm.stl", force="mesh")
bodies = [b for b in br.split(only_watertight=False) if b.volume > 50]
print("shaved bracket bodies:", len(bodies), [round(b.volume/1000,1) for b in bodies])
# URDF: copy umi_gripper_v2, drop the slab visual, swap camera gnodes to _arm
t = ET.parse("umi_gripper_v2.urdf"); r = t.getroot()
r.set("name", "umi_v2_arm")
for link in r.findall("link"):
    for v in list(link.findall("visual")):
        mesh = v.find("geometry/mesh")
        if mesh is None: continue
        fn = mesh.get("filename")
        if "aruco_cube" in fn:
            link.remove(v)
        elif fn in ("gnode_v2_cam_conn.stl", "gnode_v2_camera_mount.stl", "gnode_v2_camera_cover.stl"):
            mesh.set("filename", fn.replace(".stl", "_arm.stl"))
t.write("umi_gripper_v2_arm.urdf", encoding="unicode", xml_declaration=True)
print("wrote umi_gripper_v2_arm.urdf")
# one-piece print for the arm gripper: fixed union + shaved bracket + mount, NO slab/bridge
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)
# fuse the wrist yoke (bolts to arm servo_5) into the piece: yoke mesh -> gripper frame (+25.5 vis +46.4 attach)
wyoke = LR("yoke_gen_thin.stl")
wyoke.apply_translation([71.9, 0, 0])
hol = LR("gnode_v2_holder.stl")
he = hol.bounds[1][0]
weld = trimesh.creation.box(extents=[26.0 - 15.0, 34.0, 24.0])
weld.apply_translation([(15.0 + 26.0) / 2, -10.0, 0.0])                 # bridges holder east wall <-> yoke bridge wall
piece = U([LR("gripper_v2_print_fixed.stl"), LR("gnode_v2_cam_conn_arm.stl"), LR("gnode_v2_camera_mount_arm.stl"),
           wyoke, weld])
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 = trimesh.boolean.difference([piece, cor], engine="manifold")
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 = trimesh.boolean.difference([piece, trimesh.PointCloud(np.vstack([V + o for o in offs])).convex_hull], engine="manifold")
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_arm.stl")
nb = len(piece.split(only_watertight=False))
chk = trimesh.boolean.intersection([body, piece], engine="manifold").volume
print(f"print_fixed_arm: wt={piece.is_watertight} bodies={nb} vol={piece.volume/1000:.1f}cm3 body-clash={chk:.1f}mm3")