#!/usr/bin/env python3
"""Identify which moving object in RAM is the *player* craft, by input correlation.

Position triples are easy to find (findplayer.py); saying which one is us is the
hard part, and no static property answers it. This does: hold hard-left yaw for
a few seconds, then hard-right, and look for the object whose turn axis reverses
in phase with the stick. Every other craft is AI-flown and uncorrelated with our
input, so the discriminator is causal rather than circumstantial.

Phase 1 finds candidate triples from two whole-RAM samples; after that only the
candidates' 12 bytes are re-read, so the sampling loop is cheap.

Usage: findself.py [phase_seconds] [hz]
"""
import math
import os
import struct
import sys
import time

import numpy as np

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

FIFO = "/tmp/sylph-vgamepad.fifo"


def pad(line):
    with open(FIFO, "w") as f:
        f.write(line + "\n")


def snapshot(fd, size):
    return [(a, np.frombuffer(os.pread(fd, (b - a) // 4 * 4, a), dtype=">f4"))
            for a, b in gmem.extents(fd, size) if (b - a) >= 64]


def candidates(fd, size, dt=0.35):
    s0 = snapshot(fd, size)
    t0 = time.time()
    time.sleep(dt)
    s1 = snapshot(fd, size)
    t1 = time.time()
    out = []
    for (o, a), (o2, b) in zip(s0, s1):
        if o != o2 or len(a) != len(b):
            continue
        with np.errstate(invalid="ignore"):
            af = np.nan_to_num(a.astype(np.float64), nan=0, posinf=0, neginf=0)
            bf = np.nan_to_num(b.astype(np.float64), nan=0, posinf=0, neginf=0)
        d = bf - af
        idx = np.flatnonzero(np.abs(d) > 1e-3)
        cand = idx[np.isin(idx + 1, idx) & np.isin(idx + 2, idx)]
        for i in cand:
            v = np.array([d[i], d[i + 1], d[i + 2]])
            sp = float(np.linalg.norm(v)) / (t1 - t0)
            if 80.0 < sp < 2000.0:
                out.append(o + int(i) * 4)
    return out


def sample(fd, offs):
    t = time.time()
    pos = np.empty((len(offs), 3))
    for k, o in enumerate(offs):
        b = os.pread(fd, 12, o)
        if len(b) < 12:
            pos[k] = np.nan
            continue
        pos[k] = struct.unpack(">3f", b)
    return t, pos


def turn_axis(series):
    """Mean normalised cross(v_k, v_k+1) over a phase, per candidate."""
    ts = [t for t, _ in series]
    ps = [p for _, p in series]
    vs = []
    for k in range(len(ps) - 1):
        dt = ts[k + 1] - ts[k]
        vs.append((ps[k + 1] - ps[k]) / max(dt, 1e-3))
    ax = np.zeros((ps[0].shape[0], 3))
    n = 0
    for k in range(len(vs) - 1):
        c = np.cross(vs[k], vs[k + 1])
        nrm = np.linalg.norm(c, axis=1, keepdims=True)
        with np.errstate(invalid="ignore", divide="ignore"):
            ax += np.where(nrm > 1e-6, c / nrm, 0.0)
        n += 1
    return ax / max(n, 1)


def main():
    secs = float(sys.argv[1]) if len(sys.argv) > 1 else 3.0
    hz = float(sys.argv[2]) if len(sys.argv) > 2 else 6.0
    path = gmem.mem_path()
    fd = os.open(path, os.O_RDONLY)
    size = os.path.getsize(path)

    pad("reset")
    time.sleep(0.5)
    offs = candidates(fd, size)
    print(f"# {len(offs)} moving-triple candidates", flush=True)
    if not offs:
        sys.exit("nothing is moving — in flight?")

    phases = {}
    for name, lx in (("left", -0.9), ("right", 0.9)):
        pad(f"axis LX {lx}")
        time.sleep(0.6)                      # let the turn establish
        series = []
        n = int(secs * hz)
        for _ in range(n):
            t0 = time.time()
            series.append(sample(fd, offs))
            time.sleep(max(0, 1.0 / hz - (time.time() - t0)))
        phases[name] = turn_axis(series)
        pad("axis LX 0")
        time.sleep(1.2)
        print(f"# phase {name} done", flush=True)
    pad("reset")

    aL, aR = phases["left"], phases["right"]
    nL = np.linalg.norm(aL, axis=1)
    nR = np.linalg.norm(aR, axis=1)
    with np.errstate(invalid="ignore", divide="ignore"):
        cos = np.sum(aL * aR, axis=1) / (nL * nR)
    good = np.isfinite(cos) & (nL > 0.5) & (nR > 0.5)
    order = np.argsort(np.where(good, cos, 9e9))
    print("\n# objects whose turn axis REVERSES with the stick (most negative first)")
    shown = 0
    for k in order:
        if not good[k]:
            break
        print(f"  va {gmem.primary_va(offs[k]):#010x}  cos(axisL,axisR) = {cos[k]:+.3f}"
              f"   |L|={nL[k]:.2f} |R|={nR[k]:.2f}")
        shown += 1
        if shown >= 12:
            break
    if not shown:
        print("  (none — try longer phases)")


if __name__ == "__main__":
    main()