Dijkstra's now working. Will add more thorough comments later.