"""[feetech_calib] Inspect the servo module geometry to design the zeroing fixture.
Prints mesh frames/extents for st3215 + yoke, and the yoke placement at q=0."""
import numpy as np, trimesh

def stats(path):
    m = trimesh.load(path, force="mesh")
    lo, hi = m.bounds
    return m, lo, hi

for f in ["st3215.stl", "yoke_fillet.stl", "holder_fillet.stl", "base_holder.stl"]:
    try:
        m, lo, hi = stats(f)
        print(f"{f:20s} bbox_mm=({hi[0]-lo[0]:.1f},{hi[1]-lo[1]:.1f},{hi[2]-lo[2]:.1f}) "
              f"lo=({lo[0]:.1f},{lo[1]:.1f},{lo[2]:.1f}) hi=({hi[0]:.1f},{hi[1]:.1f},{hi[2]:.1f}) "
              f"wt={m.is_watertight}")
    except Exception as e:
        print(f"{f}: ERR {e}")

# Yoke at q=0: translate (-25.5,0,0) mm in servo frame, no rotation. Joint axis = servo-local Y.
print("\n--- yoke placed at q=0 (servo frame, mm) ---")
servo = trimesh.load("st3215.stl", force="mesh")
yoke = trimesh.load("yoke_fillet.stl", force="mesh")
yoke.apply_translation([-25.5, 0.0, 0.0])
slo, shi = servo.bounds
ylo, yhi = yoke.bounds
print(f"servo   lo={np.round(slo,1)} hi={np.round(shi,1)}")
print(f"yoke@q0 lo={np.round(ylo,1)} hi={np.round(yhi,1)}")
comb = trimesh.util.concatenate([servo, yoke])
clo, chi = comb.bounds
print(f"union   lo={np.round(clo,1)} hi={np.round(chi,1)} bbox=({chi[0]-clo[0]:.1f},{chi[1]-clo[1]:.1f},{chi[2]-clo[2]:.1f})")