#!/usr/bin/env python3
"""Derive the player craft's live transform offsets from a flight_probe capture.

Nothing here is guessed from plausible-looking numbers. Two independent
structural facts do the work:

  1. a rotation matrix is 9 floats that are orthonormal with det +1 — a
     property essentially no other data has;
  2. a position triple's frame-to-frame delta must point along the craft's
     own forward axis when it is flying straight, and its magnitude must be the
     same for every frame at constant speed.

Test 2 validates the position candidate *and* the rotation candidate against
each other, so a coincidence has to satisfy both at once.

Usage: flight_analyze.py <probe.bin> [name-substring]
"""
import math
import os
import struct
import sys

sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
import gworld  # noqa: E402


def load(path):
    f = open(path, "rb")
    assert f.read(8) == b"SYLPHPRB"
    n_inst, window, n_in = struct.unpack("<III", f.read(12))
    insts = []
    for _ in range(n_inst):
        va, off = struct.unpack("<IQ", f.read(12))
        nm = f.read(64).split(b"\0")[0].decode()
        insts.append((va, off, nm))
    frames = []
    rec = 8 + 4 * n_in + 32 + n_inst * window
    while True:
        b = f.read(rec)
        if len(b) < rec:
            break
        t = struct.unpack_from("<d", b, 0)[0]
        inp = struct.unpack_from("<%df" % n_in, b, 8)
        label = struct.unpack_from("<32s", b, 8 + 4 * n_in)[0].split(b"\0")[0].decode()
        base = 8 + 4 * n_in + 32
        bufs = [b[base + i * window: base + (i + 1) * window] for i in range(n_inst)]
        frames.append((t, inp, label, bufs))
    return insts, window, frames


def f32(buf, o):
    return struct.unpack_from(">f", buf, o)[0]


def vec(buf, o):
    return (f32(buf, o), f32(buf, o + 4), f32(buf, o + 8))


def sub(a, b):
    return (a[0] - b[0], a[1] - b[1], a[2] - b[2])


def norm(a):
    return math.sqrt(sum(c * c for c in a))


def dot(a, b):
    return sum(x * y for x, y in zip(a, b))


def analyse(insts, window, frames, want):
    idx = [i for i, (_, _, nm) in enumerate(insts) if want in nm]
    if not idx:
        sys.exit(f"no instance matching {want!r}; have: "
                 + ", ".join(sorted({nm for _, _, nm in insts})))
    i = idx[0]
    va, off, nm = insts[i]
    print(f"# player instance {va:#010x} {nm}  ({len(frames)} frames)")

    # ---- rotation blocks that hold up across every frame ------------------
    per_frame = []
    for t, inp, lab, bufs in frames:
        per_frame.append({o for o, kind, m in gworld.find_rotations(bufs[i]) if kind == "3x3"})
    stable = set.intersection(*per_frame) if per_frame else set()
    print(f"# 3x3 orthonormal blocks valid in ALL frames: "
          + (", ".join(f"{o:#05x}" for o in sorted(stable)) or "none"))

    # ---- position candidates ---------------------------------------------
    # constant-speed straight flight: |delta| equal every frame, direction fixed
    idle = [k for k, (t, inp, lab, b) in enumerate(frames) if lab.startswith("idle")]
    if len(idle) < 6:
        idle = list(range(min(20, len(frames))))
    seg = idle[:20]
    cands = []
    for o in range(0, window - 12, 4):
        ds, ok = [], True
        for a, b in zip(seg, seg[1:]):
            if b != a + 1:
                continue
            dt = frames[b][0] - frames[a][0]
            p0, p1 = vec(frames[a][3][i], o), vec(frames[b][3][i], o)
            if not all(map(math.isfinite, p0 + p1)):
                ok = False
                break
            d = sub(p1, p0)
            if dt <= 0:
                ok = False
                break
            ds.append((norm(d) / dt, d))
        if not ok or len(ds) < 4:
            continue
        sp = [s for s, _ in ds]
        mean = sum(sp) / len(sp)
        if mean < 1.0 or mean > 5000.0:      # a craft, not a counter
            continue
        spread = max(sp) - min(sp)
        if spread > 0.15 * mean:             # constant speed while coasting
            continue
        # direction must be steady too
        dirs = [(d[0] / (norm(d) or 1), d[1] / (norm(d) or 1), d[2] / (norm(d) or 1))
                for _, d in ds]
        if min(dot(dirs[0], d) for d in dirs) < 0.99:
            continue
        cands.append((o, mean, dirs[0]))

    print(f"# position candidates (steady speed + steady heading while coasting): "
          f"{len(cands)}")
    for o, sp, d in cands[:12]:
        print(f"   +{o:#05x}  speed {sp:8.2f}/s  dir ({d[0]:+.3f},{d[1]:+.3f},{d[2]:+.3f})")

    # ---- cross-validate: velocity must lie along a rotation row -----------
    print("\n# cross-check: does the motion direction match a rotation-matrix axis?")
    best = []
    for o, sp, d in cands:
        for ro in sorted(stable):
            m = struct.unpack_from(">9f", frames[seg[0]][3][i], ro)
            for r, label in ((m[0:3], "row0/right"), (m[3:6], "row1/up"), (m[6:9], "row2/fwd")):
                c = abs(dot(d, r))
                if c > 0.98:
                    best.append((c, o, ro, label, sp))
    best.sort(reverse=True)
    if not best:
        print("   none — position and orientation candidates do not corroborate")
    for c, o, ro, label, sp in best[:10]:
        print(f"   pos +{o:#05x} ∥ rot +{ro:#05x} {label}  |cos|={c:.5f}  speed {sp:.1f}")
    return stable, cands


def main():
    path = sys.argv[1]
    want = sys.argv[2] if len(sys.argv) > 2 else "Player"
    insts, window, frames = load(path)
    analyse(insts, window, frames, want)


if __name__ == "__main__":
    main()