import type { Vec3 } from '../types';

/** Tolerance used for geometric comparisons throughout the router (ft). */
export const EPS = 1e-6;

export const add = (a: Vec3, b: Vec3): Vec3 => [a[0] + b[0], a[1] + b[1], a[2] + b[2]];
export const sub = (a: Vec3, b: Vec3): Vec3 => [a[0] - b[0], a[1] - b[1], a[2] - b[2]];
export const scale = (a: Vec3, s: number): Vec3 => [a[0] * s, a[1] * s, a[2] * s];
export const dot = (a: Vec3, b: Vec3) => a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
export const cross = (a: Vec3, b: Vec3): Vec3 => [
  a[1] * b[2] - a[2] * b[1],
  a[2] * b[0] - a[0] * b[2],
  a[0] * b[1] - a[1] * b[0],
];
export const length = (a: Vec3) => Math.hypot(a[0], a[1], a[2]);
export const dist = (a: Vec3, b: Vec3) => Math.hypot(a[0] - b[0], a[1] - b[1], a[2] - b[2]);
export const manhattan = (a: Vec3, b: Vec3) =>
  Math.abs(a[0] - b[0]) + Math.abs(a[1] - b[1]) + Math.abs(a[2] - b[2]);
export const vecEquals = (a: Vec3, b: Vec3, eps = EPS) =>
  Math.abs(a[0] - b[0]) < eps && Math.abs(a[1] - b[1]) < eps && Math.abs(a[2] - b[2]) < eps;

/**
 * The six axis-aligned unit directions. Index layout: even = positive,
 * odd = negative, so `idx >> 1` is the axis (0=X 1=Y 2=Z) and `idx ^ 1`
 * is the reversed direction.
 */
export const AXIS_DIRS: readonly Vec3[] = [
  [1, 0, 0],
  [-1, 0, 0],
  [0, 1, 0],
  [0, -1, 0],
  [0, 0, 1],
  [0, 0, -1],
];

export const axisOfDir = (dirIdx: number) => dirIdx >> 1;
export const signOfDir = (dirIdx: number) => (dirIdx & 1 ? -1 : 1);
export const reverseDir = (dirIdx: number) => dirIdx ^ 1;

/** Index into AXIS_DIRS for an (exactly) axis-aligned unit vector, or -1. */
export function dirIndexOf(v: Vec3, eps = EPS): number {
  for (let i = 0; i < 6; i++) {
    const d = AXIS_DIRS[i];
    if (Math.abs(v[0] - d[0]) < eps && Math.abs(v[1] - d[1]) < eps && Math.abs(v[2] - d[2]) < eps) {
      return i;
    }
  }
  return -1;
}

/**
 * Snap a roughly axis-aligned direction to the nearest exact axis direction.
 * Returns null when the vector is degenerate or too far from any axis
 * (more than ~25° off).
 */
export function snapAxisDir(v: Vec3): Vec3 | null {
  const l = length(v);
  if (l < EPS) return null;
  const n: Vec3 = [v[0] / l, v[1] / l, v[2] / l];
  let best = -1;
  let bestDot = 0.9; // cos(~25°) — refuse wildly diagonal directions
  for (let i = 0; i < 6; i++) {
    const d = dot(n, AXIS_DIRS[i]);
    if (d > bestDot) {
      bestDot = d;
      best = i;
    }
  }
  return best >= 0 ? ([...AXIS_DIRS[best]] as Vec3) : null;
}