"""Beautify Cameron's hand-posed gripper connector blocks (nodes named connector*): keep each block's exact
pose/span (ends stay flush at his placement planes, buried in the parts they bridge), but replace the plain
box with a rounded-rect cross-section + gentle waist (the arm's beauty language). The extrude axis is chosen
as the axis that BRIDGES two different parts (both end faces buried in different neighbors); fallback = the
longest axis. Reads gripper_from_blender.glb -> writes gripper_connector*.stl + updated gripper.glb."""
import numpy as np, trimesh
from gen_curvy import _rrect, _stitch

ROUND = 6.0    # corner radius (capped by the profile)
WAIST = 0.18   # gentle mid-span pinch, only on slender connectors
NSEC = 14

s = trimesh.load("gripper_from_blender.glb")
conn_nodes = sorted(n for n in s.graph.nodes_geometry if n.startswith("connector"))
# convex hulls of every OTHER part (world mm) for the burial/bridging test
hulls = {}
for node in s.graph.nodes_geometry:
    if node in conn_nodes: continue
    T, gn = s.graph[node]
    w = trimesh.transform_points(s.geometry[gn].vertices, np.array(T, float)) * 100.0
    hulls[node] = trimesh.PointCloud(w).convex_hull

def containing(p):
    return {n for n, h in hulls.items() if h.contains([p])[0]}

new_meshes = {}
for node in conn_nodes:
    T, gn = s.graph[node]; T = np.array(T, float)
    lo, hi = s.geometry[gn].bounds
    C = np.array([[x, y, z] for x in (lo[0], hi[0]) for y in (lo[1], hi[1]) for z in (lo[2], hi[2])])
    W = trimesh.transform_points(C, T) * 100.0                     # world mm corners
    c = W.mean(0)
    axes = [W[4] - W[0], W[2] - W[0], W[1] - W[0]]                 # box edge vectors (x,y,z bit order)
    L = [float(np.linalg.norm(a)) for a in axes]
    U = [a / l for a, l in zip(axes, L)]
    # pick the axis that bridges two DIFFERENT parts
    best, why = None, "longest"
    for k in range(3):
        pa, pb = c - U[k] * L[k] / 2, c + U[k] * L[k] / 2
        A, B = containing(pa), containing(pb)
        if A and B and A != B:
            if best is None or L[k] > L[best]: best, why = k, f"bridges {sorted(A)}<->{sorted(B)}"
    if best is None: best = int(np.argmax(L))
    k = best; lat = [i for i in range(3) if i != k]
    e1, h1 = U[lat[0]], L[lat[0]] / 2
    e2, h2 = U[lat[1]], L[lat[1]] / 2
    start = c - U[k] * L[k] / 2
    slender = L[k] > 1.5 * max(2 * h1, 2 * h2)
    w = WAIST if slender else 0.0
    rings = []
    for i in range(NSEC):
        t = i / (NSEC - 1)
        sc = 1.0 - w * np.sin(np.pi * t)
        p2 = _rrect(h1 * sc, h2 * sc, min(ROUND, h1 * sc - 0.5, h2 * sc - 0.5))
        rings.append(start + U[k] * L[k] * t + np.outer(p2[:, 0], e1) + np.outer(p2[:, 1], e2))
    m = _stitch(rings)
    m.export(f"gripper_{node}.stl")
    new_meshes[node] = m
    print(f"{node}: axis={'xyz'[k]} ({why}) len={L[k]:.0f} sec={2*h1:.0f}x{2*h2:.0f} waist={w} wt={m.is_watertight}")

# rebuild the scene with the beautified connectors (same node names), export GLB
out = trimesh.Scene()
for node in s.graph.nodes_geometry:
    T, gn = s.graph[node]
    if node in new_meshes:
        m = new_meshes[node].copy(); m.apply_scale(0.01)           # mm -> GLB units
        out.add_geometry(m, node_name=node, geom_name=node)
    else:
        out.add_geometry(s.geometry[gn].copy(), node_name=node, geom_name=node, transform=np.array(T, float))
out.export("gripper.glb")
print("wrote gripper.glb with beautified connectors")