## Assert-based tests for controllers.nim.
## Run: nim c -r tests/test_controllers.nim

import std/math
import "../controllers"

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

# ---------------------------------------------------------------------------
# gotoTick tests
# ---------------------------------------------------------------------------

block forwardMovement:
  # Bot at (0,0) facing East (geometric 0°), target due east at (100,0) → bearing=0 → forward
  let (spd2, turn2) = gotoTick(100.0, 0.0, 0.0, 0.0, 0.0, 0.0)
  check spd2 > 0.0, "forward: targetSpeed should be positive"
  check abs(turn2) < 1e-9, "forward: no turn needed when already aimed"

block reverseMovement:
  # Bot at (100,0) facing East (direction=0), target at (0,0) — directly behind
  # bearing = normalizeRelativeAngle(180 - 0) = 180 → |bearing|>90 → reverse
  let (spd, _) = gotoTick(0.0, 0.0, 100.0, 0.0, 0.0, 0.0)
  check spd < 0.0, "reverse: targetSpeed should be negative when target is behind"

block turnRateClamping:
  # Bot at (0,0) facing North (game north = geometric 90°, so direction=90 in geometric)
  # Target at (100,0) = East. bearing = normalizeRelativeAngle(0 - 90) = -90 → still ≤90
  # Use large perpendicular target so bearing is 89°, and high speed → small maxTurn
  # At speed=8, calcMaxTurnRate = 10 - 0.75*8 = 4°
  # Bot facing East (0°), target at angle 89° bearing (just under 90)
  let (_, turn) = gotoTick(100.0 * cos(89.0 * PI / 180.0), 100.0 * sin(89.0 * PI / 180.0), 0.0, 0.0, 0.0, 8.0)
  check abs(turn) <= 4.0 + 1e-9, "turn rate clamped to calcMaxTurnRate at speed=8 (max 4°)"

# ---------------------------------------------------------------------------
# aimToTick tests
# ---------------------------------------------------------------------------

block gunShortestArc:
  # Gun facing East (0°), target due north (geometric 90°) → turn left +90° but clamped to 20
  let rate = aimToTick(0.0, 100.0, 0.0, 0.0, 0.0)
  check rate > 0.0, "gun shortest arc: should turn toward target"
  # Gun facing East (0°), target due south (geometric 270° → normalised -90°)
  let rate2 = aimToTick(0.0, -100.0, 0.0, 0.0, 0.0)
  check rate2 < 0.0, "gun shortest arc: should turn other way for target behind"

block gunTurnRateClamping:
  # 180° away → clamped to ±20
  let rate = aimToTick(-100.0, 0.0, 0.0, 0.0, 0.0)   # target west, gun east
  check abs(rate) <= 20.0 + 1e-9, "gun turn rate clamped to ±MAX_GUN_TURN_RATE"
  check abs(abs(rate) - 20.0) < 1e-9, "gun turn rate at max when 180° away"

echo "test_controllers: all passed"