Finding shortest path has bugs