Cleaned up Dijkstra

Still needs proper unit tests + better javadocs.