Made of little things.

import { boxCorners, type Vec } from './geometry.js';

/** A 2D rigid box: position, velocity, heading and spin. */
export interface Body {
  x: number;
  y: number;
  vx: number;
  vy: number;
  angle: number;
  av: number;
  mass: number;
  inertia: number;
  halfL: number;
  halfW: number;
}

export interface Contact {
  /** Unit normal pointing from body A towards body B. */
  nx: number;
  ny: number;
  depth: number;
  px: number;
  py: number;
}

export interface ImpactResult {
  /** Closing speed along the normal before the hit (px/s). */
  closing: number;
  impulse: number;
}

export function boxInertia(mass: number, halfL: number, halfW: number): number {
  return (mass * ((halfL * 2) ** 2 + (halfW * 2) ** 2)) / 12;
}

export function velocityAt(b: Body, px: number, py: number): Vec {
  return { x: b.vx - b.av * (py - b.y), y: b.vy + b.av * (px - b.x) };
}

export function applyImpulse(b: Body, jx: number, jy: number, px: number, py: number): void {
  b.vx += jx / b.mass;
  b.vy += jy / b.mass;
  b.av += ((px - b.x) * jy - (py - b.y) * jx) / b.inertia;
}

/** Effective mass of a body along direction n at point p (inverse). */
export function invMassAlong(b: Body, px: number, py: number, nx: number, ny: number): number {
  const rn = (px - b.x) * ny - (py - b.y) * nx;
  return 1 / b.mass + (rn * rn) / b.inertia;
}

function inside(b: Body, p: Vec, pad = 0): boolean {
  const dx = p.x - b.x;
  const dy = p.y - b.y;
  const c = Math.cos(b.angle);
  const s = Math.sin(b.angle);
  return Math.abs(dx * c + dy * s) <= b.halfL + pad && Math.abs(-dx * s + dy * c) <= b.halfW + pad;
}

/** Separating-axis test between two oriented boxes. */
export function boxContact(a: Body, b: Body): Contact | null {
  const ca = boxCorners(a.x, a.y, a.angle, a.halfL, a.halfW);
  const cb = boxCorners(b.x, b.y, b.angle, b.halfL, b.halfW);
  const axes = [a.angle, a.angle + Math.PI / 2, b.angle, b.angle + Math.PI / 2];
  let best = Infinity;
  let nx = 0;
  let ny = 0;
  for (const ang of axes) {
    const ax = Math.cos(ang);
    const ay = Math.sin(ang);
    let minA = Infinity;
    let maxA = -Infinity;
    let minB = Infinity;
    let maxB = -Infinity;
    for (const p of ca) {
      const d = p.x * ax + p.y * ay;
      minA = Math.min(minA, d);
      maxA = Math.max(maxA, d);
    }
    for (const p of cb) {
      const d = p.x * ax + p.y * ay;
      minB = Math.min(minB, d);
      maxB = Math.max(maxB, d);
    }
    const overlap = Math.min(maxA, maxB) - Math.max(minA, minB);
    if (overlap <= 0) return null;
    if (overlap < best) {
      best = overlap;
      nx = ax;
      ny = ay;
    }
  }
  if ((b.x - a.x) * nx + (b.y - a.y) * ny < 0) {
    nx = -nx;
    ny = -ny;
  }
  const pts = [...cb.filter((p) => inside(a, p, 0.5)), ...ca.filter((p) => inside(b, p, 0.5))];
  let px = (a.x + b.x) / 2;
  let py = (a.y + b.y) / 2;
  if (pts.length) {
    px = pts.reduce((s, p) => s + p.x, 0) / pts.length;
    py = pts.reduce((s, p) => s + p.y, 0) / pts.length;
  }
  return { nx, ny, depth: best, px, py };
}

/** Resolve a contact between two bodies with restitution and friction. */
export function resolveContact(a: Body, b: Body, c: Contact, restitution: number, friction: number): ImpactResult {
  const va = velocityAt(a, c.px, c.py);
  const vb = velocityAt(b, c.px, c.py);
  const rvx = vb.x - va.x;
  const rvy = vb.y - va.y;
  const vn = rvx * c.nx + rvy * c.ny;
  const share = a.mass / (a.mass + b.mass);
  const push = Math.max(0, c.depth - 0.05);
  a.x -= c.nx * push * (1 - share);
  a.y -= c.ny * push * (1 - share);
  b.x += c.nx * push * share;
  b.y += c.ny * push * share;
  if (vn >= 0) return { closing: 0, impulse: 0 };
  const k = invMassAlong(a, c.px, c.py, c.nx, c.ny) + invMassAlong(b, c.px, c.py, c.nx, c.ny);
  const j = (-(1 + restitution) * vn) / k;
  applyImpulse(a, -c.nx * j, -c.ny * j, c.px, c.py);
  applyImpulse(b, c.nx * j, c.ny * j, c.px, c.py);
  // Coulomb friction along the contact tangent.
  const tx = -c.ny;
  const ty = c.nx;
  const vt = rvx * tx + rvy * ty;
  const kt = invMassAlong(a, c.px, c.py, tx, ty) + invMassAlong(b, c.px, c.py, tx, ty);
  const jt = Math.max(-friction * j, Math.min(friction * j, -vt / kt));
  applyImpulse(a, -tx * jt, -ty * jt, c.px, c.py);
  applyImpulse(b, tx * jt, ty * jt, c.px, c.py);
  return { closing: -vn, impulse: j };
}

/** Bounce a body off static geometry given a push-out vector at a contact point. */
export function resolveStatic(b: Body, push: Vec, px: number, py: number, restitution: number, friction: number): number {
  const len = Math.hypot(push.x, push.y);
  if (!len) return 0;
  const nx = push.x / len;
  const ny = push.y / len;
  b.x += push.x;
  b.y += push.y;
  const v = velocityAt(b, px, py);
  const vn = v.x * nx + v.y * ny;
  if (vn >= 0) return 0;
  const k = invMassAlong(b, px, py, nx, ny);
  const j = (-(1 + restitution) * vn) / k;
  applyImpulse(b, nx * j, ny * j, px, py);
  const tx = -ny;
  const ty = nx;
  const vt = v.x * tx + v.y * ty;
  const kt = invMassAlong(b, px, py, tx, ty);
  const jt = Math.max(-friction * j, Math.min(friction * j, -vt / kt));
  applyImpulse(b, tx * jt, ty * jt, px, py);
  return -vn;
}

/** Lateral tyre grip at an axle: cancel sideways slip up to `maxImpulse`. Returns true if sliding. */
export function axleGrip(b: Body, px: number, py: number, heading: number, maxImpulse: number): boolean {
  const lx = -Math.sin(heading);
  const ly = Math.cos(heading);
  const v = velocityAt(b, px, py);
  const vl = v.x * lx + v.y * ly;
  const k = invMassAlong(b, px, py, lx, ly);
  let j = -vl / k;
  const sliding = Math.abs(j) > maxImpulse;
  if (sliding) j = Math.sign(j) * maxImpulse;
  applyImpulse(b, lx * j, ly * j, px, py);
  return sliding;
}

export function integrate(b: Body, dt: number): void {
  b.x += b.vx * dt;
  b.y += b.vy * dt;
  b.angle += b.av * dt;
}

export function forwardSpeed(b: Body): number {
  return b.vx * Math.cos(b.angle) + b.vy * Math.sin(b.angle);
}