1
from queue import Queue, LifoQueue, PriorityQueue
2
 
3
INFINITY = float("inf")
4
 
5
def print_node_info(current_node, path_cost, path):
6
    print("Visiting: ", current_node, " Path Cost: ", path_cost, " Path: ", path)
7
 
8
def bfs(graph, start, end, UCS=False, start_node=None):
9
    queue = Queue() if not UCS else PriorityQueue()
10
    visited = []
11
    queue.put((0, start, [start]) if start_node is None else (0, start_node, [start_node]))
12
 
13
    while not queue.empty():
14
        path_cost, current_node, path = queue.get()
15
        if current_node in visited:
16
            continue
17
        visited.append(current_node)
18
 
19
        print_node_info(current_node, path_cost, path) if start_node is None else None
20
 
21
        if current_node == end:
22
            print(f"Goal Reached! Cost: ", path_cost) if start_node is None else None
23
            break
24
 
25
        for neighbor, weight in graph[current_node].items():
26
            if neighbor not in visited:
27
                new_path_cost = path_cost + weight
28
                new_path = path + [neighbor]
29
                queue.put((new_path_cost, neighbor, new_path))
30
        
31
        print("Frontier: ", end="")
32
        for frontier in queue.queue:
33
            print((frontier[1],frontier[0]), end=" ")
34
        print()
35
    
36
    return path_cost
37
 
38
def gbfs(graph, start, end, heuristic):
39
    queue = PriorityQueue()
40
    visited = []
41
    queue.put((heuristic[start], 0, start, [start]))
42
 
43
    while not queue.empty():
44
        h_cost, path_cost, current_node, path = queue.get()
45
        if current_node in visited:
46
            continue
47
        visited.append(current_node)
48
 
49
        print_node_info(current_node, path_cost, path)
50
 
51
        if h_cost == INFINITY:
52
            continue
53
 
54
        if current_node == end:
55
            print(f"Goal Reached! Cost: ", path_cost)
56
            break
57
 
58
        for neighbor, weight in graph[current_node].items():
59
            if neighbor not in visited:
60
                new_path_cost = path_cost + weight
61
                new_path = path + [neighbor]
62
                queue.put((heuristic[neighbor], new_path_cost, neighbor, new_path))
63
 
64
def asearch(graph, start, end, astar=False, heuristic=None):
65
    queue = PriorityQueue()
66
    visited = []
67
    queue.put((heuristic[start], 0, start, [start]))
68
    hs_history = {}
69
 
70
    while not queue.empty():
71
        h_cost, path_cost, current_node, path = queue.get()
72
        if current_node in visited:
73
            continue
74
        visited.append(current_node)
75
 
76
        print_node_info(current_node, path_cost, path)
77
 
78
        if astar:
79
            hs_history[current_node] = bfs(graph, start, end, UCS=True, start_node=current_node)
80
 
81
        if h_cost == INFINITY:
82
            continue
83
 
84
        if current_node == end:
85
            print(f"Goal Reached! Cost: ", path_cost)
86
            break
87
 
88
        for neighbor, weight in graph[current_node].items():
89
            if neighbor not in visited:
90
                new_path_cost = path_cost + weight
91
                new_path = path + [neighbor]
92
                total_cost = new_path_cost + heuristic[neighbor]
93
                queue.put((total_cost, new_path_cost, neighbor, new_path))
94
    
95
    return hs_history
96
 
97
def astarsearch(graph, start, end, heuristic):
98
    hs_history = asearch(graph, start, end, astar=True, heuristic=heuristic)
99
    for node, hs in hs_history.items():
100
        if hs > heuristic[node]:
101
            print(f"Not admissible h*(n)={hs} > h(n)={heuristic[node]}")
102
        else:
103
            print(f"Admissible h*(n)={hs} > h(n)={heuristic[node]}")
104
 
105
def dfs(graph, start, end):
106
    stack = LifoQueue()
107
    visited = []
108
    stack.put((start, [start], 0))
109
 
110
    while not stack.empty():
111
        current_node, path, path_cost = stack.get()
112
 
113
        if current_node in visited:
114
            continue
115
        visited.append(current_node)
116
 
117
        print_node_info(current_node, path_cost, path)
118
 
119
        if current_node == end:
120
            print(f"Goal Reached! Cost: ", path_cost)
121
            break
122
 
123
        for neighbor, weight in graph[current_node].items():
124
            if neighbor not in visited:
125
                new_path_cost = path_cost + weight
126
                new_path = path + [neighbor]
127
                stack.put((neighbor, new_path, new_path_cost))
128
 
129
if __name__ == "__main__":
130
    graph = {
131
    'S': {'A': 5, 'B': 3, 'C': 4},
132
    'A': {'C': 2},
133
    'B': {'C': 1},
134
    'C': {'D': 3, 'E': 5, 'F': 10},
135
    'D': {},
136
    'E': {},
137
    'F': {'G': 12, 'H': 9},
138
    'G': {},
139
    'H': {'G': 1}
140
    }
141
    heuristic_values = {'S': 10, 'A': 9, 'B': 7, 'C': 8, 'D': INFINITY, 'E': INFINITY, 'F': 5, 'G': 0, 'H': 2}
142
 
143
    bfs(graph, 'S', 'G')
144
    dfs(graph, 'S', 'G')
145
    bfs(graph, 'S', 'G', UCS=True)
146
    gbfs(graph, 'S', 'G', heuristic_values)
147
    asearch(graph, 'S', 'G', heuristic_values)
148
    astarsearch(graph, 'S', 'G', heuristic_values)