"""Procedural connector between Cameron's two gripper blocks (servo_to_finger -> finger_adj), same rounded
perpendicular-block style as the arm connectors. Reads gripper_from_blender.glb (world mm), writes
gripper_connector.stl (world mm)."""
import numpy as np, trimesh
from shapely.geometry import box as sbox
import gen_two_servo as g
s=trimesh.load("gripper_from_blender.glb")
def aabb(node):
    T,gn=s.graph[node]; w=trimesh.transform_points(s.geometry[gn].vertices,np.array(T,float))*100.0
    return w.min(0), w.max(0)
Alo,Ahi=aabb("servo_to_finger"); Blo,Bhi=aabb("finger_adj")
Bc=(Blo+Bhi)/2
fA=g._box_faces(Alo,Ahi)
bidx=max(range(6), key=lambda k:(Bc-fA[k][0])@fA[k][2])          # servo-block face most facing finger block
fc,corners=fA[bidx][0],fA[bidx][1]
bdir=(Bc-fc)/np.linalg.norm(Bc-fc)
Bpts=np.array([[x,y,z] for x in (Blo[0],Bhi[0]) for y in (Blo[1],Bhi[1]) for z in (Blo[2],Bhi[2])])
depth=float((Bpts@bdir).max()-fc@bdir)+4.0                      # reach the far side of the finger block +4mm
e1,e2=corners[1]-corners[0],corners[3]-corners[0]
h1,h2=np.linalg.norm(e1)/2,np.linalg.norm(e2)/2
e1u,e2u=e1/np.linalg.norm(e1),e2/np.linalg.norm(e2)
r=float(min(g.CONNECTOR_ROUND,h1-1,h2-1))
poly=sbox(-h1+r,-h2+r,h1-r,h2-r).buffer(r,join_style=1,resolution=8)
conn=trimesh.creation.extrude_polygon(poly,1.0)
Mm=np.eye(4); Mm[:3,0],Mm[:3,1],Mm[:3,2],Mm[:3,3]=e1u,e2u,bdir*depth,fc
conn.apply_transform(Mm)
conn.export("gripper_connector.stl")
print(f"connector: wt={conn.is_watertight} face={bidx} depth={depth:.0f}mm crosssec={2*h1:.0f}x{2*h2:.0f}mm")
print(f"  bounds(mm)={np.round(conn.bounds,1).tolist()}")