A* search and heuristics

import random
from math import isqrt

from route import a_star

LESSON_TOWNS = {
    "A": (0, 3), "B": (2, 6), "C": (3, 3), "D": (2, 0), "E": (6, 6),
    "F": (6, 0), "G": (10, 3), "H": (-3, 3), "I": (-2, 6), "J": (-2, 0),
}
LESSON_ROADS = [
    ("A", "H", 3), ("A", "I", 4), ("A", "J", 4), ("H", "I", 4), ("H", "J", 4),
    ("A", "B", 5), ("A", "C", 3), ("A", "D", 4), ("B", "C", 4), ("B", "E", 4),
    ("C", "E", 5), ("C", "F", 5), ("D", "F", 4), ("E", "G", 5), ("F", "G", 6),
]


def check_route(roads, start, goal, length, route):
    """the route runs from start to goal along roads whose lengths add up to length"""
    lengths = {}
    for a, b, d in roads:
        lengths[(a, b)] = lengths[(b, a)] = d
    assert route[0] == start and route[-1] == goal, route
    assert sum(lengths[(a, b)] for a, b in zip(route, route[1:])) == length, route


def slow_length(towns, roads, start, goal):
    """relax every road both ways until nothing changes"""
    dist = {name: float("inf") for name in towns}
    dist[start] = 0
    changed = True
    while changed:
        changed = False
        for a, b, d in roads:
            for u, v in ((a, b), (b, a)):
                if dist[u] + d < dist[v]:
                    dist[v] = dist[u] + d
                    changed = True
    return -1 if dist[goal] == float("inf") else dist[goal]


def test_lesson_map():
    """the lesson's ten towns: 13 km by A, C, E, G, with at most 7 towns expanded"""
    length, route, expanded = a_star(LESSON_TOWNS, LESSON_ROADS, "A", "G")
    assert (length, route) == (13, ["A", "C", "E", "G"])
    assert expanded <= 7, expanded


def test_start_is_goal():
    """a trip from a town to itself: length 0, one town on the route, one town expanded"""
    assert a_star(LESSON_TOWNS, LESSON_ROADS, "D", "D") == (0, ["D"], 1)


def test_goal_on_another_island():
    """no road reaches the goal: -1 and an empty route"""
    towns = {"P": (0, 0), "Q": (3, 4), "R": (10, 0), "S": (10, 5)}
    roads = [("P", "Q", 5), ("R", "S", 6)]
    length, route, _ = a_star(towns, roads, "P", "S")
    assert (length, route) == (-1, [])


def test_random_maps():
    """200 random maps of 2 to 9 towns: lengths match a slow method and every route is valid"""
    rng = random.Random(8)
    for _ in range(200):
        names = [f"T{i}" for i in range(rng.randint(2, 9))]
        towns = {name: (rng.randint(0, 20), rng.randint(0, 20)) for name in names}
        roads = []
        for i in range(len(names)):
            for j in range(i + 1, len(names)):
                if rng.random() < 0.45:
                    (x1, y1), (x2, y2) = towns[names[i]], towns[names[j]]
                    d2 = (x1 - x2) ** 2 + (y1 - y2) ** 2
                    ceiling = isqrt(d2) + (isqrt(d2) ** 2 < d2)  # never shorter than the straight line
                    roads.append((names[i], names[j], max(1, ceiling + rng.randint(0, 6))))
        start, goal = rng.choice(names), rng.choice(names)
        length, route, _ = a_star(towns, roads, start, goal)
        assert length == slow_length(towns, roads, start, goal), (towns, roads, start, goal)
        if length == -1:
            assert route == []
        else:
            check_route(roads, start, goal, length, route)


def test_large_map():
    """3,600 towns on a 60 x 60 grid: the route is 6,676 long, and fewer than 1,800 towns are expanded"""
    state = 11

    def below(k):
        nonlocal state
        state = state * 48271 % 2147483647
        return state % k

    n = 60
    towns = {}
    for r in range(n):
        for c in range(n):
            towns[f"{r},{c}"] = (c * 100 + below(61) - 30, r * 100 + below(61) - 30)
    roads = []
    for r in range(n):
        for c in range(n):
            diagonal = (r + 1, c + 1) if below(2) == 0 else (r + 1, c - 1)
            for nr, nc in ((r, c + 1), (r + 1, c), diagonal):
                if 0 <= nr < n and 0 <= nc < n and below(100) < 85:
                    (x1, y1), (x2, y2) = towns[f"{r},{c}"], towns[f"{nr},{nc}"]
                    straight = isqrt((x1 - x2) ** 2 + (y1 - y2) ** 2)
                    roads.append((f"{r},{c}", f"{nr},{nc}", straight + straight * below(31) // 100 + 1))
    length, route, expanded = a_star(towns, roads, "30,2", "30,57")
    assert length == 6676, length
    check_route(roads, "30,2", "30,57", length, route)
    assert expanded < 1800, f"{expanded} towns expanded: is the estimate used in the order of the queue?"

import { test, assert } from 'mysmartcopilot:test';
import { aStar } from './route.mjs';

const LESSON_TOWNS = {
  A: [0, 3], B: [2, 6], C: [3, 3], D: [2, 0], E: [6, 6], F: [6, 0], G: [10, 3], H: [-3, 3], I: [-2, 6], J: [-2, 0],
};
const LESSON_ROADS = [
  ['A', 'H', 3], ['A', 'I', 4], ['A', 'J', 4], ['H', 'I', 4], ['H', 'J', 4],
  ['A', 'B', 5], ['A', 'C', 3], ['A', 'D', 4], ['B', 'C', 4], ['B', 'E', 4],
  ['C', 'E', 5], ['C', 'F', 5], ['D', 'F', 4], ['E', 'G', 5], ['F', 'G', 6],
];

function isqrt(n) {
  let r = Math.floor(Math.sqrt(n));
  while (r * r > n) r--;
  while ((r + 1) * (r + 1) <= n) r++;
  return r;
}

function checkRoute(roads, start, goal, length, route) {
  const lengths = new Map();
  for (const [a, b, d] of roads) {
    lengths.set(`${a}|${b}`, d);
    lengths.set(`${b}|${a}`, d);
  }
  assert.equal(route[0], start, 'the route starts at the start');
  assert.equal(route[route.length - 1], goal, 'the route ends at the goal');
  let sum = 0;
  for (let i = 1; i < route.length; i++) {
    const d = lengths.get(`${route[i - 1]}|${route[i]}`);
    assert.ok(d !== undefined, `${route[i - 1]} and ${route[i]} are joined by a road`);
    sum += d;
  }
  assert.equal(sum, length, 'the road lengths add up to the length returned');
}

function slowLength(towns, roads, start, goal) {
  const dist = {};
  for (const name of Object.keys(towns)) dist[name] = Infinity;
  dist[start] = 0;
  let changed = true;
  while (changed) {
    changed = false;
    for (const [a, b, d] of roads) {
      for (const [u, v] of [[a, b], [b, a]]) {
        if (dist[u] + d < dist[v]) {
          dist[v] = dist[u] + d;
          changed = true;
        }
      }
    }
  }
  return dist[goal] === Infinity ? -1 : dist[goal];
}

function lcg(seed) {
  let state = seed;
  return (k) => {
    state = (state * 48271) % 2147483647;
    return state % k;
  };
}

test("the lesson's ten towns: 13 km by A, C, E, G, with at most 7 towns expanded", () => {
  const [length, route, expanded] = aStar(LESSON_TOWNS, LESSON_ROADS, 'A', 'G');
  assert.deepEqual([length, route], [13, ['A', 'C', 'E', 'G']]);
  assert.ok(expanded <= 7, `${expanded} towns expanded`);
});

test('a trip from a town to itself: length 0, one town on the route, one town expanded', () => {
  assert.deepEqual(aStar(LESSON_TOWNS, LESSON_ROADS, 'D', 'D'), [0, ['D'], 1]);
});

test('no road reaches the goal: -1 and an empty route', () => {
  const towns = { P: [0, 0], Q: [3, 4], R: [10, 0], S: [10, 5] };
  const [length, route] = aStar(towns, [['P', 'Q', 5], ['R', 'S', 6]], 'P', 'S');
  assert.deepEqual([length, route], [-1, []]);
});

test('200 random maps of 2 to 9 towns: lengths match a slow method and every route is valid', () => {
  const below = lcg(8);
  for (let k = 0; k < 200; k++) {
    const names = Array.from({ length: 2 + below(8) }, (_, i) => `T${i}`);
    const towns = {};
    for (const name of names) towns[name] = [below(21), below(21)];
    const roads = [];
    for (let i = 0; i < names.length; i++) {
      for (let j = i + 1; j < names.length; j++) {
        if (below(100) < 45) {
          const [x1, y1] = towns[names[i]], [x2, y2] = towns[names[j]];
          const d2 = (x1 - x2) ** 2 + (y1 - y2) ** 2;
          const ceiling = isqrt(d2) + (isqrt(d2) ** 2 < d2 ? 1 : 0); // never shorter than the straight line
          roads.push([names[i], names[j], Math.max(1, ceiling + below(7))]);
        }
      }
    }
    const start = names[below(names.length)], goal = names[below(names.length)];
    const [length, route] = aStar(towns, roads, start, goal);
    assert.equal(length, slowLength(towns, roads, start, goal), JSON.stringify({ towns, roads, start, goal }));
    if (length === -1) assert.deepEqual(route, []);
    else checkRoute(roads, start, goal, length, route);
  }
});

test('3,600 towns on a 60 x 60 grid: the route is 6,676 long, and fewer than 1,800 towns are expanded', () => {
  const below = lcg(11);
  const n = 60, towns = {}, roads = [];
  for (let r = 0; r < n; r++) {
    for (let c = 0; c < n; c++) towns[`${r},${c}`] = [c * 100 + below(61) - 30, r * 100 + below(61) - 30];
  }
  for (let r = 0; r < n; r++) {
    for (let c = 0; c < n; c++) {
      const diagonal = below(2) === 0 ? [r + 1, c + 1] : [r + 1, c - 1];
      for (const [nr, nc] of [[r, c + 1], [r + 1, c], diagonal]) {
        if (nr >= 0 && nr < n && nc >= 0 && nc < n && below(100) < 85) {
          const [x1, y1] = towns[`${r},${c}`], [x2, y2] = towns[`${nr},${nc}`];
          const straight = isqrt((x1 - x2) ** 2 + (y1 - y2) ** 2);
          roads.push([`${r},${c}`, `${nr},${nc}`, straight + Math.floor((straight * below(31)) / 100) + 1]);
        }
      }
    }
  }
  const [length, route, expanded] = aStar(towns, roads, '30,2', '30,57');
  assert.equal(length, 6676);
  checkRoute(roads, '30,2', '30,57', length, route);
  assert.ok(expanded < 1800, `${expanded} towns expanded: is the estimate used in the order of the queue?`);
});