/* test-subpix.mjs — does sub-pixel refinement actually help, and by how much?
 *
 *   node tools/test-subpix.mjs
 *
 * Renders synthetic ArUco-like markers at known sub-pixel corner positions
 * (anti-aliased by 4x4 supersampling, with optional sensor noise and defocus),
 * quantises the corners the way a contour tracer does, then measures RMS corner
 * error before and after refineQuad.
 *
 * This exists because "the ghosts look steadier" is not a measurement. If a
 * tuning change in subpix.js makes this number worse, it was not an improvement.
 */
import { refineQuad, DEFAULT_OPTS } from '../public/js/ar/subpix.js';

// ---------- unit square -> quad homography (Heckbert) ----------
function squareToQuad(q) {
  const [p0, p1, p2, p3] = q;                       // (0,0) (1,0) (1,1) (0,1)
  const sx = p0.x - p1.x + p2.x - p3.x;
  const sy = p0.y - p1.y + p2.y - p3.y;
  let a, b, c, d, e, f, g, h;
  if (Math.abs(sx) < 1e-12 && Math.abs(sy) < 1e-12) {
    a = p1.x - p0.x; b = p2.x - p1.x; c = p0.x;
    d = p1.y - p0.y; e = p2.y - p1.y; f = p0.y;
    g = 0; h = 0;
  } else {
    const dx1 = p1.x - p2.x, dx2 = p3.x - p2.x;
    const dy1 = p1.y - p2.y, dy2 = p3.y - p2.y;
    const den = dx1 * dy2 - dx2 * dy1;
    g = (sx * dy2 - dx2 * sy) / den;
    h = (dx1 * sy - sx * dy1) / den;
    a = p1.x - p0.x + g * p1.x; b = p3.x - p0.x + h * p3.x; c = p0.x;
    d = p1.y - p0.y + g * p1.y; e = p3.y - p0.y + h * p3.y; f = p0.y;
  }
  return [a, b, c, d, e, f, g, h, 1];
}

function inv3(m) {
  const [a, b, c, d, e, f, g, h, i] = m;
  const A = e * i - f * h, B = -(d * i - f * g), C = d * h - e * g;
  const det = a * A + b * B + c * C;
  return [A / det, -(b * i - c * h) / det, (b * f - c * e) / det,
          B / det, (a * i - c * g) / det, -(a * f - c * d) / det,
          C / det, -(a * h - b * g) / det, (a * e - b * d) / det];
}

function apply(m, x, y) {
  const w = m[6] * x + m[7] * y + m[8];
  return { x: (m[0] * x + m[1] * y + m[2]) / w, y: (m[3] * x + m[4] * y + m[5]) / w };
}

// ---------- synthetic marker render ----------
function renderMarker(W, H, quad, { bits, noise = 0, blur = 0, black = 28, white = 226 }) {
  const Hm = inv3(squareToQuad(quad));
  const d = new Uint8ClampedArray(W * H * 4);
  const SS = 4, inv = 1 / (SS * SS);
  for (let py = 0; py < H; py++) {
    for (let px = 0; px < W; px++) {
      let acc = 0;
      for (let sy = 0; sy < SS; sy++) {
        for (let sx = 0; sx < SS; sx++) {
          /* Pixel CENTRES sit on integer coordinates — the convention js-aruco2
           * corners and OpenCV intrinsics both use, and the one subpix.js samples
           * with. Integrating over [px, px+1] instead would bake a half-pixel
           * offset into the ground truth and make a correct refiner look biased. */
          const p = apply(Hm, px - 0.5 + (sx + 0.5) / SS, py - 0.5 + (sy + 0.5) / SS);
          const u = p.x * 6, v = p.y * 6;           // 6x6 cells: 4x4 payload + border
          if (u < 0 || v < 0 || u >= 6 || v >= 6) { acc += white; continue; }
          const cu = u | 0, cv = v | 0;
          const border = cu === 0 || cv === 0 || cu === 5 || cv === 5;
          acc += border ? black : (bits[(cv - 1) * 4 + (cu - 1)] ? white : black);
        }
      }
      let g = acc * inv;
      if (noise) g += (Math.random() + Math.random() + Math.random() - 1.5) * noise;
      const i = (py * W + px) << 2;
      d[i] = d[i + 1] = d[i + 2] = g; d[i + 3] = 255;
    }
  }
  if (blur > 0) boxBlur(d, W, H, blur);
  return { data: d, width: W, height: H };
}

/* Separable box blur, repeated — a cheap stand-in for lens defocus. */
function boxBlur(d, W, H, r) {
  const tmp = new Float64Array(W * H);
  for (let pass = 0; pass < 2; pass++) {
    for (let y = 0; y < H; y++) for (let x = 0; x < W; x++) {
      let s = 0, n = 0;
      for (let k = -r; k <= r; k++) { const xx = x + k; if (xx >= 0 && xx < W) { s += d[((y * W + xx) << 2)]; n++; } }
      tmp[y * W + x] = s / n;
    }
    for (let y = 0; y < H; y++) for (let x = 0; x < W; x++) {
      let s = 0, n = 0;
      for (let k = -r; k <= r; k++) { const yy = y + k; if (yy >= 0 && yy < H) { s += tmp[yy * W + x]; n++; } }
      const i = (y * W + x) << 2;
      d[i] = d[i + 1] = d[i + 2] = s / n;
    }
  }
}

// ---------- scenarios ----------
const rnd = (() => { let s = 1234567; return () => (s = (s * 1103515245 + 12345) & 0x7fffffff) / 0x7fffffff; })();

function quadAt(cx, cy, size, rot, persp) {
  const base = [[-0.5, -0.5], [0.5, -0.5], [0.5, 0.5], [-0.5, 0.5]];
  const c = Math.cos(rot), s = Math.sin(rot);
  return base.map(([x, y]) => {
    const sc = size * (1 + persp * y);              // fake foreshortening
    return { x: cx + (x * c - y * s) * sc, y: cy + (x * s + y * c) * sc };
  });
}

function rms(a, b) {
  let s = 0;
  for (let i = 0; i < 4; i++) s += (a[i].x - b[i].x) ** 2 + (a[i].y - b[i].y) ** 2;
  return Math.sqrt(s / 4);
}

const cases = [
  { name: 'large, square-on       (140px)', size: 140, rot: 0.00, persp: 0.00, noise: 0, blur: 0 },
  { name: 'large, rotated 20 deg  (140px)', size: 140, rot: 0.35, persp: 0.00, noise: 0, blur: 0 },
  { name: 'oblique, foreshortened (140px)', size: 140, rot: 0.20, persp: 0.28, noise: 0, blur: 0 },
  { name: 'medium                  (90px)', size: 90,  rot: 0.25, persp: 0.15, noise: 0, blur: 0 },
  { name: 'small                   (52px)', size: 52,  rot: 0.18, persp: 0.10, noise: 0, blur: 0 },
  { name: 'noisy  (sigma 8 luma)  (110px)', size: 110, rot: 0.22, persp: 0.12, noise: 8, blur: 0 },
  { name: 'defocused (r=1 box x2) (110px)', size: 110, rot: 0.22, persp: 0.12, noise: 0, blur: 1 },
  { name: 'noisy + defocused      (110px)', size: 110, rot: 0.22, persp: 0.12, noise: 6, blur: 1 },
];

const TRIALS = 24;
console.log('sub-pixel corner refinement — RMS corner error vs ground truth (px)\n');
console.log('scenario                          raw    refined   gain   refined/total');
console.log('-'.repeat(74));

let totRaw = 0, totRef = 0, totN = 0, totTried = 0, totOk = 0;

for (const c of cases) {
  let sumRaw = 0, sumRef = 0, n = 0, ok = 0;
  for (let t = 0; t < TRIALS; t++) {
    const W = 256, H = 256;
    // sub-pixel placement: the centre lands on a fractional pixel every trial
    const quad = quadAt(128 + rnd() - 0.5, 128 + rnd() - 0.5, c.size, c.rot + (rnd() - 0.5) * 0.1, c.persp);
    const bits = Array.from({ length: 16 }, () => (rnd() > 0.45 ? 1 : 0));
    const img = renderMarker(W, H, quad, { bits, noise: c.noise, blur: c.blur });

    /* What a contour tracer hands you: corners on the pixel lattice. The extra
     * +/-0.5 jitter stands in for the tracer picking a neighbouring lattice
     * point on a staircased edge. */
    const raw = quad.map(p => ({
      x: Math.round(p.x + (rnd() - 0.5)),
      y: Math.round(p.y + (rnd() - 0.5)),
    }));

    const ref = refineQuad(img, raw, DEFAULT_OPTS);
    sumRaw += rms(raw, quad);
    sumRef += rms(ref || raw, quad);
    if (ref) ok++;
    n++;
  }
  const r = sumRaw / n, f = sumRef / n;
  totRaw += sumRaw; totRef += sumRef; totN += n; totTried += n; totOk += ok;
  console.log(
    c.name.padEnd(32) +
    r.toFixed(3).padStart(7) +
    f.toFixed(3).padStart(9) +
    (r / f).toFixed(2).padStart(7) + 'x' +
    `${ok}/${n}`.padStart(13)
  );
}

console.log('-'.repeat(74));
const R = totRaw / totN, F = totRef / totN;
console.log('overall'.padEnd(32) + R.toFixed(3).padStart(7) + F.toFixed(3).padStart(9) +
            (R / F).toFixed(2).padStart(7) + 'x' + `${totOk}/${totTried}`.padStart(13));
console.log('\nrefined/total is the accept rate: refinement that fails its guards');
console.log('returns the original corners, so a low rate costs nothing but gains nothing.');