## Assert-based tests for EnemyTracker and StateVector.
## Run: nim c -r tests/test_state.nim

import std/[math, strformat]
import arraymancer

# Import from parent dir
import "../enemy_tracker"
import "../state_vector"

template check(cond: bool, msg: string) =
  if not cond:
    quit("FAIL: " & msg, 1)

# ---------------------------------------------------------------------------
# EnemyTracker tests
# ---------------------------------------------------------------------------

block testBasicUpdate:
  var t = initEnemyTracker()
  t.update(200.0, 300.0, 90.0, 5.0, 80.0)
  check t.hasContact,                       "hasContact after update"
  check t.current.x == 200.0,              "x after update"
  check t.current.y == 300.0,              "y after update"
  check t.current.direction == 90.0,       "direction after update"
  check t.current.speed == 5.0,            "speed after update"
  check t.current.energy == 80.0,          "energy after update"
  check t.current.ticksSinceLastScan == 0, "ticksSinceLastScan reset"

block testFireDetection:
  var t = initEnemyTracker()
  # First update sets prevEnergy
  t.update(100.0, 100.0, 0.0, 0.0, 100.0)
  # Second update: energy drop of 3.0 → enemy fired power 3.0
  t.update(100.0, 100.0, 0.0, 0.0, 97.0)
  check t.current.hasFired,                "hasFired when energy drops by 3.0"
  check abs(t.current.lastFirePower - 3.0) < 0.001, "lastFirePower == 3.0"

block testNoFireOnSmallDrop:
  var t = initEnemyTracker()
  t.update(100.0, 100.0, 0.0, 0.0, 100.0)
  # Drop of 0.05 — below MIN_FIRE_POWER threshold
  t.update(100.0, 100.0, 0.0, 0.0, 99.95)
  check not t.current.hasFired, "no fire on small energy drop"

block testDeadReckoning:
  var t = initEnemyTracker()
  # direction=0° in Tank Royale means north (y increases)
  t.update(100.0, 100.0, 0.0, 5.0, 100.0)
  t.deadReckon()
  # x unchanged (sin 0° = 0), y increases by speed (cos 0° = 1)
  check abs(t.current.x - 100.0) < 0.001, "dead reckon: x unchanged for dir=0"
  check abs(t.current.y - 105.0) < 0.001, "dead reckon: y += speed for dir=0"
  check t.current.ticksSinceLastScan == 1,  "ticksSinceLastScan incremented"

block testDeadReckonEast:
  var t = initEnemyTracker()
  # direction=90° → east (sin 90° = 1, cos 90° = 0)
  t.update(100.0, 100.0, 90.0, 5.0, 100.0)
  t.deadReckon()
  check abs(t.current.x - 105.0) < 0.001, "dead reckon east: x += speed"
  check abs(t.current.y - 100.0) < 0.001, "dead reckon east: y unchanged"

block testHistoryWindow:
  var t = initEnemyTracker()
  # Feed 6 updates — history should hold last 5
  for i in 1 .. 6:
    t.update(float64(i) * 10.0, float64(i) * 20.0, 0.0, float64(i), 100.0)
  check t.historyCount == 5, "historyCount capped at 5"
  # history[0] should be the second-to-last scan (i=5)
  check abs(t.history[0].x - 50.0) < 0.001, "history[0].x == 50 (i=5)"
  check abs(t.history[4].x - 10.0) < 0.001, "history[4].x == 10 (i=1)"

# ---------------------------------------------------------------------------
# StateVector tests
# ---------------------------------------------------------------------------

block testStateVectorLength:
  var t = initEnemyTracker()
  t.update(400.0, 300.0, 45.0, 3.0, 80.0)
  let bot = BotStateData(
    x: 200.0, y: 200.0, direction: 90.0, speed: 4.0, energy: 50.0,
    gunDirection: 180.0, gunHeat: 0.5,
    arenaWidth: 800.0, arenaHeight: 600.0,
  )
  let sv = buildStateVector(bot, t)
  check sv.shape == [56], "state vector has 56 elements"

block testStateVectorRange:
  var t = initEnemyTracker()
  t.update(400.0, 300.0, 180.0, 8.0, 100.0)
  let bot = BotStateData(
    x: 800.0, y: 600.0, direction: 360.0, speed: 8.0, energy: 100.0,
    gunDirection: 360.0, gunHeat: 1.8,
    arenaWidth: 800.0, arenaHeight: 600.0,
  )
  let sv = buildStateVector(bot, t)
  for i in 0 ..< 56:
    check sv[i] >= -2.0f32 and sv[i] <= 2.0f32,
      &"sv[{i}]={sv[i]} out of [-2,2] range"

block testWallDistances:
  # Bot at (100, 200) in 800×600 arena
  # wallMax = max(800, 600) = 800
  # top    = (600 - 200) / 800 = 400/800 = 0.5
  # bottom = 200 / 800 = 0.25
  # left   = 100 / 800 = 0.125
  # right  = (800 - 100) / 800 = 700/800 = 0.875
  var t = initEnemyTracker()
  let bot = BotStateData(
    x: 100.0, y: 200.0, direction: 0.0, speed: 0.0, energy: 100.0,
    gunDirection: 0.0, gunHeat: 0.0,
    arenaWidth: 800.0, arenaHeight: 600.0,
  )
  let sv = buildStateVector(bot, t)
  check abs(sv[18] - 0.5f32) < 0.001f32,    "top wall = 0.5"
  check abs(sv[19] - 0.25f32) < 0.001f32,   "bottom wall = 0.25"
  check abs(sv[20] - 0.125f32) < 0.001f32,  "left wall = 0.125"
  check abs(sv[21] - 0.875f32) < 0.001f32,  "right wall = 0.875"

block testRelativeBearing:
  # Game convention: north=0°, CW. arctan2(dx,dy) used.
  # Bot at (0,0) dir=0°. Enemy at (0,100) → due north → absDir=0°.
  # relBearing = (0 - 0 + 540) mod 360 - 180 = 0°. normalized = 0/180 = 0.0
  var t = initEnemyTracker()
  t.update(0.0, 100.0, 0.0, 0.0, 100.0)
  let bot = BotStateData(
    x: 0.0, y: 0.0, direction: 0.0, speed: 0.0, energy: 100.0,
    gunDirection: 0.0, gunHeat: 0.0,
    arenaWidth: 800.0, arenaHeight: 600.0,
  )
  let sv = buildStateVector(bot, t)
  check abs(sv[16] - 0.0f32) < 0.01f32, "relative bearing = 0.0 (due north), got " & $sv[16]

block testRelativeBearingEast:
  # Enemy at (100,0) → due east → absDir=90°.
  # relBearing = (90 - 0 + 540) mod 360 - 180 = 90°. normalized = 90/180 = 0.5
  var t = initEnemyTracker()
  t.update(100.0, 0.0, 0.0, 0.0, 100.0)
  let bot = BotStateData(
    x: 0.0, y: 0.0, direction: 0.0, speed: 0.0, energy: 100.0,
    gunDirection: 0.0, gunHeat: 0.0,
    arenaWidth: 800.0, arenaHeight: 600.0,
  )
  let sv = buildStateVector(bot, t)
  check abs(sv[16] - 0.5f32) < 0.01f32, "relative bearing east = 0.5, got " & $sv[16]

block testHistoryPaddedWhenEmpty:
  var t = initEnemyTracker()
  let bot = BotStateData(
    x: 400.0, y: 300.0, direction: 0.0, speed: 0.0, energy: 100.0,
    gunDirection: 0.0, gunHeat: 0.0,
    arenaWidth: 800.0, arenaHeight: 600.0,
  )
  let sv = buildStateVector(bot, t)
  for i in 22 ..< 42:
    check sv[i] == 0.0f32, &"history slot {i} should be 0 when no contact"
  # indices 42-43 (goto inputs) default to 0 when not provided
  check sv[42] == 0.0f32, "sv[42] (remainingGotoDistance) should default to 0"
  check sv[43] == 0.0f32, "sv[43] (remainingGunAngle) should default to 0"
  # indices 44-55 (bullet slots) default to 0 when no bullets provided
  for i in 44 ..< 56:
    check sv[i] == 0.0f32, &"bullet slot {i} should be 0 when no bullets"

block testBulletSlots:
  var t = initEnemyTracker()
  t.update(400.0, 300.0, 0.0, 0.0, 100.0)  # enemy at (400,300)
  let bot = BotStateData(
    x: 200.0, y: 200.0, direction: 0.0, speed: 0.0, energy: 100.0,
    gunDirection: 0.0, gunHeat: 0.0,
    arenaWidth: 800.0, arenaHeight: 600.0,
  )
  # Bullet at (300,250), power 1.0 → speed = 20-3 = 17
  # relX = 300-400 = -100, relY = 250-300 = -50
  # dist to enemy = sqrt(100^2+50^2) ≈ 111.8, ticks ≈ 111.8/17 ≈ 6.58
  let b = BulletData(x: 300.0, y: 250.0, power: 1.0)
  let sv = buildStateVector(bot, t, 0.0, 0.0, [b], 1)
  check abs(sv[44] - (-100.0/800.0).float32) < 0.001f32, "bullet relX"
  check abs(sv[45] - (-50.0/600.0).float32) < 0.001f32,  "bullet relY"
  check abs(sv[46] - (17.0/20.0).float32) < 0.001f32,    "bullet speed norm"
  # second slot should be zero-padded
  for i in 48 ..< 56:
    check sv[i] == 0.0f32, &"unused bullet slot {i} should be 0"

echo "All tests passed"