astar.py

import heapq


def heuristic(a: tuple, b: tuple) -> int:
    """Manhattan distance heuristic between two grid cells."""
    return abs(a[0] - b[0]) + abs(a[1] - b[1])


def astar(grid, start: tuple, goal: tuple) -> tuple:
    """
    A* pathfinding on a grid.

    Args:
        grid: Object with a get_neighbors(x, y) method.
        start: (x, y) start cell.
        goal:  (x, y) goal cell.

    Returns:
        path: List of (x, y) from start to goal, or None if unreachable.
        explored: List of visited nodes in order (useful for visualization).
    """
    open_set = []
    heapq.heappush(open_set, (0, start))

    came_from = {}
    g_score = {start: 0}
    f_score = {start: heuristic(start, goal)}
    explored = []
    visited = set()

    while open_set:
        _, current = heapq.heappop(open_set)

        if current in visited:
            continue
        visited.add(current)
        explored.append(current)

        if current == goal:
            path = []
            while current in came_from:
                path.append(current)
                current = came_from[current]
            path.append(start)
            path.reverse()
            return path, explored

        for neighbor in grid.get_neighbors(*current):
            tentative_g = g_score[current] + 1
            if tentative_g < g_score.get(neighbor, float("inf")):
                came_from[neighbor] = current
                g_score[neighbor] = tentative_g
                f_score[neighbor] = tentative_g + heuristic(neighbor, goal)
                heapq.heappush(open_set, (f_score[neighbor], neighbor))

    return None, explored