#!/usr/bin/env python3
"""
heading_test.py -- identify the 0x1d matrix convention + shark nose axis by
correlating every candidate basis vector against the swim velocity across the
whole capture. The right candidate tracks velocity with a consistently high dot.

    python heading_test.py [capture]
"""
import sys, struct
import numpy as np
sys.path.insert(0, '.')
from vrboard import Assembler
from decode_anim import find_framed_start

path = sys.argv[1] if len(sys.argv) > 1 else 'cap7.raw.bin'
data = open(path, 'rb').read()
asm = Assembler(); asm.feed(data[find_framed_start(data):])
recs = []
for m in asm:
    if m.iserver: continue
    if m.action == 0x1d and len(m.payload) >= 56:
        h = struct.unpack_from('<I', m.payload, 4)[0]
        if h != 1:
            f = struct.unpack_from('<12f', m.payload, 8)
            recs.append((np.array(f[:9]).reshape(3, 3), np.array(f[9:12])))
print(f'{len(recs)} shark 0x1d records')

cands = {}
for name, vec in (('row0', lambda M: M[0]), ('row2', lambda M: M[2]),
                  ('col0', lambda M: M[:, 0]), ('col2', lambda M: M[:, 2])):
    dots = []
    for i in range(10, len(recs) - 10, 25):
        M, t = recs[i]
        _, t2 = recs[i + 5]
        v = t2 - t
        n = np.linalg.norm(v)
        if n < 0.5:            # skip near-stationary spans
            continue
        v = v / n
        b = vec(M); b = b / np.linalg.norm(b)
        dots.append(float(np.dot(v, b)))
    dots = np.array(dots)
    cands[name] = dots
    print(f'{name}: mean={dots.mean():6.3f}  median={np.median(dots):6.3f} '
          f'  frac>0.9={np.mean(dots > 0.9):.2f}  frac<-0.9={np.mean(dots < -0.9):.2f}'
          f'  frac|.|<0.5={np.mean(np.abs(dots) < 0.5):.2f}')