import type { GestureSignal, Landmark3D, Point2D, StepConfig } from '../types'

export type StepSignal = 'step-left' | 'step-right' | 'step-forward' | 'step-back'

export interface StepState {
  activeStep: GestureSignal
  steppedFoot: 'left' | 'right' | null
  heelBaseL: Point2D | null
  heelBaseR: Point2D | null
}

export function createStepState(): StepState {
  return { activeStep: null, steppedFoot: null, heelBaseL: null, heelBaseR: null }
}

export interface StepResult {
  signal: StepSignal
  foot: 'left' | 'right'
}

/**
 * Detect which direction a foot stepped, if any. Uses toes (31/32).
 * worldLandmarks: X+ = subject's left, Z+ = toward camera (forward)
 */
export function detectStep(
  worldLandmarks: Landmark3D[],
  toeBaseL: Point2D,
  toeBaseR: Point2D,
  cfg: StepConfig,
): StepResult | null {
  const lt = worldLandmarks[31], rt = worldLandmarks[32]
  const ldx = lt.x - toeBaseL.x, ldz = lt.z - toeBaseL.z
  const rdx = rt.x - toeBaseR.x, rdz = rt.z - toeBaseR.z
  const lDist = Math.sqrt(ldx * ldx + ldz * ldz)
  const rDist = Math.sqrt(rdx * rdx + rdz * rdz)

  const foot: 'left' | 'right' = lDist > rDist ? 'left' : 'right'
  const dx = foot === 'left' ? ldx : rdx
  const dz = foot === 'left' ? ldz : rdz

  const xOver = Math.abs(dx) >= cfg.thresholdX
  const zOver = Math.abs(dz) >= cfg.thresholdZ
  if (!xOver && !zOver) return null

  let signal: StepSignal
  if (Math.abs(dx) > Math.abs(dz) && xOver) {
    signal = dx > 0 ? 'step-left' : 'step-right'
  } else if (zOver) {
    signal = dz > 0 ? 'step-back' : 'step-forward'
  } else {
    signal = dx > 0 ? 'step-left' : 'step-right'
  }
  return { signal, foot }
}

/** Reset checks only the triggering foot on the triggering axis. */
export function checkStepReset(
  worldLandmarks: Landmark3D[],
  toeBaseL: Point2D,
  toeBaseR: Point2D,
  cfg: StepConfig,
  activeStep: GestureSignal,
  steppedFoot: 'left' | 'right',
): boolean {
  const lt = worldLandmarks[31], rt = worldLandmarks[32]
  const dx = steppedFoot === 'left' ? lt.x - toeBaseL.x : rt.x - toeBaseR.x
  const dz = steppedFoot === 'left' ? lt.z - toeBaseL.z : rt.z - toeBaseR.z

  if (activeStep === 'step-left' || activeStep === 'step-right') {
    return Math.abs(dx) < cfg.reset
  }
  return Math.abs(dz) < cfg.reset
}

/** Slowly blend baseline toward current position (drift correction). */
export function adaptBaseline(base: Point2D, current: Point2D, alpha = 0.01): Point2D {
  return {
    x: base.x * (1 - alpha) + current.x * alpha,
    z: base.z * (1 - alpha) + current.z * alpha,
  }
}
