Dijkstra improved, path is now drawn