"""The Grouper under the docking arm's clamp head, the Tiger's check (models/tender-tiger/build/tg_clamp_check.py, from the Mako's
clamp_check.py) with the Grouper's triangles, the gaps measured both ways (arm vertices to the Grouper's faces and the Grouper's
vertices to the arm's faces). Run read-only on a file with the carousel (never saved):
  * 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;
    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 (86 poses);
  * 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.
The docked pose has the antenna folded (70 deg): checks/grouper_tris.npz -> build/checks/clamp_<file>.json. With the
environment GR_VARIANT=raised the antenna stands as drawn (checks/grouper_tris_raised.npz -> clamp_<file>_raised.json).
Reports intersecting triangle pairs and the least gaps between the sub and the head, claws, links and hose."""
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__))
VARIANT = os.environ.get("GR_VARIANT", "")
d = np.load(os.path.join(HERE, "checks", "grouper_tris%s.npz" % ("_raised" if VARIANT == "raised" else "")))
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 Grouper'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 Grouper'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_Grouper (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], "grouper_part": worst[3]}
summary["antenna"] = "raised as drawn (0 deg)" if VARIANT == "raised" else "folded 70 deg (docked)"
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", "") + ("_raised" if VARIANT == "raised" else "") + ".json"), "w"), indent=1)
print("CLAMP", json.dumps(summary))
for r in out:
    if r["intersections"]:
        print("HIT", r["pose"], r["intersections"])