Fix shortestPathTo by correct selection of tentative node

Dijkstra's algorithm requires that we visit nodes in order of least tentative distance. A bug in the code meant that the ordering was different, and some incorrect paths were reported.

Also, the algorithm takes a very long period of time to run on larger graphs, if we process already visited nodes (a node may now be in the priority queue many times). So these visited nodes should not be processed again.