scada_device_layout/svelte-app/src/lib/canvas/collision.ts

import type { AABB, EpcWaypoint } from '../types.js';
import { SPACING_EXEMPT, isCurvedType, isSpurType, isEpcType, isInductionType, getCurveGeometry, EPC_CONFIG, INDUCTION_CONFIG } from '../symbols.js';
import { layout } from '../stores/layout.svelte.js';
import { orientedBoxCorners, createCurveTransforms } from './geometry.js';
import { getAABB, distPointToSegment, distToAnnularSector, pointToOBBDist, pointInConvexPolygon, pointToConvexPolygonDist, edgeDistance } from './distance.js';

// Re-export for consumers that import from collision.ts
export { getAABB, clamp } from './distance.js';
export { snapToGrid, findValidPosition } from './grid-snap.js';

// ─── OBB collision via Separating Axis Theorem ───

function obbOverlapWithSpacing(
  ax: number, ay: number, aw: number, ah: number, arot: number,
  bx: number, by: number, bw: number, bh: number, brot: number,
  spacing: number
): boolean {
  const acx = ax + aw / 2, acy = ay + ah / 2;
  const bcx = bx + bw / 2, bcy = by + bh / 2;
  const ahw = aw / 2 + spacing, ahh = ah / 2 + spacing;
  const bhw = bw / 2, bhh = bh / 2;

  const arad = (arot || 0) * Math.PI / 180;
  const brad = (brot || 0) * Math.PI / 180;
  const cosA = Math.cos(arad), sinA = Math.sin(arad);
  const cosB = Math.cos(brad), sinB = Math.sin(brad);
  const tx = bcx - acx, ty = bcy - acy;

  const axes: [number, number][] = [
    [cosA, sinA], [-sinA, cosA],
    [cosB, sinB], [-sinB, cosB],
  ];

  for (const [nx, ny] of axes) {
    const projA = ahw * Math.abs(cosA * nx + sinA * ny) + ahh * Math.abs(-sinA * nx + cosA * ny);
    const projB = bhw * Math.abs(cosB * nx + sinB * ny) + bhh * Math.abs(-sinB * nx + cosB * ny);
    const dist = Math.abs(tx * nx + ty * ny);
    if (dist > projA + projB) return false;
  }
  return true;
}

// ─── Convex polygon helpers ───

/** Get the 4 vertices of a spur trapezoid in world space (with rotation and mirror) */
function getSpurVertices(
  x: number, y: number, w: number, h: number, w2: number, rotation: number, mirrored?: boolean
): [number, number][] {
  const cx = x + w / 2;
  const cy = y + h / 2;
  let local: [number, number][];
  if (mirrored) {
    // Mirrored: angled edge on left side instead of right
    local = [
      [x + w - w2, y],   // TL (narrow end, mirrored)
      [x + w, y],        // TR
      [x + w, y + h],    // BR
      [x, y + h],        // BL (wide end, mirrored)
    ];
  } else {
    local = [
      [x, y],           // TL
      [x + w2, y],      // TR (top base end)
      [x + w, y + h],   // BR (bottom base end)
      [x, y + h],       // BL
    ];
  }
  if (!rotation) return local;
  const rad = rotation * Math.PI / 180;
  const cos = Math.cos(rad), sin = Math.sin(rad);
  return local.map(([px, py]) => {
    const dx = px - cx, dy = py - cy;
    return [cx + dx * cos - dy * sin, cy + dx * sin + dy * cos] as [number, number];
  });
}

/** Get the convex hull vertices of an induction shape (arrow + strip) in world space */
function getInductionVertices(
  x: number, y: number, w: number, h: number, rotation: number, mirrored?: boolean
): [number, number][] {
  const hw = INDUCTION_CONFIG.headWidth;
  const stripTopY = y + h * INDUCTION_CONFIG.stripTopFrac;
  const stripBottomY = y + h * INDUCTION_CONFIG.stripBottomFrac;
  const arrowPts = INDUCTION_CONFIG.arrowPoints;
  const cx = x + w / 2;
  let local: [number, number][];
  if (mirrored) {
    // Mirror arrow x-coords around center
    local = [
      [x, stripTopY],
      [x + w - arrowPts[0][0] * hw, y + arrowPts[0][1] * h],
      [x + w - arrowPts[1][0] * hw, y + arrowPts[1][1] * h],
      [x + w - arrowPts[2][0] * hw, y + arrowPts[2][1] * h],
      [x + w - arrowPts[3][0] * hw, y + arrowPts[3][1] * h],
      [x + w - arrowPts[5][0] * hw, y + arrowPts[5][1] * h],
      [x, stripBottomY],
    ];
  } else {
    local = [
      [x + w, stripTopY],
      [x + arrowPts[0][0] * hw, y + arrowPts[0][1] * h],
      [x + arrowPts[1][0] * hw, y + arrowPts[1][1] * h],
      [x + arrowPts[2][0] * hw, y + arrowPts[2][1] * h],
      [x + arrowPts[3][0] * hw, y + arrowPts[3][1] * h],
      [x + arrowPts[5][0] * hw, y + arrowPts[5][1] * h],
      [x, stripBottomY],
    ];
  }
  if (!rotation) return local;
  const cy = y + h / 2;
  const rad = rotation * Math.PI / 180;
  const cos = Math.cos(rad), sin = Math.sin(rad);
  return local.map(([px, py]) => {
    const dx = px - cx, dy = py - cy;
    return [cx + dx * cos - dy * sin, cy + dx * sin + dy * cos] as [number, number];
  });
}

/** Get the 4 vertices of an OBB in world space */
function getOBBVertices(
  x: number, y: number, w: number, h: number, rotation: number
): [number, number][] {
  const cx = x + w / 2, cy = y + h / 2;
  const rad = (rotation || 0) * Math.PI / 180;
  const cos = Math.cos(rad), sin = Math.sin(rad);
  const hw = w / 2, hh = h / 2;
  return [
    [cx - hw * cos + hh * sin, cy - hw * sin - hh * cos], // TL
    [cx + hw * cos + hh * sin, cy + hw * sin - hh * cos], // TR
    [cx + hw * cos - hh * sin, cy + hw * sin + hh * cos], // BR
    [cx - hw * cos - hh * sin, cy - hw * sin + hh * cos], // BL
  ];
}

/** General SAT for two convex polygons with spacing tolerance */
function polygonSATWithSpacing(
  aVerts: [number, number][],
  bVerts: [number, number][],
  spacing: number
): boolean {
  const allVerts = [aVerts, bVerts];
  for (const verts of allVerts) {
    for (let i = 0; i < verts.length; i++) {
      const j = (i + 1) % verts.length;
      const ex = verts[j][0] - verts[i][0];
      const ey = verts[j][1] - verts[i][1];
      const len = Math.sqrt(ex * ex + ey * ey);
      if (len === 0) continue;
      const nx = -ey / len, ny = ex / len;

      let aMin = Infinity, aMax = -Infinity;
      for (const [px, py] of aVerts) {
        const proj = px * nx + py * ny;
        aMin = Math.min(aMin, proj);
        aMax = Math.max(aMax, proj);
      }

      let bMin = Infinity, bMax = -Infinity;
      for (const [px, py] of bVerts) {
        const proj = px * nx + py * ny;
        bMin = Math.min(bMin, proj);
        bMax = Math.max(bMax, proj);
      }

      const gap = Math.max(bMin - aMax, aMin - bMax);
      if (gap > spacing) return false;
    }
  }
  return true;
}

/** Check if point is inside a convex polygon (CCW or CW winding) */
// ─── Curved vs OBB: true geometric check ───

interface CurveParams {
  x: number; y: number; w: number; h: number;
  rotation: number; symbolId: string; curveAngle?: number;
}

/**
 * Check if an OBB violates spacing against an annular sector (curved conveyor).
 * Uses true geometric distance instead of AABB approximation.
 */
function checkCurvedVsOBB(
  curve: CurveParams,
  bx: number, by: number, bw: number, bh: number, brot: number,
  spacing: number
): boolean {
  const angle = curve.curveAngle || 90;
  const { arcCx: acx, arcCy: acy, outerR, innerR } = getCurveGeometry(curve.symbolId, curve.x, curve.y, curve.w, curve.h);
  const curveRot = (curve.rotation || 0) * Math.PI / 180;
  const sweepRad = (angle * Math.PI) / 180;

  const scx = curve.x + curve.w / 2;
  const scy = curve.y + curve.h / 2;

  const { toLocal, toWorld } = createCurveTransforms(acx, acy, scx, scy, curveRot);

  const otherCx = bx + bw / 2, otherCy = by + bh / 2;
  const oRad = (brot || 0) * Math.PI / 180;
  const oCos = Math.cos(oRad), oSin = Math.sin(oRad);
  const ohw = bw / 2, ohh = bh / 2;

  const corners: [number, number][] = [
    [otherCx + ohw * oCos - ohh * oSin, otherCy + ohw * oSin + ohh * oCos],
    [otherCx - ohw * oCos - ohh * oSin, otherCy - ohw * oSin + ohh * oCos],
    [otherCx - ohw * oCos + ohh * oSin, otherCy - ohw * oSin - ohh * oCos],
    [otherCx + ohw * oCos + ohh * oSin, otherCy + ohw * oSin - ohh * oCos],
  ];

  // Check OBB corners against the annular sector
  for (const [wx, wy] of corners) {
    const [lx, ly] = toLocal(wx, wy);
    if (distToAnnularSector(lx, ly, innerR, outerR, sweepRad) < spacing) return true;
  }

  // Check edge midpoints and quarter-points for better coverage on long edges
  for (let i = 0; i < 4; i++) {
    const [x1, y1] = corners[i];
    const [x2, y2] = corners[(i + 1) % 4];
    for (const t of [0.25, 0.5, 0.75]) {
      const [lx, ly] = toLocal(x1 + (x2 - x1) * t, y1 + (y2 - y1) * t);
      if (distToAnnularSector(lx, ly, innerR, outerR, sweepRad) < spacing) return true;
    }
  }

  // Check arc sample points against the OBB
  const SAMPLES = Math.max(4, Math.ceil(angle / 10));
  for (let i = 0; i <= SAMPLES; i++) {
    const a = (i / SAMPLES) * sweepRad;
    const cos = Math.cos(a), sin = Math.sin(a);

    const [owx, owy] = toWorld(acx + outerR * cos, acy - outerR * sin);
    if (pointToOBBDist(owx, owy, bx, by, bw, bh, brot) < spacing) return true;

    if (innerR > 0) {
      const [iwx, iwy] = toWorld(acx + innerR * cos, acy - innerR * sin);
      if (pointToOBBDist(iwx, iwy, bx, by, bw, bh, brot) < spacing) return true;
    }
  }

  return false;
}

// ─── Curved vs Spur: geometric check ───

function checkCurvedVsSpur(
  curve: CurveParams,
  spurVerts: [number, number][],
  spacing: number
): boolean {
  const angle = curve.curveAngle || 90;
  const { arcCx: acx, arcCy: acy, outerR, innerR } = getCurveGeometry(curve.symbolId, curve.x, curve.y, curve.w, curve.h);
  const curveRot = (curve.rotation || 0) * Math.PI / 180;
  const sweepRad = (angle * Math.PI) / 180;

  const scx = curve.x + curve.w / 2;
  const scy = curve.y + curve.h / 2;

  const { toLocal, toWorld } = createCurveTransforms(acx, acy, scx, scy, curveRot);

  // Check spur vertices + edge sample points against annular sector
  for (const [wx, wy] of spurVerts) {
    const [lx, ly] = toLocal(wx, wy);
    if (distToAnnularSector(lx, ly, innerR, outerR, sweepRad) < spacing) return true;
  }
  for (let i = 0; i < spurVerts.length; i++) {
    const [x1, y1] = spurVerts[i];
    const [x2, y2] = spurVerts[(i + 1) % spurVerts.length];
    for (const t of [0.25, 0.5, 0.75]) {
      const [lx, ly] = toLocal(x1 + (x2 - x1) * t, y1 + (y2 - y1) * t);
      if (distToAnnularSector(lx, ly, innerR, outerR, sweepRad) < spacing) return true;
    }
  }

  // Check arc sample points against spur polygon
  const SAMPLES = Math.max(4, Math.ceil(angle / 10));
  for (let i = 0; i <= SAMPLES; i++) {
    const a = (i / SAMPLES) * sweepRad;
    const cos = Math.cos(a), sin = Math.sin(a);

    const [owx, owy] = toWorld(acx + outerR * cos, acy - outerR * sin);
    if (pointToConvexPolygonDist(owx, owy, spurVerts) < spacing) return true;

    if (innerR > 0) {
      const [iwx, iwy] = toWorld(acx + innerR * cos, acy - innerR * sin);
      if (pointToConvexPolygonDist(iwx, iwy, spurVerts) < spacing) return true;
    }
  }

  return false;
}

// ─── Curved band AABBs (only for curved vs curved fallback) ───

function getCurvedBandAABBs(sym: CurveParams): AABB[] {
  const angle = sym.curveAngle || 90;
  const { arcCx: acx, arcCy: acy, outerR, innerR } = getCurveGeometry(sym.symbolId, sym.x, sym.y, sym.w, sym.h);
  const rot = (sym.rotation || 0) * Math.PI / 180;
  const sweepRad = (angle * Math.PI) / 180;
  const scx = sym.x + sym.w / 2;
  const scy = sym.y + sym.h / 2;

  const cosR = Math.cos(rot), sinR = Math.sin(rot);
  function rotatePoint(px: number, py: number): [number, number] {
    if (!rot) return [px, py];
    const dx = px - scx, dy = py - scy;
    return [scx + dx * cosR - dy * sinR, scy + dx * sinR + dy * cosR];
  }

  const STEPS = Math.max(3, Math.ceil(angle / 10));
  const stepRad = sweepRad / STEPS;
  const boxes: AABB[] = [];

  for (let i = 0; i < STEPS; i++) {
    const a0 = i * stepRad, a1 = (i + 1) * stepRad;
    const p1 = rotatePoint(acx + outerR * Math.cos(a0), acy - outerR * Math.sin(a0));
    const p2 = rotatePoint(acx + outerR * Math.cos(a1), acy - outerR * Math.sin(a1));
    const p3 = rotatePoint(acx + innerR * Math.cos(a0), acy - innerR * Math.sin(a0));
    const p4 = rotatePoint(acx + innerR * Math.cos(a1), acy - innerR * Math.sin(a1));
    const minX = Math.min(p1[0], p2[0], p3[0], p4[0]);
    const minY = Math.min(p1[1], p2[1], p3[1], p4[1]);
    const maxX = Math.max(p1[0], p2[0], p3[0], p4[0]);
    const maxY = Math.max(p1[1], p2[1], p3[1], p4[1]);
    boxes.push({ x: minX, y: minY, w: maxX - minX, h: maxY - minY });
  }
  return boxes;
}

// ─── EPC decomposition into oriented quads ───

/** Get all collision quads for an EPC symbol in world space */
function getEpcCollisionQuads(
  symX: number, symY: number, symW: number, symH: number, rotation: number,
  waypoints: EpcWaypoint[]
): [number, number][][] {
  const quads: [number, number][][] = [];
  if (waypoints.length < 2) return quads;

  const rot = (rotation || 0) * Math.PI / 180;
  const cosR = Math.cos(rot), sinR = Math.sin(rot);
  const symCx = symX + symW / 2, symCy = symY + symH / 2;

  function toWorld(lx: number, ly: number): [number, number] {
    if (!rotation) return [lx, ly];
    const dx = lx - symCx, dy = ly - symCy;
    return [symCx + dx * cosR - dy * sinR, symCy + dx * sinR + dy * cosR];
  }

  /** Build an oriented quad from a segment (p0→p1) with given half-width */
  function segmentQuad(
    p0x: number, p0y: number, p1x: number, p1y: number, halfW: number
  ): [number, number][] {
    const dx = p1x - p0x, dy = p1y - p0y;
    const len = Math.sqrt(dx * dx + dy * dy);
    if (len === 0) return [];
    const nx = -dy / len * halfW, ny = dx / len * halfW;
    return [
      toWorld(p0x - nx, p0y - ny),
      toWorld(p1x - nx, p1y - ny),
      toWorld(p1x + nx, p1y + ny),
      toWorld(p0x + nx, p0y + ny),
    ];
  }

  /** Build an oriented box quad using shared geometry, then transform to world space. */
  function boxQuad(
    anchorX: number, anchorY: number,
    dirX: number, dirY: number,
    boxW: number, boxH: number,
    anchorSide: 'left' | 'right'
  ): [number, number][] {
    const corners = orientedBoxCorners(anchorX, anchorY, dirX, dirY, boxW, boxH, anchorSide);
    if (corners.length < 4) return [];
    return corners.map(([cx, cy]) => toWorld(cx, cy)) as [number, number][];
  }

  const ox = symX, oy = symY;

  // Line segment quads (use half-width of at least 1px for collision)
  const segHalfW = Math.max(EPC_CONFIG.lineWidth / 2, 0.5);
  for (let i = 0; i < waypoints.length - 1; i++) {
    const q = segmentQuad(
      ox + waypoints[i].x, oy + waypoints[i].y,
      ox + waypoints[i + 1].x, oy + waypoints[i + 1].y,
      segHalfW
    );
    if (q.length === 4) quads.push(q);
  }

  // Left box quad
  const p0 = waypoints[0], p1 = waypoints[1];
  const lbQ = boxQuad(
    ox + p0.x, oy + p0.y,
    p1.x - p0.x, p1.y - p0.y,
    EPC_CONFIG.leftBox.w, EPC_CONFIG.leftBox.h,
    'right'
  );
  if (lbQ.length === 4) quads.push(lbQ);

  // Right box quad
  const last = waypoints[waypoints.length - 1];
  const prev = waypoints[waypoints.length - 2];
  const rbDx = last.x - prev.x, rbDy = last.y - prev.y;
  const rbQ = boxQuad(
    ox + last.x, oy + last.y,
    rbDy, -rbDx, // rotated -90° to match perpendicular end box
    EPC_CONFIG.rightBox.w, EPC_CONFIG.rightBox.h,
    'right'
  );
  if (rbQ.length === 4) quads.push(rbQ);

  return quads;
}

/** Look up EPC waypoints for a symbol (from layout store or defaults) */
function getSymWaypoints(id: number): EpcWaypoint[] {
  const sym = layout.symbols.find(s => s.id === id);
  return sym?.epcWaypoints || EPC_CONFIG.defaultWaypoints.map(wp => ({ ...wp }));
}

// ─── Main spacing violation check ───

/** Check if any quad from a set collides with a single polygon */
function anyQuadVsPolygon(
  quads: [number, number][][],
  bVerts: [number, number][],
  spacing: number
): boolean {
  for (const q of quads) {
    if (polygonSATWithSpacing(q, bVerts, spacing)) return true;
  }
  return false;
}

/** Check if any quad from set A collides with any quad from set B */
function anyQuadVsQuads(
  aQuads: [number, number][][],
  bQuads: [number, number][][],
  spacing: number
): boolean {
  for (const aq of aQuads) {
    for (const bq of bQuads) {
      if (polygonSATWithSpacing(aq, bq, spacing)) return true;
    }
  }
  return false;
}

/** Check any EPC quad against a curved symbol */
function anyQuadVsCurved(
  quads: [number, number][][],
  curve: CurveParams,
  spacing: number
): boolean {
  // Use the curved-vs-spur logic: each quad is a convex polygon
  for (const q of quads) {
    if (checkCurvedVsSpur(curve, q, spacing)) return true;
  }
  return false;
}

/** Get collision vertices for any non-curved, non-EPC symbol based on its actual shape */
function getShapeVertices(
  x: number, y: number, w: number, h: number, rotation: number,
  symbolId: string, w2?: number, mirrored?: boolean
): [number, number][] {
  if (isSpurType(symbolId)) return getSpurVertices(x, y, w, h, w2 ?? w, rotation, mirrored);
  if (isInductionType(symbolId)) return getInductionVertices(x, y, w, h, rotation, mirrored);
  return getOBBVertices(x, y, w, h, rotation);
}

export function checkSpacingViolation(
  id: number, x: number, y: number, w: number, h: number, rotation: number,
  symbolId?: string, curveAngle?: number, w2?: number, mirrored?: boolean
): boolean {
  if (symbolId && SPACING_EXEMPT.has(symbolId)) return false;

  const spacing = layout.minSpacing;
  const isCurved = !!(symbolId && isCurvedType(symbolId));
  const isSpur = !!(symbolId && isSpurType(symbolId));
  const isEpc = !!(symbolId && isEpcType(symbolId));
  const isInduction = !!(symbolId && isInductionType(symbolId));

  // Get EPC quads for "this" symbol if it's an EPC
  let epcQuads: [number, number][][] | null = null;
  if (isEpc) {
    const wps = getSymWaypoints(id);
    epcQuads = getEpcCollisionQuads(x, y, w, h, rotation, wps);
  }

  for (const sym of layout.symbols) {
    if (sym.id === id) continue;
    if (SPACING_EXEMPT.has(sym.symbolId)) continue;

    const symIsCurved = isCurvedType(sym.symbolId);
    const symIsSpur = isSpurType(sym.symbolId);
    const symIsEpc = isEpcType(sym.symbolId);
    const symIsInduction = isInductionType(sym.symbolId);

    // Get EPC quads for "other" symbol if it's EPC
    let symEpcQuads: [number, number][][] | null = null;
    if (symIsEpc) {
      const symWps = sym.epcWaypoints || EPC_CONFIG.defaultWaypoints;
      symEpcQuads = getEpcCollisionQuads(sym.x, sym.y, sym.w, sym.h, sym.rotation, symWps);
    }

    // ── EPC vs EPC ──
    if (isEpc && symIsEpc && epcQuads && symEpcQuads) {
      if (anyQuadVsQuads(epcQuads, symEpcQuads, spacing)) return true;
      continue;
    }

    // ── EPC vs Curved ──
    if (isEpc && symIsCurved && epcQuads) {
      if (anyQuadVsCurved(epcQuads, sym, spacing)) return true;
      continue;
    }
    if (isCurved && symIsEpc && symEpcQuads) {
      if (anyQuadVsCurved(symEpcQuads, { x, y, w, h, rotation, symbolId: symbolId!, curveAngle }, spacing)) return true;
      continue;
    }

    // ── EPC vs non-curved/non-EPC ──
    if (isEpc && epcQuads && !symIsCurved && !symIsEpc) {
      const bVerts = getShapeVertices(sym.x, sym.y, sym.w, sym.h, sym.rotation, sym.symbolId, sym.w2, sym.mirrored);
      if (anyQuadVsPolygon(epcQuads, bVerts, spacing)) return true;
      continue;
    }
    if (symIsEpc && symEpcQuads && !isCurved && !isEpc) {
      const aVerts = getShapeVertices(x, y, w, h, rotation, symbolId!, w2, mirrored);
      if (anyQuadVsPolygon(symEpcQuads, aVerts, spacing)) return true;
      continue;
    }

    // ── Non-EPC, non-curved: shape polygon vs shape polygon ──
    if (!isCurved && !symIsCurved) {
      const aVerts = getShapeVertices(x, y, w, h, rotation, symbolId!, w2, mirrored);
      const bVerts = getShapeVertices(sym.x, sym.y, sym.w, sym.h, sym.rotation, sym.symbolId, sym.w2, sym.mirrored);
      if (polygonSATWithSpacing(aVerts, bVerts, spacing)) return true;
    } else if (isCurved && !symIsCurved) {
      const bVerts = getShapeVertices(sym.x, sym.y, sym.w, sym.h, sym.rotation, sym.symbolId, sym.w2, sym.mirrored);
      if (checkCurvedVsSpur(
        { x, y, w, h, rotation, symbolId: symbolId!, curveAngle },
        bVerts, spacing
      )) return true;
    } else if (!isCurved && symIsCurved) {
      const aVerts = getShapeVertices(x, y, w, h, rotation, symbolId!, w2, mirrored);
      if (checkCurvedVsSpur(sym, aVerts, spacing)) return true;
    } else {
      // Both curved: use band AABBs as fallback
      const aBoxes = getCurvedBandAABBs({ x, y, w, h, rotation, symbolId: symbolId!, curveAngle });
      const bBoxes = getCurvedBandAABBs(sym);
      for (const a of aBoxes) {
        for (const b of bBoxes) {
          if (edgeDistance(a.x, a.y, a.w, a.h, b.x, b.y, b.w, b.h) < spacing) return true;
        }
      }
    }
  }
  return false;
}