More work on Dijkstra

Refined Dijkstra algorithm, added path for round trip.
Added unit tests.