Dijkstra - working version, mess in code