/**
 * SPSIN / SPCOS from the Apollo 11 Lunar Module software (Luminary099,
 * SINGLE_PRECISION_SUBROUTINES.agc), rewritten in TypeScript.
 *
 * Units, as in the original:
 *   - The angle is a fraction of half a turn: x / 16384 * π radians,
 *     x in -16383 … 16383 (14-bit fixed point, "scaled at π").
 *   - The result is a fraction: y / 16384 (14-bit fixed point, "scaled at 1").
 *
 * The original computes sine with a fifth-order odd polynomial in the angle
 * scaled at π/2, after folding angles beyond ±90° back into range. The
 * coefficients are the flight constants C1/2, C3/2 and C5/2. Every multiply
 * keeps the high word of an exact product, which truncates toward zero.
 */

const ONE = 16384 // 1.0 in 14-bit fixed point
const MAX = ONE - 1 // the largest magnitude a single AGC word holds

// Flight constants, halved, from FIXED_FIXED_CONSTANT_POOL.agc.
const C1 = 12867 // DEC  .7853134  (octal 31103)  ≈ π/4
const C3 = -5269 // DEC -.3216147  (octal 65552)  ≈ -(π/2)³/12
const C5 = 596 // DEC  .0363551  (octal 01124)  ≈ (π/2)⁵/240

/** High word of an exact 14-bit × 14-bit product: truncates toward zero. */
const mul = (a: number, b: number) => Math.trunc((a * b) / ONE)

/** sin of an angle already folded into ±90°, t scaled at π/2. */
function polynomial(t: number): number {
  const t2 = mul(t, t)
  const a = mul(t2, C5) + C3
  const inner = mul(a, t2) + C1
  // The original doubles the full double-length product (DDOUBL), so the
  // doubling happens before truncation, not after.
  const y = Math.trunc((2 * inner * t) / ONE)
  // A result past full scale saturates, as the original's ARG90 exit does.
  return Math.max(-MAX, Math.min(MAX, y))
}

/** Fold an angle scaled at π into ±90°, scaled at π/2. */
function fold(x: number): number | 'full' {
  const t = 2 * x
  if (Math.abs(t) <= MAX) return t
  // Beyond 90°: reflect, sin(π - θ) = sin θ. The AGC's ones'-complement
  // arithmetic makes the mirror point 32768, not 32767.
  const r = t > 0 ? 32768 - t : -32768 - t
  // Exactly ±90° lands one past full scale: the original returns ±1 there.
  if (Math.abs(r) > MAX) return 'full'
  return r
}

export function spsin(x: number): number {
  const t = fold(x)
  if (t === 'full') return x > 0 ? MAX : -MAX
  return polynomial(t)
}

export function spcos(x: number): number {
  // cos θ = sin(θ + 90°). Past +180° the original wraps the shifted angle
  // back by a half turn and negates it, which is the same as 90° - θ.
  const shifted = x + ONE / 2
  return spsin(shifted <= MAX ? shifted : ONE / 2 - x)
}

/** The rewrite most developers would write first. */
export const naiveSin = (x: number) => Math.round(Math.sin((x / ONE) * Math.PI) * ONE)
export const naiveCos = (x: number) => Math.min(MAX, Math.round(Math.cos((x / ONE) * Math.PI) * ONE))