From 602b893f8130829390ae9438067d486a01d92030 Mon Sep 17 00:00:00 2001
From: Bartosz <bartek.fabisiak1@gmail.com>
Date: Sun, 31 Oct 2021 13:01:58 +0100
Subject: [PATCH] Implemented A* algorithm (needs better heuristic function).

---
 algorithms/a_star.py | 59 ++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 59 insertions(+)

diff --git a/algorithms/a_star.py b/algorithms/a_star.py
index e69de29..696cfe4 100644
--- a/algorithms/a_star.py
+++ b/algorithms/a_star.py
@@ -0,0 +1,59 @@
+def heuristic_cost(start, goal, graph):
+    return 0
+
+
+def a_star_algorithm(graph, point_set, start, goal, h=heuristic_cost):
+    for edge in graph.keys():
+        point_set[edge[0]].append(edge[1])
+        point_set[edge[1]].append(edge[0])
+    open_set = set()
+    open_set.add(start)
+    came_from = {}
+    g_score = {k: float('inf') for k in point_set.keys()}
+    g_score[start] = 0
+    f_score = {k: float('inf') for k in point_set.keys()}
+    f_score[start] = h(start, goal, graph)
+    while len(open_set) > 0:
+        current = list(open_set)[0]
+        for k in open_set:
+            if f_score[k] < f_score[current]:
+                current = k
+        if current == goal:
+            return came_from, current
+        open_set.remove(current)
+        for neighbor in point_set[current]:
+            tentative_g_score = g_score[current]
+            if current > neighbor:
+                tentative_g_score += graph[(current, neighbor)]
+            else:
+                tentative_g_score += graph[(neighbor, current)]
+            if tentative_g_score < g_score[neighbor]:
+                came_from[neighbor] = current
+                g_score[neighbor] = tentative_g_score
+                f_score[neighbor] = g_score[neighbor] + h(neighbor, goal, graph)
+                if neighbor not in open_set:
+                    open_set.add(neighbor)
+
+
+if __name__ == "__main__":
+    g = {
+        (2, 1): 3,
+        (3, 2): 2,
+        (5, 3): 1,
+        (9, 5): 5,
+        (10, 9): 4,
+        (9, 4): 3,
+        (4, 1): 4,
+        (7, 1): 6,
+        (3, 1): 4,
+        (6, 2): 3,
+        (8, 6): 8,
+        (8, 3): 2
+    }
+
+    v = dict()
+    for i in g.keys():
+        v[i[0]] = []
+        v[i[1]] = []
+
+    print(a_star_algorithm(g, v, 7, 10, heuristic_cost))