"""[feetech_calib] Functional check of servo_zero_fixture:
 - at q=0 the servo & yoke sit in the pocket with ~0 interference,
 - at q=+-8 deg the yoke collides with the fixture (the angular key rejects it)."""
import numpy as np, trimesh

fix = trimesh.load("servo_zero_fixture.stl", force="mesh")
servo = trimesh.load("st3215.stl", force="mesh")

def yoke_at(deg):
    y = trimesh.load("yoke_fillet.stl", force="mesh")
    if deg:
        p = np.array([-25.5, 0.0, 0.0])          # horn axis (Y), from mate
        R = trimesh.transformations.rotation_matrix(np.radians(deg), [0, 1, 0], p)
        y.apply_transform(R)
    return y

def ivol(a, b):
    try:
        m = trimesh.boolean.intersection([a, b], engine="manifold")
        return 0.0 if m.is_empty else m.volume / 1000.0
    except Exception:
        return -1.0

print(f"q=0  servo∩fixture = {ivol(fix, servo):.3f} cm3  (expect ~0, clearance only)")
print(f"q=0  yoke ∩fixture = {ivol(fix, yoke_at(0)):.3f} cm3  (expect ~0)")
for d in (+8, -8, +15, -15):
    print(f"q={d:+3d}° yoke∩fixture = {ivol(fix, yoke_at(d)):.3f} cm3  (expect LARGE = rejected)")

# Pocket depth actually reached by the part in Z
fl = fix.bounds
print(f"\nfixture Z span = ({fl[0][2]:.1f}, {fl[1][2]:.1f})  part Z span = (-13, 13)")