"""Strip axial roll (twist about the bone axis) from a hand-pose JSON, keeping the curl.
The pose delta vs the body's rest is swing-twist decomposed per bone; the twist factor is
dropped and the pose rebuilt as rest*swing. Thumb chains are left untouched (their roll is
functional opposition). usage: handpose_detwist.py body.glb pose_in.json pose_out.json"""
import json, math, struct, sys
from pathlib import Path

body, pose_in, pose_out = sys.argv[1:4]

data = Path(body).read_bytes()
jlen = struct.unpack_from("<I", data, 12)[0]
gltf = json.loads(data[20:20 + jlen].decode("utf-8"))
nodes = gltf["nodes"]
byname = {n.get("name"): i for i, n in enumerate(nodes)}


def qmul(a, b):
    ax, ay, az, aw = a
    bx, by, bz, bw = b
    return [aw * bx + ax * bw + ay * bz - az * by,
            aw * by - ax * bz + ay * bw + az * bx,
            aw * bz + ax * by - ay * bx + az * bw,
            aw * bw - ax * bx - ay * by - az * bz]


def qinv(q):
    return [-q[0], -q[1], -q[2], q[3]]


def qnorm(q):
    m = math.sqrt(sum(v * v for v in q))
    return [v / m for v in q]


def bone_axis(i):
    for c in nodes[i].get("children", []):
        t = nodes[c].get("translation")
        if t:
            m = math.sqrt(sum(v * v for v in t))
            if m > 1e-8:
                return [v / m for v in t]
    return None


pose = json.loads(Path(pose_in).read_text())["bones"]
out = {}
report = []
for name, p in pose.items():
    i = byname.get(name)
    if i is None or name.startswith("thumb"):
        out[name] = p
        continue
    a = bone_axis(i)
    if a is None:  # leaf tips: twist is invisible, keep as-is
        out[name] = p
        continue
    r = nodes[i].get("rotation", [0, 0, 0, 1])
    d = qmul(qinv(r), p)  # delta in the bone's rest-local frame
    dot = d[0] * a[0] + d[1] * a[1] + d[2] * a[2]
    twist = qnorm([dot * a[0], dot * a[1], dot * a[2], d[3]])
    swing = qmul(d, qinv(twist))
    out[name] = [round(v, 6) for v in qnorm(qmul(r, swing))]
    deg = 2 * math.degrees(math.atan2(abs(dot), abs(d[3])))
    if deg > 1.0:
        report.append((deg, name))

Path(pose_out).write_text(json.dumps({"bones": out}, indent=1))
report.sort(reverse=True)
print("wrote %s (%d bones, thumbs untouched)" % (pose_out, len(out)))
for deg, name in report[:6]:
    print("  stripped %5.1f deg  %s" % (deg, name))