Add Dijkstra Algorithm for routing, not entirely complete