"""Diagnose cross-midline finger bindings: for every offending ref, compare the lever arm
to the WRONG joint against the lever to its MIRRORED counterpart, so we can tell whether a
straight _r <-> _l joint remap is geometrically correct (small mirrored lever) or would tear
(vert nowhere near the mirrored bone either).

Also reports each offending vert's full influence list, and the nearest correct finger joint.

usage: crosshand_diagnose.py body.glb
"""
import json, struct, sys, math
from pathlib import Path
from collections import Counter, defaultdict

FING = ("thumb", "index", "middle", "ring", "pinky")


def read_glb(p):
    d = Path(p).read_bytes()
    length = struct.unpack_from("<I", d, 8)[0]
    off = 12
    g = b_ = None
    while off < length:
        clen, ct = struct.unpack_from("<II", d, off)
        off += 8
        if ct == 0x4E4F534A:
            g = json.loads(d[off:off + clen])
        else:
            b_ = d[off:off + clen]
        off += clen
    return g, b_


def acc(g, b, i):
    a = g["accessors"][i]
    bv = g["bufferViews"][a["bufferView"]]
    nc = {"SCALAR": 1, "VEC2": 2, "VEC3": 3, "VEC4": 4, "MAT4": 16}[a["type"]]
    fmt = {5121: "B", 5123: "H", 5125: "I", 5126: "f"}[a["componentType"]]
    size = struct.calcsize(fmt) * nc
    stride = bv.get("byteStride") or size
    off = bv.get("byteOffset", 0) + a.get("byteOffset", 0)
    return [struct.unpack_from("<%d%s" % (nc, fmt), b, off + k * stride)
            for k in range(a["count"])], a["componentType"]


def quat_mat(q):
    x, y, z, w = q
    return [[1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)],
            [2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)],
            [2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)]]


def node_local(nd):
    t = nd.get("translation", [0, 0, 0])
    r = nd.get("rotation", [0, 0, 0, 1])
    s = nd.get("scale", [1, 1, 1])
    R = quat_mat(r)
    return [[R[i][j] * s[j] for j in range(3)] + [t[i]] for i in range(3)] + [[0, 0, 0, 1]]


def matmul(A, B):
    return [[sum(A[i][k] * B[k][j] for k in range(4)) for j in range(4)] for i in range(4)]


def global_mats(g):
    loc = [node_local(nd) for nd in g["nodes"]]
    parent = {}
    for i, nd in enumerate(g["nodes"]):
        for c in nd.get("children", []):
            parent[c] = i
    memo = {}

    def gm(i):
        if i in memo:
            return memo[i]
        m = loc[i]
        p = parent.get(i)
        if p is not None:
            m = matmul(gm(p), m)
        memo[i] = m
        return m

    return [gm(i) for i in range(len(g["nodes"]))]


def mirror_name(n):
    if n.endswith("_r"):
        return n[:-2] + "_l"
    if n.endswith("_l"):
        return n[:-2] + "_r"
    return None


path = sys.argv[1]
g, b = read_glb(path)
names = [nd.get("name", "") for nd in g["nodes"]]
GM = global_mats(g)

mesh = g["meshes"][0]
prim = mesh["primitives"][0]
att = prim["attributes"]
skin_idx = next(nd.get("skin") for nd in g["nodes"] if nd.get("mesh") == 0 and "skin" in nd)
joints = g["skins"][skin_idx]["joints"]
jname = [names[j] for j in joints]
jpos = {jname[k]: (GM[j][0][3], GM[j][1][3], GM[j][2][3]) for k, j in enumerate(joints)}

P, _ = acc(g, b, att["POSITION"])
J, _ = acc(g, b, att["JOINTS_0"])
W, wt = acc(g, b, att["WEIGHTS_0"])
wsc = 1.0 if wt == 5126 else (1 / 255 if wt == 5121 else 1 / 65535)

# hand-bone anchors, to describe where verts sit
print(f"== {Path(path).name} ==")
for hb in ("hand_l", "hand_r", "middle_01_l", "middle_01_r", "middle_03_l", "middle_03_r"):
    if hb in jpos:
        p = jpos[hb]
        print(f"   {hb:14s} rest pos = ({p[0]*100:7.1f}, {p[1]*100:7.1f}, {p[2]*100:7.1f}) cm")

bad = []
for vi, (p, jrow, wrow) in enumerate(zip(P, J, W)):
    for j, w in zip(jrow, wrow):
        w *= wsc
        if w <= 0.001:
            continue
        n = jname[j]
        nl = n.lower()
        if not any(t in nl for t in FING):
            continue
        if (nl.endswith("_r") and p[0] > 0.02) or (nl.endswith("_l") and p[0] < -0.02):
            bad.append((vi, n, w, p))

print(f"\n   cross-midline finger refs: {len(bad)}")
vids = sorted({v for v, _, _, _ in bad})
print(f"   distinct verts affected  : {len(vids)}  (index range {min(vids)}..{max(vids)})")

# lever comparison: wrong joint vs mirrored joint vs nearest correct-side finger joint
print(f"\n   {'joint':16s} {'n':>5s} {'lever_wrong':>12s} {'lever_mirror':>13s} {'nearest_correct'}")
groups = defaultdict(list)
for vi, n, w, p in bad:
    groups[n].append((vi, w, p))

for n in sorted(groups):
    rows = groups[n]
    mn = mirror_name(n)
    lw = [math.dist(p, jpos[n]) * 100 for _, _, p in rows]
    lm = [math.dist(p, jpos[mn]) * 100 for _, _, p in rows] if mn in jpos else [float("nan")]
    # nearest correct-side finger joint for a sample vert
    side = "_l" if rows[0][2][0] > 0 else "_r"
    cand = [(math.dist(rows[0][2], jpos[k]) * 100, k) for k in jpos
            if any(t in k.lower() for t in FING) and k.endswith(side)]
    cand.sort()
    print(f"   {n:16s} {len(rows):5d} {sum(lw)/len(lw):9.1f}cm {sum(lm)/len(lm):10.1f}cm "
          f"  {cand[0][1]} @ {cand[0][0]:.1f}cm")

# full influence list for a few offenders
print("\n   sample offending verts (full influence list):")
for vi in vids[:6]:
    p = P[vi]
    infl = []
    for j, w in zip(J[vi], W[vi]):
        w *= wsc
        if w > 0.001:
            infl.append(f"{jname[j]}={w:.3f}")
    print(f"     v{vi} pos=({p[0]*100:6.1f},{p[1]*100:6.1f},{p[2]*100:6.1f})cm  {' '.join(infl)}")

# how many offending verts are FULLY (>0.99) bound to a wrong joint
full = sum(1 for vi, n, w, p in bad if w > 0.99)
print(f"\n   refs at weight > 0.99 (rigid, no blend to soften): {full}")

# what fraction of total left-hand-region verts are affected
hl = jpos.get("hand_l")
if hl:
    near = [vi for vi, p in enumerate(P) if math.dist(p, hl) < 0.20]
    aff = set(vids) & set(near)
    print(f"   verts within 20cm of hand_l: {len(near)}; of those affected: {len(aff)}")