// Small dense linear algebra: symmetric eigenvalues by cyclic Jacobi, and a least-squares plane fit.

/** Eigenvalues (ascending) of a symmetric n x n matrix given row-major; the input is copied. */
export function symmetricEigenvalues(A: Float64Array | number[], n: number): Float64Array {
  const a = Float64Array.from(A);
  for (let sweep = 0; sweep < 60; sweep++) {
    let off = 0;
    for (let p = 0; p < n; p++) for (let q = p + 1; q < n; q++) off += a[p * n + q] * a[p * n + q];
    let diag = 0;
    for (let p = 0; p < n; p++) diag += a[p * n + p] * a[p * n + p];
    if (off <= 1e-30 * (diag + 1e-300)) break;
    for (let p = 0; p < n; p++) {
      for (let q = p + 1; q < n; q++) {
        const apq = a[p * n + q];
        if (apq === 0) continue;
        const app = a[p * n + p], aqq = a[q * n + q];
        const theta = (aqq - app) / (2 * apq);
        const t = Math.sign(theta || 1) / (Math.abs(theta) + Math.sqrt(theta * theta + 1));
        const c = 1 / Math.sqrt(t * t + 1), s = t * c;
        for (let k = 0; k < n; k++) {
          const akp = a[k * n + p], akq = a[k * n + q];
          a[k * n + p] = c * akp - s * akq; a[k * n + q] = s * akp + c * akq;
        }
        for (let k = 0; k < n; k++) {
          const apk = a[p * n + k], aqk = a[q * n + k];
          a[p * n + k] = c * apk - s * aqk; a[q * n + k] = s * apk + c * aqk;
        }
      }
    }
  }
  const ev = new Float64Array(n);
  for (let i = 0; i < n; i++) ev[i] = a[i * n + i];
  return ev.sort();
}

/** Inverse of an n x n matrix (row-major) by Gauss-Jordan elimination with partial pivoting. */
export function invertMatrix(A: ArrayLike<number>, n: number): Float64Array {
  const a = Array.from({ length: n }, (_, i) => [...Array.from({ length: n }, (_, j) => A[n * i + j]), ...Array.from({ length: n }, (_, j) => (i === j ? 1 : 0))]);
  for (let c = 0; c < n; c++) {
    let piv = c;
    for (let r = c + 1; r < n; r++) if (Math.abs(a[r][c]) > Math.abs(a[piv][c])) piv = r;
    [a[c], a[piv]] = [a[piv], a[c]];
    const d = a[c][c];
    for (let j = 0; j < 2 * n; j++) a[c][j] /= d;
    for (let r = 0; r < n; r++) if (r !== c) { const f = a[r][c]; for (let j = 0; j < 2 * n; j++) a[r][j] -= f * a[c][j]; }
  }
  return Float64Array.from(a.flatMap((row) => row.slice(n)));
}

/** Solve the 3 x 3 system A x = b (row-major A) by Gaussian elimination with pivoting. */
export function solve3(A: number[], b: number[]): number[] {
  const M = [A.slice(0, 3).concat(b[0]), A.slice(3, 6).concat(b[1]), A.slice(6, 9).concat(b[2])];
  for (let c = 0; c < 3; c++) {
    let piv = c;
    for (let r = c + 1; r < 3; r++) if (Math.abs(M[r][c]) > Math.abs(M[piv][c])) piv = r;
    [M[c], M[piv]] = [M[piv], M[c]];
    if (Math.abs(M[c][c]) < 1e-300) throw new Error("singular 3x3 system");
    for (let r = 0; r < 3; r++) {
      if (r === c) continue;
      const f = M[r][c] / M[c][c];
      for (let k = c; k < 4; k++) M[r][k] -= f * M[c][k];
    }
  }
  return [M[0][3] / M[0][0], M[1][3] / M[1][1], M[2][3] / M[2][2]];
}

/** Least-squares fit z = a x + b y + c through points (x_i, y_i, z_i); returns the residuals z - fit.
 *
 * The fit is done by Gram-Schmidt on the columns (1, x, y), dropping a column that is (nearly)
 * dependent on the previous ones, which is what numpy's minimum-norm least squares does in the
 * Python package: the collinear points of a stick's line get a line fit, a single point a zero
 * residual. The normal equations would be singular in those cases. */
export function planeResiduals(x: ArrayLike<number>, y: ArrayLike<number>, z: ArrayLike<number>): Float64Array {
  const n = x.length;
  const res = Float64Array.from(z as number[]);
  const basis: Float64Array[] = [];
  for (const col of [Array.from({ length: n }, () => 1), Array.from(x), Array.from(y)]) {
    const v = Float64Array.from(col);
    let norm0 = 0;
    for (let i = 0; i < n; i++) norm0 += v[i] * v[i];
    for (const b of basis) {
      let d = 0;
      for (let i = 0; i < n; i++) d += b[i] * v[i];
      for (let i = 0; i < n; i++) v[i] -= d * b[i];
    }
    let norm = 0;
    for (let i = 0; i < n; i++) norm += v[i] * v[i];
    if (norm === 0 || norm <= 1e-20 * norm0) continue;
    const inv = 1 / Math.sqrt(norm);
    for (let i = 0; i < n; i++) v[i] *= inv;
    basis.push(v);
  }
  for (const b of basis) {
    let d = 0;
    for (let i = 0; i < n; i++) d += b[i] * res[i];
    for (let i = 0; i < n; i++) res[i] -= d * b[i];
  }
  return res;
}