Edge routes

/**
 * The path of an edge between two anchors: the line of a diagram, the wire of a
 * node editor. Each end leaves the box it belongs to by one of its sides, at
 * right angles to it, and the path goes round both boxes rather than through
 * them — and round the other boxes it is given, where it can.
 *
 * Two kinds of path, both a handful of numbers written into what is given:
 *
 * - **Orthogonal.** Level and upright runs only. Each end goes straight out by
 *   a stub's length; between the stubs the cheapest way is found over a grid of
 *   lines — the stubs' own, a line a stub clear of each side of each end's box and half a stub clear
 *   of each side of every other box,
 *   and the middle of the gap between the two ends' boxes — where length costs
 *   what it measures and every corner costs a stub more. A corner in the middle
 *   of the gap costs a hair less, so that of equal ways the one that turns
 *   half-way is chosen. The other boxes are at most `MAX_OBSTACLES`, so the
 *   grid, and the search over it, are bounded however many there are around.
 * - **Curve.** One cubic curve, leaving each end the way its side faces, with
 *   handles that lengthen as the ends part.
 *
 * And the questions asked of a path once it is drawn: how far a point is from
 * it, to pick it with a pointer; where the point part-way along it is, to hang
 * a label or a handle there; what box holds it; what to hand an SVG `path`.
 *
 * Coordinates are numbers in typed arrays, `[x0, y0, x1, y1, …]`. The search
 * keeps its scratch space between calls: a route allocates nothing, and the
 * same ends always give the same path.
 */

/** The side of a box an end is on, as the way it faces. */
export const SIDE = { right: 0, bottom: 1, left: 2, top: 3 } as const;
export type Side = 0 | 1 | 2 | 3;

/** The most points an orthogonal route has; `out` holds twice as many numbers. */
export const ROUTE_POINTS = 16;

/** The most boxes besides its own two that a route is kept out of. */
export const MAX_OBSTACLES = 12;

const DX = [1, 0, -1, 0] as const;
const DY = [0, 1, 0, -1] as const;

// Per axis: two stub ends, two sides of each of the two boxes, the middle of their gap, two sides of each other box.
const LINES = 7 + MAX_OBSTACLES * 2;
const STATES = LINES * LINES * 4;
/** Arrived at the far anchor itself. */
const END = STATES;

const xs = new Float64Array(LINES);
const ys = new Float64Array(LINES);
const cost = new Float64Array(STATES + 1);
const guess = new Float64Array(STATES + 1);
const via = new Int16Array(STATES + 1);
// 0 unseen, 1 reached, 2 settled.
const mark = new Uint8Array(STATES + 1);
const trail = new Float64Array((LINES * LINES + 4) * 2);
// The two boxes, each as left, top, right, bottom; and the others, as many as `others` says.
const box = new Float64Array(8);
const other = new Float64Array(MAX_OBSTACLES * 4);
let others = 0;
const NO_BOX = new Float64Array(0);
// A heap of states by their guess: every state reached is pushed again when its cost falls, and passed over once settled.
const HEAP = STATES * 4 + 8;
const heap = new Int32Array(HEAP);
const heapKey = new Float64Array(HEAP);
let heapSize = 0;

function push(state: number, key: number): void {
  let at = heapSize;
  heapSize += 1;
  while (at > 0) {
    const parent = (at - 1) >> 1;
    // Of equal guesses the state pushed with the lower number first: the same ends, the same route.
    if (heapKey[parent]! < key || (heapKey[parent] === key && heap[parent]! <= state)) break;
    heap[at] = heap[parent]!;
    heapKey[at] = heapKey[parent]!;
    at = parent;
  }
  heap[at] = state;
  heapKey[at] = key;
}

function pop(): number {
  const top = heap[0]!;
  heapSize -= 1;
  const state = heap[heapSize]!;
  const key = heapKey[heapSize]!;
  let at = 0;
  for (;;) {
    let child = at * 2 + 1;
    if (child >= heapSize) break;
    const right = child + 1;
    if (
      right < heapSize &&
      (heapKey[right]! < heapKey[child]! ||
        (heapKey[right] === heapKey[child] && heap[right]! < heap[child]!))
    ) {
      child = right;
    }
    if (key < heapKey[child]! || (key === heapKey[child] && state <= heap[child]!)) break;
    heap[at] = heap[child]!;
    heapKey[at] = heapKey[child]!;
    at = child;
  }
  heap[at] = state;
  heapKey[at] = key;
  return top;
}

/** The lines of one axis, in order and each once: how many. `axis` 0 reads boxes' left and right, 1 their top and bottom. */
function lines(
  into: Float64Array,
  start: number,
  end: number,
  axis: 0 | 1,
  clear: number,
  apart: number,
  middle: number,
): number {
  into[0] = start;
  into[1] = end;
  let count = 2;
  for (let at = 0; at < 8; at += 4) {
    into[count] = box[at + axis]! - clear;
    into[count + 1] = box[at + axis + 2]! + clear;
    count += 2;
  }
  if (!Number.isNaN(middle)) {
    into[count] = middle;
    count += 1;
  }
  for (let at = 0; at < others * 4; at += 4) {
    into[count] = other[at + axis]! - apart;
    into[count + 1] = other[at + axis + 2]! + apart;
    count += 2;
  }
  for (let at = 1; at < count; at += 1) {
    const value = into[at]!;
    let slot = at;
    while (slot > 0 && into[slot - 1]! > value) {
      into[slot] = into[slot - 1]!;
      slot -= 1;
    }
    into[slot] = value;
  }
  let kept = 1;
  for (let at = 1; at < count; at += 1) {
    if (into[at] !== into[kept - 1]) {
      into[kept] = into[at]!;
      kept += 1;
    }
  }
  return kept;
}

/** The middle of the gap between two spans, or `NaN` where they overlap. */
function gapMiddle(aLow: number, aHigh: number, bLow: number, bHigh: number): number {
  if (bLow > aHigh) return (aHigh + bLow) / 2;
  if (aLow > bHigh) return (bHigh + aLow) / 2;
  return Number.NaN;
}

function indexOf(values: Float64Array, count: number, value: number): number {
  for (let at = 0; at < count; at += 1) if (values[at] === value) return at;
  return 0;
}

/** Whether a level or upright run passes within `clear` of the inside of any of `count` boxes from `from`. */
function hits(
  boxes: Float64Array,
  count: number,
  x1: number,
  y1: number,
  x2: number,
  y2: number,
  clear: number,
): boolean {
  if (clear < 0) return false;
  const left = Math.min(x1, x2);
  const right = Math.max(x1, x2);
  const top = Math.min(y1, y2);
  const bottom = Math.max(y1, y2);
  for (let at = 0; at < count * 4; at += 4) {
    if (
      left < boxes[at + 2]! + clear &&
      right > boxes[at]! - clear &&
      top < boxes[at + 3]! + clear &&
      bottom > boxes[at + 1]! - clear
    ) {
      return true;
    }
  }
  return false;
}

/**
 * The cheapest way from the first stub's end to the far anchor, as states of
 * where one is and which way one is facing. `clear` is how far from the two
 * boxes a run must keep, and `apart` how far from the others; negative, they
 * are not there.
 */
function search(
  nx: number,
  ny: number,
  from: number,
  to: number,
  toSide: Side,
  stub: number,
  clear: number,
  apart: number,
  midX: number,
  midY: number,
): boolean {
  mark.fill(0);
  heapSize = 0;
  const goalX = xs[(to >> 2) % LINES]!;
  const goalY = ys[((to >> 2) / LINES) | 0]!;
  const into = (toSide + 2) & 3;
  cost[from] = 0;
  guess[from] = Math.abs(xs[(from >> 2) % LINES]! - goalX) + Math.abs(ys[((from >> 2) / LINES) | 0]! - goalY);
  via[from] = -1;
  mark[from] = 1;
  push(from, guess[from]!);
  while (heapSize > 0) {
    const best = pop();
    if (mark[best] === 2) continue;
    if (best === END) return true;
    mark[best] = 2;
    const facing = best & 3;
    const ix = (best >> 2) % LINES;
    const iy = ((best >> 2) / LINES) | 0;
    const x = xs[ix]!;
    const y = ys[iy]!;
    const corner = stub * (x === midX || y === midY ? 0.999 : 1);
    // From the far stub's end the anchor is reached by going in: never by backing into it.
    if ((best & ~3) === to && facing !== toSide) {
      const total = cost[best]! + stub + (facing === into ? 0 : corner);
      if (mark[END] === 0 || total < cost[END]!) {
        cost[END] = total;
        guess[END] = total;
        via[END] = best;
        mark[END] = 1;
        push(END, total);
      }
    }
    for (let turn = 0; turn < 4; turn += 1) {
      // No turning back on oneself.
      if (turn === ((facing + 2) & 3)) continue;
      const jx = ix + DX[turn]!;
      const jy = iy + DY[turn]!;
      if (jx < 0 || jy < 0 || jx >= nx || jy >= ny) continue;
      const nextX = xs[jx]!;
      const nextY = ys[jy]!;
      if (hits(box, 2, x, y, nextX, nextY, clear) || hits(other, others, x, y, nextX, nextY, apart)) continue;
      const next = ((jy * LINES + jx) << 2) | turn;
      if (mark[next] === 2) continue;
      const total = cost[best]! + Math.abs(nextX - x) + Math.abs(nextY - y) + (turn === facing ? 0 : corner);
      if (mark[next] === 0 || total < cost[next]!) {
        cost[next] = total;
        guess[next] = total + Math.abs(nextX - goalX) + Math.abs(nextY - goalY);
        via[next] = best;
        mark[next] = 1;
        push(next, guess[next]!);
      }
    }
  }
  return false;
}

// How clear of the two boxes, and of the others, each attempt keeps, in stubs; below zero, not kept clear at all.
// Clear of everything; failing that, out of everything; failing that, the others are given up before the two boxes are.
const CLEAR_OWN = [0.5, 0, 0.5, 0, -1] as const;
const CLEAR_OTHERS = [0.5, 0, -1, -1, -1] as const;

/**
 * An orthogonal route from one anchor to another, written into `out` as points;
 * returns how many. Each anchor is on the given side of its box — `boxes` holds
 * the two as `[x, y, width, height, x, y, width, height]`, and may be left out
 * for ends that belong to nothing. `obstacles` are other boxes to go round, in
 * the same form, at most `MAX_OBSTACLES` of them.
 *
 * The route leaves the first anchor and reaches the second at right angles to
 * their sides. Where the boxes are two stubs or more apart it enters neither,
 * and keeps half a stub clear of both between its stubs; where a way clear of
 * the obstacles is to be had on its grid, it takes it. Where none is, it goes
 * as it would without them; and nearer than two stubs it keeps as clear as it
 * can: out of the boxes if not clear of them, and through them only when there
 * is no way round.
 */
export function orthogonalRoute(
  ax: number,
  ay: number,
  aSide: Side,
  bx: number,
  by: number,
  bSide: Side,
  boxes: ArrayLike<number> | null,
  out: Float64Array,
  stub = 16,
  obstacles: ArrayLike<number> | null = null,
): number {
  if (out.length < ROUTE_POINTS * 2) {
    throw new RangeError(`A route is written into room for ${ROUTE_POINTS} points, not ${out.length / 2}.`);
  }
  const given = boxes ?? NO_BOX;
  if (given.length !== 0 && given.length !== 8) {
    throw new RangeError(`Boxes are two, of four numbers each, not ${given.length} numbers.`);
  }
  const around = obstacles ?? NO_BOX;
  if (around.length % 4 !== 0 || around.length > MAX_OBSTACLES * 4) {
    throw new RangeError(
      `Obstacles are boxes of four numbers each, at most ${MAX_OBSTACLES} of them, not ${around.length} numbers.`,
    );
  }
  if (given.length === 0) {
    box[0] = box[2] = ax;
    box[1] = box[3] = ay;
    box[4] = box[6] = bx;
    box[5] = box[7] = by;
  } else {
    for (let at = 0; at < 8; at += 4) {
      box[at] = given[at]!;
      box[at + 1] = given[at + 1]!;
      box[at + 2] = given[at]! + given[at + 2]!;
      box[at + 3] = given[at + 1]! + given[at + 3]!;
    }
  }
  others = around.length / 4;
  for (let at = 0; at < around.length; at += 4) {
    const x = around[at]!;
    const y = around[at + 1]!;
    other[at] = x;
    other[at + 1] = y;
    other[at + 2] = x + around[at + 2]!;
    other[at + 3] = y + around[at + 3]!;
  }
  // A stub ends its length beyond its box's side, wherever on the side — or within it — the anchor is.
  const outward = (at: number, side: Side, x: number, y: number, axis: 0 | 1): number => {
    if (axis === 0) {
      if (side === SIDE.right) return Math.max(x, box[at + 2]!) + stub;
      if (side === SIDE.left) return Math.min(x, box[at]!) - stub;
      return x;
    }
    if (side === SIDE.bottom) return Math.max(y, box[at + 3]!) + stub;
    if (side === SIDE.top) return Math.min(y, box[at + 1]!) - stub;
    return y;
  };
  const sx = outward(0, aSide, ax, ay, 0);
  const sy = outward(0, aSide, ax, ay, 1);
  const tx = outward(4, bSide, bx, by, 0);
  const ty = outward(4, bSide, bx, by, 1);

  const midX = gapMiddle(box[0]!, box[2]!, box[4]!, box[6]!);
  const midY = gapMiddle(box[1]!, box[3]!, box[5]!, box[7]!);
  // A stub clear of the two boxes, and half a stub of the others: room to pass between two of them.
  const nx = lines(xs, sx, tx, 0, stub, stub / 2, midX);
  const ny = lines(ys, sy, ty, 1, stub, stub / 2, midY);
  const from = ((indexOf(ys, ny, sy) * LINES + indexOf(xs, nx, sx)) << 2) | aSide;
  const to = (indexOf(ys, ny, ty) * LINES + indexOf(xs, nx, tx)) << 2;

  for (let attempt = others > 0 ? 0 : 2; attempt < CLEAR_OWN.length; attempt += 1) {
    const clear = CLEAR_OWN[attempt]! * stub;
    const apart = CLEAR_OTHERS[attempt]! * stub;
    // The stubs are where they must be: one that runs into another box leaves no way clear of it.
    if (hits(other, others, ax, ay, sx, sy, apart) || hits(other, others, tx, ty, bx, by, apart)) continue;
    if (!search(nx, ny, from, to, bSide, stub, clear, apart, midX, midY)) continue;
    // Read back from the far end, then written out forwards with every point that is not a corner left out.
    trail[0] = bx;
    trail[1] = by;
    let length = 1;
    for (let state = via[END]!; state !== -1; state = via[state]!) {
      trail[length * 2] = xs[(state >> 2) % LINES]!;
      trail[length * 2 + 1] = ys[((state >> 2) / LINES) | 0]!;
      length += 1;
    }
    trail[length * 2] = ax;
    trail[length * 2 + 1] = ay;
    length += 1;
    let count = 0;
    for (let at = length - 1; at >= 0; at -= 1) {
      const x = trail[at * 2]!;
      const y = trail[at * 2 + 1]!;
      if (count >= 1 && out[count * 2 - 2] === x && out[count * 2 - 1] === y) continue;
      if (count >= 2) {
        const px = out[count * 2 - 2]!;
        const py = out[count * 2 - 1]!;
        const qx = out[count * 2 - 4]!;
        const qy = out[count * 2 - 3]!;
        // The point before is on the way from the one before it to this: not a corner.
        if ((qx === px && px === x) || (qy === py && py === y)) count -= 1;
      }
      if (count === ROUTE_POINTS) {
        count = -1;
        break;
      }
      out[count * 2] = x;
      out[count * 2 + 1] = y;
      count += 1;
    }
    if (count > 0) return count;
  }
  // Never reached: with the boxes not there, the grid always has a way.
  out[0] = ax;
  out[1] = ay;
  out[2] = bx;
  out[3] = by;
  return 2;
}

/**
 * A curve from one anchor to another, written into `out` as the four points of
 * a cubic: the start, its handle, the end's handle, the end. Each handle
 * reaches out the way its side faces, by two-fifths of the distance between the
 * ends — but never less than `least`, so that ends close together still leave
 * squarely, nor more than `most`.
 */
export function curveRoute(
  ax: number,
  ay: number,
  aSide: Side,
  bx: number,
  by: number,
  bSide: Side,
  out: Float64Array,
  least = 24,
  most = 240,
): void {
  if (out.length < 8) throw new RangeError(`A curve is four points, eight numbers, not ${out.length}.`);
  const reach = Math.min(Math.max(Math.hypot(bx - ax, by - ay) * 0.4, least), most);
  out[0] = ax;
  out[1] = ay;
  out[2] = ax + DX[aSide]! * reach;
  out[3] = ay + DY[aSide]! * reach;
  out[4] = bx + DX[bSide]! * reach;
  out[5] = by + DY[bSide]! * reach;
  out[6] = bx;
  out[7] = by;
}

/** How far a point is from the run between two others. */
function segmentDistance(x: number, y: number, x1: number, y1: number, x2: number, y2: number): number {
  const dx = x2 - x1;
  const dy = y2 - y1;
  const squared = dx * dx + dy * dy;
  const along = squared === 0 ? 0 : Math.min(1, Math.max(0, ((x - x1) * dx + (y - y1) * dy) / squared));
  return Math.hypot(x - (x1 + dx * along), y - (y1 + dy * along));
}

/** How far a point is from a line through `count` points: from the nearest place on any of its runs. */
export function polylineDistance(points: ArrayLike<number>, count: number, x: number, y: number): number {
  if (count === 0) return Infinity;
  let nearest = Math.hypot(x - points[0]!, y - points[1]!);
  for (let at = 1; at < count; at += 1) {
    const distance = segmentDistance(
      x,
      y,
      points[at * 2 - 2]!,
      points[at * 2 - 1]!,
      points[at * 2]!,
      points[at * 2 + 1]!,
    );
    if (distance < nearest) nearest = distance;
  }
  return nearest;
}

/** One coordinate of a cubic at `t`, from 0 at its start to 1 at its end. */
function cubic(p0: number, p1: number, p2: number, p3: number, t: number): number {
  const u = 1 - t;
  return u * u * u * p0 + 3 * u * u * t * p1 + 3 * u * t * t * p2 + t * t * t * p3;
}

/**
 * How far a point is from a curve, taken as `steps` straight runs: exact to
 * within the little by which a run of the curve bows from its chord.
 */
export function curveDistance(curve: ArrayLike<number>, x: number, y: number, steps = 24): number {
  let nearest = Infinity;
  let fromX = curve[0]!;
  let fromY = curve[1]!;
  for (let step = 1; step <= steps; step += 1) {
    const t = step / steps;
    const toX = cubic(curve[0]!, curve[2]!, curve[4]!, curve[6]!, t);
    const toY = cubic(curve[1]!, curve[3]!, curve[5]!, curve[7]!, t);
    const distance = segmentDistance(x, y, fromX, fromY, toX, toY);
    if (distance < nearest) nearest = distance;
    fromX = toX;
    fromY = toY;
  }
  return nearest;
}

/** How long a line through `count` points is. */
export function polylineLength(points: ArrayLike<number>, count: number): number {
  let length = 0;
  for (let at = 1; at < count; at += 1) {
    length += Math.hypot(points[at * 2]! - points[at * 2 - 2]!, points[at * 2 + 1]! - points[at * 2 - 1]!);
  }
  return length;
}

/**
 * The point a distance along a line from its start, written into `out`. A
 * distance below zero is the start; one past the end, the end.
 */
export function polylinePoint(
  points: ArrayLike<number>,
  count: number,
  distance: number,
  out: Float64Array,
): void {
  if (count === 0) throw new RangeError("A line with no points has no point along it.");
  let left = Math.max(distance, 0);
  for (let at = 1; at < count; at += 1) {
    const x1 = points[at * 2 - 2]!;
    const y1 = points[at * 2 - 1]!;
    const x2 = points[at * 2]!;
    const y2 = points[at * 2 + 1]!;
    const run = Math.hypot(x2 - x1, y2 - y1);
    if (left <= run && run > 0) {
      out[0] = x1 + ((x2 - x1) * left) / run;
      out[1] = y1 + ((y2 - y1) * left) / run;
      return;
    }
    left -= run;
  }
  out[0] = points[count * 2 - 2]!;
  out[1] = points[count * 2 - 1]!;
}

/** The point of a curve at `t`, from 0 at its start to 1 at its end, written into `out`. */
export function curvePoint(curve: ArrayLike<number>, t: number, out: Float64Array): void {
  const at = Math.min(Math.max(t, 0), 1);
  out[0] = cubic(curve[0]!, curve[2]!, curve[4]!, curve[6]!, at);
  out[1] = cubic(curve[1]!, curve[3]!, curve[5]!, curve[7]!, at);
}

/**
 * The box that holds `count` points, written into `out` as left, top, right,
 * bottom. A curve lies within the box of its four points.
 */
export function pointsBounds(points: ArrayLike<number>, count: number, out: Float64Array): void {
  let left = Infinity;
  let top = Infinity;
  let right = -Infinity;
  let bottom = -Infinity;
  for (let at = 0; at < count; at += 1) {
    const x = points[at * 2]!;
    const y = points[at * 2 + 1]!;
    if (x < left) left = x;
    if (x > right) right = x;
    if (y < top) top = y;
    if (y > bottom) bottom = y;
  }
  out[0] = left;
  out[1] = top;
  out[2] = right;
  out[3] = bottom;
}

// To the hundredth: a path's text stays short, and the same from one drawing to the next.
const tidy = (value: number): number => Math.round(value * 100) / 100;

/**
 * A line through `count` points as the data of an SVG `path`, each corner
 * rounded by `radius` — or by half the shorter of its two runs, where they are
 * too short for it. Every coordinate is taken from `(originX, originY)`, so a
 * path far from zero is written in small numbers.
 */
export function polylinePath(
  points: ArrayLike<number>,
  count: number,
  radius = 0,
  originX = 0,
  originY = 0,
): string {
  if (count === 0) return "";
  let data = `M${tidy(points[0]! - originX)} ${tidy(points[1]! - originY)}`;
  for (let at = 1; at < count; at += 1) {
    const x = points[at * 2]!;
    const y = points[at * 2 + 1]!;
    if (at === count - 1 || radius <= 0) {
      data += `L${tidy(x - originX)} ${tidy(y - originY)}`;
      continue;
    }
    const before = Math.hypot(x - points[at * 2 - 2]!, y - points[at * 2 - 1]!);
    const after = Math.hypot(points[at * 2 + 2]! - x, points[at * 2 + 3]! - y);
    const round = Math.min(radius, before / 2, after / 2);
    if (!(round > 0)) {
      data += `L${tidy(x - originX)} ${tidy(y - originY)}`;
      continue;
    }
    const inX = x - ((x - points[at * 2 - 2]!) / before) * round;
    const inY = y - ((y - points[at * 2 - 1]!) / before) * round;
    const outX = x + ((points[at * 2 + 2]! - x) / after) * round;
    const outY = y + ((points[at * 2 + 3]! - y) / after) * round;
    data += `L${tidy(inX - originX)} ${tidy(inY - originY)}Q${tidy(x - originX)} ${tidy(y - originY)} ${tidy(outX - originX)} ${tidy(outY - originY)}`;
  }
  return data;
}

/** A curve as the data of an SVG `path`, every coordinate taken from `(originX, originY)`. */
export function curvePath(curve: ArrayLike<number>, originX = 0, originY = 0): string {
  const at = (index: number) => tidy(curve[index]! - (index % 2 === 0 ? originX : originY));
  return `M${at(0)} ${at(1)}C${at(2)} ${at(3)} ${at(4)} ${at(5)} ${at(6)} ${at(7)}`;
}