Started to implement dijkstra function. It needs some serious thinking to get it right.