/* fuse.js — fuseWorld: combine per-marker camera pose estimates into one world pose.
 *
 * Each detected marker yields a camera-in-world estimate:
 *   cameraWorld = anchorWorld * inverse(markerPoseInCamera)
 * Estimates are fused by confidence-weighted quaternion slerp (incremental
 * weighted average) and weighted position mean. Confidence = marker screen area
 * (bigger/closer markers dominate). A light temporal smooth removes residual jitter.
 *
 * Requiring >= 2 visible markers is handled upstream by anchor placement density;
 * fuseWorld itself works with 1..N.
 */
import * as THREE from 'three';
import { anchorWorldMatrix } from './pose.js';

const _inv = new THREE.Matrix4();
const _m = new THREE.Matrix4();
const _p = new THREE.Vector3();
const _q = new THREE.Quaternion();
const _s = new THREE.Vector3();

export class WorldFuser {
  constructor() {
    this.anchorMats = new Map(); // markerId -> Matrix4
    this.smoothPos = null;
    this.smoothQuat = null;
    /* 1€ (OneEuro) filter — the standard cure for marker-pose jitter. It low-passes
     * hard when the signal is slow (phone still => kills jitter) and eases off as
     * speed rises (phone moving => stays responsive, no lag). Two knobs:
     *   minCutoff — lower = steadier at rest (more smoothing when still)
     *   beta      — higher = snappier when moving (less lag during motion)
     * dCutoff is the cutoff for the internal speed estimate; 1.0 is standard. */
    this.oe = {
      minCutoffPos: 0.6, betaPos: 0.05,
      minCutoffAng: 0.7, betaAng: 0.06,
      dCutoff: 1.0,
      prevPos: null, dPos: new THREE.Vector3(),
      prevQuat: null, dAngRate: 0,
    };
    this.lastFuseT = 0;
  }

  // low-pass alpha from a cutoff frequency (Hz) and timestep dt (s)
  static alpha(cutoff, dt) {
    const tau = 1 / (2 * Math.PI * cutoff);
    return 1 / (1 + tau / dt);
  }

  setScene(scene) {
    this.anchorMats.clear();
    for (const a of scene.anchors || []) {
      if (a.enabled === false) continue;
      this.anchorMats.set(a.markerId, { mat: anchorWorldMatrix(a), sizeMM: a.sizeMM || 60 });
    }
  }

  sizeFor(markerId) { return this.anchorMats.get(markerId)?.sizeMM || 60; }
  knownIds() { return new Set(this.anchorMats.keys()); }

  /** estimates: [{ markerId, position(mm), quaternion, area }] */
  fuse(estimates) {
    const MM2CM = 0.1; // POS-IT translation is in mm; world units are cm
    const cams = [];
    for (const e of estimates) {
      const entry = this.anchorMats.get(e.markerId);
      if (!entry) continue;
      // marker pose in camera space -> matrix (translate mm->cm)
      _m.compose(_p.copy(e.position).multiplyScalar(MM2CM), e.quaternion, _s.set(1, 1, 1));
      _inv.copy(_m).invert();                    // camera in marker space
      const camWorld = new THREE.Matrix4().multiplyMatrices(entry.mat, _inv);
      const pos = new THREE.Vector3();
      const quat = new THREE.Quaternion();
      camWorld.decompose(pos, quat, _s);
      cams.push({ pos, quat, w: Math.max(1, e.area) });
    }
    if (!cams.length) return null;

    // weighted position mean + incremental weighted slerp for orientation
    let wSum = cams[0].w;
    const pos = cams[0].pos.clone().multiplyScalar(cams[0].w);
    const quat = cams[0].quat.clone();
    for (let i = 1; i < cams.length; i++) {
      const c = cams[i];
      // hemisphere alignment before slerp (quaternion double-cover)
      if (quat.dot(c.quat) < 0) c.quat.set(-c.quat.x, -c.quat.y, -c.quat.z, -c.quat.w);
      const t = c.w / (wSum + c.w);
      quat.slerp(c.quat, t);
      pos.add(c.pos.clone().multiplyScalar(c.w));
      wSum += c.w;
    }
    pos.multiplyScalar(1 / wSum);

    // ---- 1€ filter (position + orientation) ----
    const now = performance.now();
    const gap = now - this.lastFuseT;
    const dt = this.smoothPos ? Math.min(0.1, Math.max(0.001, gap / 1000)) : 1 / 30;
    this.lastFuseT = now;
    const oe = this.oe;

    // reset on first frame or after a real tracking gap (>1.5s)
    if (!this.smoothPos || gap > 1500) {
      this.smoothPos = pos.clone(); oe.prevPos = pos.clone(); oe.dPos.set(0, 0, 0);
      this.smoothQuat = quat.clone(); oe.prevQuat = quat.clone(); oe.dAngRate = 0;
      return { position: this.smoothPos.clone(), quaternion: this.smoothQuat.clone(), markerCount: cams.length };
    }

    // POSITION: derivative -> speed -> dynamic cutoff -> low-pass
    const dPosRaw = pos.clone().sub(oe.prevPos).multiplyScalar(1 / dt); // cm/s
    const aD = WorldFuser.alpha(oe.dCutoff, dt);
    oe.dPos.lerp(dPosRaw, aD);
    const speed = oe.dPos.length();
    const cutoffP = oe.minCutoffPos + oe.betaPos * speed;
    const aP = WorldFuser.alpha(cutoffP, dt);
    this.smoothPos.lerp(pos, aP);
    oe.prevPos.copy(pos);

    // ORIENTATION: angular speed -> dynamic cutoff -> slerp
    if (this.smoothQuat.dot(quat) < 0) quat.set(-quat.x, -quat.y, -quat.z, -quat.w);
    if (oe.prevQuat.dot(quat) < 0) oe.prevQuat.set(-oe.prevQuat.x, -oe.prevQuat.y, -oe.prevQuat.z, -oe.prevQuat.w);
    const angDelta = 2 * Math.acos(Math.min(1, Math.abs(oe.prevQuat.dot(quat)))) / dt; // rad/s
    oe.dAngRate += aD * (angDelta - oe.dAngRate);
    const cutoffA = oe.minCutoffAng + oe.betaAng * oe.dAngRate;
    const aA = WorldFuser.alpha(cutoffA, dt);
    this.smoothQuat.slerp(quat, aA);
    oe.prevQuat.copy(quat);

    return { position: this.smoothPos.clone(), quaternion: this.smoothQuat.clone(), markerCount: cams.length };
  }
}