import { HOG_LINE_Y, MAX_SPEED, MIN_SPEED, SAMPLE_RATE_HZ } from './protocol'

/** Path samples are (x, y, theta). Time is sample index / SAMPLE_RATE_HZ. */
export function sampleTime(index: number): number {
  return index / SAMPLE_RATE_HZ
}

export function trimPathToStartAtHogLine(
  path: [number, number, number][],
): [number, number, number][] {
  if (path.length < 2) return path
  const idx = path.findIndex(([, y]) => y >= HOG_LINE_Y)
  if (idx < 0) return path
  // Start just before the hog line crossing so the stone enters smoothly.
  // Theta is preserved; time is rebased via sample index on the trimmed array.
  const start = Math.max(0, idx - 1)
  return path.slice(start)
}

/** Index at which a path should start for hog-line sync (same as trim start). */
export function hogTrimStartIndex(path: [number, number, number][]): number {
  if (path.length < 2) return 0
  const idx = path.findIndex(([, y]) => y >= HOG_LINE_Y)
  if (idx < 0) return 0
  return Math.max(0, idx - 1)
}

export function velocityToWeight(velocity: number): number {
  const t = (velocity - MIN_SPEED) / (MAX_SPEED - MIN_SPEED)
  const weight = 1 + Math.round(t * 9)
  return Math.max(1, Math.min(10, weight))
}

export function weightToVelocity(weight: number): number {
  const clamped = Math.max(1, Math.min(10, weight))
  const t = (clamped - 1) / 9
  return MIN_SPEED + t * (MAX_SPEED - MIN_SPEED)
}