Dijkstra implemented, not fully tested though