From ba2ed3705d123d8489ec0b40613e6e5aa429ace8 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Maksymilian=20Miko=C5=82ajczak?= <maksguziczek@gmail.com>
Date: Thu, 4 May 2023 20:44:35 +0200
Subject: [PATCH] implemented a* instead of bfs and heuristic function

---
 __pycache__/astar.cpython-310.pyc       | Bin 0 -> 990 bytes
 __pycache__/bfs.cpython-310.pyc         | Bin 667 -> 710 bytes
 __pycache__/heuristicfn.cpython-310.pyc | Bin 0 -> 340 bytes
 __pycache__/state.cpython-310.pyc       | Bin 519 -> 697 bytes
 __pycache__/succ.cpython-310.pyc        | Bin 930 -> 1456 bytes
 astar.py                                |  61 ++++++++++++++++++++++++
 bfs.py                                  |  24 ----------
 heuristicfn.py                          |   2 +
 main.py                                 |   7 +--
 state.py                                |   7 ++-
 succ.py                                 |  27 ++++++-----
 11 files changed, 86 insertions(+), 42 deletions(-)
 create mode 100644 __pycache__/astar.cpython-310.pyc
 create mode 100644 __pycache__/heuristicfn.cpython-310.pyc
 create mode 100644 astar.py
 delete mode 100644 bfs.py
 create mode 100644 heuristicfn.py

diff --git a/__pycache__/astar.cpython-310.pyc b/__pycache__/astar.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..42aa76a6d268a4bb9cc8921e9b9f384a169d91f7
GIT binary patch
literal 990
zcmZuwO=}b}7*1xA`Pj}@i-Kq`ew@~W+oOnzpcM2_D!na82|MX_c4u~`Nw#z$14{ch
zgf5=^E&hXCJ@qFjRD3cm*n=7Jemw7I@@DFOzeljne(mKyI3Yj1a&rYx-eBu3keuYS
zBo!G^>gAves&Eu`a#*s8k2qwO^D{Dv@@S6?;tNEP0d-6-#$)jQt}1F(nB~WXTu1<X
z&j=&O%==xhK6r}#7F&Oygcx#4T4vcfwPc0TCV<W-q>X@uE26)FvlVG$8&A2Vvp~Io
z6xpDYqVqVXHeLmk08Mf-3)OQk!T&!t{nVm$liJvZmfGNywTb2Dy)L)U8c)}-vjgUP
zvB^8~X{SwXipnXhDXgitwmsXk{Fo?lwCnvndXKbgrjN|&bRGL&gpav(8n^JWY<l~c
zsxRGnGhYMoz)<HtI!U@Hj+1?b?CV8WvQ1|USZtI11ZW1H9i91De@AK~4=|8$N8v)r
z_FzMW;4W+y#)VoM$7R)+r3)vJ*-2At$CjQ(#av5eT+|39=f?H?B&F81(k>Kv?$$6*
z7=$V6xl5Ys`6@qA7m0BPr-yQ(3T=w9<44`(`$(+)=pCQbqCB}`OP4i5Yng8svdmHZ
zKo#?ebcsA^%38_XacyLyJ@c`#EM?`p;>V&~NEd=RHm<dC{i{A})jhE9L_I=7zh4ax
z>q-tQG1JScC<`$k>Z*|ALa3P-zMmUe784|#i~Sq8KsEKOSvuZH_)m!528n2oMwHP6
z-(8w@8t5Mut_Ke~ETI8#Z})!evd3T#9zuiVb8wkluub&@sdfgj^8cFhuV3Bq#1BYq
idgAZ0^Xd+9gFJ@8B`>QwUzGB-pR7(mGLoTc7W@T@(*rC3

literal 0
HcmV?d00001

diff --git a/__pycache__/bfs.cpython-310.pyc b/__pycache__/bfs.cpython-310.pyc
index f9f8ace0b0cf92919487d8962cd4d0c931f02ae7..6cd41fc78b2a22ce17a38a9563ed9dc2ade24751 100644
GIT binary patch
delta 190
zcmbQudW@AfpO=@50SJ!V4Nl>j$Sdoi0_3DHq%h_%<T6AtGJ@DlIZV0CQOrO#a|%lf
zLljF2YcPW*+e?r>O~zX+#ihx~K$<t8C^Nq(v!pVxG_^E!V`w)cqxj^9jINBVlhv7w
z{Uw2_{WMvNn1FN<Cy=<sS_o2D#0(Mu$rg#D5)vRq95%W6DWy57c8ow?F&mKJVc=op
GVFm!63nwK2

delta 147
zcmX@cI-8X@pO=@50SG1-1f{S~<dxNy0CG|oQW$d>av7o+85vTTQkYv9qL@-xf*CYf
zUxL(XGTve-E=^9}Sk%qPC^q>YqbnoxWEUo5N044WP39sdAYH@(B0wTVVyJ{Tkb8^6
WCO1E&G$+-L5y&fM0TMh6JWK!{JsL*<

diff --git a/__pycache__/heuristicfn.cpython-310.pyc b/__pycache__/heuristicfn.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..a8d87758d95a6736e9434ed9cb5aa29c7638047f
GIT binary patch
literal 340
zcmd1j<>g`kg7$C0DfK}5F^Gc<n1CDyATH(s5-AKRj4cdN3@J>(44TZzEI=U`0I^wt
z*cph6b$~<(Lk&>16hjRoh?D^8X=Vb7F@eRH5n|#DAhY~l8UgjZ1lgv^c#F9pzg&~$
z7F%&iVo^y2l&-wRnx3DSQvsnWS27fF09F5T*3Zb#P1Vm$%r36X&CJP6%+oK<%}h<s
zOf1Sy)c4FQNzKVjPt7aION>Vn&qysT$}BF)Ois(wE2zB1jl^dJnpzBUhz?^BC~W*R
bAvD-JB!UwtcZ<U&H$SB`C)EzFi-!RK>77i;

literal 0
HcmV?d00001

diff --git a/__pycache__/state.cpython-310.pyc b/__pycache__/state.cpython-310.pyc
index b39f4348da82751c4e5be3a998b9224ff7861b84..927e172c331ed437c286156d87409a0f0fb83ba6 100644
GIT binary patch
literal 697
zcmY+C&2HN;41g(HcHGoyft>dOz9cWO76o<~cFD!JTm->V8#Vq(B?V{--PS$E9-yzJ
z>z<*Poklvyn$qAWks?is_Ar}G0p<65t$s0pKV+OsipCwPn<Ku10>xfYe<o{CNexxn
zp#PI3Xx*W@F9;25reK{Yrjl1!rz%w(SFSS5&0|)3>2*_J<Vk2xulR=Q7Kp$C2nZ>X
z1&&NYhD<|(<RM39VS>yR)bPeu)g<OSX?5=-f8Gr)@&oa5um*k7o1u@nY{s?NS%ZFm
zh!-2Zx5jzXR7K=YxA&IrZYKmz$Ot)sdbAgaH?Wtu{JmLjhE6X#xpjxmv_|$z*BRXy
zX}5CuqxZTs4_LgH;>_>p@{8Re7DAZbcp>QG?h28!k7vqWuEsenPGJqjorL!P2UemH
z;1P%R2wPr*rIU({9ao8^ZymG2Z?qj7KZ25CF2n<NKoO3TO7>Eos?x!#%Bas?c3Oxy
z6`~u|zNLLB#M553#}hllTzgHR$((_zSp3C+)X^LZC|FsHz7_UJ{<HWoeQd4xdaQLj
JW>Rx=_y^7UlDhx^

delta 234
zcmdnV+Rnn8&&$ij00b8`gHo6$^7`9|06D1)QH&`JQA{a}DNHR4QOqgK!3>%#w^)Nq
z5=&B(*+A-{0K{ejVrL*OR+_j#ku!w>sFu-BlXc=XO;t^{TP($?IcY^8HANuBMa&?A
z1w^m{iIogR9Fv_H16e?hm^_D3irG(-WwI`lss~7*hy#`21aTRG1PB*{4B=qlVFZi#
OX+mfWEt3~ANdN!`$s=F@

diff --git a/__pycache__/succ.cpython-310.pyc b/__pycache__/succ.cpython-310.pyc
index 6927dcf0b724af09de00d341c5983c48bed9782c..d3465fc75de02eb6e777170701bdbb8ce625f5c4 100644
GIT binary patch
literal 1456
zcmb7E%Zl4D6qRJz@i>_>Oat@i7Z^$dT@NK?l5_$gQ<9WGC=?TEGOk}ia$o{36sC(V
z`w8u`KhY1-qFr>AeOF!e%9Dr7%*Fy;A6@BQpL6v{_V*nG?bn|n`@DzHZ#^hB0|rg#
z@;MlW7!FZ{Cm3r?LK2Y)0c^Y_6JtUd<@ky)lUbkW#AKBra_tqwAr~uin0hG(P`&00
z;mg#YUd1k1>Ab50Fp9`@O7-e>GvWIP`Z;v@5s1vt2b!VxKq4R_&gfU1k_>2o&wx&u
zkztJ)Jw+!d_v9!wH!Cud8MpMd@(l!R<F~WSDiyE)SP5u>iM{zxY8?a1lGza316@D2
zymq?E@!Hg7r>k6DKv!y|73LhHItoyMQ9<7kur0062*?*KdJiZpm+QX5vb8q!&i%(F
z*QWO^KR0D7hM1LW`|*Iba@_7~D_0vh|Ha({jXO?jjYoGHOP(sY%+<caeMNii&Pd_0
z(3e%2gYK&f7`6r0!%F9JFn&2`DLh<hhij{9B77dh?e~*dS>AlkV<w1ROmF6iRCEbe
z=!Zr*oo>6=8h2Zxi#0yKeA6Gfrm*z%!Q7LQv)96hl&R09GG~bw-rRkb%8`rdluMZi
z`5lQ92>E-~xK1M8h`c~9BR}-LxFI8-Pkm1WUZWeQJoIP4F81CP{5sFG`BG60^gAS<
zf}z+UP29u|ZmrQFpjz&$ZBZQ%#o`GB^j%AtQonn--dv-23=^WRjb4(oESOX9Df=u+
T*dpXFbOogiriN<R#)sq|c3VPM

literal 930
zcmZva&x;c=6vtmO$+V8UAYHL#4;~i=#EKw$S;S@Ax`m|+ZKY5I8Jf6`ogXEc1q<s*
z{}1)(&A-M&uAcfYc=Dyrud*A+<K?}V?<aXly1(xMj^k%zal`@s*hPIfxOjzAAE6N-
zNCYvN69R(1rE@xWgd@p&DwyD3*_;V?0)AtIwcr!OCs~k5pKhMxdh-B{p*mYoo6d*T
zZ{j?|seciGEBH)r;3JZjRK2*N-${mRVPg(XCY&$)2lC4dD>FRL*eZ9)YP%anO~yN@
zT-JeTA|)!dDoUYM@GDBeM@q&sSKRBsLs&vhrABKbp+<9%_U#T@QFEJCoz>aisoK4z
z*!%2s#deM(<@X(wLhClA;A4+>?o_GdwDx~m*&TdsZ)C{Gu#5g+YRJs+@$mIIdVPhD
z38pc)9M4Y2JwqlN8}J+EeyT#5;Lixt#JIs~B@?0SpD^sxD%FOqt@Q@O{zY##>`l*e
z|LpS3XzDu(zbTl#RiL#LgHT2Si}m$lA+=7G{t4<47W_Twen?~4jf16L$6*u(Nms|A
zT!evI2Hjzj$tb+SV<f@5THNY9U9AnX4nrON1Pvn|JtT+3Bad^hReHNIMpOqkjC^jm
owx54{G1bDDI<UrOw+F15HpWczBu>S3BwyMtX&22NG)as811(w7hyVZp

diff --git a/astar.py b/astar.py
new file mode 100644
index 0000000..834c2ee
--- /dev/null
+++ b/astar.py
@@ -0,0 +1,61 @@
+from succ import succ as successors
+from queue import PriorityQueue
+from state import State
+
+def astar(istate, goalx, goaly, passedFields):
+    fringe = PriorityQueue()
+    fringe.put(istate)
+    explored = set()
+    steps = []
+    while not fringe.empty():
+        state = fringe.get()
+        if state.xpos == goalx and state.ypos == goaly:
+            steps.insert(0, state)
+            while (state.parent != None):
+                state = state.parent
+                steps.insert(0, state)
+            return steps
+
+        element = successors(state, passedFields, goalx, goaly)
+        explored.add((state.xpos, state.ypos, state.orientation, state.priority))
+        for value in element:
+            val = (value.xpos, value.ypos, value.orientation, value.priority)
+            if val in explored:
+                continue 
+            cost = state.priority + value.priority
+            succesorState = State(state, value.action, 
+                                  value.xpos, value.ypos, 
+                                  value.orientation, cost, 
+                                  value.heuristic)
+            if value not in fringe.queue:
+                fringe.put(succesorState)
+            else :
+                for element in fringe.queue:
+                    if (element.xpos==value.xpos and element.ypos==value.ypos) and element > value:
+                        element.parent = state
+                        element.priority = value.priority
+    return False
+
+
+# def bfs(istate, goalx, goaly, passedFields):
+#     fringe = [istate]
+#     explored = []
+#     steps = []
+#     while fringe:
+#         state = fringe.pop(0)
+#         if state.xpos == goalx and state.ypos == goaly:
+#             steps.insert(0, state)
+#             while (state.parent != None):
+#                 state = state.parent
+#                 steps.insert(0, state)
+#             return steps
+
+#         element = successors(state, passedFields)
+#         explored.append((state.xpos, state.ypos, state.orientation))
+#         for value in element:
+#             val = (value.xpos, value.ypos, value.orientation)
+#             if val not in explored and value not in fringe:
+#                 fringe.append(value)
+#     return False
+
+
diff --git a/bfs.py b/bfs.py
deleted file mode 100644
index 1288543..0000000
--- a/bfs.py
+++ /dev/null
@@ -1,24 +0,0 @@
-from succ import succ as successors
-from queue import PriorityQueue
-
-
-def bfs(istate, goalx, goaly, passedFields):
-    fringe = [istate]
-    explored = []
-    steps = []
-    while fringe:
-        state = fringe.pop(0)
-        if state.xpos == goalx and state.ypos == goaly:
-            steps.insert(0, state)
-            while (state.parent != None):
-                state = state.parent
-                steps.insert(0, state)
-            return steps
-
-        element = successors(state, passedFields)
-        explored.append((state.xpos, state.ypos, state.orientation))
-        for value in element:
-            val = (value.xpos, value.ypos, value.orientation)
-            if val not in explored and value not in fringe:
-                fringe.append(value)
-    return False
diff --git a/heuristicfn.py b/heuristicfn.py
new file mode 100644
index 0000000..04d8486
--- /dev/null
+++ b/heuristicfn.py
@@ -0,0 +1,2 @@
+def heuristicfn(startx, starty, goalx, goaly):
+    return pow(((startx//50)-(starty//50)),2) + pow(((goalx//50)-(goaly//50)),2)
\ No newline at end of file
diff --git a/main.py b/main.py
index a94873f..12c9020 100644
--- a/main.py
+++ b/main.py
@@ -1,9 +1,10 @@
 import pygame
 import random
-from bfs import bfs
+from astar import astar
 from state import State
 import time
 from garbage_truck import GarbageTruck
+from heuristicfn import heuristicfn
 
 pygame.init()
 WIDTH, HEIGHT = 800, 800
@@ -44,7 +45,7 @@ def randomize_map():        # tworzenie mapy z losowymi polami
                 if 0 <= prob <= 10:
                     field_array_2.append(COBBLE)
                     temp_priority.append(3)
-                if 10 < prob <= 20:
+                elif 10 < prob <= 20:
                     field_array_2.append(SAND)
                     temp_priority.append(2)
                 else:
@@ -78,7 +79,7 @@ def main():
                 run = False
         # keys_pressed = pygame.key.get_pressed()
         draw_window(agent, fields)
-        steps = bfs(State(None, None, 0, 0, 'E'), 100, 50, fields)
+        steps = astar(State(None, None, 0, 0, 'E', priority_array[0][0], heuristicfn(0,0,300,100)), 300, 100, priority_array)
         for interm in steps:
             if interm.action == 'LEFT':
                 agent.turn_left()
diff --git a/state.py b/state.py
index 56e67df..25a0c30 100644
--- a/state.py
+++ b/state.py
@@ -1,8 +1,11 @@
 class State:
-    def __init__(self, parent, action, xpos, ypos, orientation):
+    def __init__(self, parent, action, xpos, ypos, orientation, priority, heuristic):
         self.parent = parent
         self.xpos = xpos
         self.ypos = ypos
         self.orientation = orientation
         self.action = action
-        # self.priority = priority
\ No newline at end of file
+        self.priority = priority
+        self.heuristic = heuristic
+    def __gt__(self, other):
+        return self.priority > other.priority 
\ No newline at end of file
diff --git a/succ.py b/succ.py
index a01b49a..8773dcd 100644
--- a/succ.py
+++ b/succ.py
@@ -1,30 +1,31 @@
 from state import State
+from heuristicfn import heuristicfn
 FIELDWIDTH, FIELDCOUNT = 50, 16
 
-def succ(st: State, passedFields):
+def succ(st: State, passedPriorities, goalx, goaly):
     successors = []
 
     if st.orientation == 'N':
-        successors.append(State(st, 'LEFT', st.xpos, st.ypos, 'W'))
-        successors.append(State(st, 'RIGHT', st.xpos, st.ypos, 'E'))
+        successors.append(State(st, 'LEFT', st.xpos, st.ypos, 'W', passedPriorities[st.xpos//50][st.ypos//50], heuristicfn(st.xpos, st.ypos, goalx, goaly)))
+        successors.append(State(st, 'RIGHT', st.xpos, st.ypos, 'E', passedPriorities[st.xpos//50][st.ypos//50], heuristicfn(st.xpos, st.ypos, goalx, goaly)))
         if st.ypos > 0:
-            successors.append(State(st, 'FORWARD', st.xpos, st.ypos - FIELDWIDTH , 'N'))
+            successors.append(State(st, 'FORWARD', st.xpos, st.ypos - FIELDWIDTH , 'N', passedPriorities[st.xpos//50][st.ypos//50], heuristicfn(st.xpos, st.ypos, goalx, goaly)))
     
     if st.orientation == 'S':
-        successors.append(State(st, 'LEFT', st.xpos, st.ypos, 'E'))
-        successors.append(State(st,'RIGHT', st.xpos, st.ypos, 'W'))
+        successors.append(State(st, 'LEFT', st.xpos, st.ypos, 'E', passedPriorities[st.xpos//50][st.ypos//50], heuristicfn(st.xpos, st.ypos, goalx, goaly)))
+        successors.append(State(st,'RIGHT', st.xpos, st.ypos, 'W', passedPriorities[st.xpos//50][st.ypos//50], heuristicfn(st.xpos, st.ypos, goalx, goaly)))
         if st.ypos < FIELDWIDTH * (FIELDCOUNT  - 1):
-            successors.append(State(st, 'FORWARD', st.xpos, st.ypos + FIELDWIDTH , 'S'))
+            successors.append(State(st, 'FORWARD', st.xpos, st.ypos + FIELDWIDTH , 'S', passedPriorities[st.xpos//50][st.ypos//50], heuristicfn(st.xpos, st.ypos, goalx, goaly)))
 
     if st.orientation == 'W':
-        successors.append(State(st, 'LEFT', st.xpos, st.ypos, 'S'))
-        successors.append(State(st,'RIGHT', st.xpos, st.ypos, 'N'))
+        successors.append(State(st, 'LEFT', st.xpos, st.ypos, 'S', passedPriorities[st.xpos//50][st.ypos//50], heuristicfn(st.xpos, st.ypos, goalx, goaly)))
+        successors.append(State(st,'RIGHT', st.xpos, st.ypos, 'N', passedPriorities[st.xpos//50][st.ypos//50], heuristicfn(st.xpos, st.ypos, goalx, goaly)))
         if st.xpos > 0:
-            successors.append(State(st, 'FORWARD', st.xpos - FIELDWIDTH , st.ypos, 'W'))
+            successors.append(State(st, 'FORWARD', st.xpos - FIELDWIDTH , st.ypos, 'W', passedPriorities[st.xpos//50][st.ypos//50], heuristicfn(st.xpos, st.ypos, goalx, goaly)))
 
     if st.orientation == 'E':
-        successors.append(State(st, 'LEFT', st.xpos, st.ypos, 'N'))
-        successors.append(State(st, 'RIGHT', st.xpos, st.ypos, 'S'))
+        successors.append(State(st, 'LEFT', st.xpos, st.ypos, 'N', passedPriorities[st.xpos//50][st.ypos//50], heuristicfn(st.xpos, st.ypos, goalx, goaly)))
+        successors.append(State(st, 'RIGHT', st.xpos, st.ypos, 'S', passedPriorities[st.xpos//50][st.ypos//50], heuristicfn(st.xpos, st.ypos, goalx, goaly)))
         if st.xpos < FIELDWIDTH * (FIELDCOUNT  - 1):
-            successors.append(State(st, 'FORWARD', st.xpos + FIELDWIDTH , st.ypos, 'E'))
+            successors.append(State(st, 'FORWARD', st.xpos + FIELDWIDTH , st.ypos, 'E', passedPriorities[st.xpos//50][st.ypos//50], heuristicfn(st.xpos, st.ypos, goalx, goaly)))
     return successors