# ponytail: grid accumulator replaces ring buffer; add temporal features when needed (step 3)

import std/math

const
  VPERP_BINS* = 17    # -8 to +8 inclusive (integer speed units)
  DIST_BINS*  = 8     # distance bands
  DIST_BAND*  = 125.0 # pixels per band

type
  GridCell = object
    sumSin: float
    sumCos: float
    count:  int

  LeadGrid* = object
    cells: array[VPERP_BINS * DIST_BINS, GridCell]  # 17 × 8 = 136 cells

proc initLeadGrid*(): LeadGrid =
  result = LeadGrid()

proc cellIndex(vPerp: float, distance: float): int {.inline.} =
  let vBin = clamp(int(vPerp + 8.5), 0, VPERP_BINS - 1)
  let dBin = clamp(int(distance / DIST_BAND), 0, DIST_BINS - 1)
  result = vBin * DIST_BINS + dBin

proc forward*(grid: var LeadGrid, vPerp, distance: float): float =
  ## Returns circular mean offset in degrees, or -999.0 when no data.
  let idx = cellIndex(vPerp, distance)
  let vBin = clamp(int(vPerp + 8.5), 0, VPERP_BINS - 1)
  let dBin = clamp(int(distance / DIST_BAND), 0, DIST_BINS - 1)

  # Accumulate weighted contributions: direct cell (w=1), cardinal neighbors (w=0.5), diagonals (w=0.25)
  var sinSum = 0.0
  var cosSum = 0.0
  var totalW = 0.0
  for dv in -1 .. 1:
    for dd in -1 .. 1:
      let vb = vBin + dv
      let db = dBin + dd
      if vb < 0 or vb >= VPERP_BINS or db < 0 or db >= DIST_BINS: continue
      let c = grid.cells[vb * DIST_BINS + db]
      if c.count == 0: continue
      let w = if dv == 0 and dd == 0: 1.0
              elif dv == 0 or dd == 0: 0.5
              else: 0.25
      sinSum += w * c.sumSin / float(c.count)
      cosSum += w * c.sumCos / float(c.count)
      totalW += w
  if totalW == 0.0:
    return -999.0
  result = radToDeg(arctan2(sinSum, cosSum))

proc learn*(grid: var LeadGrid, vPerp, distance, correctOffset: float) =
  let idx = cellIndex(vPerp, distance)
  grid.cells[idx].sumSin += sin(degToRad(correctOffset))
  grid.cells[idx].sumCos += cos(degToRad(correctOffset))
  inc grid.cells[idx].count

proc totalCount*(grid: LeadGrid): int =
  for c in grid.cells:
    result += c.count