SirRoboGarage/common_libs/guns/guess_factor.nim

## Guess-factor gun: statistical targeting via GF histogram.
## Bins: 31, ranging GF -1 (max CW escape) to +1 (max CCW escape).
## Learns from virtual bullet outcomes; queues one wave per (tick, power bin).

import std/[math, strformat]
import gun_harness/gun_interface
import gun_harness/virtual_bullets as vb  # PowerBins: the four power bins the harness spawns

const
  GFBins   = 31
  GFPrior  = 0.1
  DebugGF* = false
  WaveCompactAt = 64  ## compact a bin's wave seq once this many entries are consumed

type
  Wave = object
    fireX, fireY: float
    fireBearing:  float  # atan2(enemyY-selfY, enemyX-selfX) at fire tick (rad)
    # mea not stored — recomputed from FeedbackEvent.bulletPower at resolution time

  GFGun* = object
    bins:             array[GFBins, float]
    # One wave queue per power bin. The owning bin is fixed at push time (from the
    # bulletSpeed argument) and at pop time (from FeedbackEvent.bulletPower), so a
    # resolved bullet is always paired with a wave from its own bin.
    waves:            array[len(vb.PowerBins), seq[Wave]]
    waveHead:         array[len(vb.PowerBins), int]   # O(1) pop cursor into waves[bin]
    waveStoredTick:   array[len(vb.PowerBins), int]   # last tick a wave was queued for this bin
    wavePushes*:      int  # total waves enqueued (== one per (tick, bin))
    waveStarved*:     int  # onResult found an empty queue for its own bin
    debugGraphics*:   bool

proc initGFGun*(): GFGun =
  result.debugGraphics = false
  for b in 0..<len(vb.PowerBins):
    result.waveStoredTick[b] = -1
  # Seed with a head-on prior: triangular bump at bin 15 (GF=0).
  # Prevents the cold-start tie-break to GF=-1 (bin 0) that poisons early fitness.
  let center = (GFBins - 1) div 2  # = 15
  for i in 0..<GFBins:
    let d = abs(i - center)
    result.bins[i] = GFPrior + 0.5 / float(1 + d)

proc gfToIndex(gf: float): int {.inline.} =
  clamp(int(round((gf + 1.0) * 0.5 * float(GFBins - 1))), 0, GFBins - 1)

proc indexToGF(idx: int): float {.inline.} =
  float(idx) / float(GFBins - 1) * 2.0 - 1.0

proc peakBin(g: GFGun): int =
  var best = 0
  for i in 1..<GFBins:
    if g.bins[i] > g.bins[best]:
      best = i
  best

proc binForSpeed(spd: float): int {.inline.} =
  ## Map a virtual-bullet speed back to its power-bin index. All four bin speeds
  ## are exactly representable floats; the epsilon is belt-and-braces only.
  for i in 0..<len(vb.PowerBins):
    if abs(spd - bulletSpeed(vb.PowerBins[i])) < 1e-6:
      return i
  -1

proc binForPower(power: float): int {.inline.} =
  ## Map a FeedbackEvent.bulletPower back to its power-bin index.
  for i in 0..<len(vb.PowerBins):
    if abs(power - vb.PowerBins[i]) < 1e-6:
      return i
  -1

proc takeOldestWave(g: var GFGun, binIdx: int): (bool, Wave) =
  ## Pop the oldest unresolved wave for this bin (O(1) amortized via waveHead).
  ## Returns (false, default) when the bin's queue is empty.
  if binIdx < 0 or g.waveHead[binIdx] >= g.waves[binIdx].len:
    return (false, Wave())
  result = (true, g.waves[binIdx][g.waveHead[binIdx]])
  inc g.waveHead[binIdx]
  # Amortized O(1): drop the consumed prefix once it dominates the queue.
  if g.waveHead[binIdx] >= WaveCompactAt and
     g.waveHead[binIdx] * 2 >= g.waves[binIdx].len:
    g.waves[binIdx] = g.waves[binIdx][g.waveHead[binIdx] .. g.waves[binIdx].high]
    g.waveHead[binIdx] = 0

proc predict*(g: var GFGun, state: WorldState, bulletSpeed: float): GunPrediction =
  if bulletSpeed <= 0.0:
    return GunPrediction(x: state.enemyX, y: state.enemyY)

  let dx = state.enemyX - state.selfX
  let dy = state.enemyY - state.selfY
  let dist   = sqrt(dx*dx + dy*dy)
  let bearing = arctan2(dy, dx)
  let mea    = arcsin(clamp(8.0 / bulletSpeed, -1.0, 1.0))

  # Queue at most one wave per (tick, power bin). The fire site's extra predict()
  # call for the selected bin lands on the same tick and reuses the queued wave.
  let binIdx = binForSpeed(bulletSpeed)
  if binIdx >= 0 and g.waveStoredTick[binIdx] != state.tick:
    g.waves[binIdx].add Wave(
      fireX:       state.selfX,
      fireY:       state.selfY,
      fireBearing: bearing,
    )
    g.waveStoredTick[binIdx] = state.tick
    inc g.wavePushes

  let peak = g.peakBin()
  let peakGF = indexToGF(peak)
  let gfAngle = bearing + peakGF * mea
  # Aim from self at gfAngle, at current dist (angular targeting)
  let px = state.selfX + cos(gfAngle) * dist
  let py = state.selfY + sin(gfAngle) * dist

  when DebugGF:
    echo fmt"[gf-dbg] predict: peakGF={peakGF:.2f} peakBin={peak} mea={radToDeg(mea):.1f}° aimAngle={radToDeg(gfAngle):.1f}° waves={g.waves[binIdx].len}"

  GunPrediction(
    x: clamp(px, BotRadius, state.arenaWidth  - BotRadius),
    y: clamp(py, BotRadius, state.arenaHeight - BotRadius),
  )

proc onResult*(g: var GFGun, e: FeedbackEvent) =
  ## Called when a virtual bullet resolves. Pop the OLDEST unresolved wave from
  ## this bullet's own power-bin queue, compute the actual GF, and smooth-add it.
  let binIdx = binForPower(e.bulletPower)
  if binIdx < 0: return

  let (found, w) = g.takeOldestWave(binIdx)
  if not found:
    inc g.waveStarved
    return

  # Recompute mea from the actual bullet power (correct per-bin, not the cached first-bin mea)
  let speed = bulletSpeed(e.bulletPower)
  let mea   = arcsin(clamp(8.0 / speed, -1.0, 1.0))

  # Compute actual bearing from fire position to where the enemy actually was
  let actualDx = e.actualX - w.fireX
  let actualDy = e.actualY - w.fireY
  let actualBearing = arctan2(actualDy, actualDx)
  var bearingDelta = actualBearing - w.fireBearing
  # Normalize to [-PI, PI]
  while bearingDelta >  PI: bearingDelta -= 2.0*PI
  while bearingDelta < -PI: bearingDelta += 2.0*PI

  let gf = if mea > 1e-10: clamp(bearingDelta / mea, -1.0, 1.0) else: 0.0
  let centerIdx = gfToIndex(gf)

  when DebugGF:
    echo fmt"[gf-dbg] onResult: fireBearing={radToDeg(w.fireBearing):.1f}° actualBearing={radToDeg(actualBearing):.1f}° delta={radToDeg(bearingDelta):.1f}° MEA={radToDeg(mea):.1f}° GF={gf:.2f} peakBin={centerIdx}"

  # Triangular smoothing kernel over adjacent bins
  for i in 0..<GFBins:
    let dist = abs(i - centerIdx)
    g.bins[i] += 1.0 / float(1 + dist)