"""Beautify Cameron rough gripper blocks:
- connector* nodes: rounded prism along the bridging axis, HIS oriented envelope kept (flush ends).
- camera bracket blocks (Cube / cam_conn / cam_conn.001): AXIS-ALIGNED (straight) rounded prisms from the
  world bbox of his placement (Cameron 2026-07-07: straight, not angled), west face clamped clear of the
  servo (+0.6mm), then MERGED into one cam_conn bracket.
Writes gripper_connector*/gripper_cam_conn.stl + updated gripper.glb."""
import numpy as np, trimesh
from gen_curvy import _rrect, _stitch

ROUND = 6.0
WAIST = 0.18
NSEC = 14
SERVO_CLEAR = 0.6


def _swept_relief(cpts, body_hull):
    """Relief = camera hull (+2mm) SWEPT 120mm along +optical so the cover can slide on (Cameron 2026-07-07:
    clear the whole approach corridor, not just the seated pocket)."""
    ch = trimesh.PointCloud(cpts).convex_hull
    n, a = body_hull.face_normals, body_hull.area_faces
    key = np.round(n, 2)
    uniq, inv = np.unique(key, axis=0, return_inverse=True)
    w = np.bincount(inv, weights=a)
    axis = uniq[np.argmax(w)]; axis = axis / np.linalg.norm(axis)
    t = body_hull.vertices @ axis
    tm = float(np.median(t))
    optical = axis if (t.max() - tm) > (tm - t.min()) else -axis
    base = ch.vertices + ch.vertex_normals * 2.0
    return trimesh.PointCloud(np.vstack([base, base + optical * 120.0])).convex_hull

s = trimesh.load("gripper_from_blender.glb")
CAM_BLOCKS = [n for n in s.graph.nodes_geometry if n in ("Cube", "cam_conn", "cam_conn.001")]
conn_nodes = sorted(n for n in s.graph.nodes_geometry if n.startswith("connector"))
skip_out = set(CAM_BLOCKS)

def wpts(node):
    T, gn = s.graph[node]
    return trimesh.transform_points(s.geometry[gn].vertices, np.array(T, float)) * 100.0

hulls = {n: trimesh.PointCloud(wpts(n)).convex_hull
         for n in s.graph.nodes_geometry if n not in conn_nodes and n not in CAM_BLOCKS}
servo_xmax = float(hulls["servo"].bounds[1][0])

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

def prism_aa(lo, hi, r=ROUND):
    """Straight (world-axis) rounded prism along the longest axis of an AABB."""
    L = hi - lo
    k = int(np.argmax(L)); lat = [i for i in range(3) if i != k]
    e1 = np.eye(3)[lat[0]]; e2v = np.eye(3)[lat[1]]
    h1, h2 = L[lat[0]] / 2, L[lat[1]] / 2
    c = (lo + hi) / 2
    start = c.copy(); start[k] = lo[k]
    rings = []
    for i in range(NSEC):
        t = i / (NSEC - 1)
        q = _rrect(h1, h2, min(r, h1 - 0.5, h2 - 0.5))
        p = start.copy(); p[k] = lo[k] + L[k] * t
        rings.append(p + np.outer(q[:, 0], e1) + np.outer(q[:, 1], e2v))
    return _stitch(rings)

new_meshes = {}
# --- connector* : oriented treatment (unchanged behaviour) ---
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
    c = W.mean(0)
    axes = [W[4] - W[0], W[2] - W[0], W[1] - W[0]]
    L = [float(np.linalg.norm(a)) for a in axes]
    U = [a / l for a, l in zip(axes, L)]
    best, why = None, "longest"
    for k in range(3):
        A, B = containing(c - U[k] * L[k] / 2), containing(c + U[k] * L[k] / 2)
        if A and B and A != B and (best is None or L[k] > L[best]):
            best, why = k, "bridges"
    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
    w = WAIST if L[k] > 1.5 * max(2 * h1, 2 * h2) else 0.0
    rings = []
    for i in range(NSEC):
        t = i / (NSEC - 1)
        sc = 1.0 - w * np.sin(np.pi * t)
        q = _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(q[:, 0], e1) + np.outer(q[:, 1], e2))
    m = _stitch(rings)
    m.export(f"gripper_{node}.stl")
    new_meshes[node] = m
    ax = "xyz"[k]
    print(f"{node}: axis={ax} ({why}) wt={m.is_watertight}")

# --- camera bracket: FULLY PARAMETRIC to the camera_mount + holder positions (2026-07-07) ---
cam_parts = []
if "camera_mount" in hulls:
    mlo, mhi = hulls["camera_mount"].bounds
    hlo, hhi = hulls["holder"].bounds
    z0, z1 = mlo[2] + 4.0, mhi[2] - 4.0
    strut_lo = np.array([max(servo_xmax + SERVO_CLEAR, hlo[0] + 4.0), hhi[1] - 3.6, z0])
    strut_hi = np.array([mhi[0] - 4.0, mlo[1] + 5.0, z1])              # bury 5mm into the mount foot
    cam_parts.append(prism_aa(strut_lo, strut_hi))
    brace_lo = np.array([hhi[0] - 0.75, hlo[1], z0])                   # hug the holder east face, full height
    brace_hi = np.array([min(hhi[0] + 25.0, strut_hi[0]), hhi[1] + 7.7, z1])
    cam_parts.append(prism_aa(brace_lo, brace_hi))
if cam_parts:
    cam = trimesh.boolean.union(cam_parts, engine="manifold") if len(cam_parts) > 1 else cam_parts[0]
    # NOTE: camera relief/corridor is carved ONLY in gen_camera_wedge (post-tilt, correct optical) —
    # carving here used the pre-tilt direction and left dead voids in the bracket (Cameron 2026-07-07).
    cam.export("gripper_cam_conn.stl")
    print(f"cam_conn (parametric bracket): wt={cam.is_watertight} bodies={len(cam.split(only_watertight=False))} vol={cam.volume/1000:.1f}cm3")

out = trimesh.Scene()
for node in s.graph.nodes_geometry:
    if node in skip_out: continue
    T, gn = s.graph[node]
    if node in new_meshes:
        m = new_meshes[node].copy(); m.apply_scale(0.01)
        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))
if cam_parts:
    m = cam.copy(); m.apply_scale(0.01)
    out.add_geometry(m, node_name="cam_conn", geom_name="cam_conn")
out.export("gripper.glb")
print("wrote gripper.glb")