import { classifyTriangle } from './Polygon';

export interface Triangle {
  a: Vector3;
  b: Vector3;
  c: Vector3;
  bounds: AABB;
}

export interface AABB {
  minX: number;
  minY: number;
  minZ: number;

  maxX: number;
  maxY: number;
  maxZ: number;
}

const EPS = 1e-100;

// --- Fast Vector Math Helpers ---
const sub = (a: Vector3, b: Vector3) => new Vector3(a.x - b.x, a.y - b.y, a.z - b.z);
const dot = (a: Vector3, b: Vector3) => a.x * b.x + a.y * b.y + a.z * b.z;
const cross = (a: Vector3, b: Vector3) =>
  new Vector3(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x);

const normalize = (v: Vector3): Vector3 => {
  const len = Math.sqrt(dot(v, v));
  return len === 0 ? new Vector3(0, 0, 0) : new Vector3(v.x / len, v.y / len, v.z / len);
};

// --- Main Intersection Function ---

function rotateVector(v: Vector3, q: Quat): Vector3 {
  const { x: qx, y: qy, z: qz, w: qw } = q;
  const { x: vx, y: vy, z: vz } = v;

  // Quaternion vector multiplication (q * v * q^-1)
  const ix = qw * vx + qy * vz - qz * vy;
  const iy = qw * vy + qz * vx - qx * vz;
  const iz = qw * vz + qx * vy - qy * vx;
  const iw = -qx * vx - qy * vy - qz * vz;

  return new Vector3(
    ix * qw + iw * -qx + iy * -qz - iz * -qy,
    iy * qw + iw * -qy + iz * -qx - ix * -qz,
    iz * qw + iw * -qz + ix * -qy - iy * -qx
  );
}

export function getworldcoord(tri: Triangle, newOrigin: Vector3, r: Quat): Triangle {
  const transformPoint = (p: Vector3) => {
    const rotated = rotateVector(p, r);
    return new Vector3(
      rotated.x + newOrigin.x,
      rotated.y + newOrigin.y,
      rotated.z + newOrigin.z
    );
  };

  const A = transformPoint(tri.a);
  const B = transformPoint(tri.b);
  const C = transformPoint(tri.c);

  return {
    a: A,
    b: B,
    c: C,
    bounds: getTriangleAABB(A, B, C),
  };
}
export function intersectTriangles3D(triA: Triangle, triB: Triangle): Vector3[] {
  // Plane A normal & signed vertex distances of TriB
  const nA = cross(sub(triA.b, triA.a), sub(triA.c, triA.a));
  const dB = getPlaneDistances(triB, nA, triA.a);
  if (isDisjoint(dB)) return [];

  // Plane B normal & signed vertex distances of TriA
  const nB = cross(sub(triB.b, triB.a), sub(triB.c, triB.a));
  const dA = getPlaneDistances(triA, nB, triB.a);
  if (isDisjoint(dA)) return [];

  // Coplanar Triangles
  if (dB[0] === 0 && dB[1] === 0 && dB[2] === 0) {
    return intersectCoplanarTriangles(triA, triB, nA);
  }

  // Intersection line direction & intervals
  const dL = cross(nA, nB);
  const intA = computeInterval(triA, dA, dL);
  const intB = computeInterval(triB, dB, dL);
  if (!intA || !intB) return [];

  // Overlap interval check
  const tStart = Math.max(intA.min.t, intB.min.t);
  const tEnd = Math.min(intA.max.t, intB.max.t);

  if (tStart > tEnd + EPS) return []; // No overlap

  const p1 = getPointOnLine(intA, intB, tStart);
  if (Math.abs(tStart - tEnd) <= EPS) return [p1]; // Point contact

  return [p1, getPointOnLine(intA, intB, tEnd)]; // Line segment
}

// --- Helper Utilities ---

function getPlaneDistances(
  tri: Triangle,
  norm: Vector3,
  origin: Vector3
): [number, number, number] {
  return [
    clean(dot(norm, sub(tri.a, origin))),
    clean(dot(norm, sub(tri.b, origin))),
    clean(dot(norm, sub(tri.c, origin))),
  ];
}

const clean = (v: number) => (Math.abs(v) < EPS ? 0 : v);

const isDisjoint = (d: number[]) =>
  (d[0] > 0 && d[1] > 0 && d[2] > 0) || (d[0] < 0 && d[1] < 0 && d[2] < 0);

interface IntervalPt {
  t: number;
  point: Vector3;
}
interface Interval {
  min: IntervalPt;
  max: IntervalPt;
}

function computeInterval(tri: Triangle, d: number[], dL: Vector3): Interval | null {
  const v = [tri.a, tri.b, tri.c];
  const pts: IntervalPt[] = [];
  for (let i = 0; i < 3; i++) {
    const j = (i + 1) % 3;
    if (d[i] * d[j] <= 0 && d[i] !== d[j]) {
      const a = d[i] / (d[i] - d[j]);
      const p = new Vector3(
        v[i].x + a * (v[j].x - v[i].x),
        v[i].y + a * (v[j].y - v[i].y),
        v[i].z + a * (v[j].z - v[i].z)
      );
      pts.push({ t: dot(dL, p), point: p });
    } else if (d[i] === 0) {
      pts.push({ t: dot(dL, v[i]), point: v[i] });
    }
  }
  if (pts.length < 2) return null;
  return pts[0].t > pts[1].t ? { min: pts[1], max: pts[0] } : { min: pts[0], max: pts[1] };
}

function getPointOnLine(intA: Interval, intB: Interval, t: number): Vector3 {
  const pts = [intA.min, intA.max, intB.min, intB.max];
  let best = pts[0],
    minDiff = Math.abs(pts[0].t - t);
  for (let i = 1; i < pts.length; i++) {
    const diff = Math.abs(pts[i].t - t);
    if (diff < minDiff) {
      minDiff = diff;
      best = pts[i];
    }
  }
  return best.point;
}

// --- Coplanar Fallback ---

function intersectCoplanarTriangles(triA: Triangle, triB: Triangle, norm: Vector3): Vector3[] {
  const ax = Math.abs(norm.x),
    ay = Math.abs(norm.y),
    az = Math.abs(norm.z);
  const drop = ax >= ay && ax >= az ? 0 : ay >= ax && ay >= az ? 1 : 2;

  const proj = (p: Vector3) =>
    drop === 0
      ? new Vector2(p.y, p.z)
      : drop === 1
      ? new Vector2(p.x, p.z)
      : new Vector2(p.x, p.y);

  let output = [triA.a, triA.b, triA.c].map(proj);
  const polyB = [triB.a, triB.b, triB.c].map(proj);

  for (let i = 0; i < 3; i++) {
    const input = output;
    output = [];
    const b1 = polyB[i],
      b2 = polyB[(i + 1) % 3];

    for (let j = 0; j < input.length; j++) {
      const p1 = input[j],
        p2 = input[(j + 1) % input.length];
      const in1 = (b2.x - b1.x) * (p1.y - b1.y) - (b2.y - b1.y) * (p1.x - b1.x) >= -EPS;
      const in2 = (b2.x - b1.x) * (p2.y - b1.y) - (b2.y - b1.y) * (p2.x - b1.x) >= -EPS;

      if (in1) {
        output.push(p1);
        if (!in2) output.push(intersectLines2D(p1, p2, b1, b2));
      } else if (in2) {
        output.push(intersectLines2D(p1, p2, b1, b2));
      }
    }
  }

  const d = -dot(norm, triA.a);
  return output.map((p) => {
    if (drop === 0) return new Vector3(-(norm.y * p.x + norm.z * p.y + d) / norm.x, p.x, p.y);
    if (drop === 1) return new Vector3(p.x, -(norm.x * p.x + norm.z * p.y + d) / norm.y, p.y);
    return new Vector3(p.x, p.y, -(norm.x * p.x + norm.y * p.y + d) / norm.z);
  });
}

function intersectLines2D(p1: Vector2, p2: Vector2, q1: Vector2, q2: Vector2): Vector2 {
  const a1 = p2.y - p1.y,
    b1 = p1.x - p2.x,
    c1 = a1 * p1.x + b1 * p1.y;
  const a2 = q2.y - q1.y,
    b2 = q1.x - q2.x,
    c2 = a2 * q1.x + b2 * q1.y;
  const det = a1 * b2 - a2 * b1;
  return new Vector2((b2 * c1 - b1 * c2) / det, (a1 * c2 - a2 * c1) / det);
}

export function intersectsAABB(a: AABB, b: AABB): boolean {
  return (
    a.minX <= b.maxX &&
    a.maxX >= b.minX &&
    a.minY <= b.maxY &&
    a.maxY >= b.minY &&
    a.minZ <= b.maxZ &&
    a.maxZ >= b.minZ
  );
}

export function getTriangleAABB(p1: Vector3, p2: Vector3, p3: Vector3): AABB {
  return {
    minX: Math.min(p1.x, p2.x, p3.x),
    minY: Math.min(p1.y, p2.y, p3.y),
    minZ: Math.min(p1.z, p2.z, p3.z),
    maxX: Math.max(p1.x, p2.x, p3.x),
    maxY: Math.max(p1.y, p2.y, p3.y),
    maxZ: Math.max(p1.z, p2.z, p3.z),
  };
}

// --- Triangle Cache (per-object memoization of edges & normal) ---
// Safe as long as Triangle objects are never mutated in place after
// creation, which holds throughout this file — getworldcoord and every
// clip/triangulate step always construct a brand-new Triangle rather
// than editing .a/.b/.c on an existing one. This turns "recompute edges
// and normal on every SAT comparison" into "compute once per triangle,
// reuse on every subsequent comparison."

interface TriCache {
  normal: Vector3;
  edges: [Vector3, Vector3, Vector3];
}

const triCache = new WeakMap<Triangle, TriCache>();

function getTriCache(t: Triangle): TriCache {
  let c = triCache.get(t);
  if (!c) {
    const e0 = sub(t.b, t.a);
    const e1 = sub(t.c, t.b);
    const e2 = sub(t.a, t.c);
    const normal = cross(e0, sub(t.c, t.a)); // matches original intersectsSAT arithmetic exactly
    c = { normal, edges: [e0, e1, e2] };
    triCache.set(t, c);
  }
  return c;
}

function isSeparatingAxis(axis: Vector3, t1: Triangle, t2: Triangle): boolean {
  const lengthSq = dot(axis, axis);
  if (lengthSq < 1e-100) return false;

  const p1a = dot(t1.a, axis);
  const p1b = dot(t1.b, axis);
  const p1c = dot(t1.c, axis);
  const min1 = Math.min(p1a, p1b, p1c);
  const max1 = Math.max(p1a, p1b, p1c);

  const p2a = dot(t2.a, axis);
  const p2b = dot(t2.b, axis);
  const p2c = dot(t2.c, axis);
  const min2 = Math.min(p2a, p2b, p2c);
  const max2 = Math.max(p2a, p2b, p2c);

  return max1 < min2 || max2 < min1;
}

export function intersectsSAT(t1: Triangle, t2: Triangle): boolean {
  const c1 = getTriCache(t1);
  const c2 = getTriCache(t2);

  if (isSeparatingAxis(c1.normal, t1, t2)) return false;
  if (isSeparatingAxis(c2.normal, t1, t2)) return false;

  const edges1 = c1.edges;
  const edges2 = c2.edges;

  for (let i = 0; i < 3; i++) {
    for (let j = 0; j < 3; j++) {
      const axis = cross(edges1[i], edges2[j]);
      if (isSeparatingAxis(axis, t1, t2)) return false;
    }
  }

  return true;
}

export function isIntersect(t1: Triangle, t2: Triangle): boolean {
  if (!intersectsAABB(t1.bounds, t2.bounds)) return false;
  return intersectsSAT(t1, t2);
}

/**
 * Robust point-in-mesh test via generalized winding number.
 */
export function isPointInsideMesh(point: Vector3, triangles: Triangle[]): boolean {
  let solidAngleSum = 0;

  for (let i = 0; i < triangles.length; i++) {
    const ra = sub(triangles[i].a, point);
    const rb = sub(triangles[i].b, point);
    const rc = sub(triangles[i].c, point);

    const la = Math.sqrt(dot(ra, ra));
    const lb = Math.sqrt(dot(rb, rb));
    const lc = Math.sqrt(dot(rc, rc));

    if (la < EPS || lb < EPS || lc < EPS) continue;

    const numerator = dot(ra, cross(rb, rc));
    const denominator =
      la * lb * lc +
      dot(ra, rb) * lc +
      dot(rb, rc) * la +
      dot(rc, ra) * lb;

    solidAngleSum += 2 * Math.atan2(numerator, denominator);
  }

  return solidAngleSum / (4 * Math.PI) > 0.5;
}

// --- Polygon Triangulation ---

const MERGE_EPS_SQ = 1e-100;

function buildPlaneBasis(normal: Vector3): { u: Vector3; v: Vector3 } {
  const n = normalize(normal);
  const helper = Math.abs(n.x) < 0.9 ? new Vector3(1, 0, 0) : new Vector3(0, 1, 0);
  const u = normalize(cross(helper, n));
  const v = cross(n, u);
  return { u, v };
}

function makeTriangle(a: Vector3, b: Vector3, c: Vector3): Triangle {
  return { a, b, c, bounds: getTriangleAABB(a, b, c) };
}

function dedupePoints(points: Vector3[]): Vector3[] {
  const out: Vector3[] = [];
  for (const p of points) {
    const isDup = out.some((q) => {
      const d = sub(p, q);
      return dot(d, d) < MERGE_EPS_SQ;
    });
    if (!isDup) out.push(p);
  }
  return out;
}

export function triangulateCoplanarPoints(points: Vector3[], normal: Vector3): Triangle[] {
  const unique = dedupePoints(points);
  if (unique.length < 3) return [];

  if (unique.length === 3) {
    const [a, b, c] = unique;
    return classifyTriangle(a, b, c) !== 'degenerate' ? [makeTriangle(a, b, c)] : [];
  }

  const { u, v } = buildPlaneBasis(normal);

  let cx = 0,
    cy = 0,
    cz = 0;
  unique.forEach((p) => {
    cx += p.x;
    cy += p.y;
    cz += p.z;
  });
  const centroid = new Vector3(cx / unique.length, cy / unique.length, cz / unique.length);

  const projected = unique.map((p) => {
    const rel = sub(p, centroid);
    return { point: p, x: dot(rel, u), y: dot(rel, v) };
  });

  projected.sort((p1, p2) => Math.atan2(p1.y, p1.x) - Math.atan2(p2.y, p2.x));

  const triangles: Triangle[] = [];
  for (let i = 1; i < projected.length - 1; i++) {
    const a = projected[0].point,
      b = projected[i].point,
      c = projected[i + 1].point;
    if (classifyTriangle(a, b, c) !== 'degenerate') {
      triangles.push(makeTriangle(a, b, c));
    }
  }
  return triangles;
}

export const triangleNormal = (t: Triangle) => getTriCache(t).normal;

// --- Multi-plane Clipping ---

function clipPolygonByPlane(
  poly: Vector3[],
  planeNormal: Vector3,
  planePoint: Vector3
): { insidePoly: Vector3[]; outsidePoly: Vector3[] } {
  const insidePoly: Vector3[] = [];
  const outsidePoly: Vector3[] = [];
  const n = poly.length;
  if (n === 0) return { insidePoly, outsidePoly };

  const dists = poly.map((p) => clean(dot(planeNormal, sub(p, planePoint))));

  for (let i = 0; i < n; i++) {
    const j = (i + 1) % n;
    const pi = poly[i],
      pj = poly[j];
    const di = dists[i],
      dj = dists[j];

    if (di <= 0) insidePoly.push(pi);
    if (di >= 0) outsidePoly.push(pi);

    if ((di < 0 && dj > 0) || (di > 0 && dj < 0)) {
      const t = di / (di - dj);
      const ip = new Vector3(
        pi.x + t * (pj.x - pi.x),
        pi.y + t * (pj.y - pi.y),
        pi.z + t * (pj.z - pi.z)
      );
      insidePoly.push(ip);
      outsidePoly.push(ip);
    }
  }
  return { insidePoly, outsidePoly };
}

function fragmentCentroid(poly: Vector3[]): Vector3 {
  let x = 0,
    y = 0,
    z = 0;
  poly.forEach((p) => {
    x += p.x;
    y += p.y;
    z += p.z;
  });
  const n = poly.length;
  return new Vector3(x / n, y / n, z / n);
}

// --- AABB Tree / BVH for broad-phase culling ---

export interface BVHNode {
  bounds: AABB;
  left: BVHNode | null;
  right: BVHNode | null;
  triangles: Triangle[] | null; // set only on leaf nodes
}

const LEAF_SIZE = 4; // stop splitting once a node holds this few triangles

function mergeAABB(a: AABB, b: AABB): AABB {
  return {
    minX: Math.min(a.minX, b.minX), minY: Math.min(a.minY, b.minY), minZ: Math.min(a.minZ, b.minZ),
    maxX: Math.max(a.maxX, b.maxX), maxY: Math.max(a.maxY, b.maxY), maxZ: Math.max(a.maxZ, b.maxZ),
  };
}

function triangleCentroid(t: Triangle): Vector3 {
  return new Vector3((t.a.x + t.b.x + t.c.x) / 3, (t.a.y + t.b.y + t.c.y) / 3, (t.a.z + t.b.z + t.c.z) / 3);
}

function axisValue(v: Vector3, axis: 'x' | 'y' | 'z'): number {
  return axis === 'x' ? v.x : axis === 'y' ? v.y : v.z;
}

/** Builds a BVH once per mesh, reusing each triangle's existing `.bounds`. */
export function buildBVH(triangles: Triangle[]): BVHNode {
  const bounds = triangles.reduce((acc, t) => mergeAABB(acc, t.bounds), triangles[0].bounds);

  if (triangles.length <= LEAF_SIZE) {
    return { bounds, left: null, right: null, triangles };
  }

  // Median split along whichever axis this node's bounds are longest on —
  // adapts to local density instead of a fixed grid.
  const extX = bounds.maxX - bounds.minX, extY = bounds.maxY - bounds.minY, extZ = bounds.maxZ - bounds.minZ;
  const axis: 'x' | 'y' | 'z' = extX >= extY && extX >= extZ ? 'x' : extY >= extZ ? 'y' : 'z';

  // Compute each centroid once, up front, instead of re-deriving it on
  // every comparator call during sort (default sort makes ~n log n calls).
  const withKeys = triangles.map(t => ({ t, key: axisValue(triangleCentroid(t), axis) }));
  withKeys.sort((p, q) => p.key - q.key);
  const sorted = withKeys.map(x => x.t);

  const mid = Math.floor(sorted.length / 2);

  return { bounds, left: buildBVH(sorted.slice(0, mid)), right: buildBVH(sorted.slice(mid)), triangles: null };
}

/** Returns candidate triangles whose bounds overlap `queryBounds` — a superset that still needs an exact test (e.g. SAT) for confirmation. */
export function queryBVH(node: BVHNode, queryBounds: AABB, out: Triangle[] = []): Triangle[] {
  if (!intersectsAABB(node.bounds, queryBounds)) return out;
  if (node.triangles) {
    for (const t of node.triangles) {
      if (intersectsAABB(t.bounds, queryBounds)) out.push(t);
    }
    return out;
  }
  if (node.left) queryBVH(node.left, queryBounds, out);
  if (node.right) queryBVH(node.right, queryBounds, out);
  return out;
}

export function clipTriangleAgainstMesh(
  tri: Triangle,
  otherTriangles: Triangle[],
  keepOutside: boolean,
  otherBVH?: BVHNode
): Triangle[] {
  const cutters = otherBVH
    ? queryBVH(otherBVH, tri.bounds).filter(o => intersectsSAT(tri, o)) // AABB already confirmed by queryBVH
    : otherTriangles.filter(o => isIntersect(tri, o)); // fallback still needs the AABB check

  if (cutters.length === 0) {
    const inside = isPointInsideMesh(fragmentCentroid([tri.a, tri.b, tri.c]), otherTriangles);
    return inside !== keepOutside ? [tri] : [];
  }

  let fragments: Vector3[][] = [[tri.a, tri.b, tri.c]];
  for (const cutter of cutters) {
    const next: Vector3[][] = [];
    const n = triangleNormal(cutter);
    for (const frag of fragments) {
      const { insidePoly, outsidePoly } = clipPolygonByPlane(frag, n, cutter.a);
      if (insidePoly.length >= 3) next.push(insidePoly);
      if (outsidePoly.length >= 3) next.push(outsidePoly);
    }
    fragments = next;
  }

  const triNormal = triangleNormal(tri);
  const result: Triangle[] = [];
  for (const frag of fragments) {
    const inside = isPointInsideMesh(fragmentCentroid(frag), otherTriangles);
    if (inside !== keepOutside) {
      result.push(...triangulateCoplanarPoints(frag, triNormal));
    }
  }
  return result;
}

export function clipTriangleOutside(tri: Triangle, otherTriangles: Triangle[], otherBVH?: BVHNode): Triangle[] {
  return clipTriangleAgainstMesh(tri, otherTriangles, true, otherBVH);
}

export function clipTriangleInside(tri: Triangle, otherTriangles: Triangle[], otherBVH?: BVHNode): Triangle[] {
  return clipTriangleAgainstMesh(tri, otherTriangles, false, otherBVH);
}