#!/usr/bin/env python3
"""Does `pilot.py`'s stick sign REDUCE its own aim error, or grow it?

With the pad finally reaching the game (`pilot-never-fires.md`), the pilot still
fires 0 times in 150 s and `|aim yaw|` stays pinned at exactly 90° — the `ez < 0`
branch, i.e. the target is astern and stays astern. A controller whose stick sign
is inverted looks exactly like that: it turns away from the error it is nulling,
so the target never comes round the front.

The test is direct and needs no target. Pick a direction 45° off the nose in the
ship's own frame, freeze it in WORLD coordinates, command the deflection
`sticks()` would command for that error, and see whether the error falls or
rises. Both signs are tested, because one of them agreeing by luck would prove
nothing.

`sticks()`'s convention, replicated exactly:

    fwd   = M[fwd_row] * fwd_sign
    right = M[(fwd_row + 1) % 3]
    up    = cross(fwd, right)
    yaw   = atan2(dot(want, right), dot(want, fwd))
    sx    =  KP*yaw   - KD*(body rate about up)
    sy    = -(KP*pitch - KD*(body rate about right))

The KD terms are dropped here: the P term saturates the stick anyway, and a
damping term cannot flip a sign.

Usage: stick_sign.py <nav-live.json> [seconds_per_phase]
"""
import json
import math
import os
import sys
import time

import numpy as np

sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
import frozen     # noqa: E402
import navigator  # noqa: E402
from flight_probe import Pad  # noqa: E402


def main():
    cfg = json.load(open(sys.argv[1]))
    secs = float(sys.argv[2]) if len(sys.argv) > 2 else 3.0
    W = navigator.World(cfg)
    W.scan()
    pad = Pad()

    def frame():
        ents = W.sample(time.time())
        me = next((e for e in ents if "Player" in e[1]), None)
        if me is None:
            return None
        M = W.rot(me[0])
        if M is None:
            return None
        fwd = M[W.fwd_row] * W.fwd_sign
        right = M[(W.fwd_row + 1) % 3]
        return fwd, right, np.cross(fwd, right)

    def err(want, f):
        fwd, right, up = f
        yaw = math.atan2(float(np.dot(want, right)), float(np.dot(want, fwd)))
        pitch = math.atan2(float(np.dot(want, up)), float(np.dot(want, fwd)))
        return math.degrees(yaw), math.degrees(pitch)

    def trial(axis, side):
        f0 = frame()
        if f0 is None:
            print(f"{axis}{side:+d}: no player/orientation")
            return
        fwd, right, up = f0
        base = right if axis == "yaw" else up
        want = fwd * math.cos(math.radians(45)) + base * side * math.sin(math.radians(45))
        want = want / np.linalg.norm(want)
        y0, p0 = err(want, f0)
        # what sticks() would command for this error
        if axis == "yaw":
            pad.axis("LX", math.copysign(1.0, y0))
            cmd = f"LX={math.copysign(1.0, y0):+.0f}"
        else:
            pad.axis("LY", -math.copysign(1.0, p0))
            cmd = f"LY={-math.copysign(1.0, p0):+.0f}"
        time.sleep(secs)
        pad.reset()
        f1 = frame()
        froze = frozen.frozen(3.0)[0]
        if f1 is None:
            print(f"{axis}{side:+d}: lost the player")
            return
        y1, p1 = err(want, f1)
        e0, e1 = (abs(y0), abs(y1)) if axis == "yaw" else (abs(p0), abs(p1))
        verdict = "REDUCES (sign correct)" if e1 < e0 - 1.0 else (
            "GROWS (sign INVERTED)" if e1 > e0 + 1.0 else "no change")
        print(f"{axis} want{side:+d} {cmd}: |err| {e0:6.2f} -> {e1:6.2f} deg   "
              f"{verdict}{'   *** FROZE - DISCARD ***' if froze else ''}")

    if frozen.frozen(3.0)[0]:
        print("# GUEST IS ALREADY FROZEN — nothing to measure")
        return 3
    for axis in ("yaw", "pitch"):
        for side in (+1, -1):
            trial(axis, side)
            time.sleep(1.0)
    pad.reset()
    return 0


if __name__ == "__main__":
    raise SystemExit(main())