diff --git a/__pycache__/astar.cpython-310.pyc b/__pycache__/astar.cpython-310.pyc
new file mode 100644
index 0000000..42aa76a
Binary files /dev/null and b/__pycache__/astar.cpython-310.pyc differ
diff --git a/__pycache__/bfs.cpython-310.pyc b/__pycache__/bfs.cpython-310.pyc
index f9f8ace..6cd41fc 100644
Binary files a/__pycache__/bfs.cpython-310.pyc and b/__pycache__/bfs.cpython-310.pyc differ
diff --git a/__pycache__/heuristicfn.cpython-310.pyc b/__pycache__/heuristicfn.cpython-310.pyc
new file mode 100644
index 0000000..a8d8775
Binary files /dev/null and b/__pycache__/heuristicfn.cpython-310.pyc differ
diff --git a/__pycache__/state.cpython-310.pyc b/__pycache__/state.cpython-310.pyc
index b39f434..927e172 100644
Binary files a/__pycache__/state.cpython-310.pyc and b/__pycache__/state.cpython-310.pyc differ
diff --git a/__pycache__/succ.cpython-310.pyc b/__pycache__/succ.cpython-310.pyc
index 6927dcf..d3465fc 100644
Binary files a/__pycache__/succ.cpython-310.pyc and b/__pycache__/succ.cpython-310.pyc differ
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