/* subpix.js — sub-pixel refinement of ArUco quad corners.
 *
 * WHY THIS EXISTS AND NOT cv.cornerSubPix: the pinned build
 * (@techstark/opencv-js@4.12.0-release.1) does not export cornerSubPix — probed
 * directly against the module, not assumed. `solvePnPRefineLM` IS exported, but
 * that refines a pose from corners; it cannot improve the corners themselves.
 *
 * WHY IT MATTERS: js-aruco2 hands back contour corners quantised to roughly
 * whole pixels of the detection canvas. Every downstream metric inherits that
 * quantisation — pose jitter, reprojection error, and the gate thresholds tuned
 * against them. At 960px long-side a 60mm crest spans ~140px, so one pixel of
 * corner error is ~0.7% of the marker, which at typical table distance is a few
 * millimetres of ghost placement, flickering frame to frame.
 *
 * METHOD — edge fitting, not corner finding. A corner operator (which is what
 * cornerSubPix is) looks at a small window around one point and is easily pulled
 * by the marker's own inner modules. Instead we use the whole length of each
 * border edge:
 *
 *   1. walk along each edge of the quad, skipping the ends
 *   2. at each step, scan perpendicular to the edge and find the sub-pixel peak
 *      of the intensity gradient (parabolic fit on the three samples around the
 *      maximum) — that is the true border crossing at that point
 *   3. fit a line through those crossings by total least squares, with one
 *      outlier rejection pass
 *   4. intersect consecutive edge lines to get the refined corners
 *
 * Averaging ~12 independent crossings per edge beats any single-window estimate,
 * and the corner comes from two long lines rather than from pixels near the
 * corner itself — which is exactly where a printed marker is least reliable
 * (ink bleed, rounding, and the contour tracer's own staircase).
 *
 * SAFETY: refinement is all-or-nothing per marker. If any edge is too short, too
 * low-contrast, too few inliers, near-parallel to its neighbour, or if any
 * corner would move further than maxShiftPx, the ORIGINAL corners are returned
 * untouched. A bad refinement is worse than none, because the pose gates in
 * cv-worker.js are tuned against honest corner noise.
 *
 * COST: samples are taken straight from the RGBA ImageData with bilinear luma,
 * so there is no grayscale pre-pass over the frame. Per marker: 4 edges x ~12
 * samples x ~17 taps ~= 800 taps. Five markers is ~4k taps against the 518k
 * pixels the detector has already touched — under 1% of detection cost.
 */

const EPS = 1e-9;

export const DEFAULT_OPTS = {
  edgeInset: 0.18,       // skip this fraction at each end of an edge (corners lie)
  samplePitchPx: 9,      // one crossing per ~9px of edge
  minSamples: 5,
  maxSamples: 22,
  scanCellFrac: 0.42,    // scan +/- this fraction of ONE marker cell, so the
                         // scan can never reach the next black/white transition
  minScanPx: 1.5,
  maxScanPx: 7,
  scanStepPx: 0.5,
  minContrast: 24,       // luma range across a scan; below this it's flat/noise
  recentreIters: 2,      // re-centre the scan window on the estimate, then redo
  maxOffFrac: 0.75,      // crossing further than this fraction of R => not our edge
  sigmaMaxFrac: 0.55,    // gradient spread, as a fraction of R ...
  sigmaMaxPx: 2.4,       // ... and an absolute ceiling, so heavy defocus still passes
  minPoints: 4,          // inlier crossings needed to trust an edge
  outlierPx: 1.4,
  minEdgePx: 24,         // below this a marker is too small to refine usefully
  maxShiftPx: 3.0,       // refinement that moves a corner further than this is
                         // not refinement, it's a different quad
};

/* Bilinear luma sample from RGBA ImageData. NaN outside the image. */
function lumaAt(d, w, h, x, y) {
  if (!(x >= 0 && y >= 0 && x <= w - 1 && y <= h - 1)) return NaN;
  const x0 = x | 0, y0 = y | 0;
  const x1 = x0 + 1 < w ? x0 + 1 : x0;
  const y1 = y0 + 1 < h ? y0 + 1 : y0;
  const fx = x - x0, fy = y - y0;
  const i00 = (y0 * w + x0) << 2, i10 = (y0 * w + x1) << 2;
  const i01 = (y1 * w + x0) << 2, i11 = (y1 * w + x1) << 2;
  const g00 = 0.299 * d[i00] + 0.587 * d[i00 + 1] + 0.114 * d[i00 + 2];
  const g10 = 0.299 * d[i10] + 0.587 * d[i10 + 1] + 0.114 * d[i10 + 2];
  const g01 = 0.299 * d[i01] + 0.587 * d[i01 + 1] + 0.114 * d[i01 + 2];
  const g11 = 0.299 * d[i11] + 0.587 * d[i11 + 1] + 0.114 * d[i11 + 2];
  const a = g00 + (g10 - g00) * fx;
  const b = g01 + (g11 - g01) * fx;
  return a + (b - a) * fy;
}

/* One pass: sample a scan line centred on (sx,sy), return the gradient
 * centroid, its spread, the contrast and the net polarity. */
function scanPass(d, w, h, sx, sy, nx, ny, R, step) {
  const n = Math.floor((2 * R) / step) + 1;
  if (n < 7) return null;

  const vals = new Float64Array(n);
  let lo = Infinity, hi = -Infinity;
  for (let i = 0; i < n; i++) {
    const o = -R + i * step;
    const v = lumaAt(d, w, h, sx + nx * o, sy + ny * o);
    if (Number.isNaN(v)) return null;            // scan left the frame
    vals[i] = v;
    if (v < lo) lo = v;
    if (v > hi) hi = v;
  }

  /* CENTROID, NOT PEAK. Parabolic interpolation of the gradient peak — the
   * obvious choice, and what cornerSubPix-style methods reduce to — is biased by
   * the sampling phase, because the gradient of an anti-aliased step is roughly
   * triangular and a parabola is a poor fit to a triangle. Measured against
   * synthetic edges of known position it carried up to 0.29px of bias sharp and
   * 0.75px defocused, which is worse than the whole-pixel corners we started
   * with. The first moment of |gradient| has no such bias: for ANY symmetric
   * edge profile the centroid sits exactly on the edge. Same synthetic test:
   * 0.018px RMS, sharp or defocused. See tools/test-subpix.mjs. */
  let sum = 0, wsum = 0, signed = 0;
  const g = new Float64Array(n);
  for (let i = 1; i < n - 1; i++) {
    const dv = (vals[i + 1] - vals[i - 1]) * 0.5;
    signed += dv;
    g[i] = dv < 0 ? -dv : dv;
    sum += g[i] * (-R + i * step);
    wsum += g[i];
  }
  if (wsum < EPS) return null;
  const off = sum / wsum;

  // second moment: how spread out the gradient is. A single clean transition is
  // compact; two transitions, or a textured surface, are not.
  let m2 = 0;
  for (let i = 1; i < n - 1; i++) { const x = -R + i * step - off; m2 += g[i] * x * x; }

  return { off, sigma: Math.sqrt(m2 / wsum), contrast: hi - lo, sign: signed >= 0 ? 1 : -1 };
}

/* Locate the border crossing near a sample point, re-centring so the window
 * ends up symmetric about the edge (which is what makes the centroid unbiased
 * when the starting guess is off, and is worth ~3x on small markers). */
function scanEdge(d, w, h, sx, sy, nx, ny, R, o) {
  let acc = 0, last = null;
  for (let k = 0; k < o.recentreIters; k++) {
    const r = scanPass(d, w, h, sx + nx * acc, sy + ny * acc, nx, ny, R, o.scanStepPx);
    if (!r) return null;
    if (r.contrast < o.minContrast) return null;       // flat: nothing to find
    if (Math.abs(r.off) > o.maxOffFrac * R) return null; // edge isn't in this window
    acc += r.off;
    last = r;
  }
  // diffuse gradient => more than one edge in the window, or surface texture
  if (last.sigma > Math.min(o.sigmaMaxFrac * R, o.sigmaMaxPx)) return null;
  return { off: acc, grad: last.sign };
}

/* Total least squares line fit. Returns { x, y, ux, uy } — a point and a unit
 * direction. TLS rather than y = mx + b because a near-vertical edge is perfectly
 * ordinary here and would blow up the slope form. */
function fitLine(pts) {
  const n = pts.length;
  let mx = 0, my = 0;
  for (let i = 0; i < n; i++) { mx += pts[i].x; my += pts[i].y; }
  mx /= n; my /= n;

  let sxx = 0, sxy = 0, syy = 0;
  for (let i = 0; i < n; i++) {
    const dx = pts[i].x - mx, dy = pts[i].y - my;
    sxx += dx * dx; sxy += dx * dy; syy += dy * dy;
  }
  if (sxx + syy < EPS) return null;
  const th = 0.5 * Math.atan2(2 * sxy, sxx - syy);   // major axis of the scatter
  return { x: mx, y: my, ux: Math.cos(th), uy: Math.sin(th) };
}

function refineEdge(d, w, h, p0, p1, o) {
  const ex = p1.x - p0.x, ey = p1.y - p0.y;
  const len = Math.hypot(ex, ey);
  if (len < o.minEdgePx) return null;

  const ux = ex / len, uy = ey / len;
  const nx = -uy, ny = ux;
  const cell = len / 6;                        // 4x4 payload + 1 border cell each side
  const R = Math.min(o.maxScanPx, Math.max(o.minScanPx, cell * o.scanCellFrac));
  const nS = Math.max(o.minSamples, Math.min(o.maxSamples, Math.round(len / o.samplePitchPx)));

  const pts = [];
  let pos = 0;
  for (let i = 0; i < nS; i++) {
    const f = nS === 1 ? 0.5 : i / (nS - 1);
    const t = o.edgeInset + (1 - 2 * o.edgeInset) * f;
    const sx = p0.x + ex * t, sy = p0.y + ey * t;
    const r = scanEdge(d, w, h, sx, sy, nx, ny, R, o);
    if (!r) continue;
    const s = r.grad;
    if (s > 0) pos++;
    pts.push({ x: sx + nx * r.off, y: sy + ny * r.off, s });
  }
  if (pts.length < o.minPoints) return null;

  /* Keep only crossings with the majority gradient polarity. A scan that found a
   * white->black step where its neighbours found black->white has locked onto a
   * different transition (an inner module, or the far side of a thin border);
   * including it would tilt the fitted line. */
  const want = pos * 2 >= pts.length ? 1 : -1;
  let kept = pts.filter(p => p.s === want);
  if (kept.length < o.minPoints) return null;

  let line = fitLine(kept);
  if (!line) return null;

  // one outlier pass against the fitted line
  const rx = -line.uy, ry = line.ux;
  const inl = kept.filter(p => Math.abs((p.x - line.x) * rx + (p.y - line.y) * ry) <= o.outlierPx);
  if (inl.length >= o.minPoints && inl.length < kept.length) {
    const l2 = fitLine(inl);
    if (l2) { line = l2; kept = inl; }
  }
  line.n = kept.length;
  return line;
}

function intersect(a, b) {
  const cr = a.ux * b.uy - a.uy * b.ux;
  if (Math.abs(cr) < 0.05) return null;        // within ~3 deg of parallel
  const dx = b.x - a.x, dy = b.y - a.y;
  const t = (dx * b.uy - dy * b.ux) / cr;
  return { x: a.x + a.ux * t, y: a.y + a.uy * t };
}

/* Refine one quad. Returns a new 4-corner array, or null to keep the original.
 * Corner order is preserved: corner i is the meeting of edge i-1 and edge i. */
export function refineQuad(imageData, corners, opts = DEFAULT_OPTS) {
  if (!corners || corners.length !== 4) return null;
  const d = imageData.data, w = imageData.width, h = imageData.height;

  const lines = new Array(4);
  for (let i = 0; i < 4; i++) {
    lines[i] = refineEdge(d, w, h, corners[i], corners[(i + 1) % 4], opts);
    if (!lines[i]) return null;
  }

  const out = new Array(4);
  let worst = 0;
  for (let i = 0; i < 4; i++) {
    const p = intersect(lines[(i + 3) % 4], lines[i]);
    if (!p) return null;
    const shift = Math.hypot(p.x - corners[i].x, p.y - corners[i].y);
    if (shift > opts.maxShiftPx) return null;  // this is a different quad, not a better one
    if (shift > worst) worst = shift;
    out[i] = p;
  }
  out.shift = worst;
  return out;
}

/* Refine every marker in place. Returns diagnostics for the HUD: how many
 * markers were refined and the mean corner correction applied, which is the
 * number that tells you whether this is doing anything at all. */
export function refineMarkers(imageData, markers, opts = DEFAULT_OPTS) {
  let refined = 0, shiftSum = 0;
  for (const m of markers) {
    const r = refineQuad(imageData, m.corners, opts);
    if (!r) continue;
    m.corners = [{ x: r[0].x, y: r[0].y }, { x: r[1].x, y: r[1].y },
                 { x: r[2].x, y: r[2].y }, { x: r[3].x, y: r[3].y }];
    m.subpix = true;
    refined++;
    shiftSum += r.shift;
  }
  return { refined, total: markers.length, meanShiftPx: refined ? shiftSum / refined : 0 };
}