Implemented default Dijkstra's algorithm.
Refactored to a mode STL-like design