A* Grid Pathfinding With Heuristic

export type GridPoint = readonly [row: number, column: number];

interface OpenEntry {
  readonly point: GridPoint;
  readonly score: number;
}

export function findPath(grid: readonly string[], start: GridPoint, goal: GridPoint): GridPoint[] | null {
  if (!Array.isArray(grid) || grid.length === 0) throw new RangeError("grid must be a non-empty array");
  const width = grid[0]?.length ?? 0;
  if (width === 0) throw new RangeError("grid rows must not be empty");
  for (const row of grid) {
    if (typeof row !== "string" || row.length !== width) {
      throw new RangeError("grid must be rectangular");
    }
  }
  if (!isWalkable(grid, start)) throw new RangeError("start is blocked or out of bounds");
  if (!isWalkable(grid, goal)) throw new RangeError("goal is blocked or out of bounds");

  const startKey = keyOf(start);
  const goalKey = keyOf(goal);
  const open = new Map<string, OpenEntry>([[startKey, { point: start, score: heuristic(start, goal) }]]);
  const cameFrom = new Map<string, string>();
  const cost = new Map<string, number>([[startKey, 0]]);
  const closed = new Set<string>();

  while (open.size > 0) {
    let currentKey: string | null = null;
    let currentEntry: OpenEntry | null = null;
    for (const [candidateKey, candidate] of open) {
      if (currentEntry === null || candidate.score < currentEntry.score) {
        currentKey = candidateKey;
        currentEntry = candidate;
      }
    }
    if (currentKey === null || currentEntry === null) break;
    open.delete(currentKey);
    if (closed.has(currentKey)) continue;
    closed.add(currentKey);

    if (currentKey === goalKey) {
      return reconstruct(cameFrom, currentKey).map(parsePoint);
    }

    const baseCost = cost.get(currentKey) ?? 0;
    for (const neighbor of neighbors(currentEntry.point)) {
      if (!isWalkable(grid, neighbor)) continue;
      const neighborKey = keyOf(neighbor);
      if (closed.has(neighborKey)) continue;
      const tentative = baseCost + 1;
      if (tentative >= (cost.get(neighborKey) ?? Number.POSITIVE_INFINITY)) continue;
      cost.set(neighborKey, tentative);
      cameFrom.set(neighborKey, currentKey);
      open.set(neighborKey, { point: neighbor, score: tentative + heuristic(neighbor, goal) });
    }
  }
  return null;
}

function isWalkable(grid: readonly string[], point: GridPoint): boolean {
  const [row, column] = point;
  if (!Number.isInteger(row) || !Number.isInteger(column)) return false;
  if (row < 0 || row >= grid.length) return false;
  const line = grid[row];
  if (line === undefined) return false;
  if (column < 0 || column >= line.length) return false;
  return line[column] !== "#";
}

function heuristic(point: GridPoint, goal: GridPoint): number {
  return Math.abs(point[0] - goal[0]) + Math.abs(point[1] - goal[1]);
}

function neighbors(point: GridPoint): GridPoint[] {
  const [row, column] = point;
  return [
    [row - 1, column],
    [row + 1, column],
    [row, column - 1],
    [row, column + 1],
  ];
}

function keyOf(point: GridPoint): string {
  return `${point[0]},${point[1]}`;
}

function parsePoint(key: string): GridPoint {
  const [row, column] = key.split(",");
  return [Number(row), Number(column)];
}

function reconstruct(cameFrom: ReadonlyMap<string, string>, goalKey: string): string[] {
  const keys = [goalKey];
  let cursor = cameFrom.get(goalKey);
  while (cursor !== undefined) {
    keys.push(cursor);
    cursor = cameFrom.get(cursor);
  }
  return keys.reverse();
}

const maze = [
  "S....",
  ".###.",
  ".#.#.",
  ".#.#.",
  ".###.",
  "....G",
];
console.log(findPath(maze, [0, 0], [5, 4]));
console.log(findPath(maze, [0, 0], [2, 2]));