#!/usr/bin/env python3
"""A pilot that tries to stay alive, not just to shoot.

Every earlier loop flew a straight pursuit and was shot down; the trace from
ctrl_probe.py shows why — 1500 hull points gone in twelve seconds while sitting
in a turret's line of fire, with no reaction of any kind. Three measured facts
make a reaction possible:

* **Hull is `position + 0x154`** — at spawn it equals the unit definition's own
  `HP` (1500 for the Delta Saber), it steps down 30/60/90 per hit, and it goes
  negative at death. So damage is observable *as it happens*, not inferred.
* **`RT` accelerates and `LT` brakes**, and the setting persists: measured
  ground speed went 488 → 1510 under RT, 488 → 174 under LT and then *stayed*
  near 130 with the sticks neutral. (The older note "RT is not the throttle" was
  drawn from a value-scan for a speed field, not from measuring the speed.)
* **`RB` fires** (autopilot-memory-driven.md).

So the loop is a state machine on damage rather than a pure pursuit:

    ENGAGE  chase and shoot the nearest hostile fighter
    EVADE   entered the moment the hull drops — turn away from the threats,
            full throttle, jink; leave only after several quiet seconds
    RETIRE  hull below a floor: break for the friendly capital ship, which the
            mission's own hint says is where you resupply

Turrets are treated as threats to be *kept at a distance*, not as targets: the
objective is the invading fighters, and the turret is what killed every previous
run.

Usage: pilot.py <config.json> [seconds] [--dry]
"""
import json
import math
import os
import struct
import sys
import time
from collections import deque

import numpy as np

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

HULL_OFF = 0x154          # confirmed: == definition HP at spawn, falls when hit
SHIELD_OFF = 0x430        # candidate: == definition Shield MaxValue at spawn


class Pilot:
    KP, KD = 2.2, 0.45
    FIRE_CONE = math.radians(9)
    FIRE_RANGE = 5000.0
    TURRET_KEEPOUT = 2500.0   # ...and stay this far from things that shoot back
    EVADE_QUIET = 5.0         # seconds without damage before re-engaging
    RETIRE_FRAC = 0.30        # hull fraction that sends us home
    HZ = 8.0

    def __init__(self, W, pad, dry=False, log=sys.stdout):
        self.W = W
        self.pad = pad
        self.dry = dry
        self.log = log
        # closest-point-of-approach avoidance is navigator.py's, reused as-is
        self.av = navigator.Navigator(W, pad, dry=True, log=log)
        self.prevM = None
        self.firing = False
        self.throttle = 0      # -1 brake, 0 coast, +1 accelerate
        self.mode = "ENGAGE"
        self.hp_hist = deque(maxlen=64)
        self.hp0 = None
        self.last_hit = -1e9
        self.threat_dir = None

    # ------------------------------------------------------------ own state
    def own(self, off):
        b = os.pread(self.W.fd, 8, off + HULL_OFF)
        hull = struct.unpack_from(">f", b, 0)[0] if len(b) >= 4 else float("nan")
        b2 = os.pread(self.W.fd, 4, off + SHIELD_OFF)
        shield = struct.unpack(">f", b2)[0] if len(b2) == 4 else float("nan")
        return hull, shield

    def set_throttle(self, want):
        """RT / LT are a persistent setting, so only send the change."""
        if want == self.throttle or self.dry:
            return
        self.pad.trig("RT", 1.0 if want > 0 else 0.0)
        self.pad.trig("LT", 1.0 if want < 0 else 0.0)
        self.throttle = want

    # -------------------------------------------------------------- targets
    def hostiles(self, ents, me_off):
        out = []
        for off, nm, p, v, r in ents:
            if off == me_off or navigator.faction(nm) != "ADAN":
                continue
            out.append((off, nm, p, v, r, "Turret" in nm or r >= navigator.Navigator.BIG_RADIUS))
        return out

    def pick(self, me_p, me_v, fwd, hos):
        """Nearest *fighter*, weighted by how far off the nose it is."""
        speed = max(float(np.linalg.norm(me_v)), 1.0)
        best, bestscore = None, 1e18
        for off, nm, p, v, r, hard in hos:
            if hard:
                continue          # turrets and hulls are not the objective
            rel = p - me_p
            d = float(np.linalg.norm(rel))
            if d < 1e-3:
                continue
            lead = p + v * (d / max(speed, 300.0))
            theta = ang(lead - me_p, fwd)
            score = d * (1.0 + 3.0 * (theta / math.pi) ** 2)
            if score < bestscore:
                best, bestscore = (off, nm, lead, lead - me_p, d), score
        return best

    def threat_vector(self, me_p, hos):
        """Where the danger is: inverse-square weighted direction to shooters."""
        acc = np.zeros(3)
        for off, nm, p, v, r, hard in hos:
            rel = p - me_p
            d = float(np.linalg.norm(rel))
            if d < 1.0 or d > 8000.0:
                continue
            w = (1500.0 / d) ** 2 * (3.0 if hard else 1.0)
            acc += norm(rel) * w
        return norm(acc) if np.linalg.norm(acc) > 1e-6 else None

    def friendly_base(self, ents, me_off):
        """The biggest friendly — the carrier the briefing says to resupply at."""
        best = None
        for off, nm, p, v, r in ents:
            if off == me_off or navigator.faction(nm) != "TCAF":
                continue
            if best is None or r > best[4]:
                best = (off, nm, p, v, r)
        return best

    # ------------------------------------------------------------ steering
    def sticks(self, want, M, w):
        fwd = M[self.W.fwd_row] * self.W.fwd_sign
        right = M[(self.W.fwd_row + 1) % 3]
        up = np.cross(fwd, right)
        ex, ey, ez = (float(np.dot(want, right)), float(np.dot(want, up)),
                      float(np.dot(want, fwd)))
        yaw = math.atan2(ex, ez if abs(ez) > 1e-3 else 1e-3)
        pitch = math.atan2(ey, ez if abs(ez) > 1e-3 else 1e-3)
        if ez < 0:                        # target behind: commit to a full turn
            yaw = math.copysign(math.pi / 2, ex if ex else 1.0)
        sx = max(-1.0, min(1.0, self.KP * yaw - self.KD * float(np.dot(w, up))))
        sy = max(-1.0, min(1.0, -(self.KP * pitch - self.KD * float(np.dot(w, right)))))
        return sx, sy, yaw, pitch

    # ---------------------------------------------------------------- step
    def step(self, t, dt):
        ents = self.W.sample(t)
        me = next((e for e in ents if "Player" in e[1]), None)
        if me is None:
            return None
        me_off, me_nm, me_p, me_v, me_r = me
        M = self.W.rot(me_off)
        if M is None:
            return "no-orientation"
        fwd = M[self.W.fwd_row] * self.W.fwd_sign
        right = M[(self.W.fwd_row + 1) % 3]
        up = np.cross(fwd, right)
        speed = float(np.linalg.norm(me_v))

        hull, shield = self.own(me_off)
        if self.hp0 is None and math.isfinite(hull) and hull > 0:
            self.hp0 = hull
        self.hp_hist.append((t, hull))
        # damage over the last ~2 s; the hull only ever falls, so any drop is a hit
        recent = [h for (ts, h) in self.hp_hist if t - ts <= 2.0]
        dmg = (max(recent) - hull) if recent else 0.0
        if dmg > 0.5:
            self.last_hit = t
        if hull <= 0:
            return "DEAD"

        # body angular velocity, for the damping term
        w = np.zeros(3)
        if self.prevM is not None and dt > 1e-3:
            D = self.prevM @ M.T
            w = np.array([D[2, 1] - D[1, 2], D[0, 2] - D[2, 0], D[1, 0] - D[0, 1]]) / (2 * dt)
        self.prevM = M

        hos = self.hostiles(ents, me_off)
        self.threat_dir = self.threat_vector(me_p, hos)
        frac = hull / self.hp0 if self.hp0 else 1.0

        # ---- mode
        if frac <= self.RETIRE_FRAC:
            self.mode = "RETIRE"
        elif t - self.last_hit < self.EVADE_QUIET:
            self.mode = "EVADE"
        else:
            self.mode = "ENGAGE"

        tgt = self.pick(me_p, me_v, fwd, hos)
        push, worst = self.av.avoidance(me_p, me_v, me_r, ents, me_off)

        if self.mode == "EVADE":
            # Away from the guns, plus a jink so a straight escape line is not
            # itself an easy solution for whatever is shooting.
            away = -self.threat_dir if self.threat_dir is not None else fwd
            jink = right * math.sin(t * 1.7) * 0.5 + up * math.cos(t * 2.3) * 0.35
            want = norm(away + jink)
            self.set_throttle(+1)
            fire = False
        elif self.mode == "RETIRE":
            base = self.friendly_base(ents, me_off)
            if base is not None:
                rel = base[2] - me_p
                d = float(np.linalg.norm(rel))
                want = norm(rel) if d > base[4] + 400.0 else norm(np.cross(rel, up))
            else:
                want = -self.threat_dir if self.threat_dir is not None else fwd
            self.set_throttle(+1)
            fire = False
        else:
            # Straight lead pursuit and fly *through*. An earlier version orbited
            # once inside a standoff radius and braked while doing it: it then
            # circled one attacker for 40 s at ~700 m, at 60-100 units/s, never
            # inside the firing cone. Overshooting and re-acquiring is better
            # than a stall in the middle of a battle; collision avoidance already
            # keeps a fighter-sized margin.
            want = norm(tgt[3]) if tgt else fwd
            if tgt and tgt[4] > 2500.0:
                self.set_throttle(+1)
            elif tgt and tgt[4] < 500.0 and speed > 900.0:
                self.set_throttle(-1)
            else:
                self.set_throttle(0)
            fire = True

        # a turret inside its keep-out radius outranks the target
        for off, nm, p, v, r, hard in hos:
            if not hard:
                continue
            d = float(np.linalg.norm(p - me_p))
            if d < self.TURRET_KEEPOUT:
                want = norm(want + norm(me_p - p) * (2.0 * (1.0 - d / self.TURRET_KEEPOUT)))
                break

        pn = float(np.linalg.norm(push))
        if pn > 1e-6:
            want = norm(want + push * (3.0 if pn > 0.6 else 1.5))

        sx, sy, yaw, pitch = self.sticks(want, M, w)
        # The firing gate has to be measured against the TARGET, not against the
        # commanded direction: `want` carries the avoidance and keep-out terms,
        # so gating on it means the guns stay cold exactly when the loop is
        # manoeuvring — which is most of a dogfight.
        aim = self.sticks(norm(tgt[3]), M, w)[2:] if tgt else (math.pi, math.pi)
        aim_ok = abs(aim[0]) < self.FIRE_CONE and abs(aim[1]) < self.FIRE_CONE
        fire = bool(fire and tgt and aim_ok and tgt[4] < self.FIRE_RANGE and pn < 1.2)

        if not self.dry:
            self.pad.axis("LX", sx)
            self.pad.axis("LY", sy)
            if fire != self.firing:
                (self.pad.press if fire else self.pad.release)("RB")
                self.firing = fire

        msg = (f"{self.mode:<7} hull={hull:6.0f} shd={shield:6.0f} spd={speed:6.0f} "
               f"thr={self.throttle:+d} yaw={math.degrees(yaw):+6.1f} "
               f"pit={math.degrees(pitch):+6.1f} aim={math.degrees(aim[0]):+6.1f}"
               f"/{math.degrees(aim[1]):+6.1f} fire={int(fire)}")
        if dmg > 0.5:
            msg += f" HIT -{dmg:.0f}"
        if tgt:
            msg += f" tgt={tgt[1][3:24]:<21} d={tgt[4]:6.0f}"
        if worst:
            msg += f" | AVOID {worst[1][3:18]} miss={worst[3]:5.0f} t={worst[4]:4.1f}"
        return msg

    def run(self, secs):
        self.W.scan()
        t0 = time.time()
        last, last_scan = t0, 0.0
        hostiles0 = None
        while time.time() - t0 < secs:
            t = time.time()
            if t - last_scan > 5.0:
                ents = self.W.scan()
                last_scan = t
                n_ad = sum(1 for _, va in ents
                           if navigator.faction(self.W.defs[va]) == "ADAN")
                if hostiles0 is None:
                    hostiles0 = n_ad
                print(f"[{t-t0:6.1f}] scan: {len(ents)} entities, {n_ad} ADAN "
                      f"(start {hostiles0})", file=self.log, flush=True)
            msg = self.step(t, t - last)
            last = t
            if msg is None:
                print(f"[{t-t0:6.1f}] player object gone — stopping",
                      file=self.log, flush=True)
                break
            print(f"[{t-t0:6.1f}] {msg}", file=self.log, flush=True)
            if msg == "DEAD":
                break
            time.sleep(max(0.0, 1.0 / self.HZ - (time.time() - t)))
        if not self.dry:
            self.pad.reset()
        print(f"# flew {time.time()-t0:.0f}s", file=self.log, flush=True)


def main():
    cfg = json.load(open(sys.argv[1]))
    secs = float(sys.argv[2]) if len(sys.argv) > 2 else 180.0
    W = navigator.World(cfg)
    Pilot(W, Pad(), dry="--dry" in sys.argv).run(secs)


if __name__ == "__main__":
    main()