Added initial implementation based on Dijkstra.