"""Grouper: a quick docking-interface check against the docking arm's head (read-only; nothing saved anywhere). The arm's head,
claws and hose in the Tender frame (clamp axis at (0, -0.09), origin 1.94 m under the wrist) come from the Tiger's probe dump,
models/tender-tiger/build/checks/arm_head_tris.npz (20 poses: lowered 0 .. 5.3 m, claws shut and open, the nose either way;
read only, that folder is not touched). Tests the Grouper's static hull (collar, deck fittings), the hatch shut, and the antenna
as drawn and folded (70 deg): intersecting triangle pairs and the least gap, per pose, and which head part is nearest.
The full clamp check in the arm and ship files (86 poses) is the next step's.
    blender.exe -b Tender_Grouper.blend --python build/gr_clamp_probe.py   -> build/checks/clamp_probe.json"""
import bpy, os, json, math
import numpy as np
from mathutils import Vector
from mathutils.bvhtree import BVHTree

HERE = os.path.dirname(os.path.abspath(__file__))
NPZ = os.path.join(os.path.dirname(os.path.dirname(HERE)), "tender-tiger", "build", "checks", "arm_head_tris.npz")
os.makedirs(os.path.join(HERE, "checks"), exist_ok=True)
sc = bpy.context.scene
root = bpy.data.objects["Grouper_Root"]


def rearm():
    for ob in bpy.data.objects:
        if ob.animation_data:
            for fc in ob.animation_data.drivers:
                fc.driver.expression = fc.driver.expression
                fc.driver.is_valid = True


def pose(prop, val):
    root[prop] = val
    root.update_tag()
    for o in bpy.data.objects:
        if o.name.startswith("SM_Grouper"):
            o.update_tag()
    sc.frame_set(sc.frame_current + 1)
    bpy.context.view_layer.update()


def geo(o, near=None):
    dg = bpy.context.evaluated_depsgraph_get()
    ev = o.evaluated_get(dg)
    me = ev.to_mesh()
    me.calc_loop_triangles()
    Mx = ev.matrix_world.copy()
    vs = [Mx @ v.co for v in me.vertices]
    ts = [tuple(t.vertices) for t in me.loop_triangles]
    ev.to_mesh_clear()
    if near is not None:
        ts = [t for t in ts if near((vs[t[0]] + vs[t[1]] + vs[t[2]]) / 3)]
    return vs, ts


near_top = lambda c: c.z > 0.95 and math.hypot(c.x, c.y + 0.09) < 1.6
rearm()
d = np.load(NPZ)
poses = sorted({k.split("_")[0] for k in d.files})
res = {}
cases = [("hull", "SM_Grouper_Hull", None), ("hatch_shut", "SM_Grouper_Hatch", ("hatch_open_deg", 0.0)),
         ("antenna_as_drawn", "SM_Grouper_Antenna", ("antenna_fold_deg", 0.0)), ("antenna_folded_70", "SM_Grouper_Antenna", ("antenna_fold_deg", 70.0))]
for case, nm, pz in cases:
    if pz:
        pose(*pz)
    vs, ts = geo(bpy.data.objects[nm], near_top)
    tree = BVHTree.FromPolygons(vs, ts)
    vlist = sorted({i for t in ts for i in t})
    rows = []
    for p in poses:
        V = d[p + "_v"]; T = d[p + "_t"]; N = d[p + "_n"]
        keep = np.where((V[T].mean(axis=1)[:, 2] < 2.6))[0]
        hv = [Vector(v) for v in V]
        htree = BVHTree.FromPolygons(hv, [tuple(T[i]) for i in keep])
        ov = tree.overlap(htree)
        best = (9.0, None)
        for i in vlist:
            r = htree.find_nearest(vs[i])
            if r[0] is not None and r[3] < best[0]:
                best = (r[3], str(N[keep[r[2]]]))
        rows.append({"pose": str(d[p + "_label"]), "pairs": len(ov), "least_gap_m": round(best[0], 4), "nearest_head_part": best[1]})
    res[case] = {"poses": len(rows), "with_intersections": sum(1 for r in rows if r["pairs"]),
                 "least_gap_m": min(r["least_gap_m"] for r in rows),
                 "worst": min(rows, key=lambda r: r["least_gap_m"]), "rows": rows}
pose("antenna_fold_deg", 0.0)
summary = {k: {kk: v[kk] for kk in ("poses", "with_intersections", "least_gap_m", "worst")} for k, v in res.items()}
json.dump({"source": NPZ, "summary": summary, "cases": res}, open(os.path.join(HERE, "checks", "clamp_probe.json"), "w"), indent=1)
print("CLAMP", json.dumps(summary))