Dijkstra search algorithm
TODO tidy up