SirRoboGarage/common_libs/movements/wave_surfer.nim

## wave_surfer.nim — Wave-surfing movement module.
## Detects enemy fire via energy drops, maintains a danger histogram (31 GF bins),
## and steers toward the lowest-danger GF bin on approaching waves.
## All angles in radians internally; output degrees for bot API.

import std/[math]
import gun_harness/gun_interface
import movement_harness/movement_interface

const WS_BINS       = 31
const WallMargin    = 40.0
const DodgeTicks    = 15.0   # approximate ticks to reach dodge position
const WS_PrefDist   = 400.0  # optimal engagement distance
const WS_DistBand   = 50.0   # deadband: pure strafe within ±50px of preferred
const WS_RadialFrac = 0.35   # radial blend fraction (0=pure strafe, 1=pure radial)

type
  WSWave = object
    originX, originY: float64
    bearing:          float64  ## direction from enemy to us at fire time (radians)
    speed:            float64
    radius:           float64
    startDist:        float64

  WaveSurferModule* = object
    bins:            array[WS_BINS, float64]
    waves:           seq[WSWave]
    prevEnergy:      float64
    strafeDir:       float64  ## +1.0 or -1.0
    debugGraphics*:  bool

proc initWaveSurfer*(): WaveSurferModule =
  var m = WaveSurferModule(prevEnergy: 100.0, strafeDir: 1.0, debugGraphics: false)
  for i in 0..<WS_BINS: m.bins[i] = 1.0
  m

proc resetRound*(m: var WaveSurferModule) =
  m.waves      = @[]
  m.prevEnergy = 100.0
  m.strafeDir  = 1.0

proc gfToBin(gf: float64): int {.inline.} =
  clamp(int(round((gf.clamp(-1.0, 1.0) + 1.0) * 0.5 * float64(WS_BINS - 1))), 0, WS_BINS - 1)

proc binToGF(idx: int): float64 {.inline.} =
  float64(idx) / float64(WS_BINS - 1) * 2.0 - 1.0

proc mea(speed: float64): float64 {.inline.} = arcsin(min(8.0 / speed, 1.0))

proc nearestWave(m: WaveSurferModule, botX, botY: float64): int =
  ## Index of the wave closest to reaching us (largest radius relative to startDist).
  result = -1
  var bestRatio = -1.0
  for i in 0..<m.waves.len:
    let d = sqrt((botX - m.waves[i].originX)^2 + (botY - m.waves[i].originY)^2)
    let ratio = m.waves[i].radius / d
    if ratio > bestRatio:
      bestRatio = ratio
      result = i

proc computeMove*(m: var WaveSurferModule, ws: WorldState): MoveCommand =
  let botX    = ws.selfX
  let botY    = ws.selfY
  let enemyX  = ws.enemyX
  let enemyY  = ws.enemyY

  # Fire detection
  let drop = m.prevEnergy - ws.enemyEnergy
  m.prevEnergy = ws.enemyEnergy
  if drop >= 0.1 and drop <= 3.0:
    let bspeed  = 20.0 - 3.0 * drop
    let bearing = arctan2(botY - enemyY, botX - enemyX)
    let d       = sqrt((botX - enemyX)^2 + (botY - enemyY)^2)
    m.waves.add WSWave(
      originX: enemyX, originY: enemyY,
      bearing: bearing,
      speed:   bspeed,
      radius:  0.0,
      startDist: d,
    )

  # Advance waves; collect hits
  var i = 0
  while i < m.waves.len:
    m.waves[i].radius += m.waves[i].speed
    let d = sqrt((botX - m.waves[i].originX)^2 + (botY - m.waves[i].originY)^2)
    if m.waves[i].radius >= d:
      let toBot = arctan2(botY - m.waves[i].originY, botX - m.waves[i].originX)
      var off   = toBot - m.waves[i].bearing
      while off >  PI: off -= 2.0*PI
      while off < -PI: off += 2.0*PI
      let maxA = mea(m.waves[i].speed)
      if maxA >= 1e-9:
        let gf = clamp(off / maxA, -1.0, 1.0)
        m.bins[gfToBin(gf)] += 1.0
      m.waves.del(i)
    else:
      inc i

  # Dodge logic: find nearest wave, pick safest GF bin direction
  let nearest = m.nearestWave(botX, botY)
  if nearest >= 0:
    let w = m.waves[nearest]
    let toBot = arctan2(botY - w.originY, botX - w.originX)
    var off   = toBot - w.bearing
    while off >  PI: off -= 2.0*PI
    while off < -PI: off += 2.0*PI
    let maxA = mea(w.speed)
    let curGF = if maxA >= 1e-9: clamp(off / maxA, -1.0, 1.0) else: 0.0
    let curBin = gfToBin(curGF)

    # Average danger
    var total = 0.0
    for b in m.bins: total += b
    let avg = total / float64(WS_BINS)

    if m.bins[curBin] > avg:
      # Find lowest-danger bin, penalizing positions near walls
      let dodgeDist = ws.selfSpeed * DodgeTicks
      var bestBin = 0
      for j in 1..<WS_BINS:
        let gfJ      = binToGF(j)
        let angleJ   = w.bearing + gfJ * maxA
        let futureX  = botX + cos(angleJ) * dodgeDist
        let futureY  = botY + sin(angleJ) * dodgeDist
        let wallHit  = futureX < WallMargin or futureX > ws.arenaWidth  - WallMargin or
                       futureY < WallMargin or futureY > ws.arenaHeight - WallMargin
        let dangerJ  = if wallHit: m.bins[j] * 5.0 else: m.bins[j]
        let gfBest   = binToGF(bestBin)
        let angleB   = w.bearing + gfBest * maxA
        let futureXB = botX + cos(angleB) * dodgeDist
        let futureYB = botY + sin(angleB) * dodgeDist
        let wallHitB = futureXB < WallMargin or futureXB > ws.arenaWidth  - WallMargin or
                       futureYB < WallMargin or futureYB > ws.arenaHeight - WallMargin
        let dangerB  = if wallHitB: m.bins[bestBin] * 5.0 else: m.bins[bestBin]
        if dangerJ < dangerB: bestBin = j
      let bestGF = binToGF(bestBin)
      # Move in direction of best GF: positive = CCW (orbit left), negative = CW
      m.strafeDir = if bestGF >= curGF: 1.0 else: -1.0

  # Perpendicular strafe (same body trick as phantom_meteor)
  let enemyBearingRad = arctan2(enemyY - botY, enemyX - botX)
  # Perpendicular in chosen strafe direction
  var perpAngle =
    if m.strafeDir >= 0.0: enemyBearingRad + PI * 0.5
    else:                   enemyBearingRad - PI * 0.5

  # Hard wall escape: if near any wall, blend toward arena center
  # ponytail: linear blend, upgrade to override if blending proves too weak
  let nearLeft   = botX < WallMargin
  let nearRight  = botX > ws.arenaWidth  - WallMargin
  let nearBottom = botY < WallMargin
  let nearTop    = botY > ws.arenaHeight - WallMargin
  if nearLeft or nearRight or nearBottom or nearTop:
    # Flip strafe if it pushes further into the wall
    let px = cos(perpAngle)
    let py = sin(perpAngle)
    if (nearLeft   and px < 0.0) or (nearRight  and px > 0.0) or
       (nearBottom and py < 0.0) or (nearTop    and py > 0.0):
      m.strafeDir = -m.strafeDir
      perpAngle   = if m.strafeDir >= 0.0: enemyBearingRad + PI * 0.5
                    else:                   enemyBearingRad - PI * 0.5
    # Blend 50% toward arena center
    let escapeAngle = arctan2(ws.arenaHeight * 0.5 - botY, ws.arenaWidth * 0.5 - botX)
    let ex = cos(escapeAngle) + cos(perpAngle)
    let ey = sin(escapeAngle) + sin(perpAngle)
    perpAngle = arctan2(ey, ex)

  # Distance control: blend a radial component when outside the deadband.
  # Secondary to bullet dodge — capped at WS_RadialFrac of travel direction.
  # ponytail: linear blend, tune WS_RadialFrac if approach/retreat feels sluggish
  let enemyDist  = sqrt((enemyX - botX)^2 + (enemyY - botY)^2)
  let distErr    = enemyDist - WS_PrefDist
  let radialFrac =
    if   distErr >  WS_DistBand: WS_RadialFrac                     # too far  → approach
    elif distErr < -WS_DistBand: -WS_RadialFrac                    # too close → retreat
    else: 0.0                                                       # deadband  → pure strafe
  if abs(radialFrac) > 1e-9:
    # Radial direction: toward enemy (positive) or away (negative)
    let radialAngle = arctan2(enemyY - botY, enemyX - botX) +
                      (if radialFrac < 0.0: PI else: 0.0)
    let rx = cos(perpAngle) * (1.0 - abs(radialFrac)) + cos(radialAngle) * abs(radialFrac)
    let ry = sin(perpAngle) * (1.0 - abs(radialFrac)) + sin(radialAngle) * abs(radialFrac)
    perpAngle = arctan2(ry, rx)

  let desiredDeg = radToDeg(perpAngle)

  var delta = desiredDeg - ws.selfHeading
  while delta >  180.0: delta -= 360.0
  while delta < -180.0: delta += 360.0

  let goForward = abs(delta) <= 90.0
  if not goForward:
    delta = if delta >= 0.0: delta - 180.0 else: delta + 180.0

  (speed: if goForward: 8.0 else: -8.0,
   turnRate: delta.clamp(-10.0, 10.0))