/* pose.js — marker pose estimation with the confirmed fixes:
 *   1. POS-IT rotation used AS-IS; translation Y and Z negated (-t[1], -t[2]).
 *   2. POS-IT planar ambiguity resolved by temporal consistency:
 *      pick the solution (bestError vs alternativeError) closest to the previous frame.
 *
 * Requires vendor chain loaded in order: cv -> svd -> posit1 -> aruco -> dictionary.
 */
import * as THREE from 'three';

const _m = new THREE.Matrix4();
const _q = new THREE.Quaternion();

export class PoseEstimator {
  constructor(focalLength) {
    this.focal = focalLength;
    this.posits = new Map();   // sizeMM -> POS.Posit
    this.prev = new Map();     // markerId -> { quat, pos, t }
    this.prevTTL = 1500;       // ms before history is considered stale
  }

  positFor(sizeMM) {
    if (!this.posits.has(sizeMM)) this.posits.set(sizeMM, new POS.Posit(sizeMM, this.focal));
    return this.posits.get(sizeMM);
  }

  /** corners: aruco marker corners, image-space; cx/cy: image center.
   *  Returns { position: THREE.Vector3 (mm, marker->camera), quaternion, error } */
  estimate(markerId, corners, cx, cy, sizeMM) {
    const centered = corners.map(c => ({ x: c.x - cx, y: (cy - c.y) }));
    const pose = this.positFor(sizeMM).pose(centered);
    if (!pose) return null;

    const cand = [
      this.candidate(pose.bestRotation, pose.bestTranslation, pose.bestError),
      this.candidate(pose.alternativeRotation, pose.alternativeTranslation, pose.alternativeError),
    ];

    // Temporal consistency: prefer the solution nearest the previous frame's quat.
    const prev = this.prev.get(markerId);
    let pick;
    if (prev && (performance.now() - prev.t) < this.prevTTL) {
      const d0 = Math.abs(cand[0].quaternion.dot(prev.quat));
      const d1 = Math.abs(cand[1].quaternion.dot(prev.quat));
      // only override error-order if the alternative is clearly more consistent
      pick = (d1 > d0 + 0.05) ? cand[1] : (d0 > d1 + 0.05 ? cand[0] : (cand[0].error <= cand[1].error ? cand[0] : cand[1]));
    } else {
      pick = cand[0].error <= cand[1].error ? cand[0] : cand[1];
    }

    this.prev.set(markerId, { quat: pick.quaternion.clone(), pos: pick.position.clone(), t: performance.now() });
    return pick;
  }

  candidate(rot, t, error) {
    // Rotation as-is (row-major 3x3 -> Matrix4)
    _m.set(
      rot[0][0], rot[0][1], rot[0][2], 0,
      rot[1][0], rot[1][1], rot[1][2], 0,
      rot[2][0], rot[2][1], rot[2][2], 0,
      0, 0, 0, 1
    );
    const quaternion = new THREE.Quaternion().setFromRotationMatrix(_m);
    // Translation: negate Y and Z only (confirmed fix)
    const position = new THREE.Vector3(t[0], -t[1], -t[2]);
    return { position, quaternion, error };
  }
}

/** Build the marker->world transform for an anchor.
 *  mount 'flat': marker printed face-up on a horizontal surface.
 *  mount 'wall': marker on a vertical surface; yawDeg = facing direction.
 *  mount 'custom': explicit yaw/pitch/roll (deg) applied in YXZ order.
 *  All mounts additionally honour yaw/pitch/roll offsets for fine trim.
 */
export function anchorWorldMatrix(anchor) {
  const pos = new THREE.Vector3(...anchor.position);
  const yaw = THREE.MathUtils.degToRad(anchor.yawDeg || 0);
  const pitch = THREE.MathUtils.degToRad(anchor.pitchDeg || 0);
  const roll = THREE.MathUtils.degToRad(anchor.rollDeg || 0);

  // Base orientation by mount:
  //  flat: marker face-up — marker +Z (out of print face) -> world +Y
  //  wall: marker vertical — marker +Z faces world +Z when yawDeg = 0
  const base = new THREE.Quaternion();
  if (anchor.mount !== 'wall' && anchor.mount !== 'custom') {
    base.setFromAxisAngle(new THREE.Vector3(1, 0, 0), -Math.PI / 2);
  }

  // Trim (fully editable): yaw about world Y, then pitch/roll fine adjustment
  const trim = new THREE.Quaternion().setFromEuler(new THREE.Euler(pitch, yaw, roll, 'YXZ'));
  const q = trim.multiply(base);
  return new THREE.Matrix4().compose(pos, q, new THREE.Vector3(1, 1, 1));
}