First part of dijkstra in, untested.
Most of dijkstra in!
Added citation