Dijkstra shortest path. A little bit dirty code but anyway.