import * as THREE from 'three';
import { useModelStore } from '@/stores/useModelStore';

export type XRCalibrationStep =
  | 'idle'
  | 'await-cube-1'
  | 'await-model-1'
  | 'await-cube-2'
  | 'await-model-2'
  | 'await-cube-3' // opcional verificação
  | 'await-model-3'
  | 'await-real-1'
  | 'await-virtual-1'
  | 'await-real-2'
  | 'await-virtual-2'
  | 'await-real-3'
  | 'await-virtual-3'
  | 'done';

interface XRPair {
  cube: THREE.Vector3;
  model: THREE.Vector3;
}

interface XRCalState {
  step: XRCalibrationStep;
  modelId: string | null;
  alignType: 'cube' | 'virt-real' | null;
  pairs: XRPair[];
  realPoints: THREE.Vector3[];
  virtualPoints: THREE.Vector3[];
  pendingCube: THREE.Vector3 | null;
  progress: number;
  verifyErrorDeg: number;
  listeners: Set<() => void>;
}

export const xrCalibration: XRCalState = {
  step: 'idle',
  modelId: null,
  alignType: null,
  pairs: [],
  realPoints: [],
  virtualPoints: [],
  pendingCube: null,
  progress: 0,
  verifyErrorDeg: NaN,
  listeners: new Set(),
};

function notify() {
  xrCalibration.listeners.forEach((fn) => fn());
}

export function subscribeXRCalibration(fn: () => void): () => void {
  xrCalibration.listeners.add(fn);
  return () => {
    xrCalibration.listeners.delete(fn);
  };
}

export function startXRCalibration(modelId: string, type: 'cube' | 'virt-real' = 'cube') {
  xrCalibration.modelId = modelId;
  xrCalibration.alignType = type;
  xrCalibration.pairs = [];
  xrCalibration.realPoints = [];
  xrCalibration.virtualPoints = [];
  xrCalibration.pendingCube = null;
  xrCalibration.progress = 0;
  xrCalibration.verifyErrorDeg = NaN;
  
  if (type === 'cube') {
    xrCalibration.step = 'await-cube-1';
  } else {
    xrCalibration.step = 'await-real-1';
  }
  notify();
}

export function cancelXRCalibration() {
  xrCalibration.step = 'idle';
  xrCalibration.modelId = null;
  xrCalibration.alignType = null;
  xrCalibration.pairs = [];
  xrCalibration.realPoints = [];
  xrCalibration.virtualPoints = [];
  xrCalibration.pendingCube = null;
  xrCalibration.progress = 0;
  xrCalibration.verifyErrorDeg = NaN;
  notify();
}

export function pushXRCubeFace(dirWorld: THREE.Vector3) {
  if (xrCalibration.step === 'await-cube-1') {
    xrCalibration.pendingCube = dirWorld.clone().normalize();
    xrCalibration.step = 'await-model-1';
  } else if (xrCalibration.step === 'await-cube-2') {
    xrCalibration.pendingCube = dirWorld.clone().normalize();
    xrCalibration.step = 'await-model-2';
  } else if (xrCalibration.step === 'await-cube-3') {
    xrCalibration.pendingCube = dirWorld.clone().normalize();
    xrCalibration.step = 'await-model-3';
  } else {
    return;
  }
  notify();
}

export function pushXRRealPoint(pointWorld: THREE.Vector3) {
  if (xrCalibration.alignType !== 'virt-real') return;
  const p = pointWorld.clone();
  if (xrCalibration.step === 'await-real-1') {
    xrCalibration.realPoints[0] = p;
    xrCalibration.step = 'await-virtual-1';
    xrCalibration.progress = 0.16;
  } else if (xrCalibration.step === 'await-real-2') {
    xrCalibration.realPoints[1] = p;
    xrCalibration.step = 'await-virtual-2';
    xrCalibration.progress = 0.5;
  } else if (xrCalibration.step === 'await-real-3') {
    xrCalibration.realPoints[2] = p;
    xrCalibration.step = 'await-virtual-3';
    xrCalibration.progress = 0.83;
  }
  notify();
}

export function pushXRVirtualPoint(pointLocalPivot: THREE.Vector3) {
  if (xrCalibration.alignType !== 'virt-real') return;
  const p = pointLocalPivot.clone();
  if (xrCalibration.step === 'await-virtual-1') {
    xrCalibration.virtualPoints[0] = p;
    xrCalibration.step = 'await-real-2';
    xrCalibration.progress = 0.33;
  } else if (xrCalibration.step === 'await-virtual-2') {
    xrCalibration.virtualPoints[1] = p;
    xrCalibration.step = 'await-real-3';
    xrCalibration.progress = 0.66;
  } else if (xrCalibration.step === 'await-virtual-3') {
    xrCalibration.virtualPoints[2] = p;
    xrCalibration.step = 'done';
    xrCalibration.progress = 1.0;
  }
  notify();
}

function snapToPrincipalAxis(v: THREE.Vector3, maxDeg = 12): THREE.Vector3 {
  const cosT = Math.cos(THREE.MathUtils.degToRad(maxDeg));
  const axes = [
    new THREE.Vector3(1, 0, 0),
    new THREE.Vector3(-1, 0, 0),
    new THREE.Vector3(0, 1, 0),
    new THREE.Vector3(0, -1, 0),
    new THREE.Vector3(0, 0, 1),
    new THREE.Vector3(0, 0, -1),
  ];
  let best = v;
  let bestDot = cosT;
  for (const a of axes) {
    const d = v.dot(a);
    if (d > bestDot) {
      bestDot = d;
      best = a.clone();
    }
  }
  return best;
}

export function pushXRModelFaceNormal(normalWorld: THREE.Vector3, calGroupWorldQuat: THREE.Quaternion) {
  if (!xrCalibration.pendingCube) return;
  const cubeWorld = xrCalibration.pendingCube;
  const inv = calGroupWorldQuat.clone().invert();
  const cubeLocal = cubeWorld.clone().applyQuaternion(inv).normalize();
  const modelLocalRaw = normalWorld.clone().applyQuaternion(inv).normalize();
  const modelLocal = snapToPrincipalAxis(modelLocalRaw, 12);
  xrCalibration.pairs.push({ cube: cubeLocal, model: modelLocal });
  xrCalibration.pendingCube = null;

  if (xrCalibration.step === 'await-model-1') {
    xrCalibration.step = 'await-cube-2';
    xrCalibration.progress = 0.33;
  } else if (xrCalibration.step === 'await-model-2') {
    xrCalibration.progress = 0.75;
    xrCalibration.step = 'await-cube-3';
  } else if (xrCalibration.step === 'await-model-3') {
    xrCalibration.progress = 1;
    xrCalibration.verifyErrorDeg = computeXRVerifyError(xrCalibration.pairs);
    xrCalibration.step = 'done';
  }
  notify();
}

export function computeXRCalibrationQuaternion(pairs: XRPair[]): THREE.Quaternion | null {
  if (pairs.length < 2) return null;
  const n1 = pairs[0].model.clone().normalize();
  const n2raw = pairs[1].model.clone();
  const n2 = n2raw.sub(n1.clone().multiplyScalar(n2raw.dot(n1)));
  if (n2.lengthSq() < 1e-6) return null;
  n2.normalize();
  const n3 = new THREE.Vector3().crossVectors(n1, n2);

  const c1 = pairs[0].cube.clone().normalize();
  const c2raw = pairs[1].cube.clone();
  const c2 = c2raw.sub(c1.clone().multiplyScalar(c2raw.dot(c1)));
  if (c2.lengthSq() < 1e-6) return null;
  c2.normalize();
  const c3 = new THREE.Vector3().crossVectors(c1, c2);

  const N = new THREE.Matrix4().makeBasis(n1, n2, n3);
  const C = new THREE.Matrix4().makeBasis(c1, c2, c3);
  const Nt = N.clone().transpose();
  const R = new THREE.Matrix4().multiplyMatrices(C, Nt);
  const q = new THREE.Quaternion().setFromRotationMatrix(R);
  return q;
}

export function computeXRVerifyError(pairs: XRPair[]): number {
  if (pairs.length < 3) return NaN;
  const q = computeXRCalibrationQuaternion(pairs.slice(0, 2));
  if (!q) return NaN;
  const predicted = pairs[2].model.clone().applyQuaternion(q).normalize();
  const target = pairs[2].cube.clone().normalize();
  const cos = THREE.MathUtils.clamp(predicted.dot(target), -1, 1);
  return THREE.MathUtils.radToDeg(Math.acos(cos));
}

export function computeVirtRealTransform(
  v: THREE.Vector3[],
  r: THREE.Vector3[]
): { quaternion: THREE.Quaternion; position: THREE.Vector3 } | null {
  if (v.length < 3 || r.length < 3) return null;

  const v1 = v[0];
  const v2 = v[1];
  const v3 = v[2];

  const r1 = r[0];
  const r2 = r[1];
  const r3 = r[2];

  // 1. Basis local virtual
  const xv = new THREE.Vector3().subVectors(v2, v1).normalize();
  const v3_proj = new THREE.Vector3().subVectors(v3, v1);
  const dot_v = v3_proj.dot(xv);
  const yv = v3_proj.clone().sub(xv.clone().multiplyScalar(dot_v));
  if (yv.lengthSq() < 1e-6) return null;
  yv.normalize();
  const zv = new THREE.Vector3().crossVectors(xv, yv).normalize();

  // 2. Basis de mundo real
  const xr = new THREE.Vector3().subVectors(r2, r1).normalize();
  const r3_proj = new THREE.Vector3().subVectors(r3, r1);
  const dot_r = r3_proj.dot(xr);
  const yr = r3_proj.clone().sub(xr.clone().multiplyScalar(dot_r));
  if (yr.lengthSq() < 1e-6) return null;
  yr.normalize();
  const zr = new THREE.Vector3().crossVectors(xr, yr).normalize();

  // 3. Matriz de rotação R que mapeia virtual para real
  const Mv = new THREE.Matrix4().makeBasis(xv, yv, zv);
  const Mr = new THREE.Matrix4().makeBasis(xr, yr, zr);
  const Mv_t = Mv.clone().transpose();
  const R = Mr.clone().multiply(Mv_t);

  const quaternion = new THREE.Quaternion().setFromRotationMatrix(R);

  // 4. Translação T = r1 - R * v1
  const rv1 = v1.clone().applyQuaternion(quaternion);
  const position = new THREE.Vector3().subVectors(r1, rv1);

  return { quaternion, position };
}

export const activeModelGroupRef = { current: null as THREE.Group | null };
