initial dijkstra

1 files changed, 22 insertions, 1 deletions

diff --git a/algorithms.py b/algorithms.py
index 7140a17..06d768c 100644
--- a/algorithms.py
+++ b/algorithms.py
@@ -1,3 +1,4 @@
+import heapq
 import math
 
 
@@ -32,4 +33,24 @@ def get_closest_node_id(nodes, source_node):
 
 def find_shortest_path(nodes, source_id, target_id):
     """ Return the shortest path using Dijkstra's algortihm. """
-    return []
+    queue = []
+    visited = set()
+
+    print("neighbours", nodes[source_id].neighbours)
+    for neighbour in nodes[source_id].neighbours:
+        heapq.heappush(queue, (length_haversine(nodes[source_id], neighbour), (source_id, neighbour.id)))
+
+    while queue:
+        cand_dist, cand_path = heapq.heappop(queue)
+        walk_end = cand_path[-1]
+        if walk_end == target_id:
+            return cand_path
+        if walk_end in visited:
+            continue
+        visited.add(walk_end)
+        for neighbour in nodes[walk_end].neighbours:
+            print(neighbour)
+            if neighbour not in visited:
+                heapq.heappush(queue, (cand_dist + length_haversine(nodes[walk_end], neighbour), cand_path + (neighbour.id, )))
+    # no path found
+    return None