SteelXR2/src/components/three/SmartMeasure.ts

import * as THREE from 'three';

const _v = new THREE.Vector3();
const _projected = new THREE.Vector3();

/**
 * Find the nearest vertex in a mesh to a world-space point,
 * returning it only if it's within `thresholdPx` pixels on screen.
 */
export function findNearestVertex(
  mesh: THREE.Mesh,
  worldPoint: THREE.Vector3,
  camera: THREE.Camera,
  canvasSize: { width: number; height: number },
  thresholdPx: number = 10
): THREE.Vector3 | null {
  const geo = mesh.geometry;
  const posAttr = geo.attributes.position;
  if (!posAttr) return null;

  // Project hit point to screen
  const hitScreen = worldPoint.clone().project(camera);
  const hitX = (hitScreen.x * 0.5 + 0.5) * canvasSize.width;
  const hitY = (-hitScreen.y * 0.5 + 0.5) * canvasSize.height;

  let bestDist = Infinity;
  let bestVertex: THREE.Vector3 | null = null;

  for (let i = 0; i < posAttr.count; i++) {
    _v.fromBufferAttribute(posAttr, i);
    _v.applyMatrix4(mesh.matrixWorld);

    _projected.copy(_v).project(camera);
    const sx = (_projected.x * 0.5 + 0.5) * canvasSize.width;
    const sy = (-_projected.y * 0.5 + 0.5) * canvasSize.height;

    const dx = sx - hitX;
    const dy = sy - hitY;
    const dist = Math.sqrt(dx * dx + dy * dy);

    if (dist < bestDist) {
      bestDist = dist;
      bestVertex = _v.clone();
    }
  }

  if (bestDist <= thresholdPx && bestVertex) {
    return bestVertex;
  }
  return null;
}

/**
 * Detect if the face under the cursor belongs to a cylindrical hole.
 * Uses face normals around the hit to detect radial patterns → circle fit.
 * Returns diameter in mm, or null if not a hole.
 */
export function detectHoleAtFace(
  mesh: THREE.Mesh,
  faceIndex: number
): { center: THREE.Vector3; diameterMM: number } | null {
  const geo = mesh.geometry;
  const posAttr = geo.attributes.position;
  const normalAttr = geo.attributes.normal;
  const indexAttr = geo.index;

  if (!posAttr || !normalAttr || !indexAttr) return null;

  // Get hit face normal
  const i0 = indexAttr.getX(faceIndex * 3);
  const i1 = indexAttr.getX(faceIndex * 3 + 1);
  const i2 = indexAttr.getX(faceIndex * 3 + 2);

  const hitNormal = new THREE.Vector3();
  const n0 = new THREE.Vector3().fromBufferAttribute(normalAttr, i0);
  const n1 = new THREE.Vector3().fromBufferAttribute(normalAttr, i1);
  const n2 = new THREE.Vector3().fromBufferAttribute(normalAttr, i2);
  hitNormal.addVectors(n0, n1).add(n2).normalize();

  const hitCenter = new THREE.Vector3();
  const p0 = new THREE.Vector3().fromBufferAttribute(posAttr, i0);
  const p1 = new THREE.Vector3().fromBufferAttribute(posAttr, i1);
  const p2 = new THREE.Vector3().fromBufferAttribute(posAttr, i2);
  hitCenter.addVectors(p0, p1).add(p2).divideScalar(3);

  // Collect neighboring faces with similar "cylindrical" normal pattern
  // Cylindrical faces have normals perpendicular to the cylinder axis
  // We look for faces whose normals are roughly perpendicular to each other
  // but share a common axis (the hole axis)

  const faceCount = indexAttr.count / 3;
  const cylinderVertices: THREE.Vector3[] = [];
  const faceNormals: THREE.Vector3[] = [];

  // Threshold for proximity (local search)
  const searchRadius = 0.05; // 50mm in model units

  for (let f = 0; f < faceCount; f++) {
    const fi0 = indexAttr.getX(f * 3);
    const fi1 = indexAttr.getX(f * 3 + 1);
    const fi2 = indexAttr.getX(f * 3 + 2);

    const fp0 = new THREE.Vector3().fromBufferAttribute(posAttr, fi0);
    const fp1 = new THREE.Vector3().fromBufferAttribute(posAttr, fi1);
    const fp2 = new THREE.Vector3().fromBufferAttribute(posAttr, fi2);

    const fc = new THREE.Vector3().addVectors(fp0, fp1).add(fp2).divideScalar(3);
    if (fc.distanceTo(hitCenter) > searchRadius) continue;

    const fn = new THREE.Vector3();
    const fn0 = new THREE.Vector3().fromBufferAttribute(normalAttr, fi0);
    const fn1 = new THREE.Vector3().fromBufferAttribute(normalAttr, fi1);
    const fn2 = new THREE.Vector3().fromBufferAttribute(normalAttr, fi2);
    fn.addVectors(fn0, fn1).add(fn2).normalize();

    // For cylindrical surfaces, normals should be roughly perpendicular to hole axis
    // and the dot product between hit normal and face normal reveals if they share curvature
    const dot = Math.abs(hitNormal.dot(fn));
    // Cylindrical faces: normals vary (dot < 0.95) but aren't opposite (dot > -0.5)
    if (dot < 0.98) {
      cylinderVertices.push(fp0, fp1, fp2);
      faceNormals.push(fn);
    }
  }

  if (cylinderVertices.length < 9) return null; // Need at least 3 faces

  // Try to find a common axis and fit a circle
  // Estimate axis as cross product of two differing normals
  let axis: THREE.Vector3 | null = null;
  for (let i = 1; i < faceNormals.length; i++) {
    const cross = new THREE.Vector3().crossVectors(faceNormals[0], faceNormals[i]);
    if (cross.length() > 0.1) {
      axis = cross.normalize();
      break;
    }
  }
  if (!axis) return null;

  // Project vertices onto plane perpendicular to axis
  // Use least-squares circle fit on 2D projections
  const basisU = new THREE.Vector3();
  const basisV = new THREE.Vector3();
  
  // Create orthonormal basis
  if (Math.abs(axis.x) < 0.9) {
    basisU.crossVectors(axis, new THREE.Vector3(1, 0, 0)).normalize();
  } else {
    basisU.crossVectors(axis, new THREE.Vector3(0, 1, 0)).normalize();
  }
  basisV.crossVectors(axis, basisU).normalize();

  // Project unique vertices to 2D
  const seen = new Set<string>();
  const points2D: { u: number; v: number }[] = [];
  
  for (const vert of cylinderVertices) {
    const key = `${vert.x.toFixed(6)},${vert.y.toFixed(6)},${vert.z.toFixed(6)}`;
    if (seen.has(key)) continue;
    seen.add(key);
    
    const rel = vert.clone().sub(hitCenter);
    points2D.push({
      u: rel.dot(basisU),
      v: rel.dot(basisV),
    });
  }

  if (points2D.length < 4) return null;

  // Least-squares circle fit (Kasa method)
  const result = circleFitKasa(points2D);
  if (!result) return null;

  const radiusModel = result.radius;
  const diameterMM = radiusModel * 2 * 1000;

  // Filter: reasonable hole sizes (2mm to 200mm diameter)
  if (diameterMM < 2 || diameterMM > 200) return null;

  // Reconstruct 3D center
  const center3D = hitCenter.clone()
    .add(basisU.clone().multiplyScalar(result.cx))
    .add(basisV.clone().multiplyScalar(result.cy));

  center3D.applyMatrix4(mesh.matrixWorld);

  return { center: center3D, diameterMM };
}

/** Kasa circle fit: returns { cx, cy, radius } in the input coordinate system */
function circleFitKasa(points: { u: number; v: number }[]): { cx: number; cy: number; radius: number } | null {
  const n = points.length;
  if (n < 3) return null;

  let su = 0, sv = 0, suu = 0, svv = 0, suv = 0, suuu = 0, svvv = 0, suvv = 0, svuu = 0;
  
  for (const p of points) {
    su += p.u; sv += p.v;
    suu += p.u * p.u; svv += p.v * p.v;
    suv += p.u * p.v;
    suuu += p.u * p.u * p.u; svvv += p.v * p.v * p.v;
    suvv += p.u * p.v * p.v; svuu += p.v * p.u * p.u;
  }

  const A = n * suu - su * su;
  const B = n * suv - su * sv;
  const C = n * svv - sv * sv;
  const D = 0.5 * (n * suuu + n * suvv - su * suu - su * svv);
  const E = 0.5 * (n * svvv + n * svuu - sv * suu - sv * svv);

  const denom = A * C - B * B;
  if (Math.abs(denom) < 1e-12) return null;

  const cx = (D * C - E * B) / denom;
  const cy = (A * E - B * D) / denom;

  let r2sum = 0;
  for (const p of points) {
    r2sum += (p.u - cx) ** 2 + (p.v - cy) ** 2;
  }
  const radius = Math.sqrt(r2sum / n);

  // Check fit quality: standard deviation of radii should be small relative to radius
  let variance = 0;
  for (const p of points) {
    const r = Math.sqrt((p.u - cx) ** 2 + (p.v - cy) ** 2);
    variance += (r - radius) ** 2;
  }
  const stdDev = Math.sqrt(variance / n);
  if (stdDev / radius > 0.15) return null; // Poor fit, not a circle

  return { cx, cy, radius };
}

/**
 * Find nearest edge segment from EdgesGeometry data.
 * Returns distance in mm or null.
 */
export function findNearestEdgeSegment(
  mesh: THREE.Mesh,
  worldPoint: THREE.Vector3,
  camera: THREE.Camera,
  canvasSize: { width: number; height: number },
  thresholdPx: number = 12
): { midpoint: THREE.Vector3; lengthMM: number } | null {
  // Look for edge line segments children
  let edgeLines: THREE.LineSegments | null = null;
  mesh.children.forEach(c => {
    if (c.userData.__edgeLine && c instanceof THREE.LineSegments) {
      edgeLines = c;
    }
  });

  // If no edge lines, generate from EdgesGeometry
  const geo = edgeLines?.geometry ?? new THREE.EdgesGeometry(mesh.geometry, 15);
  const posAttr = geo.attributes.position;
  if (!posAttr) return null;

  // Project hit point to screen
  const hitScreen = worldPoint.clone().project(camera);
  const hitX = (hitScreen.x * 0.5 + 0.5) * canvasSize.width;
  const hitY = (-hitScreen.y * 0.5 + 0.5) * canvasSize.height;

  const matrix = edgeLines ? edgeLines.matrixWorld.clone().premultiply(mesh.matrixWorld) : mesh.matrixWorld;
  
  let bestDist = Infinity;
  let bestMid: THREE.Vector3 | null = null;
  let bestLen = 0;

  const segCount = posAttr.count / 2;
  const a = new THREE.Vector3();
  const b = new THREE.Vector3();
  const pa = new THREE.Vector3();
  const pb = new THREE.Vector3();

  for (let i = 0; i < segCount; i++) {
    a.fromBufferAttribute(posAttr, i * 2).applyMatrix4(matrix);
    b.fromBufferAttribute(posAttr, i * 2 + 1).applyMatrix4(matrix);

    // Project to screen
    pa.copy(a).project(camera);
    pb.copy(b).project(camera);
    const ax = (pa.x * 0.5 + 0.5) * canvasSize.width;
    const ay = (-pa.y * 0.5 + 0.5) * canvasSize.height;
    const bx = (pb.x * 0.5 + 0.5) * canvasSize.width;
    const by = (-pb.y * 0.5 + 0.5) * canvasSize.height;

    // Distance from point to line segment in screen space
    const dist = pointToSegmentDist(hitX, hitY, ax, ay, bx, by);

    if (dist < bestDist) {
      bestDist = dist;
      bestMid = a.clone().add(b).multiplyScalar(0.5);
      bestLen = a.distanceTo(b) * 1000; // mm
    }
  }

  if (!edgeLines) geo.dispose();

  if (bestDist <= thresholdPx && bestMid && bestLen > 0.5) {
    return { midpoint: bestMid, lengthMM: bestLen };
  }
  return null;
}

function pointToSegmentDist(px: number, py: number, ax: number, ay: number, bx: number, by: number): number {
  const dx = bx - ax;
  const dy = by - ay;
  const lenSq = dx * dx + dy * dy;
  if (lenSq < 0.01) return Math.sqrt((px - ax) ** 2 + (py - ay) ** 2);

  let t = ((px - ax) * dx + (py - ay) * dy) / lenSq;
  t = Math.max(0, Math.min(1, t));
  const closestX = ax + t * dx;
  const closestY = ay + t * dy;
  return Math.sqrt((px - closestX) ** 2 + (py - closestY) ** 2);
}

/**
 * Combined snap resolver: tries vertex first, then edge midpoint,
 * falling back to the raw hit point. Returns { point, type }.
 */
export function resolveSnap(
  mesh: THREE.Mesh,
  hitPoint: THREE.Vector3,
  camera: THREE.Camera,
  canvasSize: { width: number; height: number },
  vertexThresholdPx: number = 12,
  edgeThresholdPx: number = 16
): { point: THREE.Vector3; type: 'vertex' | 'edge' | 'surface' } {
  const v = findNearestVertex(mesh, hitPoint, camera, canvasSize, vertexThresholdPx);
  if (v) return { point: v, type: 'vertex' };
  const e = findNearestEdgeSegment(mesh, hitPoint, camera, canvasSize, edgeThresholdPx);
  if (e) return { point: e.midpoint, type: 'edge' };
  return { point: hitPoint.clone(), type: 'surface' };
}