Implemented Dijkstra, agrees with A*