Add comments to shortestPath and clean it up