"""The Tiger under the docking arm's clamp head, the Mako's check (models/tender-mako/build/clamp_check.py) with the Tiger's
triangles (tool arm folded, as docked), the gaps measured both ways (arm vertices to the Tiger's faces and the Tiger's vertices
to the arm's faces; the Critique of 2026-10-22). Run read-only on a file with the carousel:
  * models/docking-arm/DockArm_Carousel.blend: arm 1 let down 0, 1.5, 3.0, 4.5, 5.3 m, claws shut and open, the sub hanging
    from the head (origin 1.94 m under the wrist axis, the clamp axis 9 cm ahead of it), nose either way along the tangent;
    and the poses the Critique added: the claws part-way (0.1 .. 0.95) at 0 and 5.3 m; reach_in 2.34 (the ship's launch arm)
    from 1.5 m down to POOL (9.3 m), claws shut and open; reach_in 0 at 7.3, 8.8 and 9.3 m;
  * models/M_Ship_v2_interior_season2.blend: the sub in the place of each preview Tender (_Tender1..3, children of the head
    bones, arm 1 at BOARD with reach_in), against every posed carousel part.
Reports intersecting triangle pairs and the least gaps between the sub and the head, claws, links and hose:
build/checks/clamp_<file>.json"""
import bpy, os, math, json
import numpy as np
from mathutils import Vector, Matrix
from mathutils.bvhtree import BVHTree
HERE = os.path.dirname(os.path.abspath(__file__))
d = np.load(os.path.join(HERE, "checks", "tiger_tris.npz"))
RV, RT, RN = d["v"], d["t"], d["names"]
dg = bpy.context.evaluated_depsgraph_get()


def posed(o):
    ev = o.evaluated_get(dg)
    me = ev.to_mesh()
    me.calc_loop_triangles()
    M = ev.matrix_world
    vs = [M @ v.co for v in me.vertices]
    ts = [tuple(t.vertices) for t in me.loop_triangles]
    ev.to_mesh_clear()
    return vs, ts


def run(parts, M, label):
    M = np.array(M)
    w = RV @ M[:3, :3].T + M[:3, 3]
    tree = BVHTree.FromPolygons([Vector(p) for p in w], [tuple(int(i) for i in t) for t in RT])
    res = {"pose": label, "intersections": {}, "least_gap_m": {}, "nearest_sub_part": {}}
    for o in parts:
        vs, ts = posed(o)
        if not ts:
            continue
        t2 = BVHTree.FromPolygons(vs, ts)
        ov = tree.overlap(t2)
        if ov:
            res["intersections"][o.name] = {"pairs": len(ov), "sub_parts": sorted(set(str(RN[i]) for i, j in ov))}
        best = (9.0, None)
        for p in vs:                                             # arm vertices to the Tiger's faces
            hit = tree.find_nearest(p)
            if hit[0] is not None and hit[3] < best[0]:
                best = (hit[3], str(RN[hit[2]]))
        A = np.array([tuple(v) for v in vs])
        lo, hi = A.min(0) - 0.3, A.max(0) + 0.3
        sel = np.all((w > lo) & (w < hi), axis=1)
        for k in np.nonzero(sel)[0]:                             # the Tiger's vertices to the arm's faces
            hit = t2.find_nearest(Vector(w[k]))
            if hit[0] is not None and hit[3] < best[0]:
                best = (hit[3], "SM_Tiger (vertex %d)" % k)
        res["least_gap_m"][o.name] = round(best[0], 4)
        res["nearest_sub_part"][o.name] = best[1]
    return res


out = []
fn = os.path.basename(bpy.data.filepath)
if fn.startswith("DockArm_Carousel"):
    arm = bpy.data.objects["Arm1"]
    parts = [o for o in bpy.data.objects if o.type == 'MESH' and (o.name.startswith("Arm1_") or o.name == "SM_DockArm_Turret")]
    keep = {k: arm.get(k, 0.0) for k in ("lower_m", "claw_open", "reach_in")}

    def pose(lower, claw, reach, label):
        arm["lower_m"] = lower
        arm["claw_open"] = claw
        arm["reach_in"] = reach
        for ob in bpy.data.objects:
            if ob.name.startswith("Arm1"):
                ob.update_tag()
        bpy.context.view_layer.update()
        dg.update()
        head = bpy.data.objects["Arm1_Head"].matrix_world
        o = head.translation - Vector((0, 0, 1.94))
        for nose in (1, -1):
            M = Matrix.Translation(o) @ Matrix.Rotation(nose * math.pi / 2, 4, 'Z') @ Matrix.Translation((0, 0.09, 0))
            out.append(run(parts, M, label + ", nose %s" % ("+x" if nose > 0 else "-x")))
    for lower in (0.0, 1.5, 3.0, 4.5, 5.3):
        for claw in (0.0, 1.0):
            pose(lower, claw, 0.0, "lower %.1f m, claws %s" % (lower, "open" if claw else "shut"))
    for lower in (0.0, 5.3):
        for claw in (0.1, 0.25, 0.4, 0.55, 0.7, 0.85, 0.95):
            pose(lower, claw, 0.0, "lower %.1f m, claws %.2f open" % (lower, claw))
    for lower in (1.5, 3.0, 4.5, 5.3, 6.3, 7.3, 8.3, 9.3):
        for claw in (0.0, 1.0):
            pose(lower, claw, 2.34, "lower %.1f m, reach_in 2.34, claws %s" % (lower, "open" if claw else "shut"))
    for lower in (7.3, 8.8, 9.3):
        pose(lower, 0.0, 0.0, "lower %.1f m (pool), claws shut" % lower)
    for k, v in keep.items():
        arm[k] = v
else:
    rig = next(o for o in bpy.data.objects if o.name.startswith("DockArm_Rig"))
    parts = [o for o in bpy.data.objects if o.type == 'MESH' and (o.parent == rig or any(m.type == 'ARMATURE' and m.object == rig for m in o.modifiers))]
    for t in sorted([o for o in bpy.data.objects if o.name.startswith("_Tender")], key=lambda o: o.name):
        out.append(run(parts, t.matrix_world, "ship file, in the place of " + t.name))
        out[-1]["origin"] = [round(c, 3) for c in t.matrix_world.translation]
summary = {"file": fn, "poses": len(out), "poses_with_intersections": sum(1 for r in out if r["intersections"]),
           "least_gap_overall_m": min(min(r["least_gap_m"].values()) for r in out), "gaps_measured": "both ways"}
worst = min(((v, r["pose"], k, r["nearest_sub_part"][k]) for r in out for k, v in r["least_gap_m"].items()), key=lambda q: q[0])
summary["least_gap_where"] = {"pose": worst[1], "arm_part": worst[2], "tiger_part": worst[3]}
per_part = {}
for r in out:
    for k, v in r["least_gap_m"].items():
        kk = k.replace("SK_DockArm_", "").replace("Arm2_", "Arm1_").replace("Arm3_", "Arm1_")
        per_part[kk] = min(per_part.get(kk, 9.0), v)
summary["least_gap_per_part_m"] = dict(sorted(per_part.items(), key=lambda kv: kv[1]))
json.dump({"summary": summary, "poses": out}, open(os.path.join(HERE, "checks", "clamp_" + fn.replace(".blend", "") + ".json"), "w"), indent=1)
print("CLAMP", json.dumps(summary))
for r in out:
    if r["intersections"]:
        print("HIT", r["pose"], r["intersections"])