Implemented Dijkstra!
TODO implement check if nodes are connected
Applied some formatting