"""Voxel-dilation servo clearance (robust uniform inflate; OCP offset can't handle the
geared servo). Build holder un-trimmed (build123d), uniformly dilate the servo on a voxel
grid, then mesh-boolean subtract (manifold3d). Writes wrap_holder.stl.
Usage: voxel_trim.py [clearance_mm]
"""
import sys
import numpy as np
import trimesh
from scipy.ndimage import binary_dilation
from skimage import measure

sys.path.insert(0, "/data/cameron/repos/smith300_para_stuff")
from wrap_holder import make_wrap_holder
from build123d import export_stl

D = "/data/cameron/repos/smith300_para_stuff"
CLEAR = float(sys.argv[1]) if len(sys.argv) > 1 else 0.3
PITCH = 0.15
iters = max(1, round(CLEAR / PITCH))
print(f"voxel inflate: pitch={PITCH} iters={iters} -> ~{iters*PITCH:.2f}mm clearance", flush=True)


def as_volume(m, name):
    m.merge_vertices()
    m.update_faces(m.nondegenerate_faces())
    m.update_faces(m.unique_faces())
    m.remove_unreferenced_vertices()
    if not m.is_watertight:
        m.fill_holes()
    m.fix_normals()
    print(f"  {name}: watertight={m.is_watertight} tris={len(m.faces)}", flush=True)
    return m


# 1. holder geometry, no servo trim
export_stl(make_wrap_holder(trim=False), "/tmp/holder_raw.stl", tolerance=0.02, ascii_format=False)
holder = as_volume(trimesh.load("/tmp/holder_raw.stl", force="mesh"), "holder")

# 2. servo -> solid voxel grid -> meshes (world mm)
servo = trimesh.load(f"{D}/st3215.stl", force="mesh")
vox = servo.voxelized(pitch=PITCH).fill()
origin = np.asarray(vox.transform, float)[:3, 3]
pad = iters + 2
mat = np.pad(vox.matrix, pad)


def grid_to_mesh(boolgrid):
    v, f, _, _ = measure.marching_cubes(boolgrid.astype(np.float32), level=0.5)
    return trimesh.Trimesh(vertices=origin + (v - pad) * PITCH, faces=f)


servo_solid = as_volume(grid_to_mesh(mat), "servo_solid")
inflated = as_volume(grid_to_mesh(binary_dilation(mat, iterations=iters)), "inflated")
print("servo bounds      :", np.round(servo.bounds, 2).tolist(), flush=True)
print("servo_solid bounds:", np.round(servo_solid.bounds, 2).tolist(), "(match servo)", flush=True)
print("inflated bounds   :", np.round(inflated.bounds, 2).tolist(), "(servo +/- clear)", flush=True)

# 3. holder - inflated servo
result = trimesh.boolean.difference([holder, inflated])
parts = result.split(only_watertight=False)
if len(parts) > 1:
    result = max(parts, key=lambda m: m.volume)
result.export(f"{D}/wrap_holder.stl")
print(f"RESULT watertight={result.is_watertight} bodies={len(parts)} vol={result.volume:.0f} "
      f"(untrimmed ~5342)", flush=True)

# 4. verify no overlap with servo
inter = trimesh.boolean.intersection([result, servo_solid])
iv = inter.volume if inter is not None and len(inter.vertices) else 0.0
print(f"holder INTER servo = {iv:.2f} mm^3  ({'CLEAR' if iv < 1 else 'INTERFERENCE'})", flush=True)