"""Tiger: the tool arm through its whole travel (0 = aft as the picture draws it .. 90 deg = folded down), 2.5 deg steps, against
the hull: intersecting triangle pairs and the least gap. Its pin (through the arm's eye, by design) is left out: the hull's
faces within 15 mm of the hinge axis. blender -b Tender_Tiger.blend --python build/tg_motion_check.py -> checks/motion.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__))
root = bpy.data.objects["Tiger_Root"]
arm = bpy.data.objects["SM_Tiger_ToolArm"]
hull = bpy.data.objects["SM_Tiger_Hull"]
hinge = arm.matrix_world.translation.copy()
dg = bpy.context.evaluated_depsgraph_get()


def tris(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


hv, ht = tris(hull)
keep = []
for t in ht:
    c = (hv[t[0]] + hv[t[1]] + hv[t[2]]) / 3
    if math.hypot(c.y - hinge.y, c.z - hinge.z) > 0.015 or abs(c.x) > 0.081:
        keep.append(t)
htree = BVHTree.FromPolygons(hv, keep)
res = []
start = root["tool_arm_down_deg"]
for k in range(37):
    deg = 2.5 * k
    root["tool_arm_down_deg"] = deg
    for ob in (root, arm):
        ob.update_tag()
    if arm.animation_data:
        for fc in arm.animation_data.drivers:
            fc.driver.expression = fc.driver.expression
    bpy.context.view_layer.update()
    dg.update()
    av, at = tris(arm)
    atree = BVHTree.FromPolygons(av, at)
    ov = atree.overlap(htree)
    g = min(htree.find_nearest(p)[3] for p in av)
    where = sorted({tuple(round(c, 2) for c in ((hv[keep[j][0]] + hv[keep[j][1]] + hv[keep[j][2]]) / 3)) for i, j in ov})[:4]
    res.append({"deg": deg, "rot_x_deg": round(math.degrees(arm.matrix_world.to_euler().x), 2), "pairs": len(ov),
                "least_gap_m": round(g, 4), "hull_faces_hit": [list(w) for w in where]})
root["tool_arm_down_deg"] = start
summary = {"steps": len(res), "with_intersections": sum(1 for r in res if r["pairs"]), "least_gap_m": min(r["least_gap_m"] for r in res),
           "driver_follows": abs(res[-1]["rot_x_deg"] + 90.0) < 0.5}
json.dump({"summary": summary, "steps": res}, open(os.path.join(HERE, "checks", "motion.json"), "w"), indent=1)
print("MOTION", json.dumps(summary))