"""THE CANONICAL YOKE GENERATOR (Cameron 2026-07-08: "use our own yoke now since that is what we
switched to"). Supersedes yoke_exact.py (recovered from the lab, kept for reference/provenance only —
its constants independently validated this reconstruction: PT, plate extents, holes, IDLER_SNUG all match).

Parametric yoke, servo frame, mm. Knobs: PLATE_T (kit-era 4.0 -> 2.0 for screw engagement), HOLE_D
(2.9 -> 3.05). Contact faces (idler inner -27.3, horn inner 9.6) are FIXED; thickness changes only remove
OUTER material so screws bite deeper. Output: yoke_gen_thin.stl -> copied over yoke_fillet.stl +
yoke_exact.stl (arm chain inputs) and into gnode_v2_yoke.stl (v2 gripper chain, via gen_umi_v2.py)."""
import numpy as np, trimesh
from gen_curvy import _rrect, _stitch

PLATE_T = 2.0          # halved from 4.0 (Cameron 2026-07-08: screws barely reached the horn holes)
HOLE_D = 3.05          # was 2.9 ("a tiny bit larger" ok'd)
BORE_D = 9.0
HOLES_XZ = [(-32.5, 0.0), (-25.5, 7.0), (-25.5, -7.0), (-18.5, 0.0)]
PX0, PX1 = -48.0, -15.0        # plate x span (measured 33)
Z = 13.0                       # half-width
IDLER_IN, HORN_IN = -27.3, 9.6 # inner (contact) faces - DO NOT MOVE
BX0 = -53.0                    # bridge outer x
BW = 6.0                       # bridge wall thickness
R = 5.0                        # fillet radius (rounded prisms)

def rprism(lo, hi, r, axis):
    L = hi - lo
    lat = [i for i in range(3) if i != axis]
    e1, e2 = np.eye(3)[lat[0]], np.eye(3)[lat[1]]
    h1, h2 = L[lat[0]] / 2, L[lat[1]] / 2
    c = (lo + hi) / 2
    rings = []
    for i in range(14):
        t = i / 13.0
        q = _rrect(h1, h2, min(r, h1 - 0.3, h2 - 0.3))
        p = c.copy(); p[axis] = lo[axis] + L[axis] * t
        rings.append(p + np.outer(q[:, 0], e1) + np.outer(q[:, 1], e2))
    return _stitch(rings)

def U(p): return trimesh.boolean.union(p, engine="manifold")
def D(a, b): return trimesh.boolean.difference([a, b], engine="manifold")
def cylY(x, z, r, y0, y1):
    c = trimesh.creation.cylinder(radius=r, height=y1 - y0, sections=48)
    c.apply_transform(trimesh.transformations.rotation_matrix(np.pi / 2, [1, 0, 0]))
    c.apply_translation([x, (y0 + y1) / 2, z])
    return c

plates = []
for (yin, yout) in [(IDLER_IN, IDLER_IN - PLATE_T), (HORN_IN, HORN_IN + PLATE_T)]:
    y0, y1 = sorted([yin, yout])
    p = rprism(np.array([PX0, y0, -Z]), np.array([PX1, y1, Z]), R, 1)
    for hx, hz in HOLES_XZ:
        p = D(p, cylY(hx, hz, HOLE_D / 2, y0 - 1, y1 + 1))
    p = D(p, cylY(-25.5, 0.0, BORE_D / 2, y0 - 1, y1 + 1))
    plates.append(p)
# bridge wall spanning the plates, with fillet wedges at the junctions
ylo, yhi = IDLER_IN - PLATE_T, HORN_IN + PLATE_T
bridge = rprism(np.array([BX0, ylo, -Z]), np.array([BX0 + BW, yhi, Z]), R, 1)
fil1 = rprism(np.array([BX0, IDLER_IN - PLATE_T, -Z]), np.array([BX0 + BW + 5.0, IDLER_IN + 4.0, Z]), R, 0)
fil2 = rprism(np.array([BX0, HORN_IN - 4.0, -Z]), np.array([BX0 + BW + 5.0, HORN_IN + PLATE_T, Z]), R, 0)
yoke = U(plates + [bridge, fil1, fil2])
yoke.export("yoke_gen_thin.stl")
print(f"yoke_gen_thin: wt={yoke.is_watertight} vol={yoke.volume/1000:.1f}cm3 bounds={np.round(yoke.bounds,1).tolist()}")
# validation vs original (where geometry SHOULD match: plates inner faces, bridge, holes)
orig = trimesh.load("yoke_fillet.stl", force="mesh")
# hole check: rays through each hole position must pass clear
ok = True
for hx, hz in HOLES_XZ:
    for yq in (IDLER_IN - PLATE_T / 2, HORN_IN + PLATE_T / 2):
        d = trimesh.proximity.ProximityQuery(yoke).signed_distance(np.array([[hx, yq, hz]]))
        if d[0] > -(HOLE_D / 2 - 1.0):
            pass
        else:
            ok = False; print("hole blocked at", hx, hz, yq)
print("hole clearance check:", "PASS" if ok else "FAIL")
# servo clearance: same audit as always
servo_hull = trimesh.PointCloud(trimesh.load("st3215.stl", force="mesh").vertices).convex_hull
pts, _ = trimesh.sample.sample_surface(yoke, 9000)
pen = float(trimesh.proximity.ProximityQuery(servo_hull).signed_distance(pts).max())
print(f"vs servo hull: {max(0.0, pen):.2f}mm max (plates ride boss/disc: small contact expected)")