"""Tiger: read-only probe of models/docking-arm/DockArm_Carousel.blend (never saved). The docking arm's head, claws and hose,
posed as clamp_check does it (arm 1 let down 0 .. 5.3 m, claws shut and open, the Tender's nose either way along the tangent),
written in the Tender's frame with the clamp axis at (0, -0.09) (the Kazah's convention: origin 1.94 m under the wrist, 9 cm
behind the axis): checks/arm_head_tris.npz (per pose: vertices, triangles, part names). tg_clamp_place.py measures the
Tiger against it for any clamp position without opening Blender again.

blender -b ../docking-arm/DockArm_Carousel.blend --python build/tg_probe_arm.py
"""
import bpy
import os
import math
import numpy as np
from mathutils import Vector, Matrix

HERE = os.path.dirname(os.path.abspath(__file__))
os.makedirs(os.path.join(HERE, "checks"), exist_ok=True)
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")]
dg = bpy.context.evaluated_depsgraph_get()
out = {}
k = 0
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:
            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))
            Mi = np.array(M.inverted())
            V, T, N = [], [], []
            off = 0
            for ob in parts:
                ev = ob.evaluated_get(dg)
                me = ev.to_mesh()
                me.calc_loop_triangles()
                W = np.array(ev.matrix_world)
                co = np.empty(len(me.vertices) * 3)
                me.vertices.foreach_get("co", co)
                co = co.reshape(-1, 3) @ W[:3, :3].T + W[:3, 3]
                co = co @ Mi[:3, :3].T + Mi[:3, 3]
                tr = np.empty(len(me.loop_triangles) * 3, np.int64)
                me.loop_triangles.foreach_get("vertices", tr)
                ev.to_mesh_clear()
                keep = np.linalg.norm(co[:, :2] - np.array([0, -0.09]), axis=1) < 3.0
                if not keep.any():
                    continue
                V.append(co)
                T.append(tr.reshape(-1, 3) + off)
                N += [ob.name] * (len(tr) // 3)
                off += len(co)
            tag = "p%02d" % k
            out[tag + "_v"] = np.concatenate(V)
            out[tag + "_t"] = np.concatenate(T)
            out[tag + "_n"] = np.array(N)
            out[tag + "_label"] = np.array("lower %.1f m, claws %s, nose %s" % (lower, "open" if claw else "shut",
                                                                                "+x" if nose > 0 else "-x"))
            k += 1
np.savez_compressed(os.path.join(HERE, "checks", "arm_head_tris.npz"), **out)
print("PROBE OK", k, "poses")