Initial Dijkstra Implementation Algorithm