Step two

Algo Dijkstra - almost done, need to fix.