First working version of Dijkstra done