/* 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;
    this.posAlpha = 0.35;  // smoothing factors (higher = snappier)
    this.quatAlpha = 0.35;
    this.lastFuseT = 0;
  }

  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 MM = 0.001;
    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->m)
      _m.compose(_p.copy(e.position).multiplyScalar(MM), 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);

    // temporal smoothing
    const now = performance.now();
    if (this.smoothPos && now - this.lastFuseT < 500) {
      this.smoothPos.lerp(pos, this.posAlpha);
      if (this.smoothQuat.dot(quat) < 0) quat.set(-quat.x, -quat.y, -quat.z, -quat.w);
      this.smoothQuat.slerp(quat, this.quatAlpha);
    } else {
      this.smoothPos = pos.clone();
      this.smoothQuat = quat.clone();
    }
    this.lastFuseT = now;
    return { position: this.smoothPos.clone(), quaternion: this.smoothQuat.clone(), markerCount: cams.length };
  }
}