Refactor shortest path code