"""Swept collision check: rotate the yoke through the joint range about the real
output axis (Y through (-25.5,0,0)) and report holder-yoke intersection at each angle.
All-clear => the joint sweeps without the moving yoke hitting the fixed wrap holder.
"""
import sys
sys.path.insert(0, "/data/cameron/repos/smith300_para_stuff")
from build123d import Axis, Vector
from wrap_holder import make_wrap_holder
from yoke_exact import make_yoke_exact

holder = make_wrap_holder(trim=False)   # servo-trim is body-side; doesn't affect yoke clearance
yoke0 = make_yoke_exact()
axis = Axis(Vector(-25.5, 0, 0), Vector(0, 1, 0))

worst = 0.0
worst_c = None
worst_deg = 0
for deg in range(-100, 101, 10):   # +-1.74 rad = +-100 deg (declared joint range)
    y = yoke0.rotate(axis, deg)
    try:
        c = holder & y
        v = 0.0 if c is None else float(c.volume)
    except Exception as e:
        print(f"  {deg:+4d} deg: ERR {type(e).__name__}")
        continue
    if v > worst:
        worst, worst_c, worst_deg = v, c, deg
    if v >= 1.0:
        print(f"  {deg:+4d} deg: {v:.1f} mm^3  *** HIT ***")
print(f"worst-case holder-yoke overlap over sweep = {worst:.2f} mm^3  -> "
      + ("CLEAR" if worst < 1.0 else "INTERFERENCE"))
if worst_c is not None and worst >= 1.0:
    bb = worst_c.bounding_box()
    mn, mx = bb.min, bb.max
    print(f"worst @ {worst_deg:+d} deg overlap bbox: "
          f"X[{mn.X:.1f},{mx.X:.1f}] Y[{mn.Y:.1f},{mx.Y:.1f}] Z[{mn.Z:.1f},{mx.Z:.1f}]")