Refactoring and first try at dijkstra's, not tested yet