Initial dijkstra implementation, not tested