#!/usr/bin/env catnip
# A* sur une grille 2D
#
# Déplacements orthogonaux de coût unitaire, obstacles fixes et heuristique
# de Manhattan. La recherche et la reconstruction du chemin sont écrites en
# Catnip ; le module standard Python heapq fournit seulement le tas binaire.
#
# Avec une heuristique cohérente, chaque cellule est développée au plus une
# fois. Sur cette grille où E = O(V), le coût est O(V log V) en temps et O(V)
# en mémoire.
#
# Source:
#   Hart, Nilsson et Raphael (1968), "A Formal Basis for the Heuristic
#   Determination of Minimum Cost Paths", IEEE Transactions on Systems
#   Science and Cybernetics, 4(2), 100-107.

heapq = import('heapq')

struct Cell {
    x: int; y: int;

    display(self): str => { f"({self.x}, {self.y})" }
}

struct SearchResult {
    path: list[Cell]; cost: int; expanded: int;
}

# Voisinage de von Neumann : droite, bas, gauche, haut.
neighbors = (cell: Cell, width: int, height: int, blocked: set[Cell]): list[Cell] => {
    directions = list(tuple(1, 0), tuple(0, 1), tuple(-1, 0), tuple(0, -1))
    result = list()

    for direction in directions {
        candidate = Cell(cell.x + direction[0], cell.y + direction[1])
        inside = candidate.x >= 0 and candidate.x < width and candidate.y >= 0 and candidate.y < height
        if inside and candidate not in blocked {
            result = result + list(candidate)
        }
    }
    result
}

manhattan = (cell: Cell, goal: Cell): int => {
    abs(cell.x - goal.x) + abs(cell.y - goal.y)
}

zero_heuristic = (_cell: Cell, _goal: Cell): int => { 0 }

reconstruct_path = (came_from: dict[Cell, Cell], goal: Cell): list[Cell] => {
    current = goal
    path = list(current)
    while current in came_from {
        current = came_from[current]
        path = list(current) + path
    }
    path
}

# Les entrées du tas sont (f, ordre, g, cellule). L'ordre est unique : heapq
# n'a donc jamais besoin de comparer deux Cell lorsque leurs priorités f sont
# égales. Une amélioration ajoute une nouvelle entrée ; les anciennes sont
# ignorées à leur sortie du tas.
shortest_path = (width: int, height: int, blocked: set[Cell], start: Cell, goal: Cell, heuristic): SearchResult => {
    frontier = list()
    order = 0
    heapq.heappush(frontier, tuple(heuristic(start, goal), order, 0, start))

    came_from = dict()
    g_score = dict((start, 0))
    expanded = 0

    while len(frontier) > 0 {
        entry = heapq.heappop(frontier)
        queued_g = entry[2]
        current = entry[3]

        # Entrée périmée après la découverte d'un chemin moins coûteux.
        if queued_g != g_score[current] {
            continue
        }

        expanded = expanded + 1
        if current == goal {
            return SearchResult(reconstruct_path(came_from, current), queued_g, expanded)
        }

        for neighbor in neighbors(current, width, height, blocked) {
            tentative_g = queued_g + 1
            known_g = g_score.get(neighbor)
            if known_g is None or tentative_g < known_g {
                came_from[neighbor] = current
                g_score[neighbor] = tentative_g
                order = order + 1
                priority = tentative_g + heuristic(neighbor, goal)
                heapq.heappush(frontier, tuple(priority, order, tentative_g, neighbor))
            }
        }
    }

    SearchResult(list(), -1, expanded)
}

valid_path = (path: list[Cell], blocked: set[Cell], start: Cell, goal: Cell): bool => {
    if len(path) == 0 or path[0] != start or path[len(path) - 1] != goal {
        return False
    }

    i = 0
    while i < len(path) {
        if path[i] in blocked {
            return False
        }
        if i > 0 {
            dx = abs(path[i].x - path[i - 1].x)
            dy = abs(path[i].y - path[i - 1].y)
            if dx + dy != 1 {
                return False
            }
        }
        i = i + 1
    }
    True
}

render = (width: int, height: int, blocked: set[Cell], path: list[Cell], start: Cell, goal: Cell) => {
    path_cells = set(*path)
    for y in range(height) {
        row = ''
        for x in range(width) {
            cell = Cell(x, y)
            marker = if cell == start { 'S' }
            elif cell == goal { 'G' }
            elif cell in blocked { '#' }
            elif cell in path_cells { '*' }
            else { '.' }
            row = row + marker
        }
        print(f"  {row}")
    }
}

# Deux murs décalés imposent des détours sans fermer la grille.
width = 18
height = 10
start = Cell(1, 1)
goal = Cell(16, 8)
blocked = set()

for y in range(8) {
    if y != 4 {
        blocked.add(Cell(5, y))
    }
}
for y in range(2, height) {
    if y != 6 {
        blocked.add(Cell(11, y))
    }
}

a_star = shortest_path(width, height, blocked, start, goal, manhattan)
dijkstra = shortest_path(width, height, blocked, start, goal, zero_heuristic)

if a_star.cost < 0 or dijkstra.cost < 0 {
    raise RuntimeError("aucun chemin entre le départ et l'arrivée")
}

# Dijkstra est A* avec h = 0 : il sert d'oracle indépendant de l'heuristique.
if not valid_path(a_star.path, blocked, start, goal) or a_star.cost != len(a_star.path) - 1 {
    raise RuntimeError("A* a produit un chemin invalide")
}
if not valid_path(dijkstra.path, blocked, start, goal) or dijkstra.cost != len(dijkstra.path) - 1 {
    raise RuntimeError("Dijkstra a produit un chemin invalide")
}
if a_star.cost != dijkstra.cost {
    raise RuntimeError(f"A* n'est pas optimal : coût {a_star.cost}, oracle {dijkstra.cost}")
}

print("⇒ A* sur une grille 2D")
print(f"  Départ {start.display()} → arrivée {goal.display()}")
print(f"  Coût optimal : {a_star.cost}")
print(f"  Cellules développées : A* {a_star.expanded}, Dijkstra {dijkstra.expanded}")
print()
render(width, height, blocked, a_star.path, start, goal)