Implementation of Dijkstra's algorithm with various heuristics