"""For the current Blender layout: the holder face facing the yoke, then every yoke-arm face's gap /
facing / perpendicularity to it — to work out the right yoke-face rule."""
import numpy as np
import trimesh
import gen_two_servo as g

g.LAYOUT = g.read_layout("two_servo_from_blender.glb")
yoke = trimesh.load("yoke_exact.stl", force="mesh")
holder = trimesh.load("holder_approx_drilled.stl", force="mesh")
Ty, Th = g._mm(g.LAYOUT[0]), g._mm(g.LAYOUT[1])
arm = yoke.vertices[yoke.vertices[:, 0] < g.YOKE_ARM_X]
yf, hf = g._box_faces(arm.min(0), arm.max(0)), g._box_faces(*holder.bounds)


def nm(n):
    ax = int(np.argmax(np.abs(n)))
    return ("+" if n[ax] > 0 else "-") + "XYZ"[ax]


yoke_ctr = trimesh.transform_points([arm.mean(0)], Ty)[0]
bh = None
for b, (hc, hcorn, hn) in enumerate(hf):
    hcw, hnw = trimesh.transform_points([hc], Th)[0], Th[:3, :3] @ hn
    to_y = yoke_ctr - hcw
    algn = (to_y / np.linalg.norm(to_y)) @ hnw
    if bh is None or algn > bh[0]:
        bh = (algn, b, hnw, hcw)
_, hb, hnw, hcw = bh
hcornw = trimesh.transform_points(hf[hb][1], Th)
print("HOLDER face facing yoke:", nm(hf[hb][2]), "(local)  world-normal", np.round(hnw, 2).tolist())
print("yoke arm faces vs that holder face:")
for a, (yc, ycorn, yn) in enumerate(yf):
    ynw, ycw = Ty[:3, :3] @ yn, trimesh.transform_points([yc], Ty)[0]
    ycornw = trimesh.transform_points(ycorn, Ty)
    gap = min(float(np.linalg.norm(p - q)) for p in ycornw for q in hcornw)
    print(f"  {nm(yn)} (local): gap={gap:5.0f}mm  facesHolder={ (hcw-ycw)@ynw > 0 }"
          f"  |dot·holderN|={abs(float(ynw @ hnw)):.2f}  (0=perp,1=parallel)")