Finding the shortest path in a grid with BFS