Refactorings for shortest path algo (not finished)