Made of little things.

export interface Vec {
  x: number;
  y: number;
}

export interface Pose {
  x: number;
  y: number;
  angle: number;
}

/** A polyline with cumulative lengths and a bounding box. */
export interface Path {
  pts: Vec[];
  cum: number[];
  len: number;
  x0: number;
  y0: number;
  x1: number;
  y1: number;
}

export const clamp = (v: number, lo: number, hi: number) => Math.max(lo, Math.min(hi, v));

export function makePath(pts: Vec[]): Path {
  const cum = [0];
  let x0 = Infinity;
  let y0 = Infinity;
  let x1 = -Infinity;
  let y1 = -Infinity;
  for (let i = 0; i < pts.length; i++) {
    const p = pts[i];
    if (i > 0) cum.push(cum[i - 1] + Math.hypot(p.x - pts[i - 1].x, p.y - pts[i - 1].y));
    x0 = Math.min(x0, p.x);
    y0 = Math.min(y0, p.y);
    x1 = Math.max(x1, p.x);
    y1 = Math.max(y1, p.y);
  }
  return { pts, cum, len: cum[cum.length - 1], x0, y0, x1, y1 };
}

export function pointAt(path: Path, s: number): Vec {
  const { pts, cum } = path;
  if (pts.length === 1) return pts[0];
  const d = clamp(s, 0, path.len);
  let lo = 0;
  let hi = cum.length - 1;
  while (hi - lo > 1) {
    const mid = (lo + hi) >> 1;
    if (cum[mid] <= d) lo = mid;
    else hi = mid;
  }
  const a = pts[lo];
  const b = pts[hi];
  const t = (d - cum[lo]) / (cum[hi] - cum[lo] || 1);
  return { x: a.x + (b.x - a.x) * t, y: a.y + (b.y - a.y) * t };
}

/** Position and a smoothed heading (looks a little ahead and behind). */
export function poseAt(path: Path, s: number): Pose {
  const p = pointAt(path, s);
  const a = pointAt(path, s - 2);
  const b = pointAt(path, s + 2);
  return { x: p.x, y: p.y, angle: Math.atan2(b.y - a.y, b.x - a.x) };
}

export function cubicPoints(p0: Vec, p1: Vec, p2: Vec, p3: Vec, n: number): Vec[] {
  const out: Vec[] = [];
  for (let i = 0; i <= n; i++) {
    const t = i / n;
    const u = 1 - t;
    out.push({
      x: u * u * u * p0.x + 3 * u * u * t * p1.x + 3 * u * t * t * p2.x + t * t * t * p3.x,
      y: u * u * u * p0.y + 3 * u * u * t * p1.y + 3 * u * t * t * p2.y + t * t * t * p3.y,
    });
  }
  return out;
}

export function quadPoints(p0: Vec, p1: Vec, p2: Vec, n: number): Vec[] {
  const out: Vec[] = [];
  for (let i = 0; i <= n; i++) {
    const t = i / n;
    const u = 1 - t;
    out.push({ x: u * u * p0.x + 2 * u * t * p1.x + t * t * p2.x, y: u * u * p0.y + 2 * u * t * p1.y + t * t * p2.y });
  }
  return out;
}

/** Offset a polyline to the right of its travel direction (canvas y points down). */
export function offsetPoints(pts: Vec[], off: number, closed = false): Vec[] {
  const n = pts.length;
  return pts.map((p, i) => {
    const prev = i > 0 ? pts[i - 1] : closed ? pts[n - 1] : p;
    const next = i < n - 1 ? pts[i + 1] : closed ? pts[0] : p;
    const tx = next.x - prev.x;
    const ty = next.y - prev.y;
    const l = Math.hypot(tx, ty) || 1;
    return { x: p.x - (ty / l) * off, y: p.y + (tx / l) * off };
  });
}

export function tracePath(ctx: CanvasRenderingContext2D, pts: Vec[], closed = false): void {
  ctx.beginPath();
  ctx.moveTo(pts[0].x, pts[0].y);
  for (let i = 1; i < pts.length; i++) ctx.lineTo(pts[i].x, pts[i].y);
  if (closed) ctx.closePath();
}

export const lerp = (a: number, b: number, t: number) => a + (b - a) * t;

export function angleDiff(a: number, b: number): number {
  let d = a - b;
  while (d > Math.PI) d -= Math.PI * 2;
  while (d < -Math.PI) d += Math.PI * 2;
  return d;
}

/** Smooth curve through control points (Catmull-Rom), resampled about every `step` units. */
export function smoothCurve(ctrl: Vec[], step: number): Vec[] {
  if (ctrl.length < 3) return resample(ctrl, step);
  const dense: Vec[] = [];
  for (let i = 0; i < ctrl.length - 1; i++) {
    const p0 = ctrl[Math.max(0, i - 1)];
    const p1 = ctrl[i];
    const p2 = ctrl[i + 1];
    const p3 = ctrl[Math.min(ctrl.length - 1, i + 2)];
    const n = Math.max(4, Math.ceil(Math.hypot(p2.x - p1.x, p2.y - p1.y) / (step * 0.5)));
    for (let k = 0; k < n; k++) {
      const t = k / n;
      const t2 = t * t;
      const t3 = t2 * t;
      dense.push({
        x: 0.5 * (2 * p1.x + (-p0.x + p2.x) * t + (2 * p0.x - 5 * p1.x + 4 * p2.x - p3.x) * t2 + (-p0.x + 3 * p1.x - 3 * p2.x + p3.x) * t3),
        y: 0.5 * (2 * p1.y + (-p0.y + p2.y) * t + (2 * p0.y - 5 * p1.y + 4 * p2.y - p3.y) * t2 + (-p0.y + 3 * p1.y - 3 * p2.y + p3.y) * t3),
      });
    }
  }
  dense.push(ctrl[ctrl.length - 1]);
  return resample(dense, step);
}

/** Evenly respace a polyline (keeps both ends). */
export function resample(pts: Vec[], step: number): Vec[] {
  const path = makePath(pts);
  const n = Math.max(1, Math.round(path.len / step));
  const out: Vec[] = [];
  for (let i = 0; i <= n; i++) out.push(pointAt(path, (path.len * i) / n));
  return out;
}

/** The part of a polyline between distances s0 and s1 along it. */
export function slicePath(pts: Vec[], s0: number, s1: number): Vec[] {
  const path = makePath(pts);
  const a = clamp(s0, 0, path.len);
  const b = clamp(s1, a, path.len);
  const out: Vec[] = [pointAt(path, a)];
  for (let i = 1; i < pts.length - 1; i++) if (path.cum[i] > a && path.cum[i] < b) out.push(pts[i]);
  out.push(pointAt(path, b));
  return out;
}

export function convexHull(points: Vec[]): Vec[] {
  const pts = [...points].sort((a, b) => a.x - b.x || a.y - b.y);
  if (pts.length < 3) return pts;
  const cross = (o: Vec, a: Vec, b: Vec) => (a.x - o.x) * (b.y - o.y) - (a.y - o.y) * (b.x - o.x);
  const lower: Vec[] = [];
  for (const p of pts) {
    while (lower.length >= 2 && cross(lower[lower.length - 2], lower[lower.length - 1], p) <= 0) lower.pop();
    lower.push(p);
  }
  const upper: Vec[] = [];
  for (let i = pts.length - 1; i >= 0; i--) {
    const p = pts[i];
    while (upper.length >= 2 && cross(upper[upper.length - 2], upper[upper.length - 1], p) <= 0) upper.pop();
    upper.push(p);
  }
  upper.pop();
  lower.pop();
  return lower.concat(upper);
}

/** Corners of a rectangle centred at (x, y), rotated by `angle`. */
export function boxCorners(x: number, y: number, angle: number, halfL: number, halfW: number): Vec[] {
  const c = Math.cos(angle);
  const s = Math.sin(angle);
  return [
    { x: x + c * halfL - s * halfW, y: y + s * halfL + c * halfW },
    { x: x + c * halfL + s * halfW, y: y + s * halfL - c * halfW },
    { x: x - c * halfL + s * halfW, y: y - s * halfL - c * halfW },
    { x: x - c * halfL - s * halfW, y: y - s * halfL + c * halfW },
  ];
}