Extract Dijkstra algorithm parts to make them reusable.
Add Bellman-Ford algorithm implementation. NOT FINISHED!