arah2/routing/src/astar.ts

/*
 * astar.ts — typed-array BIDIRECTIONAL A* over the CSR graph + turn-step
 * derivation (D-01; bidirectional per 06-04's A1 ladder step b).
 * ----------------------------------------------------------------------------
 * The engine-A router core: routeAstar finds the minimum-seconds path
 * between two graph nodes. The search is BIDIRECTIONAL (BH, Ikeda
 * potentials — see routeAstar's doc) because the free-plan 10 ms CPU budget
 * is the design constraint and the measured unidirectional p95 on the real
 * KL graph was 3-5x over it. The hot loops remain allocation-free: binary
 * heaps of node indices over preallocated typed arrays, typed-array
 * distances/closed sets/chains, lazy on-demand potentials, no Map, no
 * objects per node. Optimality is not presumed — astar.test.mjs A1
 * exhaustively proves equivalence to brute-force Dijkstra on the fixture
 * (625 pairs), and the oneway/unreachable laws have dedicated cases.
 *
 * Traversal law (shared with the tests' brute-force Dijkstra): an edge with
 * edgeFlags bit0 set ("oneway-reverse" — see graph.ts) is NEVER relaxed.
 * Snapping (snap.ts) ignores the flag; only A* enforces direction.
 *
 * classifyTurn is the ONE authority for "what is a turn" — plan 07's cron
 * projection imports this exact classifier. Convention: compass bearings
 * increase clockwise (N=0, E=90), so a NEGATIVE bearing delta is a left
 * turn.
 *
 * deriveSteps walks a node path and splits it into maneuver steps at
 * bearings that leave "straight" or at street-name changes; steps carry
 * kind/name/distanceM and polyline6 geometry (RouteStep.kind — the field
 * this phase added to types.ts; fixture-world steps stay empty arrays,
 * D-10).
 *
 * Pure: no I/O, no logging, no storage (T-06-05).
 */

import type { RoadGraph } from "./graph.ts";
import { haversineM } from "./geo.ts";
import { encodePolyline6 } from "./polyline.ts";
import type { RoutePoint, RouteStep } from "./types.ts";

/** Maneuver kinds — structurally the RouteStep.kind union (one authority). */
export type GuidanceTurnKind = NonNullable<RouteStep["kind"]>;

/** |delta| <= this is "straight" (degrees). */
export const TURN_STRAIGHT_MAX_DEG = 25;
/** |delta| <= this (and > straight) is a "slight" turn (degrees). */
export const TURN_SLIGHT_MAX_DEG = 60;
/** |delta| >= this is a "uturn" (degrees). */
export const TURN_UTURN_MIN_DEG = 150;

const DEG_TO_RAD = Math.PI / 180;
/** Same mean-earth sphere as geo.ts haversineM (6 371 000 m). */
const EARTH_RADIUS_M = 6_371_000;

export interface AstarResult {
  /** Node path, origin first, goal last. */
  nodes: number[];
  /** Total travel seconds (sum of traversed edgeSeconds). */
  seconds: number;
  /** Meters over the node path (haversineM post-hoc — the plan's law). */
  distanceM: number;
}

/* ————— binary min-heap of node indices over preallocated typed arrays ————— */

const HEAP_INITIAL_CAPACITY = 1024;

class NodeHeap {
  private keys: Float64Array;
  private nodes: Uint32Array;
  size = 0;

  constructor() {
    this.keys = new Float64Array(HEAP_INITIAL_CAPACITY);
    this.nodes = new Uint32Array(HEAP_INITIAL_CAPACITY);
  }

  push(key: number, node: number): void {
    if (this.size === this.keys.length) this.grow();
    let i = this.size;
    this.size += 1;
    while (i > 0) {
      const parent = (i - 1) >> 1;
      if (this.keys[parent]! <= key) break;
      this.keys[i] = this.keys[parent]!;
      this.nodes[i] = this.nodes[parent]!;
      i = parent;
    }
    this.keys[i] = key;
    this.nodes[i] = node;
  }

  /** The min key without popping; Infinity when empty (the BH stop rule reads this). */
  peekKey(): number {
    return this.size === 0 ? Number.POSITIVE_INFINITY : this.keys[0]!;
  }

  /** Returns the min-key node, or -1 when empty. Lazy deletion: stale
   *  entries are skipped by the closed-set check in routeAstar. */
  pop(): number {
    if (this.size === 0) return -1;
    const top = this.nodes[0]!;
    this.size -= 1;
    const lastKey = this.keys[this.size]!;
    const lastNode = this.nodes[this.size]!;
    let i = 0;
    for (;;) {
      const left = 2 * i + 1;
      if (left >= this.size) break;
      const right = left + 1;
      const child = right < this.size && this.keys[right]! < this.keys[left]! ? right : left;
      if (this.keys[child]! >= lastKey) break;
      this.keys[i] = this.keys[child]!;
      this.nodes[i] = this.nodes[child]!;
      i = child;
    }
    if (this.size > 0) {
      this.keys[i] = lastKey;
      this.nodes[i] = lastNode;
    }
    return top;
  }

  private grow(): void {
    const keys = new Float64Array(this.keys.length * 2);
    keys.set(this.keys);
    const nodes = new Uint32Array(this.nodes.length * 2);
    nodes.set(this.nodes);
    this.keys = keys;
    this.nodes = nodes;
  }
}

/* ————— A* — bidirectional (BH, Ikeda potentials) ————— */

/** Inline haversine from a graph node to a fixed (lat, lng), in METERS. */
function metersTo(graph: RoadGraph, v: number, lat2: number, lng2: number): number {
  const lat1 = graph.lat[v]! * DEG_TO_RAD;
  const lat2r = lat2 * DEG_TO_RAD;
  const dLat = lat2r - lat1;
  const dLng = (lng2 - graph.lng[v]!) * DEG_TO_RAD;
  const sinLat = Math.sin(dLat / 2);
  const sinLng = Math.sin(dLng / 2);
  const h = sinLat * sinLat + Math.cos(lat1) * Math.cos(lat2r) * sinLng * sinLng;
  return 2 * EARTH_RADIUS_M * Math.asin(Math.min(1, Math.sqrt(h)));
}

/* ————— reverse adjacency (the backward search's in-edge scan) ————— */

/**
 * The reverse CSR (in-edges per node), built ONCE per RoadGraph and
 * memoized in a module-scope WeakMap — the snap-grid LAW 1 pattern. The
 * backward search cannot iterate a node's in-edges out of a source-grouped
 * CSR (that was the 06-04 GREEN bug the A1/A2 gates caught: out-edges read
 * as in-edges fabricate phantom arcs and traverse oneway mirrors); this
 * index makes them addressable without touching the pinned binary layout.
 * One-time build is O(V+E) per isolate, paid by the first route (the CPU
 * gate's warm-up covers it), amortized across the isolate's requests.
 */
interface ReverseAdj {
  /** rowStart[u]..rowStart[u+1] index u's IN-edges inside targets/edgeIds. */
  rowStart: Uint32Array;
  /** The in-edge's SOURCE node (v, for an edge v -> u). */
  targets: Uint32Array;
  /** The edge's index in the forward CSR (weights/flags/seconds lookup). */
  edgeIds: Uint32Array;
}

const reverseGraphs = new WeakMap<RoadGraph, ReverseAdj>();

function reverseFor(graph: RoadGraph): ReverseAdj {
  const memoized = reverseGraphs.get(graph);
  if (memoized !== undefined) return memoized;
  const n = graph.nodeCount;
  const edgeCount = graph.edgeCount;
  const inDegree = new Uint32Array(n + 1);
  for (let e = 0; e < edgeCount; e++) inDegree[graph.edgeTarget[e]! + 1] = inDegree[graph.edgeTarget[e]! + 1]! + 1;
  for (let i = 0; i < n; i++) inDegree[i + 1] = inDegree[i + 1]! + inDegree[i]!;
  const targets = new Uint32Array(edgeCount);
  const edgeIds = new Uint32Array(edgeCount);
  const cursor = inDegree.slice(0, n);
  for (let u = 0; u < n; u++) {
    for (let e = graph.edgeStart[u]!; e < graph.edgeStart[u + 1]!; e++) {
      const v = graph.edgeTarget[e]!; // the edge u -> v is an IN-edge of v
      const pos = cursor[v]!;
      cursor[v] = pos + 1;
      targets[pos] = u;
      edgeIds[pos] = e;
    }
  }
  const adj: ReverseAdj = { rowStart: inDegree, targets, edgeIds };
  reverseGraphs.set(graph, adj);
  return adj;
}

/**
 * Minimum-seconds path origin -> goal over the graph, or null when the pair
 * is unreachable (the honest miss). Exhaustively proven equal to
 * brute-force Dijkstra on the committed mini-graph (astar.test.mjs A1).
 *
 * BIDIRECTIONAL (A1 ladder step b, 06-04): the free-plan 10 ms CPU ceiling
 * demands it — measured on the real 311k-node KL graph, the unidirectional
 * search's weak max-speed heuristic balloons on 20-30 km routes (p95 38 ms).
 * BH construction (Ikeda et al.): with directional heuristics
 * h_f(v)=hav(v,goal)/maxSpeed and h_b(v)=hav(v,origin)/maxSpeed (each
 * consistent on its own search graph) and the opposite potentials
 *   p_f(v) = (h_f(v) - h_b(v))/2,  p_b(v) = -p_f(v),
 * the reductions
 *   forward, edge u->v:   c + p_f(u) - p_f(v)
 *   backward, in-edge v->u of settled u: c + p_b(u) - p_b(v)
 * are NON-NEGATIVE (each feasibility is the sum of the two consistencies,
 * each <= c), and the meet keys satisfy
 *   d_f(v) + d_b(v) = real(o->g through v) - p_f(o) + p_f(g)
 * — a v-independent constant — so stopping when top_f + top_b >= mu (the
 * best meeting cost seen) settles the goal PROVABLY optimally, at roughly
 * half the explored radius per side. Alternation balance is a work knob,
 * not a correctness knob (the stop rule carries optimality); A1's
 * exhaustive 625-pair Dijkstra-equivalence sweep is the standing proof,
 * re-run on every pass.
 */
export function routeAstar(graph: RoadGraph, originNode: number, goalNode: number): AstarResult | null {
  const n = graph.nodeCount;
  if (!Number.isInteger(originNode) || originNode < 0 || originNode >= n) return null;
  if (!Number.isInteger(goalNode) || goalNode < 0 || goalNode >= n) return null;
  if (originNode === goalNode) return { nodes: [originNode], seconds: 0, distanceM: 0 };

  const maxSpeedMps = graph.maxSpeedKmh / 3.6;
  const heuristicScale = maxSpeedMps > 0 ? 1 / maxSpeedMps : 1;
  const goalLat = graph.lat[goalNode]!;
  const goalLng = graph.lng[goalNode]!;
  const origLat = graph.lat[originNode]!;
  const origLng = graph.lng[originNode]!;

  // Directional heuristics + opposite potentials, lazy per node — only
  // explored nodes ever pay, and the hot loops stay allocation-free.
  const hfCache = new Float64Array(n).fill(Number.NaN); // hav(v, goal) * scale
  const hbCache = new Float64Array(n).fill(Number.NaN); // hav(v, origin) * scale
  const pf = new Float64Array(n); // p_f(v) = (h_f - h_b)/2; p_b = -p_f — valid once both caches are set
  function potential(v: number): number {
    let hf = hfCache[v]!;
    if (Number.isNaN(hf)) {
      hf = metersTo(graph, v, goalLat, goalLng) * heuristicScale;
      hfCache[v] = hf;
    }
    let hb = hbCache[v]!;
    if (Number.isNaN(hb)) {
      hb = metersTo(graph, v, origLat, origLng) * heuristicScale;
      hbCache[v] = hb;
    }
    const value = (hf - hb) / 2;
    pf[v] = value;
    return value;
  }

  // Reduced distances (BH keys), chain arrays, closed sets, one heap per side.
  const df = new Float64Array(n).fill(Number.POSITIVE_INFINITY); // reduced origin->v
  const db = new Float64Array(n).fill(Number.POSITIVE_INFINITY); // reduced v->goal
  const fPrev = new Int32Array(n).fill(-1); // predecessor on the origin->v chain
  const bNext = new Int32Array(n).fill(-1); // successor on the v->goal chain
  const fClosed = new Uint8Array(n);
  const bClosed = new Uint8Array(n);
  const fHeap = new NodeHeap();
  const bHeap = new NodeHeap();

  df[originNode] = 0;
  potential(originNode); // pf[o] is needed by the first forward relaxations
  fHeap.push(0, originNode); // key = the REDUCED distance itself (the heuristic lives in the reduction)
  db[goalNode] = 0;
  potential(goalNode); // p_b(g) likewise for the first backward relaxations
  bHeap.push(0, goalNode);

  let mu = Number.POSITIVE_INFINITY; // best meeting key sum (real + const)
  let meetFrom = -1; // bridge edge meetFrom -> meetTo (direction of travel)
  let meetTo = -1;

  while (fHeap.size > 0 && bHeap.size > 0) {
    // THE BH STOP RULE: with consistent potentials, no future meeting can
    // beat mu once both frontiers' minima sum past it.
    if (fHeap.peekKey() + bHeap.peekKey() >= mu) break;
    // BALANCED ALTERNATION: expand the side with the smaller frontier.
    // (Measured 06-04: comparing raw tops starves the backward side — its
    // keys start near the full route cost while the forward side's start
    // near zero, so `top_f <= top_b` ran the forward search almost alone
    // (19501:3 settles) and landed 4x SLOWER than unidirectional. Heap
    // size keeps both rings growing; correctness lives entirely in the
    // stop rule — alternation order is a work-balancing knob only.)
    const expandForward = fHeap.size <= bHeap.size;
    const u = (expandForward ? fHeap : bHeap).pop()!;
    if (expandForward ? fClosed[u]! !== 0 : bClosed[u]! !== 0) continue; // lazy deletion
    if (expandForward) fClosed[u] = 1;
    else bClosed[u] = 1;

    if (expandForward) {
      const du = df[u]!;
      const pfu = pf[u]!;
      for (let e = graph.edgeStart[u]!; e < graph.edgeStart[u + 1]!; e++) {
        if ((graph.edgeFlags[e]! & 1) !== 0) continue; // oneway-reverse: never traversed
        const v = graph.edgeTarget[e]!;
        const pv = potential(v);
        const candidate = du + graph.edgeSeconds[e]! + pfu - pv;
        if (candidate < df[v]!) {
          df[v] = candidate;
          fPrev[v] = u;
          fHeap.push(candidate, v);
        }
        // Meet probe: v already reached backward? (candidate uses v's BEST d_b)
        if (db[v]! < Number.POSITIVE_INFINITY) {
          const meet = candidate + db[v]!;
          if (meet < mu) {
            mu = meet;
            meetFrom = u;
            meetTo = v;
          }
        }
      }
    } else {
      const rev = reverseFor(graph);
      const du = db[u]!;
      const pfu = pf[u]!;
      const end = rev.rowStart[u + 1]!;
      for (let r = rev.rowStart[u]!; r < end; r++) {
        const e = rev.edgeIds[r]!; // the edge v -> u
        if ((graph.edgeFlags[e]! & 1) !== 0) continue; // oneway-reverse: never traversed
        const v = rev.targets[r]!;
        const pvv = potential(v);
        const candidate = du + graph.edgeSeconds[e]! - pfu + pvv;
        if (candidate < db[v]!) {
          db[v] = candidate;
          bNext[v] = u;
          bHeap.push(candidate, v);
        }
        if (df[v]! < Number.POSITIVE_INFINITY) {
          const meet = candidate + df[v]!;
          if (meet < mu) {
            mu = meet;
            meetFrom = v; // the path travels v -> u here
            meetTo = u;
          }
        }
      }
    }
  }

  if (meetFrom < 0) return null; // never met — unreachable (honest miss)

  // Reconstruct: origin ->(fPrev)-> meetFrom -[bridge]-> meetTo ->(bNext)-> goal.
  const fPart: number[] = [];
  for (let x = meetFrom; x >= 0; x = fPrev[x]!) fPart.push(x);
  fPart.reverse();
  const bPart: number[] = [];
  for (let y = meetTo; y >= 0; y = bNext[y]!) bPart.push(y);
  const path = fPart.concat(bPart);

  // REAL costs recomputed along the node path (exact; no reduced-cost
  // arithmetic leaks into the returned seconds).
  let seconds = 0;
  let distanceM = 0;
  for (let i = 1; i < path.length; i++) {
    const a = path[i - 1]!;
    const b = path[i]!;
    const from = { lat: graph.lat[a]!, lng: graph.lng[a]! };
    const to = { lat: graph.lat[b]!, lng: graph.lng[b]! };
    distanceM += haversineM(from, to);
    const edge = findTraversableEdge(graph, a, b);
    seconds += edge >= 0 ? graph.edgeSeconds[edge]! : 0; // -1 only on a corrupt chain — loud in deriveSteps
  }
  return { nodes: path, seconds, distanceM };
}

/* ————— turn classification (the one authority; plan 07 imports this) ————— */

/**
 * Classify the maneuver between two compass bearings (degrees, 0..360,
 * clockwise). The delta normalizes to (-180, 180]; NEGATIVE delta is a
 * LEFT turn (counter-clockwise). Thresholds are the exported TURN_*
 * constants.
 */
export function classifyTurn(prevBearingDeg: number, nextBearingDeg: number): GuidanceTurnKind {
  let delta = (nextBearingDeg - prevBearingDeg) % 360;
  if (delta > 180) delta -= 360;
  if (delta <= -180) delta += 360;
  const abs = Math.abs(delta);
  if (abs >= TURN_UTURN_MIN_DEG) return "uturn";
  if (abs <= TURN_STRAIGHT_MAX_DEG) return "straight";
  if (abs <= TURN_SLIGHT_MAX_DEG) return delta < 0 ? "slight-left" : "slight-right";
  return delta < 0 ? "left" : "right";
}

/* ————— step derivation ————— */

function initialBearingDeg(a: RoutePoint, b: RoutePoint): number {
  const lat1 = a.lat * DEG_TO_RAD;
  const lat2 = b.lat * DEG_TO_RAD;
  const dLng = (b.lng - a.lng) * DEG_TO_RAD;
  const y = Math.sin(dLng) * Math.cos(lat2);
  const x = Math.cos(lat1) * Math.sin(lat2) - Math.sin(lat1) * Math.cos(lat2) * Math.cos(dLng);
  return (Math.atan2(y, x) / DEG_TO_RAD + 360) % 360;
}

/** The traversable (bit0 clear) edge u -> v, or -1. */
function findTraversableEdge(graph: RoadGraph, u: number, v: number): number {
  for (let e = graph.edgeStart[u]!; e < graph.edgeStart[u + 1]!; e++) {
    if (graph.edgeTarget[e]! === v && (graph.edgeFlags[e]! & 1) === 0) return e;
  }
  return -1;
}

interface StepAccumulator {
  kind: GuidanceTurnKind;
  name: string | undefined;
  points: RoutePoint[];
  distanceM: number;
}

/**
 * Derive maneuver steps from a node path: split where the bearing leaves
 * "straight" or the street name changes. Step 0 is always "depart"; a
 * terminal "arrive" step anchors the goal node. Each step carries its
 * street name (omitted when unnamed), haversine meters, and polyline6
 * geometry (the RouteStep.kind field this phase added to types.ts —
 * fixture-world steps stay empty arrays, D-10).
 *
 * Throws on a malformed path (a non-traversable consecutive pair) — a
 * caller bug is loud, unlike a route miss which stays null.
 */
export function deriveSteps(graph: RoadGraph, nodes: number[]): RouteStep[] {
  if (nodes.length === 0) return [];
  if (nodes.length === 1) return [{ kind: "arrive" }];
  const pointAt = (node: number): RoutePoint => ({ lat: graph.lat[node]!, lng: graph.lng[node]! });
  const steps: RouteStep[] = [];
  let current: StepAccumulator | null = null;
  let prevBearing = 0;
  for (let i = 0; i < nodes.length - 1; i++) {
    const u = nodes[i]!;
    const v = nodes[i + 1]!;
    const edge = findTraversableEdge(graph, u, v);
    if (edge < 0) {
      throw new Error(`deriveSteps: nodes[${i}]->nodes[${i + 1}] (${u}->${v}) is not a traversable edge`);
    }
    const from = pointAt(u);
    const to = pointAt(v);
    const bearing = initialBearingDeg(from, to);
    const kind: GuidanceTurnKind = i === 0 ? "depart" : classifyTurn(prevBearing, bearing);
    prevBearing = bearing;
    const name = graph.nameAt(graph.edgeNameId[edge]!);
    const segmentM = haversineM(from, to);
    const splits = current !== null && (kind !== "straight" || name !== current.name);
    if (splits || current === null) {
      if (current !== null) {
        steps.push(toStep(current));
      }
      current = { kind, name, points: [from, to], distanceM: segmentM };
    } else {
      current.points.push(to);
      current.distanceM += segmentM;
    }
  }
  if (current !== null) steps.push(toStep(current));
  const goal = pointAt(nodes[nodes.length - 1]!);
  steps.push({ kind: "arrive", geometry: encodePolyline6([goal]) });
  return steps;
}

function toStep(acc: StepAccumulator): RouteStep {
  const step: RouteStep = {
    kind: acc.kind,
    distanceM: acc.distanceM,
    geometry: encodePolyline6(acc.points),
  };
  if (acc.name !== undefined) step.name = acc.name;
  return step;
}