// COLMAP world->camera pose  ->  Three.js camera pose (spec M5).
//
// FROZEN CONTRACT. This is the JavaScript mirror of `backend/festival4d/geometry.py`
// (`colmap_to_threejs`, `quat_to_mat`). The math and the POSE_TEST_VECTORS below are
// duplicated verbatim on the Python side (`backend/tests/test_geometry.py`) so both agree.
// Lanes B and C CONSUME this; do not reimplement the conversion elsewhere. Changing it
// requires a change request and a synchronized edit to both files + both vector sets.
//
// Conventions:
//   - Quaternions are COLMAP order [w, x, y, z] (scalar first), unit norm.
//   - Pose (q, t) is world->camera: x_cam = R(q) * x_world + t.
//   - COLMAP camera axes: +x right, +y down, +z forward.
//   - Three.js cameras look down -z with +y up; hence the diag(1, -1, -1) flip.

const SQRT1_2 = 0.7071067811865476;

// Camera-axis flip: COLMAP camera-local (x right, y down, z forward) -> Three.js (x right,
// y up, z backward). Same _FLIP_YZ as the Python side.
const FLIP_YZ = [
  [1, 0, 0],
  [0, -1, 0],
  [0, 0, -1],
];

/**
 * Convert a unit quaternion [w, x, y, z] to a 3x3 rotation matrix (row-major nested array).
 * Hamilton convention, right-handed, active rotation (COLMAP world->camera when q is a
 * COLMAP pose quaternion).
 * @param {number[]} q - [w, x, y, z]
 * @returns {number[][]} 3x3 rotation matrix
 */
export function quatToMat(q) {
  const [w, x, y, z] = q;
  const n = w * w + x * x + y * y + z * z;
  if (n < 1e-12) throw new Error("quaternion has near-zero norm");
  const s = 2.0 / n;
  const wx = s * w * x, wy = s * w * y, wz = s * w * z;
  const xx = s * x * x, xy = s * x * y, xz = s * x * z;
  const yy = s * y * y, yz = s * y * z, zz = s * z * z;
  return [
    [1.0 - (yy + zz), xy - wz, xz + wy],
    [xy + wz, 1.0 - (xx + zz), yz - wx],
    [xz - wy, yz + wx, 1.0 - (xx + yy)],
  ];
}

function transpose3(m) {
  return [
    [m[0][0], m[1][0], m[2][0]],
    [m[0][1], m[1][1], m[2][1]],
    [m[0][2], m[1][2], m[2][2]],
  ];
}

function matMul3(a, b) {
  const out = [[0, 0, 0], [0, 0, 0], [0, 0, 0]];
  for (let i = 0; i < 3; i++)
    for (let j = 0; j < 3; j++)
      out[i][j] = a[i][0] * b[0][j] + a[i][1] * b[1][j] + a[i][2] * b[2][j];
  return out;
}

function matVec3(m, v) {
  return [
    m[0][0] * v[0] + m[0][1] * v[1] + m[0][2] * v[2],
    m[1][0] * v[0] + m[1][1] * v[1] + m[1][2] * v[2],
    m[2][0] * v[0] + m[2][1] * v[1] + m[2][2] * v[2],
  ];
}

/**
 * Convert a COLMAP world->camera pose to a Three.js camera pose (spec M5). FROZEN.
 * @param {number[]} q - COLMAP world->camera quaternion [w, x, y, z]
 * @param {number[]} t - COLMAP world->camera translation [tx, ty, tz]
 * @returns {{position: number[], rotation: number[][], matrixWorld: number[]}}
 *   position       - camera center in world coords, C = -R^T t (assign to camera.position)
 *   rotation       - Three.js camera world rotation R_three = R^T * diag(1,-1,-1),
 *                    3x3 row-major (feed to camera.setRotationFromMatrix via a Matrix4)
 *   matrixWorld    - column-major 16-array [R_three | position], ready for
 *                    THREE.Matrix4().fromArray(...) when driving the camera by matrix.
 */
export function colmapToThreejs(q, t) {
  const R = quatToMat(q); // world -> cam
  const Rc2w = transpose3(R); // cam -> world
  const position = matVec3(Rc2w, [-t[0], -t[1], -t[2]]); // C = -R^T t
  const rotation = matMul3(Rc2w, FLIP_YZ); // R_three

  // Column-major Matrix4 with rotation in the upper-left 3x3 and translation = position.
  const m = rotation;
  const p = position;
  const matrixWorld = [
    m[0][0], m[1][0], m[2][0], 0,
    m[0][1], m[1][1], m[2][1], 0,
    m[0][2], m[1][2], m[2][2], 0,
    p[0], p[1], p[2], 1,
  ];
  return { position, rotation, matrixWorld };
}

// --- FROZEN test vectors (must equal POSE_TEST_VECTORS in backend/tests/test_geometry.py).
export const POSE_TEST_VECTORS = [
  {
    name: "identity",
    q: [1.0, 0.0, 0.0, 0.0],
    t: [0.0, 0.0, -10.0],
    position: [0.0, 0.0, 10.0],
    rotation: [[1.0, 0.0, 0.0], [0.0, -1.0, 0.0], [0.0, 0.0, -1.0]],
  },
  {
    name: "yaw90",
    q: [SQRT1_2, 0.0, SQRT1_2, 0.0],
    t: [0.0, 0.0, 10.0],
    position: [10.0, 0.0, 0.0],
    rotation: [[0.0, 0.0, 1.0], [0.0, -1.0, 0.0], [1.0, 0.0, 0.0]],
  },
  {
    name: "lookat",
    q: [0.0, 1.0, 0.0, 0.0],
    t: [0.0, 0.0, 8.0],
    position: [0.0, 0.0, 8.0],
    rotation: [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]],
  },
];

/**
 * Self-check the conversion against POSE_TEST_VECTORS. Returns true on success, throws on
 * mismatch. Called by main.js so the frozen contract is verified live in the browser.
 * @param {number} [atol=1e-9]
 */
export function selfTest(atol = 1e-9) {
  for (const vec of POSE_TEST_VECTORS) {
    const { position, rotation } = colmapToThreejs(vec.q, vec.t);
    for (let i = 0; i < 3; i++) {
      if (Math.abs(position[i] - vec.position[i]) > atol)
        throw new Error(`pose.js selfTest[${vec.name}]: position mismatch at ${i}`);
      for (let j = 0; j < 3; j++) {
        if (Math.abs(rotation[i][j] - vec.rotation[i][j]) > atol)
          throw new Error(`pose.js selfTest[${vec.name}]: rotation mismatch at ${i},${j}`);
      }
    }
  }
  return true;
}