/* geom.js — is this layout's crest geometry actually solvable without ambiguity?
 *
 * A coplanar set of markers admits two camera poses that reproject almost
 * identically: the true one and a mirrored twin. cv-worker.js suppresses the
 * twin with physics gates (camera above the table, no teleporting), but those
 * are a safety net over a geometry problem. The cure is a crest that does not
 * share the plane of the others — the A3 test rig does this with a 160mm tower
 * carrying crests at 10cm and 16cm.
 *
 * The thing is, "the anchors are coplanar" is invisible. A layout saved from the
 * editor, or a tower knocked over and re-entered as flat, looks completely
 * normal in the JSON and only shows up later as ghosts intermittently flipping
 * to the wrong side of the table — the most expensive bug in this project to
 * date. So: measure it once when the scene loads, and put the number on screen.
 *
 * The measure is the RMS distance of every anchor CORNER from the best-fit plane
 * through all of them. Corners rather than anchor centres, because a crest
 * mounted vertically among flat ones contributes out-of-plane spread through its
 * own extent even if its centre happens to sit near the common plane.
 */

/* Eigen-decomposition of a symmetric 3x3 by cyclic Jacobi. Small, exact enough,
 * and no dependency — three.js has no symmetric eigensolver. */
function jacobiEigen(m) {
  const a = [[m[0], m[1], m[2]], [m[1], m[3], m[4]], [m[2], m[4], m[5]]];
  const v = [[1, 0, 0], [0, 1, 0], [0, 0, 1]];
  for (let sweep = 0; sweep < 24; sweep++) {
    let off = 0;
    for (let p = 0; p < 2; p++) for (let q = p + 1; q < 3; q++) off += a[p][q] * a[p][q];
    if (off < 1e-20) break;
    for (let p = 0; p < 2; p++) {
      for (let q = p + 1; q < 3; q++) {
        if (Math.abs(a[p][q]) < 1e-18) continue;
        const theta = (a[q][q] - a[p][p]) / (2 * a[p][q]);
        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 < 3; k++) {
          const akp = a[k][p], akq = a[k][q];
          a[k][p] = c * akp - s * akq;
          a[k][q] = s * akp + c * akq;
        }
        for (let k = 0; k < 3; k++) {
          const apk = a[p][k], aqk = a[q][k];
          a[p][k] = c * apk - s * aqk;
          a[q][k] = s * apk + c * aqk;
        }
        for (let k = 0; k < 3; k++) {
          const vkp = v[k][p], vkq = v[k][q];
          v[k][p] = c * vkp - s * vkq;
          v[k][q] = s * vkp + c * vkq;
        }
      }
    }
  }
  return { values: [a[0][0], a[1][1], a[2][2]], vectors: v };
}

/* points: flat [x,y,z, x,y,z, ...] in world cm.
 * Returns null if there is nothing meaningful to measure. */
export function planarity(points) {
  const n = points.length / 3;
  if (n < 3) return null;

  let cx = 0, cy = 0, cz = 0;
  for (let i = 0; i < n; i++) { cx += points[3 * i]; cy += points[3 * i + 1]; cz += points[3 * i + 2]; }
  cx /= n; cy /= n; cz /= n;

  let xx = 0, xy = 0, xz = 0, yy = 0, yz = 0, zz = 0;
  for (let i = 0; i < n; i++) {
    const dx = points[3 * i] - cx, dy = points[3 * i + 1] - cy, dz = points[3 * i + 2] - cz;
    xx += dx * dx; xy += dx * dy; xz += dx * dz;
    yy += dy * dy; yz += dy * dz; zz += dz * dz;
  }
  const { values, vectors } = jacobiEigen([xx, xy, xz, yy, yz, zz]);

  // smallest eigenvalue -> normal of the best-fit plane
  let k = 0;
  if (values[1] < values[k]) k = 1;
  if (values[2] < values[k]) k = 2;
  const nx = vectors[0][k], ny = vectors[1][k], nz = vectors[2][k];

  let s2 = 0, mx = 0;
  for (let i = 0; i < n; i++) {
    const d = (points[3 * i] - cx) * nx + (points[3 * i + 1] - cy) * ny + (points[3 * i + 2] - cz) * nz;
    s2 += d * d;
    if (Math.abs(d) > mx) mx = Math.abs(d);
  }
  return {
    points: n,
    rmsCm: Math.sqrt(s2 / n),
    maxCm: mx,
    normal: [nx, ny, nz],
  };
}

/* COPLANAR_CM: below this the layout is effectively flat and the mirrored pose
 * is live. The A3 rig's tower gives ~5cm RMS; print tolerance and a crest taped
 * slightly proud of the table give well under 0.5cm, so 1.5cm separates a real
 * height difference from noise without being so strict that a modest raised
 * crest fails to register. */
export const COPLANAR_CM = 1.5;

export function describePlanarity(p) {
  if (!p) return 'geom —';
  if (p.rmsCm < COPLANAR_CM) return `geom COPLANAR ${p.rmsCm.toFixed(1)}cm — raise a crest`;
  return `geom spread ${p.rmsCm.toFixed(1)}cm (max ${p.maxCm.toFixed(1)})`;
}