"""Checks of the rigged model, run headless on the saved file:
    blender -b Tender_Aethel.blend --python build/ae_check.py
* rotors: every rotor turned through a full turn in 10 deg steps; its vertices' distance to the nearest duct / hub / stator
  vertex (dense surfaces) must stay over 4 mm;
* hatch: the lid swept from shut to open (0..1 in 0.05) against the fin, the roof block and the hull skin;
* arms: each slider swept over its range, the others at rest, and a grid of shoulder x elbow poses: no arm vertex inside
  the hull (analytic inside test on the hull surface) or within 5 mm of the fittings;
writes build/checks/motion.json.
"""
import os
import sys
import json
import math
import bpy
import numpy as np

ROOT = os.path.dirname(bpy.data.filepath)
HERE = os.path.join(ROOT, 'build')
sys.path.insert(0, HERE)
import ae_lib as L

OUT = os.path.join(HERE, 'checks'); os.makedirs(OUT, exist_ok=True)
dg = bpy.context.evaluated_depsgraph_get()
root = bpy.data.objects['Aethel_Root']


def world_verts(ob):
    ob_eval = ob.evaluated_get(dg)
    me = ob_eval.to_mesh()
    V = np.empty(len(me.vertices) * 3); me.vertices.foreach_get('co', V)
    V = V.reshape(-1, 3)
    M = np.array(ob_eval.matrix_world)
    ob_eval.to_mesh_clear()
    return V @ M[:3, :3].T + M[:3, 3]


def update():
    # a custom property change does not re-run drivers by itself: tag the root and re-set the frame
    root.update_tag()
    bpy.context.scene.frame_set(bpy.context.scene.frame_current)
    bpy.context.view_layer.update()
    global dg
    dg = bpy.context.evaluated_depsgraph_get()
    dg.update()


def densify(V, F_ob, step=0.01):
    """Vertices plus points spread over the faces (so thin gaps between coarse faces are measured)."""
    ob = F_ob
    me = ob.data
    pts = [V]
    for p in me.polygons:
        idx = list(p.vertices)
        q = V[idx]
        c = q.mean(0)
        size = np.linalg.norm(q - c, axis=1).max()
        n = int(min(6, size / step))
        for k in range(1, n + 1):
            f = k / (n + 1)
            pts.append(c + (q - c) * f)
    return np.vstack(pts)


def min_dist(A, B, chunk=4000):
    best = np.inf
    for i in range(0, len(A), chunk):
        a = A[i:i + chunk]
        d = ((a[:, None, :] - B[None, :, :]) ** 2).sum(-1)
        best = min(best, float(np.sqrt(d.min())))
    return best


def inside_hull(P):
    """Points inside the hull surface (analytic: SIDE x, SIDE y and the section's half-width at that height)."""
    v = P[:, 1] / L.S + L.V0
    y = L.Y0 - P[:, 2] / L.S
    X = np.abs(P[:, 0]) / L.S
    yt, yb, hw = L.lines(v)
    t = (y - yt) / (yb - yt)
    tg = np.array(L.PROFILE_TG)
    g = np.interp(t, tg[:, 0], tg[:, 1])
    return (v > L.V_TIP) & (v < L.V_END) & (t > 0.01) & (t < 0.99) & (X < g * hw - 0.5)


report = {}
# proof that the sliders move the parts (a check that moves nothing passes vacuously)
def _pos(n):
    return np.array(bpy.data.objects[n].matrix_world.translation)
update(); p0 = _pos('SM_Aethel_Finger_A_L')
root['arm_L_shoulder'] = 60.0; update(); p1 = _pos('SM_Aethel_Finger_A_L')
root['arm_L_shoulder'] = 0.0; update()
report['proof_arm_moves_m'] = round(float(np.linalg.norm(p1 - p0)), 3)
# ---- rotors: analytic, in each rotor's axis frame (radius and axial position of every vertex, turned through a turn) --
fit = bpy.data.objects['SM_Aethel_Fittings']
update()
FV = world_verts(fit)
S = L.S
# bore radius along the axis (sheet px, from ae_parts' duct profiles), and the axial span of stator and hub
DUCT = {'Lower': dict(prof=[(31.0, 340.0), (31.5, 318.0), (32.5, 314.0), (34.5, 312.2)], stator=(330.5, 333.5),
                      hub=(330.0, 350.0), hub_r=8.5),
        'Upper': dict(prof=[(26.0, 605.5), (26.5, 552.0), (27.5, 549.0), (29.5, 548.0)], stator=(570.5, 573.5),
                      hub=(562.0, 591.6), hub_r=7.0)}
rot_rep = {}
for name in ('SM_Aethel_Rotor_Lower_L', 'SM_Aethel_Rotor_Lower_R', 'SM_Aethel_Rotor_Upper_L', 'SM_Aethel_Rotor_Upper_R'):
    ob = bpy.data.objects[name]
    d = DUCT[name.split('_')[3]]
    fc = ob.animation_data.drivers[0] if ob.animation_data else None
    if fc:
        fc.mute = True
    worst_r, worst_a = np.inf, np.inf
    for k in range(36):
        ob.rotation_euler[1] = math.radians(10 * k)
        update()
        R = world_verts(ob)
        piv = np.array(ob.matrix_world.translation)
        rel = R - piv
        r = np.hypot(rel[:, 0], rel[:, 2]) / S
        a = R[:, 1] / S + L.V0
        pr = sorted(d['prof'], key=lambda p: p[1])
        rb = np.interp(a, [p[1] for p in pr], [p[0] for p in pr]) * math.cos(math.pi / 48)
        worst_r = min(worst_r, float((rb - r).min()) * S)
        # axial: blade points beyond the hub radius must stay forward of the stator; the spinner forward of the hub
        blades = r > d['hub_r'] + 0.5
        worst_a = min(worst_a, float((d['stator'][0] - a[blades]).min()) * S)
        sp = ~blades
        worst_a = min(worst_a, float((d['hub'][0] - a[sp]).min()) * S)
    if fc:
        fc.mute = False
    rot_rep[name] = dict(radial_mm=round(worst_r * 1000, 1), axial_mm=round(worst_a * 1000, 1))
report['rotor_clearance'] = rot_rep

# ---- hatch -------------------------------------------------------------------------------------------------------------
hull = bpy.data.objects['SM_Aethel_Hull']
HV = world_verts(hull)
hatch = bpy.data.objects['SM_Aethel_Hatch']
piv = np.array(hatch.matrix_world.translation)
FVn = FV[np.linalg.norm(FV - piv, axis=1) < 1.6]
HVn = HV[np.linalg.norm(HV - piv, axis=1) < 1.6]
worst_f, worst_h = np.inf, np.inf
for k in range(1, 21):
    root['hatch_open'] = k / 20
    update()
    Hd = world_verts(hatch)
    worst_f = min(worst_f, min_dist(Hd, FVn))
    if k >= 4:
        worst_h = min(worst_h, min_dist(Hd, HVn))
root['hatch_open'] = 0.0
update()
report['hatch'] = dict(min_to_fittings_mm=round(worst_f * 1000, 1), min_to_skin_mm_from_20pct=round(worst_h * 1000, 1))

# ---- arms --------------------------------------------------------------------------------------------------------------
arm_names = ['Arm_Upper', 'Arm_Fore', 'Arm_Wrist', 'Finger_A', 'Finger_B']
def arm_points(side):
    return np.vstack([world_verts(bpy.data.objects[f'SM_Aethel_{n}_{side}']) for n in arm_names])


# the fittings near the arms, minus the arm's own mount collar (which the shoulder sits in by design)
def fittings_near(side):
    sh = np.array(bpy.data.objects[f'SM_Aethel_Arm_Upper_{side}'].matrix_world.translation)
    far = np.linalg.norm(FV - sh, axis=1) > 0.30
    near = np.linalg.norm(FV - sh, axis=1) < 2.5
    return FV[far & near]


def inside_parts(P):
    v = P[:, 1] / L.S + L.V0; y = L.Y0 - P[:, 2] / L.S; X = P[:, 0] / L.S
    c = L.CHIN
    chin = (v > c['v0'] - 2) & (v < c['v1']) & (np.abs(X) < c['w'] + 0.5) & (y > c['y0'] - 0.5) & (y < c['y1'] + 0.5)
    n = L.NACELLE
    nac = (v > n['v_duct0'] - 0.5) & (v < 455) & (np.hypot(np.abs(X) - n['X'], y - n['y']) < n['r'] + 0.5)
    return chin | nac


arm_rep = {}
ranges = {'yaw': (-20, 17.5), 'shoulder': (-40, 70), 'elbow': (-50, 80), 'wrist': (-180, 180), 'claw': (0, 1)}
for side in ('L', 'R'):
    FN = fittings_near(side)
    res = {}
    for key, (lo, hi) in ranges.items():
        bad = []
        for val in np.linspace(lo, hi, 9):
            for k in ranges:
                root[f'arm_{side}_{k}'] = 0.0
            root[f'arm_{side}_{key}'] = float(val)
            update()
            P = arm_points(side)
            # only the moving pieces beyond the shoulder ball (the ball sits in the collar on the skin)
            sh = np.array(bpy.data.objects[f'SM_Aethel_Arm_Upper_{side}'].matrix_world.translation)
            Pm = P[np.linalg.norm(P - sh, axis=1) > 0.20]
            ins = int(inside_hull(Pm).sum()) + int(inside_parts(Pm).sum())
            d = min_dist(Pm, FN)
            if ins or d < 0.005:
                bad.append((round(float(val), 1), ins, round(d * 1000, 1)))
        res[key] = bad
    # grid of shoulder x elbow (yaw 0)
    grid_bad = []
    for s_ in np.linspace(-40, 70, 12):
        for e_ in np.linspace(-50, 80, 14):
            for k in ranges:
                root[f'arm_{side}_{k}'] = 0.0
            root[f'arm_{side}_shoulder'] = float(s_); root[f'arm_{side}_elbow'] = float(e_)
            update()
            P = arm_points(side)
            sh = np.array(bpy.data.objects[f'SM_Aethel_Arm_Upper_{side}'].matrix_world.translation)
            Pm = P[np.linalg.norm(P - sh, axis=1) > 0.20]
            ins = int(inside_hull(Pm).sum()) + int(inside_parts(Pm).sum())
            d = min_dist(Pm, FN)
            if ins or d < 0.005:
                grid_bad.append((round(float(s_)), round(float(e_)), ins, round(d * 1000, 1)))
    res['shoulder_x_elbow_bad'] = grid_bad
    corner_bad = []
    for y_ in np.linspace(-20, 17.5, 6):
        for s_ in np.linspace(-40, 70, 7):
            for e_ in np.linspace(-50, 80, 7):
                for k in ranges:
                    root[f'arm_{side}_{k}'] = 0.0
                root[f'arm_{side}_yaw'] = float(y_); root[f'arm_{side}_shoulder'] = float(s_); root[f'arm_{side}_elbow'] = float(e_)
                update()
                P = arm_points(side)
                sh = np.array(bpy.data.objects[f'SM_Aethel_Arm_Upper_{side}'].matrix_world.translation)
                Pm = P[np.linalg.norm(P - sh, axis=1) > 0.20]
                ins = int(inside_hull(Pm).sum()) + int(inside_parts(Pm).sum())
                d = min_dist(Pm, FN)
                if ins or d < 0.005:
                    corner_bad.append((y_, s_, e_, ins, round(d * 1000, 1)))
    res['yaw_shoulder_elbow_grid_bad'] = corner_bad
    arm_rep[side] = res
for side in ('L', 'R'):
    for k in ranges:
        root[f'arm_{side}_{k}'] = 0.0
update()
report['arms'] = arm_rep
json.dump(report, open(os.path.join(OUT, 'motion.json'), 'w'), indent=1)
print(json.dumps(report)[:3000])