"""The Manta under the docking arm's clamp head. 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 closed and open, the sub hanging
    from the head (origin 1.94 m under the wrist axis), nose either way along the tangent;
  * 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."""
import bpy, os, math, json, sys
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", "manta_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 sub_tree(M):
    M = np.array(M)
    w = RV @ M[:3, :3].T + M[:3, 3]
    return BVHTree.FromPolygons([Vector(p) for p in w], [tuple(int(i) for i in t) for t in RT]), w

def gap(vs, tree):
    best = 9.0
    for p in vs[::2]:
        hit = tree.find_nearest(p)
        if hit[0] is not None:
            best = min(best, hit[3])
    return best

def run(parts, M, label):
    tree, w = sub_tree(M)
    res = {"pose": label, "intersections": {}, "least_gap_m": {}}
    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))}
        res["least_gap_m"][o.name] = round(gap(vs, tree), 4)
    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")]
    for lower in (0.0, 1.5, 3.0, 4.5, 5.3):
        for claw in (0.0, 1.0):
            arm["lower_m"] = lower
            arm["claw_open"] = claw
            for ob in bpy.data.objects:                       # tag each, as the arm's own pose() does
                if ob.name.startswith("Arm1"):
                    ob.update_tag()
            bpy.context.view_layer.update()
            dg.update()
            head = bpy.data.objects["Arm1_Head"].matrix_world
            c1 = bpy.data.objects["Arm1_Claw1"].matrix_world
            print("POSE", lower, claw, "wrist z %.3f" % head.translation.z, "claw1 tilt %.1f" % math.degrees(math.acos(max(-1, min(1, (c1.to_3x3() @ Vector((0, 0, 1))).z)))))
            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))   # collar (0,-0.09) under the head
                out.append(run(parts, M, "lower %.1f m, claws %s, nose %s" % (lower, "open" if claw else "closed", "+x" if nose > 0 else "-x")))
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)}
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"])