Breadth First Search implementation for Dijkstra
Merging Astar and BFS