SirRoboGarage/common_libs/movements/minimum_risk.nim

## minimum_risk.nim — Minimum-risk point movement for melee (multiple enemies).
## Generates candidate points, scores each by threat proximity, wall/corner risk,
## and travel distance, then drives toward the lowest-risk point.
## Uses the same perpendicular-body trick as phantom_meteor.nim.

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


const
  CandidateRadius = 150.0   # px radius for ring candidates
  NumRingPoints   = 16      # ring of 16 + 4 random = 20 candidates
  NumRandPoints   = 4
  WallMargin      = 80.0    # below this dist-to-wall = risk
  CornerMargin    = 150.0   # below this dist-to-corner = risk
  RecalcInterval  = 10      # ticks between full recalculations
  KEnemy          = 1.0     # inverse-square weight for enemy threat
  KWall           = 0.5     # linear wall penalty weight
  KCorner         = 0.8     # corner penalty weight
  KTravel         = 0.003   # penalty per pixel of travel distance

type
  Vec2 = object
    x, y: float

proc vec2(x, y: float): Vec2 {.inline.} = Vec2(x: x, y: y)
proc dist(a, b: Vec2): float {.inline.} =
  let dx = a.x - b.x; let dy = a.y - b.y
  sqrt(dx*dx + dy*dy)

type MinimumRiskModule* = object
  targetX, targetY: float
  ticksSinceCalc:   int
  hasTarget:        bool
  debugGraphics*:   bool

proc initMinimumRisk*(): MinimumRiskModule =
  MinimumRiskModule(hasTarget: false, ticksSinceCalc: RecalcInterval, debugGraphics: false)

proc wallRisk(p: Vec2, w, h: float): float {.inline.} =
  ## Linear penalty that ramps up inside WallMargin.
  let dL = p.x
  let dR = w - p.x
  let dB = p.y
  let dT = h - p.y
  let minD = min(min(dL, dR), min(dB, dT))
  if minD >= WallMargin: 0.0
  else: KWall * (1.0 - minD / WallMargin)

proc cornerRisk(p: Vec2, w, h: float): float {.inline.} =
  ## Penalty for proximity to any of the four corners.
  let corners = [vec2(0.0,0.0), vec2(w,0.0), vec2(0.0,h), vec2(w,h)]
  var worst = 0.0
  for c in corners:
    let d = dist(p, c)
    if d < CornerMargin:
      worst = max(worst, KCorner * (1.0 - d / CornerMargin))
  worst

proc scorePoint(p, bot: Vec2, threats: openArray[Vec2], w, h: float): float =
  var risk = 0.0
  # Inverse-square enemy threat
  for t in threats:
    let d = max(dist(p, t), 1.0)
    risk += KEnemy / (d * d) * 1e4   # scale so numbers are comparable
  risk += wallRisk(p, w, h)
  risk += cornerRisk(p, w, h)
  risk += KTravel * dist(p, bot)
  risk

proc clampToArena(p: Vec2, w, h: float): Vec2 {.inline.} =
  vec2(p.x.clamp(WallMargin, w - WallMargin),
       p.y.clamp(WallMargin, h - WallMargin))

proc recalcTarget(m: var MinimumRiskModule, ws: WorldState) =
  let bot = vec2(ws.selfX, ws.selfY)
  let w   = ws.arenaWidth
  let h   = ws.arenaHeight

  # Use all alive enemies; fall back to single target when seq is empty
  var threats: seq[Vec2]
  if ws.enemies.len > 0:
    for ei in ws.enemies:
      threats.add vec2(ei.x, ei.y)
  else:
    threats.add vec2(ws.enemyX, ws.enemyY)

  # Build candidates: ring + random (deterministic via tick-seeded offsets)
  var best     = bot          # fallback: stay put
  var bestRisk = scorePoint(bot, bot, threats, w, h)

  for i in 0..<NumRingPoints:
    let angle = float(i) / float(NumRingPoints) * 2.0 * PI
    let p = clampToArena(
      vec2(bot.x + CandidateRadius * cos(angle),
           bot.y + CandidateRadius * sin(angle)), w, h)
    let r = scorePoint(p, bot, threats, w, h)
    if r < bestRisk:
      bestRisk = r
      best     = p

  # 4 random-ish candidates via golden-angle spread (no RNG state needed)
  for i in 0..<NumRandPoints:
    let angle = float(i) * 2.399963  # golden angle ~137.5°
    let radius = CandidateRadius * 0.5 * (1.0 + float(i) / float(NumRandPoints))
    let p = clampToArena(vec2(bot.x + radius * cos(angle),
                               bot.y + radius * sin(angle)), w, h)
    let r = scorePoint(p, bot, threats, w, h)
    if r < bestRisk:
      bestRisk = r
      best     = p

  m.targetX  = best.x
  m.targetY  = best.y
  m.hasTarget = true

proc computeMove*(m: var MinimumRiskModule, ws: WorldState): MoveCommand =
  inc m.ticksSinceCalc
  if m.ticksSinceCalc >= RecalcInterval or not m.hasTarget:
    m.recalcTarget(ws)
    m.ticksSinceCalc = 0

  let bot    = vec2(ws.selfX, ws.selfY)
  let target = vec2(m.targetX, m.targetY)
  let d      = dist(bot, target)

  if d < 5.0:
    # Already at target — force recalc next tick
    m.hasTarget = false
    return (speed: 0.0, turnRate: 0.0)

  # Direction to target (math convention: 0=East, CCW+)
  let toTargetRad = arctan2(target.y - bot.y, target.x - bot.x)
  let toTargetDeg = radToDeg(toTargetRad)

  # Delta from current body heading
  var delta = toTargetDeg - ws.selfHeading
  while delta >  180.0: delta -= 360.0
  while delta < -180.0: delta += 360.0

  # Dot-product trick: reverse if |delta| > 90 to save turning
  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))