SirRoboGarage/common_libs/tests/test_adaptive_radar.nim

## Offline unit tests for the adaptive melee radar's geometry and phase logic.
##
## No Java, no server, no battle. Run with plain
##   nim c -r common_libs/tests/test_adaptive_radar.nim
##
## The bugs live in the wraparound geometry and the phase hysteresis, so those
## are exercised exhaustively here: minimal covering arc (clustered, straddling
## 0/360, single, ~180, ~350, exact boundaries, duplicates), the turn-rate cap,
## and the acquire/track/fallback state machine.

import std/[math, sets]
import gun_harness/gun_interface
import radars/adaptive_melee_radar

var failures = 0
proc check(name: string, ok: bool) =
  if ok:
    echo "PASS: ", name
  else:
    echo "FAIL: ", name
    inc failures

# The harness contract: the module must satisfy the duck-typed `isRadarModule`.
static:
  doAssert isRadarModule(AdaptiveMeleeRadarModule),
    "AdaptiveMeleeRadarModule must implement computeScan(var, WorldState): float"

const
  SelfX = 1000.0
  SelfY = 1000.0

proc approx(a, b, eps = 1e-9): bool {.inline.} = abs(a - b) <= eps

proc enemyAt(id: int, bearing, dist, lastSeen: float): EnemyInfo =
  let br = degToRad(bearing)
  EnemyInfo(id: id,
            x: SelfX + dist * cos(br),
            y: SelfY + dist * sin(br),
            heading: 0.0, speed: 0.0, energy: 100.0,
            lastSeenTick: lastSeen.int)

proc ws(tick: int, radarHeading: float, enemies: seq[EnemyInfo]): WorldState =
  WorldState(selfX: SelfX, selfY: SelfY,
             selfRadarHeading: radarHeading,
             arenaWidth: 2000.0, arenaHeight: 2000.0,
             tick: tick, enemies: enemies)

proc arcOf(bearings: openArray[float]): tuple[lo, hi, width: float] =
  minimalCoveringArc(bearings)

# ── minimal covering arc ─────────────────────────────────────────────────────

proc testClustered() =
  let a = arcOf([100.0, 110.0, 120.0])
  check "arc: clustered 100/110/120 -> lo=100 hi=120 width=20",
        approx(a.lo, 100.0) and approx(a.hi, 120.0) and approx(a.width, 20.0)
  check "arc: clustered arc contains every input",
        isInsideArc(100.0, a.lo, a.hi, a.width) and
        isInsideArc(110.0, a.lo, a.hi, a.width) and
        isInsideArc(120.0, a.lo, a.hi, a.width)

proc testStraddleZero() =
  # THE classic case: 350 and 10 must be a 20 deg arc through 0, not 340.
  let a = arcOf([350.0, 10.0])
  check "arc: 350/10 -> width 20 (not 340)",
        approx(a.width, 20.0)
  check "arc: 350/10 arc runs lo=350 CCW to hi=10",
        approx(a.lo, 350.0) and approx(a.hi, 10.0)
  check "arc: 350/10 arc contains both ends and 0",
        isInsideArc(350.0, a.lo, a.hi, a.width) and
        isInsideArc(10.0, a.lo, a.hi, a.width) and
        isInsideArc(0.0, a.lo, a.hi, a.width)

proc testStraddleZeroNegative() =
  # Negative inputs must normalize to the same arc.
  let a = arcOf([-10.0, 10.0])
  check "arc: -10/10 normalizes to a 20 deg arc",
        approx(a.width, 20.0) and isInsideArc(0.0, a.lo, a.hi, a.width)

proc testSingleEnemy() =
  let a = arcOf([42.0])
  check "arc: single bearing -> width 0, lo=hi=42",
        approx(a.lo, 42.0) and approx(a.hi, 42.0) and approx(a.width, 0.0)
  let a0 = arcOf([0.0])
  check "arc: single bearing at 0 -> width 0", approx(a0.width, 0.0)

proc testEmpty() =
  let a = arcOf([])
  check "arc: empty -> zero arc", approx(a.width, 0.0)

proc testHalfCircle() =
  # Two exactly opposite bearings: either 180 deg arc covers them; both are
  # valid, the key is the width is exactly 180 and both are inside.
  let a = arcOf([0.0, 180.0])
  check "arc: opposite bearings -> width 180",
        approx(a.width, 180.0)
  check "arc: opposite bearings arc contains both",
        isInsideArc(0.0, a.lo, a.hi, a.width) and
        isInsideArc(180.0, a.lo, a.hi, a.width)
  let b = arcOf([90.0, 270.0])
  check "arc: 90/270 -> width 180",
        approx(b.width, 180.0) and
        isInsideArc(90.0, b.lo, b.hi, b.width) and
        isInsideArc(270.0, b.lo, b.hi, b.width)

proc testNearFull() =
  # Bearings every 10 deg: the largest gap is 10 deg, so width = 350.
  var bearings: seq[float]
  for i in 0 ..< 36: bearings.add(float(i) * 10.0)
  let a = arcOf(bearings)
  check "arc: ~full circle (10 deg gaps) -> width 350",
        approx(a.width, 350.0, 1e-6)
  var allIn = true
  for x in bearings:
    if not isInsideArc(x, a.lo, a.hi, a.width): allIn = false
  check "arc: ~full circle arc contains all 36 bearings", allIn

proc testDuplicates() =
  let a = arcOf([77.0, 77.0, 77.0])
  check "arc: duplicate bearings -> width 0", approx(a.width, 0.0)
  let b = arcOf([200.0, 200.0, 210.0, 210.0])
  check "arc: duplicated clustered ends -> width 10",
        approx(b.width, 10.0) and isInsideArc(200.0, b.lo, b.hi, b.width)

proc testWrapExtremes() =
  # An arc that includes 359 and 1 -> 2 deg through 0.
  let a = arcOf([359.0, 1.0])
  check "arc: 359/1 -> width 2 through 0",
        approx(a.width, 2.0) and isInsideArc(0.0, a.lo, a.hi, a.width)
  # Exact 0/360 alias.
  let b = arcOf([0.0, 360.0])
  check "arc: 0/360 alias -> width 0", approx(b.width, 0.0)

# ── turn-rate cap ────────────────────────────────────────────────────────────

proc testRateCapStatic() =
  var ok = true
  for width in [0.0, 10.0, 45.0, 90.0, 180.0, 269.0]:
    for h in 0 ..< 360:
      for dir in [-1, 1]:
        let (rate, _) = sweepRate(h.float, 10.0, 10.0 + width, width,
                                  MarginDeg, dir)
        if abs(rate) > MaxRadarTurnRate + 1e-9: ok = false
  check "rate: sweepRate never exceeds the 45 deg/tick cap (static sweep)",
        ok

proc testRateCapDynamicSweep() =
  # Drive a real sweep over a 90 deg arc and check the cap + containment.
  var m = initAdaptiveMeleeRadar()
  let lo = 0.0
  let hi = 90.0
  let width = 90.0
  var heading = 45.0
  var dir = 1
  var capOk = true
  var contained = true
  for _ in 0 ..< 400:
    let (rate, nd) = sweepRate(heading, lo, hi, width, MarginDeg, dir)
    if abs(rate) > MaxRadarTurnRate + 1e-9: capOk = false
    heading = normalizeDeg(heading + rate)
    dir = nd
    if not isInsideArc(heading, normalizeDeg(lo - MarginDeg),
                       normalizeDeg(hi + MarginDeg), width + 2.0 * MarginDeg):
      contained = false
  check "rate: dynamic sweep never exceeds the 45 deg/tick cap", capOk
  check "rate: dynamic sweep stays inside the widened arc", contained
  discard m

proc testAcquireRateIsMax() =
  var m = initAdaptiveMeleeRadar()
  let s = ws(1, 0.0, @[])
  check "phase: empty enemies -> full spin 45",
        m.computeScan(s) == MaxRadarTurnRate and m.phase == rpAcquire

# ── phase machine ────────────────────────────────────────────────────────────

proc testAcquireToTrack() =
  var m = initAdaptiveMeleeRadar()
  var t = 0
  var rate = 0.0
  # Fresh every tick; no new id after the first tick.
  for _ in 0 ..< FreshStreakTicks + 2:
    inc t
    rate = m.computeScan(ws(t, 0.0, @[enemyAt(1, 100.0, 300.0, float(t))]))
  check "phase: full spin while acquiring, then tracking after the fresh streak",
        m.phase == rpTrack
  check "phase: acquisition emits the max rate",
        rate <= MaxRadarTurnRate

proc testTrackSingleEnemySmallArc() =
  var m = initAdaptiveMeleeRadar()
  m.setExpectedEnemies(1)
  var t = 0
  var rate = 0.0
  for _ in 0 ..< FreshStreakTicks + 6:
    inc t
    rate = m.computeScan(ws(t, 0.0, @[enemyAt(7, 123.0, 300.0, float(t))]))
  check "single enemy: enters tracking", m.phase == rpTrack
  check "single enemy: swept width is the 2*margin corridor",
        approx(m.lastSweptWidth, 2.0 * MarginDeg)
  check "single enemy: rate is capped", abs(rate) <= MaxRadarTurnRate

proc testStaleFallback() =
  var m = initAdaptiveMeleeRadar()
  m.setExpectedEnemies(1)
  var t = 0
  for _ in 0 ..< FreshStreakTicks + 2:
    inc t
    discard m.computeScan(ws(t, 0.0, @[enemyAt(1, 50.0, 300.0, float(t))]))
  check "fallback: reached tracking before the staleness test", m.phase == rpTrack
  inc t
  # last seen 20 ticks ago > FreshnessTicks (16)
  let rate = m.computeScan(ws(t, 0.0, @[enemyAt(1, 50.0, 300.0, float(t - 20))]))
  check "fallback: a stale enemy drops back to full spin",
        m.phase == rpAcquire and rate == MaxRadarTurnRate

proc testNewIdFallback() =
  var m = initAdaptiveMeleeRadar()
  m.setExpectedEnemies(1)
  var t = 0
  for _ in 0 ..< FreshStreakTicks + 2:
    inc t
    discard m.computeScan(ws(t, 0.0, @[enemyAt(1, 50.0, 300.0, float(t))]))
  check "fallback: tracking a single enemy", m.phase == rpTrack
  inc t
  let rate = m.computeScan(ws(t, 0.0, @[
    enemyAt(1, 50.0, 300.0, float(t)),
    enemyAt(2, 60.0, 300.0, float(t)),
  ]))
  check "fallback: a brand-new enemy id drops back to full spin",
        m.phase == rpAcquire and rate == MaxRadarTurnRate

proc testExpectedCountGate() =
  # expectedEnemies = 3 but only 2 are ever known -> must stay in acquisition
  # (full spin), never enter tracking against a partial crowd.
  var m = initAdaptiveMeleeRadar()
  m.setExpectedEnemies(3)
  var t = 0
  var phase = rpAcquire
  for _ in 0 ..< 40:
    inc t
    discard m.computeScan(ws(t, 0.0, @[
      enemyAt(1, 0.0, 300.0, float(t)),
      enemyAt(2, 10.0, 300.0, float(t)),
    ]))
    phase = m.phase
  check "gate: a missing expected enemy keeps the radar in full-spin acquisition",
        phase == rpAcquire
  # Once the third appears, it acquires and tracks.
  for _ in 0 ..< FreshStreakTicks + 2:
    inc t
    discard m.computeScan(ws(t, 0.0, @[
      enemyAt(1, 0.0, 300.0, float(t)),
      enemyAt(2, 10.0, 300.0, float(t)),
      enemyAt(3, 20.0, 300.0, float(t)),
    ]))
  check "gate: tracking resumes once all expected enemies are known",
        m.phase == rpTrack

proc testWideArcFallback() =
  # Three enemies evenly spread: covering arc is 240 deg, +40 margin = 280,
  # below the 300 exit threshold. Four spread at 0/90/180/270 -> 270+40 = 310,
  # above it, so the radar must stay in acquisition (full spin).
  var m = initAdaptiveMeleeRadar()
  m.setExpectedEnemies(4)
  var t = 0
  for _ in 0 ..< FreshStreakTicks + 10:
    inc t
    discard m.computeScan(ws(t, 0.0, @[
      enemyAt(1, 0.0, 300.0, float(t)),
      enemyAt(2, 90.0, 300.0, float(t)),
      enemyAt(3, 180.0, 300.0, float(t)),
      enemyAt(4, 270.0, 300.0, float(t)),
    ]))
  check "fallback: a ~310 deg swept arc stays in full-spin acquisition",
        m.phase == rpAcquire

proc testWideArcExitAndEnterHysteresis() =
  var m = initAdaptiveMeleeRadar()
  m.setExpectedEnemies(3)
  var t = 0
  # A narrow cluster enters tracking.
  for _ in 0 ..< FreshStreakTicks + 2:
    inc t
    discard m.computeScan(ws(t, 0.0, @[
      enemyAt(1, 0.0, 300.0, float(t)),
      enemyAt(2, 10.0, 300.0, float(t)),
      enemyAt(3, 20.0, 300.0, float(t)),
    ]))
  check "hysteresis: narrow cluster tracks", m.phase == rpTrack
  # Widen to ~310 swept -> exit.
  t += 1
  discard m.computeScan(ws(t, 0.0, @[
    enemyAt(1, 0.0, 300.0, float(t)),
    enemyAt(2, 90.0, 300.0, float(t)),
    enemyAt(3, 180.0, 300.0, float(t)),
    enemyAt(4, 270.0, 300.0, float(t)),
  ]))
  check "hysteresis: wide arc exits tracking", m.phase == rpAcquire

proc testNoKnownEnemyStaysAcquire() =
  var m = initAdaptiveMeleeRadar()
  m.setExpectedEnemies(3)
  var t = 0
  var rate = 0.0
  for _ in 0 ..< 20:
    inc t
    rate = m.computeScan(ws(t, 0.0, @[]))
  check "gate: no known enemies -> always full spin",
        m.phase == rpAcquire and rate == MaxRadarTurnRate

proc testExpectedDisabled() =
  # -1 disables the gate: a single known enemy acquires even if more exist.
  var m = initAdaptiveMeleeRadar()
  var t = 0
  for _ in 0 ..< FreshStreakTicks + 2:
    inc t
    discard m.computeScan(ws(t, 0.0, @[enemyAt(1, 0.0, 300.0, float(t))]))
  check "gate: expectedEnemies = -1 does not block acquisition", m.phase == rpTrack

proc testStaleLiveEnemyTrusted() =
  # With the corpse filter retired the radar trusts the tracker: a tracked
  # enemy with a very old lastSeenTick is a stale LIVE enemy, not a corpse. It
  # is still included in the covering arc (so the sweep covers it) and, being
  # stale, keeps the radar in acquisition until it has been re-scanned.
  var m = initAdaptiveMeleeRadar()
  m.setExpectedEnemies(1)
  var t = 0
  for _ in 0 ..< FreshStreakTicks + 2:
    inc t
    discard m.computeScan(ws(t, 0.0, @[
      enemyAt(1, 100.0, 300.0, float(t)),
      enemyAt(2, 300.0, 300.0, float(t - 100)),  # stale, but alive
    ]))
  # Bearings 100 and 300: the minimal covering arc runs 300 -> 100 (width 160).
  check "trust: a long-unseen tracked enemy is not dropped from the arc",
        approx(m.lastSweptWidth, 160.0 + 2.0 * MarginDeg)
  check "trust: a long-unseen tracked enemy keeps the radar in acquisition",
        m.phase == rpAcquire
  # When the tracker stops reporting it (onBotDeath marked it dead, so it is
  # absent from `state.enemies`) the radar trusts that and tracks the survivor.
  for _ in 0 ..< FreshStreakTicks + 2:
    inc t
    discard m.computeScan(ws(t, 0.0, @[enemyAt(1, 100.0, 300.0, float(t))]))
  check "trust: once the dead enemy leaves the tracker the radar tracks",
        m.phase == rpTrack

# ── driver ───────────────────────────────────────────────────────────────────

testClustered()
testStraddleZero()
testStraddleZeroNegative()
testSingleEnemy()
testEmpty()
testHalfCircle()
testNearFull()
testDuplicates()
testWrapExtremes()
testRateCapStatic()
testRateCapDynamicSweep()
testAcquireRateIsMax()
testAcquireToTrack()
testTrackSingleEnemySmallArc()
testStaleFallback()
testNewIdFallback()
testExpectedCountGate()
testWideArcFallback()
testWideArcExitAndEnterHysteresis()
testNoKnownEnemyStaysAcquire()
testExpectedDisabled()
testStaleLiveEnemyTrusted()

if failures > 0:
  echo "\n", failures, " check(s) FAILED"
  quit(1)
echo "\nAll adaptive-radar checks passed."