## GotoTest — throwaway prototype to validate goto(x,y) + aimTo(x,y) controllers.
## Diamond pattern with wall-smash north waypoint to test stuck recovery.
## Tank Royale: Y=0 is south, Y increases northward. Arena 800x600.

import std/[strformat, os]
import tankroyale_botapi

const botJsonPath = currentSourcePath().parentDir / "GotoTest.json"

# Diamond waypoints. (400,600) is AT the north wall — physically impossible, tests wall recovery.
const waypoints = [
  (400.0, 300.0),  # center
  (400.0, 600.0),  # north wall — AT wall, unreachable, tests wall-stuck recovery
  ( 36.0, 300.0),  # west wall (left)
  (400.0,  36.0),  # south wall (bottom)
  (764.0, 300.0),  # east wall (right)
  (400.0, 300.0),  # center
]

type GotoBot = ref object of Bot
  waypointIdx: int
  oldX, oldY: float   # position 3 ticks ago for stuck detection
  tickCount: int      # ticks since oldX/oldY was last updated
  stuckTicks: int     # remaining ticks of reverse override


method onRoundStarted*(bot: GotoBot, e: RoundStartedEvent) =
  setAdjustGunForBodyTurn(true)
  bot.waypointIdx = 0
  bot.oldX = 0.0; bot.oldY = 0.0
  bot.tickCount = 0; bot.stuckTicks = 0

method run(bot: GotoBot) =
  while isRunning():
    let bx = getX()
    let by = getY()
    let (tx, ty) = waypoints[bot.waypointIdx]
    let dist = distanceTo(bx, by, tx, ty)

    if dist < 40.0:
      bot.waypointIdx = (bot.waypointIdx + 1) mod waypoints.len

    # goto controller — explicit forward/reverse proportional steering
    # ponytail: no speed-taper on heading error; add when overshooting observed
    let rawBearing = normalizeRelativeAngle(directionTo(tx, ty) - getDirection())
    let (dirSign, effBearing) =
      if abs(rawBearing) > 90.0:
        (-1.0, normalizeRelativeAngle(rawBearing + 180.0))
      else:
        (1.0, rawBearing)
    let maxTurn = calcMaxTurnRate(getSpeed())
    setTurnRate(effBearing.clamp(-maxTurn, maxTurn))

    # stuck detector — ponytail: 3-tick sample, upgrade to wall-nav if needed
    inc bot.tickCount
    if bot.tickCount >= 3:
      let moved = distanceTo(bot.oldX, bot.oldY, bx, by)
      if moved < 1.0 and dist >= 30.0:
        bot.stuckTicks = 6
        echo &"STUCK at ({bx:.1f},{by:.1f}) moved={moved:.2f} — reversing"
      bot.oldX = bx; bot.oldY = by
      bot.tickCount = 0

    if bot.stuckTicks > 0:
      # reverse current speed direction to unstick
      let targetSpd = if getSpeed() >= 0.0: -8.0 else: 8.0
      setTargetSpeed(targetSpd)
      dec bot.stuckTicks
    else:
      let rawSpeed = getNewTargetSpeed(MAX_SPEED, abs(getSpeed()), dist)
      setTargetSpeed(dirSign * rawSpeed)

    # aimTo arena center — radarBearingTo reuses getX/getY/getGunDirection internally
    let cx = getArenaWidth().float / 2.0
    let cy = getArenaHeight().float / 2.0
    let gunTurnNeeded = normalizeRelativeAngle(directionTo(cx, cy) - getGunDirection())
    setGunTurnRate(gunTurnNeeded.clamp(-MAX_GUN_TURN_RATE, MAX_GUN_TURN_RATE))

    echo &"pos=({bx:.1f},{by:.1f}) target=({tx:.1f},{ty:.1f}) dist={dist:.1f} spd={getSpeed():.2f} stuck={bot.stuckTicks}"

    # Debug graphics: waypoint circles + line to current target
    for i, (wx, wy) in waypoints:
      if i == bot.waypointIdx:
        setStrokeColor(RED)
      else:
        setStrokeColor(GRAY)
      drawCircle(wx, wy, 10.0)
    setStrokeColor(YELLOW)
    drawLine(bx, by, tx, ty)

    go()

when isMainModule:
  var bot = GotoBot()
  start(bot, botJsonPath)