arah2/routing/src/snap.ts

/*
 * snap.ts — server-side GPS-fix snapping onto the CSR road graph (D-05).
 * ------------------------------------------------------------------------
 * Engine A owns snapping — ONE authority for road geometry, so the client
 * and the router can never drift. This is the SERVER authority (a reroute
 * origin lands here); the CLIENT progress-projection (phase 6) walks the
 * selected route polyline instead and never calls this module.
 *
 * An honest miss resolves NULL: a fix outside the graph bbox (plus
 * SNAP_BBOX_MARGIN_DEG) or outside any local grid neighborhood is the
 * designed "router-pending" signal (D-03) — never a fabricated node,
 * never a throw.
 *
 * Direction plays no part here: a oneway segment's flagged reverse edge
 * (edgeFlags bit0 — see graph.ts) still carries segment geometry and its
 * street name, so fixes near a oneway road snap to its endpoints exactly
 * like two-way traffic. A* enforces the direction law; snapping ignores it.
 *
 * Cost: the coarse grid index (~0.002 deg cells) is built ONCE per
 * RoadGraph and memoized in a module-scope WeakMap (LAW 1 lazy memoize —
 * the fixture-source precedent; per-isolate in a Worker, per-process in
 * Node). Each fix then touches only the 3x3 cell neighborhood — O(local),
 * never O(nodes) (RESEARCH anti-pattern: global nearest-node scans).
 *
 * Pitfall 8 law: projection math scales lng by cos(lat) — never raw
 * degree deltas; the RETURNED distance is haversineM (geo.ts, the only
 * distance authority that leaves this module).
 *
 * Pure: no I/O, no logging, no storage (T-06-05). Coordinates exist only
 * in the caller's frame.
 *
 * Known limitation (documented, honest): candidate segments come from the
 * 3x3 cell neighborhood of the FIX's cell — a segment both of whose
 * endpoints sit more than one cell (~0.002 deg) away from the fix's cell
 * is not a candidate. KL-metro build data (plan 04) densifies at
 * intersections, well inside this window; a miss degrades to null
 * (router-pending), never a wrong snap.
 */

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

/** Fixes beyond bbox+margin are honest misses (router-pending), in degrees. */
export const SNAP_BBOX_MARGIN_DEG = 0.01;

/** Coarse grid cell edge, degrees — tuned to urban block spacing. */
const GRID_CELL_DEG = 0.002;

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

/** A snap outcome: the graph node a fix resolves to. */
export interface SnapResult {
  /** Node index into the graph's coordinate arrays. */
  node: number;
  /** haversineM(fix, snapped node) — meters. */
  distanceM: number;
  /**
   * The best segment's edgeNameId (edges carry names; nodes don't — plan
   * 07's cron street-hop consumes this). null when the edge is unnamed
   * (UNNAMED_NAME_ID sentinel).
   */
  nameId: number | null;
}

/* ————— per-graph coarse grid (built once, memoized by WeakMap) ————— */

type Grid = Map<string, number[]>;

const grids = new WeakMap<RoadGraph, Grid>();

let gridBuilds = 0;

/** Instrumentation (tests/instruments): how many grids have been built. */
export function gridBuildCount(): number {
  return gridBuilds;
}

function cellKey(cx: number, cy: number): string {
  return `${cx}:${cy}`;
}

function gridFor(graph: RoadGraph): Grid {
  const memoized = grids.get(graph);
  if (memoized) return memoized;
  const grid: Grid = new Map();
  const inv = 1 / GRID_CELL_DEG;
  for (let n = 0; n < graph.nodeCount; n++) {
    const cx = Math.floor((graph.lng[n]! - graph.bbox.minLng) * inv);
    const cy = Math.floor((graph.lat[n]! - graph.bbox.minLat) * inv);
    const key = cellKey(cx, cy);
    const bucket = grid.get(key);
    if (bucket) bucket.push(n);
    else grid.set(key, [n]);
  }
  gridBuilds += 1;
  grids.set(graph, grid);
  return grid;
}

/**
 * Snap a raw GPS fix to the nearest graph node by point-to-SEGMENT
 * projection over the 3x3 cell neighborhood. Returns the closer endpoint
 * of the best segment together with that segment's nameId, or null when
 * the fix is an honest miss.
 */
export function snapToGraph(graph: RoadGraph, point: RoutePoint): SnapResult | null {
  const margin = SNAP_BBOX_MARGIN_DEG;
  const { minLat, minLng, maxLat, maxLng } = graph.bbox;
  if (
    point.lat < minLat - margin ||
    point.lat > maxLat + margin ||
    point.lng < minLng - margin ||
    point.lng > maxLng + margin
  ) {
    return null; // honest miss — router-pending upstream (D-03)
  }
  const grid = gridFor(graph);
  const inv = 1 / GRID_CELL_DEG;
  const pcx = Math.floor((point.lng - minLng) * inv);
  const pcy = Math.floor((point.lat - minLat) * inv);
  // Pitfall 8: scale ALL lng deltas by cos(lat) before any comparison.
  const cosLat = Math.cos(point.lat * DEG_TO_RAD);
  const px = point.lng * cosLat;
  const py = point.lat;

  let bestDist2 = Infinity;
  let bestNode = -1;
  let bestNameId: number | null = null;
  for (let dy = -1; dy <= 1; dy++) {
    for (let dx = -1; dx <= 1; dx++) {
      const bucket = grid.get(cellKey(pcx + dx, pcy + dy));
      if (bucket === undefined) continue;
      for (const u of bucket) {
        for (let e = graph.edgeStart[u]!; e < graph.edgeStart[u + 1]!; e++) {
          const v = graph.edgeTarget[e]!;
          // Point-to-segment projection in cos-scaled degree space.
          const ax = graph.lng[u]! * cosLat;
          const ay = graph.lat[u]!;
          const bx = graph.lng[v]! * cosLat;
          const by = graph.lat[v]!;
          const abx = bx - ax;
          const aby = by - ay;
          const len2 = abx * abx + aby * aby;
          let t = 0;
          if (len2 > 0) {
            t = ((px - ax) * abx + (py - ay) * aby) / len2;
            t = t < 0 ? 0 : t > 1 ? 1 : t;
          }
          const qx = ax + t * abx - px;
          const qy = ay + t * aby - py;
          // Squared meters — ranking only; the reported distance is haversine.
          const d2 = (qx * qx + qy * qy) * M_PER_DEG * M_PER_DEG;
          if (d2 < bestDist2) {
            bestDist2 = d2;
            bestNode = t <= 0.5 ? u : v;
            const nameId = graph.edgeNameId[e]!;
            bestNameId = nameId === UNNAMED_NAME_ID ? null : nameId;
          }
        }
      }
    }
  }
  if (bestNode < 0) return null; // in margin band but no local roads — honest miss
  return {
    node: bestNode,
    distanceM: haversineM(point, { lat: graph.lat[bestNode]!, lng: graph.lng[bestNode]! }),
    nameId: bestNameId,
  };
}