Dijkstra implemented, needs heavy debugging.