SirRoboGarage/common_libs/tests/measure_strafe_range_stall.nim

## STRAFE range-control + corner-stall gates (job j111). OFFLINE / cheap: no
## Java, no server, no battle. Run with:
##
##   nim c -r --nimcache:/tmp/nc_j111 --path:common_libs \
##       common_libs/tests/measure_strafe_range_stall.nim
##
## Reuses the fixture loader + stats shape of j108's
## `common_libs/tests/measure_strafe_gates.nim`. It only calls the PUBLIC
## `initStrafe()` / `resetRound()` / `computeMove()` / `loadStrafeEnv()` API, so
## it compiles BOTH against the pre-fix module (where the new env names are
## simply ignored) and the post-fix module. That is what makes the before/after
## comparison honest: the SAME gate binary source, run on two tree states.
##
##   GATE A  TILE AVAILABILITY on the corrected (j110) heat field WITH the tilt
##           active: mean candidates / safe candidates on the tilted line and
##           the fraction of picks where the safe pool is empty.
##
##   GATE B  STALL DETECTOR. The recorded fixtures are replayed as a KINEMATIC
##           simulation (the enemy keeps its recorded path; OUR bot is
##           integrated from the mover's own speed/turn commands, with wall and
##           body-radius clamping). Per tick we record whether the mover was
##           COMMANDED to move yet produced no displacement (stuck) and whether
##           its tile did not change. We report the fraction of commanded ticks
##           that are stuck and the LONGEST stuck run. The corner stall is a
##           longest-run diverge-to-infinity defect, so the longest run is the
##           number that matters.
##
##   GATE C  RANGE. Same kinematic replay, range control OFF vs ON, same seed,
##           same enemy path. We report the achieved distance distribution
##           (mean/median/p10/p90 and the fraction inside the dead band). The
##           claim "range control moves the distance toward TR_STRAFE_RANGE" is
##           only MEASURED if the ON distribution is closer to the target than
##           the OFF one.
##
##   CORNER  A targeted test: the bot is placed in each of the four corners with
##           the enemy on the inward diagonal, so the strafe line points out of
##           the arena. That is the zero-candidate case that used to keep driving
##           into the wall. We report the longest stuck run and the escape
##           distance from the corner.

import std/[os, json, math, random, strutils, algorithm, sets]
import gun_harness/gun_interface
import movements/strafe

const repoRoot = currentSourcePath().parentDir.parentDir.parentDir
const ArenaW   = 800.0
const ArenaH   = 600.0
const GridSize = 36.0
const Cols     = int(ArenaW / GridSize)   # 22
const Rows     = int(ArenaH / GridSize)   # 16
const MarginX  = (ArenaW - Cols.float * GridSize) / 2.0
const MarginY  = (ArenaH - Rows.float * GridSize) / 2.0
const Seed     = 20250923

# ── fixture loading ───────────────────────────────────────────────────────────

type Sample = object
  enemyX, enemyY, enemyEnergy: float
  selfX, selfY, selfHeading, selfEnergy: float

proc loadSamples(fixture: string): seq[Sample] =
  let path = repoRoot / "tools" / "fixtures" / fixture
  for rawLine in lines(path):
    let line = rawLine.strip()
    if line.len == 0: continue
    let n = parseJson(line)
    if n.hasKey("meta") or n.hasKey("end"): continue
    result.add Sample(
      enemyX: n["ex"].getFloat(), enemyY: n["ey"].getFloat(),
      enemyEnergy: n["ee"].getFloat(),
      selfX: n["sx"].getFloat(), selfY: n["sy"].getFloat(),
      selfHeading: n["sh"].getFloat(), selfEnergy: n["se"].getFloat())

proc loadRoundStarts(fixture: string): seq[int] =
  let side = repoRoot / "tools" / "fixtures" / "drussgt_meta" /
             (fixture & ".rounds.json")
  if not fileExists(side): return
  for r in parseFile(side)["rounds"]:
    result.add r["startTick"].getInt()

proc makeWs(s: Sample, x, y, h, sp: float): WorldState =
  WorldState(
    enemyX: s.enemyX, enemyY: s.enemyY, enemyHeading: 0.0, enemySpeed: 0.0,
    enemyEnergy: s.enemyEnergy,
    selfX: x, selfY: y, selfHeading: h, selfSpeed: sp,
    selfEnergy: s.selfEnergy, arenaWidth: ArenaW, arenaHeight: ArenaH,
    enemies: @[EnemyInfo(id: 1, x: s.enemyX, y: s.enemyY, energy: s.enemyEnergy)])

proc tileOf(x, y: float): int =
  let c = clamp(int((x - MarginX) / GridSize), 0, Cols - 1)
  let r = clamp(int((y - MarginY) / GridSize), 0, Rows - 1)
  r * Cols + c

# ── kinematic replay ──────────────────────────────────────────────────────────

type SimResult = object
  dist:            seq[float]   ## distance to the enemy per tick
  commandedTicks:  int
  stuckTicks:      int         ## commanded but no displacement
  longestStuck:    int
  trappedTicks:    int         ## commanded but < TrapRadius from 25 ticks ago
  longestTrapped:  int
  noTileTicks:     int         ## commanded but same tile as the previous tick
  longestNoTile:   int
  picks:           int
  safeSum:         int
  candSum:         int
  fallbackPicks:   int
  turnSum:         float       ## sum of |turnRate|
  absSpeedSum:     float
  turnedTicks:     int         ## |turnRate| >= 0.5

const TrapWindow = 25
const TrapRadius = 30.0

proc simFixture(samples: seq[Sample], starts: seq[int]): SimResult =
  randomize(Seed)
  var m = initStrafe()
  var x, y, h, sp = 0.0
  var prevTile  = -1
  var runStuck  = 0
  var runTrapped = 0
  var runNoTile = 0
  var prevPicks = 0
  var posHist: seq[(float, float)]
  for i in 0..<samples.len:
    let s = samples[i]
    if i == 0 or i in starts:
      m.resetRound()
      x = s.selfX; y = s.selfY; h = s.selfHeading; sp = 0.0
      prevTile = -1; runStuck = 0; runTrapped = 0; runNoTile = 0; prevPicks = 0
      posHist.setLen(0)
    let ws  = makeWs(s, x, y, h, sp)
    let cmd = m.computeMove(ws)
    if m.picks > prevPicks:
      prevPicks = m.picks
      inc result.picks
      result.safeSum += m.lastSafeCount
      result.candSum += m.lastCandCount
      if m.lastSafeCount == 0: inc result.fallbackPicks
    if abs(cmd.turnRate) >= 0.5: inc result.turnedTicks
    result.turnSum += abs(cmd.turnRate)
    result.absSpeedSum += abs(cmd.speed)
    # apply turn + speed (immediate: this is a stall detector, not a physics model)
    h += cmd.turnRate
    sp = cmd.speed
    let nx = clamp(x + sp * cos(h * PI / 180.0), 18.0, ArenaW - 18.0)
    let ny = clamp(y + sp * sin(h * PI / 180.0), 18.0, ArenaH - 18.0)
    let disp = hypot(nx - x, ny - y)
    x = nx; y = ny
    let tile  = tileOf(x, y)
    let cmdOn = abs(cmd.speed) > 0.5
    if cmdOn: inc result.commandedTicks
    if prevTile >= 0 and tile == prevTile and cmdOn:
      inc runNoTile; inc result.noTileTicks
    else:
      result.longestNoTile = max(result.longestNoTile, runNoTile); runNoTile = 0
    if cmdOn and disp < 0.2:
      inc runStuck; inc result.stuckTicks
    else:
      result.longestStuck = max(result.longestStuck, runStuck); runStuck = 0
    # trap = commanded, yet still inside TrapRadius of where we were 25 ticks
    # ago: the corner oscillation ("never comes back") that a pure disp==0
    # detector misses because the bot bounces between two adjacent tiles.
    if posHist.len >= TrapWindow:
      let old = posHist[posHist.len - TrapWindow]
      if cmdOn and hypot(x - old[0], y - old[1]) < TrapRadius:
        inc runTrapped; inc result.trappedTicks
      else:
        result.longestTrapped = max(result.longestTrapped, runTrapped); runTrapped = 0
    posHist.add (x, y)
    prevTile = tile
    result.dist.add hypot(s.enemyX - x, s.enemyY - y)
  result.longestNoTile = max(result.longestNoTile, runNoTile)
  result.longestStuck  = max(result.longestStuck, runStuck)
  result.longestTrapped = max(result.longestTrapped, runTrapped)

# ── stats ─────────────────────────────────────────────────────────────────────

proc mean(v: seq[float]): float =
  if v.len == 0: return 0.0
  var t = 0.0
  for x in v: t += x
  t / v.len.float

proc pct(v: seq[float], q: float): float =
  if v.len == 0: return 0.0
  var s = v
  s.sort()
  s[clamp(int(q * (s.len - 1).float), 0, s.len - 1)]

proc fracWithin(v: seq[float], lo, hi: float): float =
  if v.len == 0: return 0.0
  var n = 0
  for x in v:
    if x >= lo and x <= hi: inc n
  n.float / v.len.float

proc fmtF(x: float, d = 1): string = formatFloat(x, ffDecimal, d)

# ── range mode = the ONLY knob the gate has to touch ──────────────────────────

proc setRangeMode(on: bool) =
  ## OFF == pure perpendicular strafe: TILT_MAX 0. ON == the module default.
  ## On the pre-fix module these names are not read at all, so OFF and ON
  ## behave identically there (which is exactly the honest "before" baseline).
  for n in ["TR_STRAFE_RANGE", "TR_STRAFE_RANGE_TOL",
            "TR_STRAFE_TILT_GAIN", "TR_STRAFE_TILT_MAX"]:
    delEnv(n)
  if not on:
    putEnv("TR_STRAFE_TILT_MAX", "0")
  loadStrafeEnv()

# ── targeted corner test ──────────────────────────────────────────────────────

type CornerResult = object
  name:         string
  longestStuck: int
  stuckFrac:    float
  within72:     float    ## fraction of ticks still inside 72 px of the corner
  escapes:      int      ## distinct tiles visited after tick 20
  maxFromCorner: float

proc simCorner(name: string, bx, by, ex, ey: float, nTicks: int): CornerResult =
  randomize(Seed)
  var m = initStrafe()
  var x = bx; var y = by
  # Adversarial stall setup: heading EXACTLY on the strafe line (perpendicular
  # to the inward enemy bearing), with dir = +1, so forward presses into the
  # adjacent wall and the heading band has nothing to correct. This is the
  # "corner whose outward direction is the last sign" the defect needs.
  var h = arctan2(ey - by, ex - bx) * 180.0 / PI + 90.0
  var sp = 0.0
  var runStuck = 0
  var stuck = 0
  var commanded = 0
  var within72 = 0
  var seen = initHashSet[int]()
  result.name = name
  for i in 0..<nTicks:
    let s = Sample(enemyX: ex, enemyY: ey, enemyEnergy: 100.0,
                   selfX: x, selfY: y, selfHeading: h, selfEnergy: 100.0)
    let cmd = m.computeMove(makeWs(s, x, y, h, sp))
    h += cmd.turnRate
    sp = cmd.speed
    let nx = clamp(x + sp * cos(h * PI / 180.0), 18.0, ArenaW - 18.0)
    let ny = clamp(y + sp * sin(h * PI / 180.0), 18.0, ArenaH - 18.0)
    let disp = hypot(nx - x, ny - y)
    x = nx; y = ny
    let tile  = tileOf(x, y)
    let cmdOn = abs(cmd.speed) > 0.5
    if cmdOn: inc commanded
    if cmdOn and disp < 0.2:
      inc runStuck; inc stuck
    else:
      result.longestStuck = max(result.longestStuck, runStuck); runStuck = 0
    if i >= 20:
      seen.incl tile
      if hypot(x - bx, y - by) < 72.0: inc within72
    result.maxFromCorner = max(result.maxFromCorner, hypot(x - bx, y - by))
  result.longestStuck = max(result.longestStuck, runStuck)
  result.stuckFrac = if commanded == 0: 0.0 else: stuck.float / commanded.float
  result.within72 = if nTicks <= 20: 0.0
                    else: within72.float / (nTicks - 20).float
  result.escapes = seen.len

proc cornerTest(rangeOn: bool): seq[CornerResult] =
  setRangeMode(rangeOn)
  for (name, cx, cy, ux, uy) in [
      ("bottom-left ", 18.0, 18.0,  1.0,  1.0),
      ("bottom-right", 782.0, 18.0, -1.0,  1.0),
      ("top-right   ", 782.0, 582.0, -1.0, -1.0),
      ("top-left    ", 18.0, 582.0,  1.0, -1.0)]:
    let ex = cx + ux * 240.0
    let ey = cy + uy * 240.0
    result.add simCorner(name, cx, cy, ex, ey, 400)

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

const Fixtures = [
  "tr_drussgt_vs_modularbot.jsonl",
  "tr_drussgt_vs_corners.jsonl",
  "tr_drussgt_vs_spinbot.jsonl",
  "tr_drussgt_vs_crazy.jsonl",
]

type Loaded = object
  name:    string
  samples: seq[Sample]
  starts:  seq[int]

var loaded: seq[Loaded]
for f in Fixtures:
  loaded.add Loaded(name: f, samples: loadSamples(f), starts: loadRoundStarts(f))

echo "STRAFE range/stall gates (j111) — kinematic replay of DrussGT fixtures, seed=", Seed
echo ""

# ── GATE A: tile availability with the tilt active ───────────────────────────
echo "=== GATE A — TILE AVAILABILITY (tilt active, corrected j110 field) ==="
for L in loaded:
  setRangeMode(true)
  let r = simFixture(L.samples, L.starts)
  if r.picks == 0: continue
  echo "  ", L.name
  echo "    picks=", r.picks,
       "  mean candidates=", fmtF(r.candSum.float / r.picks.float, 2),
       "  mean SAFE=", fmtF(r.safeSum.float / r.picks.float, 2),
       "  zero-safe picks=", r.fallbackPicks, " (",
       fmtF(100.0 * r.fallbackPicks.float / r.picks.float, 1), "%)"
echo ""

# ── GATE B: stall detector, range OFF vs ON ──────────────────────────────────
proc stallRow(L: Loaded, rangeOn: bool): SimResult =
  setRangeMode(rangeOn)
  simFixture(L.samples, L.starts)

echo "=== GATE B — STALL DETECTOR (commanded move, no progress) ==="
echo "  stuck  = commanded but zero displacement (true freeze)"
echo "  trapped= commanded but still <30 px from where it was 25 ticks ago"
echo "           (the corner oscillation the user saw; the defect the gate is for)"
echo "  fixture                        range  commanded   stuck% longest noTile% longest trapped% longest"
for L in loaded:
  for on in [false, true]:
    let r = stallRow(L, on)
    let stuckPct = if r.commandedTicks == 0: 0.0 else: 100.0 * r.stuckTicks.float / r.commandedTicks.float
    let noTilePct = if r.commandedTicks == 0: 0.0 else: 100.0 * r.noTileTicks.float / r.commandedTicks.float
    let trappedPct = if r.commandedTicks == 0: 0.0 else: 100.0 * r.trappedTicks.float / r.commandedTicks.float
    echo "  ", alignLeft(L.name, 30), " ",
         (if on: "ON " else: "OFF"), "  ",
         align($r.commandedTicks, 9), "  ",
         align(fmtF(stuckPct, 2), 6), " ", align($r.longestStuck, 5), "  ",
         align(fmtF(noTilePct, 1), 7), " ", align($r.longestNoTile, 5), "  ",
         align(fmtF(trappedPct, 1), 7), " ", align($r.longestTrapped, 6)
echo ""

# ── GATE C: achieved range distribution, OFF vs ON ───────────────────────────
proc rangeRow(L: Loaded, rangeOn: bool): seq[float] =
  setRangeMode(rangeOn)
  simFixture(L.samples, L.starts).dist

echo "=== GATE C — ACHIEVED RANGE DISTRIBUTION (range control OFF vs ON) ==="
echo "  target TR_STRAFE_RANGE = 200 px (== TR_POWER_FAR_DIST); rng default"
echo "  fixture                        range    mean   p10   med   p90   <=tol(225)  |d-200|"
for L in loaded:
  for on in [false, true]:
    let d = rangeRow(L, on)
    let dIn = @[d.pct(0.1), d.pct(0.5), d.pct(0.9)]
    var absErr = 0.0
    for v in d: absErr += abs(v - 200.0)
    let mae = if d.len == 0: 0.0 else: absErr / d.len.float
    echo "  ", alignLeft(L.name, 30), " ",
         (if on: "ON " else: "OFF"), "  ",
         align(fmtF(d.mean, 0), 6), " ",
         align(fmtF(dIn[0], 0), 5), " ",
         align(fmtF(dIn[1], 0), 5), " ",
         align(fmtF(dIn[2], 0), 5), "  ",
         align(fmtF(100.0 * fracWithin(d, 175.0, 225.0), 1), 9), "%  ",
         align(fmtF(mae, 1), 8)
echo ""

# ── GATE D: the no-turn property ──────────────────────────────────────────────
proc turnRow(L: Loaded, rangeOn: bool): SimResult =
  setRangeMode(rangeOn)
  simFixture(L.samples, L.starts)

echo "=== GATE D — NO-TURN PROPERTY (range OFF vs ON) ==="
echo "  the tilt must NOT turn the body more; heading still only corrected to the band"
echo "  fixture                        range   mean|turn|   no-turn%   mean|speed|"
for L in loaded:
  for on in [false, true]:
    let r = turnRow(L, on)
    let n = r.commandedTicks
    echo "  ", alignLeft(L.name, 30), " ",
         (if on: "ON " else: "OFF"), "  ",
         align(fmtF(if n == 0: 0.0 else: r.turnSum / n.float, 2), 9), "   ",
         align(fmtF(if n == 0: 0.0 else: 100.0 * (n - r.turnedTicks).float / n.float, 1), 8), "   ",
         align(fmtF(if n == 0: 0.0 else: r.absSpeedSum / n.float, 2), 9)
echo ""

# ── GATE B2: the corner test ─────────────────────────────────────────────────
echo "=== GATE B2 — CORNER ESCAPE (line points out of the arena, 400 ticks) ==="
echo "  within72 = fraction of ticks still inside 72 px (2 tiles) of the corner"
for (tag, spread) in [("SPREAD=0 (zero-candidate line)", 0), ("SPREAD=1 (default)", 1)]:
  putEnv("TR_STRAFE_SPREAD", $spread)
  echo "  ", tag, ":"
  for on in [false, true]:
    echo "    range ", (if on: "ON" else: "OFF"), ":"
    for c in cornerTest(on):
      echo "      ", c.name, "  longestStuck=", align($c.longestStuck, 3),
           "  stuck%=", align(fmtF(100.0 * c.stuckFrac, 1), 5),
           "  within72%=", align(fmtF(100.0 * c.within72, 1), 5),
           "  tilesVisited=", align($c.escapes, 3),
           "  maxFromCorner=", fmtF(c.maxFromCorner, 1), " px"
delEnv("TR_STRAFE_SPREAD")
loadStrafeEnv()
echo ""
echo "=== BUILD IDENTITY (which tree this gate measured) ==="
when declared(StrafeRange):
  echo "  module exposes the range knobs: YES (post-fix tree)"
else:
  echo "  module exposes the range knobs: no (pre-fix baseline tree)"