/* solve-cv.js — v3 tracking core: joint multi-marker "board" solve via OpenCV.js.
 *
 * Replaces the per-marker POS-IT -> fuse pipeline. Every detected marker corner
 * becomes a 3D world point (from scene.json anchors, same anchorWorldMatrix
 * convention as v2), and ONE solvePnP call recovers the camera pose directly:
 *   - stability scales with marker spread instead of degrading into fusion tuning
 *   - the wall crest makes the point set non-coplanar => planar ambiguity gone
 *   - single visible marker still works (IPPE init + iterative refine w/ prior)
 *
 * Frame conversion (validated against synthetic projections, error ~0):
 *   solvePnP gives world->cvCamera (R, t).  cv cam: +X right, +Y down, +Z fwd.
 *   camera position in world:  C = -R^T t
 *   three.js world-from-camera rotation:  R^T * diag(1,-1,-1)
 *
 * OpenCV.js (~13MB WASM) is lazy-loaded after Start; the POS-IT engine remains
 * available as a runtime A/B fallback (HUD top-third tap cycles engines).
 */
import * as THREE from 'three';
import { anchorWorldMatrix } from './pose.js';

// ---------------- OpenCV.js loader ----------------
let _cv = null;
let _loading = null;

export function isCvReady() { return !!(_cv && _cv.Mat); }
export function getCv() { return _cv; }
export function _injectCv(m) { _cv = m; }   // test hook (node)

export function loadOpenCV(url = '/vendor/opencv.js') {
  if (isCvReady()) return Promise.resolve(_cv);
  if (_loading) return _loading;
  _loading = new Promise((resolve, reject) => {
    const s = document.createElement('script');
    s.src = url;
    s.async = true;
    s.onerror = () => reject(new Error('opencv.js failed to load'));
    s.onload = () => {
      const mod = window.cv;
      const finish = (m) => { _cv = m; resolve(m); };
      if (!mod) return reject(new Error('opencv.js loaded but window.cv missing'));
      if (mod.Mat) return finish(mod);                    // already initialized
      if (typeof mod.then === 'function') return mod.then(finish, reject); // promise build
      mod.onRuntimeInitialized = () => finish(mod);       // classic emscripten
      // safety: poll in case onRuntimeInitialized was already consumed
      const t0 = performance.now();
      (function poll() {
        if (_cv) return;
        if (mod.Mat) return finish(mod);
        if (performance.now() - t0 > 30000) return reject(new Error('opencv.js init timeout'));
        setTimeout(poll, 100);
      })();
    };
    document.head.appendChild(s);
  });
  return _loading;
}

// ---------------- marker corner templates ----------------
/* js-aruco2 corner order (canonical, matches POS-IT buildModel): TL, TR, BR, BL.
 * Marker-local frame: +X right, +Y up (toward printed top edge), +Z out of face.
 * 'ymirror' axis mode flips local Y — on-device insurance in case the effective
 * marker frame v2 calibrated against differs by a Y mirror (cycle via HUD). */
export const CV_AXIS_MODES = ['std', 'ymirror'];

function markerLocalCorners(sizeMM, mode) {
  const h = sizeMM / 20;                  // mm -> cm, half size
  const y = mode === 'ymirror' ? -1 : 1;
  return [
    new THREE.Vector3(-h,  h * y, 0),
    new THREE.Vector3( h,  h * y, 0),
    new THREE.Vector3( h, -h * y, 0),
    new THREE.Vector3(-h, -h * y, 0),
  ];
}

// ---------------- 1€ filter (same maths as v2 fuse.js, standalone) ----------------
class OneEuroPose {
  /* Raw board-solve noise is far below POS-IT's, so cutoffs sit higher (less
   * smoothing => less lag) than v2's fuse.js tuning. Retune on-device if needed. */
  constructor({ minCutoffPos = 1.2, betaPos = 0.10, minCutoffAng = 1.4, betaAng = 0.12, dCutoff = 1.0 } = {}) {
    Object.assign(this, { minCutoffPos, betaPos, minCutoffAng, betaAng, dCutoff });
    this.reset();
  }
  reset() {
    this.pos = null; this.quat = null;
    this.prevPos = null; this.dPos = new THREE.Vector3();
    this.prevQuat = null; this.dAngRate = 0;
    this.lastT = 0;
  }
  static alpha(cutoff, dt) { const tau = 1 / (2 * Math.PI * cutoff); return 1 / (1 + tau / dt); }
  apply(pos, quat, now) {
    const gap = now - this.lastT;
    const dt = this.pos ? Math.min(0.1, Math.max(0.001, gap / 1000)) : 1 / 30;
    this.lastT = now;
    if (!this.pos || gap > 1500) {
      this.pos = pos.clone(); this.prevPos = pos.clone(); this.dPos.set(0, 0, 0);
      this.quat = quat.clone(); this.prevQuat = quat.clone(); this.dAngRate = 0;
      return { position: this.pos.clone(), quaternion: this.quat.clone() };
    }
    const aD = OneEuroPose.alpha(this.dCutoff, dt);
    // position
    const dRaw = pos.clone().sub(this.prevPos).multiplyScalar(1 / dt);
    this.dPos.lerp(dRaw, aD);
    const aP = OneEuroPose.alpha(this.minCutoffPos + this.betaPos * this.dPos.length(), dt);
    this.pos.lerp(pos, aP);
    this.prevPos.copy(pos);
    // orientation
    if (this.quat.dot(quat) < 0) quat.set(-quat.x, -quat.y, -quat.z, -quat.w);
    if (this.prevQuat.dot(quat) < 0) this.prevQuat.set(-this.prevQuat.x, -this.prevQuat.y, -this.prevQuat.z, -this.prevQuat.w);
    const ang = 2 * Math.acos(Math.min(1, Math.abs(this.prevQuat.dot(quat)))) / dt;
    this.dAngRate += aD * (ang - this.dAngRate);
    const aA = OneEuroPose.alpha(this.minCutoffAng + this.betaAng * this.dAngRate, dt);
    this.quat.slerp(quat, aA);
    this.prevQuat.copy(quat);
    return { position: this.pos.clone(), quaternion: this.quat.clone() };
  }
}

// ---------------- the solver ----------------
export class CvBoardSolver {
  constructor() {
    this.anchors = new Map();       // markerId -> { mat: Matrix4, sizeMM }
    this.worldCorners = new Map();  // `${axisMode}:${markerId}` -> [x,y,z]*4 flat
    this.axisMode = 'std';
    this.prev = null;               // { r:[3], t:[3], at:ms }
    this.prevTTL = 1500;
    this.filter = new OneEuroPose();
    this.K = null; this.dist = null; this.Kw = 0; this.Kf = 0;
    this.maxReprojPx = 8;           // reject frames worse than this (bad detect)
    this.lastReproj = 0;
    this._v = new THREE.Vector3();
  }

  setAxisMode(m) { if (CV_AXIS_MODES.includes(m)) { this.axisMode = m; this.prev = null; this.filter.reset(); } }
  getAxisMode() { return this.axisMode; }

  setScene(scene) {
    this.anchors.clear(); this.worldCorners.clear();
    for (const a of scene.anchors || []) {
      if (a.enabled === false) continue;
      this.anchors.set(a.markerId, { mat: anchorWorldMatrix(a), sizeMM: a.sizeMM || 60 });
    }
    this.prev = null; this.filter.reset();
  }
  knownIds() { return new Set(this.anchors.keys()); }

  cornersFor(markerId) {
    const key = `${this.axisMode}:${markerId}`;
    let flat = this.worldCorners.get(key);
    if (!flat) {
      const a = this.anchors.get(markerId);
      if (!a) return null;
      flat = [];
      for (const c of markerLocalCorners(a.sizeMM, this.axisMode)) {
        this._v.copy(c).applyMatrix4(a.mat);
        flat.push(this._v.x, this._v.y, this._v.z);
      }
      this.worldCorners.set(key, flat);
    }
    return flat;
  }

  /** markers: [{ id, corners:[{x,y}*4] }] in detection-canvas pixels.
   *  Returns { position(cm world), quaternion(three), markerCount, reprojPx } or null. */
  solve(markers, width, height, focalPx) {
    const cv = _cv;
    if (!cv || !cv.Mat) return null;

    // intrinsics (cy depends on height; rebuild K when geometry changes)
    if (!this.K || this.Kw !== width || this.Kf !== focalPx || this._h !== height) {
      if (this.K) { this.K.delete(); this.dist.delete(); }
      this.K = cv.matFromArray(3, 3, cv.CV_64F, [focalPx, 0, width / 2, 0, focalPx, height / 2, 0, 0, 1]);
      this.dist = cv.Mat.zeros(4, 1, cv.CV_64F);
      this.Kw = width; this.Kf = focalPx; this._h = height;
    }

    const obj = [], img = [];
    let used = 0;
    for (const m of markers) {
      const flat = this.cornersFor(m.id);
      if (!flat) continue;
      obj.push(...flat);
      for (const c of m.corners) img.push(c.x, c.y);
      used++;
    }
    if (!used) return null;
    const n = obj.length / 3;

    const objM = cv.matFromArray(n, 3, cv.CV_64F, obj);
    const imgM = cv.matFromArray(n, 2, cv.CV_64F, img);
    const rvec = new cv.Mat(3, 1, cv.CV_64F);
    const tvec = new cv.Mat(3, 1, cv.CV_64F);
    const R = new cv.Mat();
    const proj = new cv.Mat();
    const jac = new cv.Mat();
    let out = null;

    try {
      const fresh = this.prev && (performance.now() - this.prev.at) < this.prevTTL;
      if (fresh) {
        // warm start: iterative LM from last frame — fast, and implicitly
        // resolves single-marker planar flips by temporal continuity
        rvec.data64F.set(this.prev.r); tvec.data64F.set(this.prev.t);
        cv.solvePnP(objM, imgM, this.K, this.dist, rvec, tvec, true, cv.SOLVEPNP_ITERATIVE);
      } else {
        // cold start: SQPNP (any geometry) -> fall back to IPPE (planar) -> refine
        let ok = false;
        try { ok = cv.solvePnP(objM, imgM, this.K, this.dist, rvec, tvec, false, cv.SOLVEPNP_SQPNP); } catch { ok = false; }
        if (!ok) {
          try { ok = cv.solvePnP(objM, imgM, this.K, this.dist, rvec, tvec, false, cv.SOLVEPNP_IPPE); } catch { ok = false; }
        }
        if (!ok) return null;
        cv.solvePnP(objM, imgM, this.K, this.dist, rvec, tvec, true, cv.SOLVEPNP_ITERATIVE);
      }

      // reprojection error gate (mean px) — rejects poisoned frames before the filter
      cv.projectPoints(objM, rvec, tvec, this.K, this.dist, proj, jac);
      let err = 0;
      for (let i = 0; i < n; i++) {
        err += Math.hypot(proj.data64F[2 * i] - img[2 * i], proj.data64F[2 * i + 1] - img[2 * i + 1]);
      }
      err /= n;
      this.lastReproj = err;
      if (err > this.maxReprojPx) { this.prev = null; return null; }

      this.prev = { r: [...rvec.data64F], t: [...tvec.data64F], at: performance.now() };

      // ---- convert to three.js world pose ----
      cv.Rodrigues(rvec, R);
      const d = R.data64F;                       // row-major world->cvCam
      const t = tvec.data64F;
      // C = -R^T t
      const Cx = -(d[0] * t[0] + d[3] * t[1] + d[6] * t[2]);
      const Cy = -(d[1] * t[0] + d[4] * t[1] + d[7] * t[2]);
      const Cz = -(d[2] * t[0] + d[5] * t[1] + d[8] * t[2]);
      // world-from-threeCam = R^T * diag(1,-1,-1): columns [R^T_col0, -R^T_col1, -R^T_col2]
      const m4 = new THREE.Matrix4().set(
        d[0], -d[3], -d[6], 0,
        d[1], -d[4], -d[7], 0,
        d[2], -d[5], -d[8], 0,
        0, 0, 0, 1
      );
      const quat = new THREE.Quaternion().setFromRotationMatrix(m4);
      const sm = this.filter.apply(new THREE.Vector3(Cx, Cy, Cz), quat, performance.now());
      out = { position: sm.position, quaternion: sm.quaternion, markerCount: used, reprojPx: err };
    } finally {
      objM.delete(); imgM.delete(); rvec.delete(); tvec.delete(); R.delete(); proj.delete(); jac.delete();
    }
    return out;
  }
}