diff --git a/Astar.py b/Astar.py
index c339af2..1226c39 100644
--- a/Astar.py
+++ b/Astar.py
@@ -1,63 +1,21 @@
 import math
+from operator import ne
 
 import pygame
 from Global_variables import Global_variables as G_var
+from Min_heap import Min_heap
+from Node import Node, State
 from Package import Package
 from Shelf import Shelf
 
 
-class State:
-
-    def __init__(self, direction, x, y):
-        self.direction = direction  # kierunek w ktorym "patrzy wozek"
-        self.x = x
-        self.y = y
-
-    def get_direction(self):
-        return self.direction
-
-    def get_x(self):
-        return self.x
-
-    def get_y(self):
-        return self.y
-
-    def goal_test(self, goal):    # sprawdza czy osiagnelismy cel
-        if self.x == goal[0] and self.y == goal[1]:
-            return True
-        else:
-            return False
-
-
-class Node:
-    def __init__(self, state, walkable):
-        self.state = state
-        self.direction = state.direction
-        self.walkable = walkable
-        self.g_cost = 0
-        self.h_cost = 0
-        self.parent = None
-
-    def get_action(self):
-        return self.action
-
-    def get_direction(self):
-        return self.direction
-
-    def get_parent(self):
-        return self.parent
-
-    def f_cost(self):
-        if self.walkable:
-            return self.g_cost + self.h_cost
-        else:
-            # return 0
-            return math.inf
-
-
 class Pathfinding:
     def __init__(self, enviroment_2d):
-        # self.grid = []
+        self.enviroment_2d = enviroment_2d
+        self.reset_grid()
+        self.path = []
+
+    def reset_grid(self):
         self.grid = [[      # tworze pustej tablicy o wymiarach naszej kraty
             None 
             for y in range(G_var().DIMENSION_Y)]
@@ -66,12 +24,13 @@ class Pathfinding:
         for x in range(G_var().DIMENSION_X):       # zapełnianie tablicy obiektami Node
             for y in range(G_var().DIMENSION_Y):
                 is_walkable = True
-                if isinstance(enviroment_2d[x][y], Shelf):
+                to_check_type = self.enviroment_2d[x][y]
+                if isinstance(to_check_type, Shelf):
+                    is_walkable = False
+                elif isinstance(to_check_type, Package) and to_check_type.is_placed:
                     is_walkable = False
                 self.grid[x][y] = Node(State(1, x, y), is_walkable)
         
-        self.path = []
-
     def succ(self,node):    #funckja zwraca sąsiadów noda w argumencie
         node_x = node.state.x
         node_y = node.state.y
@@ -84,53 +43,98 @@ class Pathfinding:
                 neighbours.append(self.grid[neighbour_x][neighbour_y])
         return neighbours
 
-    def find_path(self, starting_state, target_state):   # algorytm wyszukiwania trasy
+################## TO REMOVE 
+    def set_text(self, string, coordx, coordy, fontSize): #Function to set text
+
+        font = pygame.font.Font('freesansbold.ttf', fontSize) 
+        #(0, 0, 0) is black, to make black text
+        string = str(string)
+        text = font.render(string, True, (0, 0, 0)) 
+        textRect = text.get_rect()
+        textRect.center = (coordx, coordy) 
+        return (text, textRect)
+
+    def draw_node(self,node, window, color):
+        node_x = node.state.x
+        node_y = node.state.y
+        #######################SET TEXT
+        f_cost_text = self.set_text(node.f_cost(), node_x * G_var().RECT_SIZE + (G_var().RECT_SIZE/2), node_y *
+            G_var().RECT_SIZE + (G_var().RECT_SIZE/2), 10)
+        g_cost_text = self.set_text(node.g_cost, node_x * G_var().RECT_SIZE + (G_var().RECT_SIZE/4), node_y *
+            G_var().RECT_SIZE + (G_var().RECT_SIZE/4), 10)
+        h_cost_text = self.set_text(node.h_cost, node_x * G_var().RECT_SIZE + (G_var().RECT_SIZE/4*3), node_y *
+            G_var().RECT_SIZE + (G_var().RECT_SIZE/4), 10)
+
+        ###############################
+        node_x = node.state.x
+        node_y = node.state.y
+        block = pygame.Rect(
+            node_x * G_var().RECT_SIZE, node_y *
+            G_var().RECT_SIZE, G_var().RECT_SIZE, G_var().RECT_SIZE
+        )
+        pygame.draw.rect(window,
+                         color,
+                         block)
+        window.blit(f_cost_text[0], f_cost_text[1])
+        window.blit(g_cost_text[0], g_cost_text[1])
+        window.blit(h_cost_text[0], h_cost_text[1])
+        
+###############################
+
+    def find_path(self, starting_state, target_state, window):   # algorytm wyszukiwania trasy
         start_node = self.grid[starting_state.x][starting_state.y]
         target_node = self.grid[target_state.x][target_state.y]
 
-        fringe = []
+        fringe = Min_heap()
         explored = []
         
         is_target_node_walkable = True
         if not target_node.walkable:
             target_node.walkable = True
             is_target_node_walkable = False
-        fringe.append(start_node)
+        fringe.insert(start_node)
 
-        while len(fringe) > 0:
-            current_node = fringe[0]
-            for i in range(1, len(fringe)):
-                if fringe[i].f_cost() < current_node.f_cost() or (fringe[i].f_cost() == current_node.f_cost() and fringe[i].h_cost < current_node.h_cost):
-                    current_node = fringe[i]
-            
-            fringe.remove(current_node)
+        while fringe.count() > 0:
+            # current_node = fringe[0]
+            current_node = fringe.extract()
+            #################################################### TEST
+            # current_node_color = (213, 55, 221)
+            # to_check_color = (55, 213, 55)
+            # current_color = (233,55,55)
+            # # self.draw_node(current_node,window,current_node_color)
+            # for node_to_check in explored:
+            #     self.draw_node(node_to_check,window, current_node_color)
+            # for node_to_check in fringe.items:
+            #     self.draw_node(node_to_check,window, to_check_color)
+            # self.draw_node(current_node,window,current_color)
+            # pygame.display.flip()
+            ###############################################################
             explored.append(current_node)
 
             if current_node.state == target_node.state:
                 path = self.retrace_path(start_node,target_node)
                 self.path = path
+                target_node.walkable = is_target_node_walkable
+                return
             
             for neighbour in self.succ(current_node):
-                if not neighbour.walkable or neighbour in explored:
-                # if neighbour in explored:
+                neighbour_in_explored = [e for e in explored if e.state == neighbour.state]
+                if not neighbour.walkable or len(neighbour_in_explored) > 0:
                     continue
                 new_movement_cost_to_neighbour = current_node.g_cost + self.get_distance(current_node,neighbour)
-                if new_movement_cost_to_neighbour < neighbour.g_cost or not neighbour in fringe:
+                if new_movement_cost_to_neighbour < neighbour.g_cost or not fringe.contains(neighbour):
                     neighbour.g_cost = new_movement_cost_to_neighbour
                     neighbour.h_cost = self.get_distance(neighbour,target_node)
                     neighbour.parent = current_node
-                    if not neighbour in fringe:
-                        fringe.append(neighbour)
-
+                    if not fringe.contains(neighbour):
+                        fringe.insert(neighbour)
         target_node.walkable = is_target_node_walkable
     
     def get_distance(self, node_a, node_b): # funckja liczy dystans dla odległości między dwoma nodami
         dist_x = abs(node_a.state.x - node_b.state.x)
         dist_y = abs(node_a.state.y - node_b.state.y)
 
-        if dist_x > dist_y:
-            return 10 * (dist_x - dist_y)
-        return 10 * (dist_y - dist_x)
+        return (dist_x + dist_y) * 10
     
     def retrace_path(self, start_node, end_node): # funkcja zwraca tablice która ma w sobie wartosci pola parent
         # od end_node do start_node
diff --git a/Environment.py b/Environment.py
index 4898e0f..fd2b459 100644
--- a/Environment.py
+++ b/Environment.py
@@ -28,9 +28,9 @@ class Environment:
         new_truck = Truck(window, 14, 7)
         self.enviroment_2d[14][7] = new_truck
         self.truck = new_truck
-        self.moving_truck = Moving_truck(
-            self.window, self.enviroment_2d, self.truck, self.package_spawner)
         self.astar = Pathfinding(self.enviroment_2d)
+        self.moving_truck = Moving_truck(
+            self.window, self.enviroment_2d, self.truck, self.package_spawner, self.astar)
         self.finding_fields = Finding_fields(self.enviroment_2d)
         self.weekend = random.randint(0, 1)
 
@@ -47,18 +47,6 @@ class Environment:
         self.update_truck()
         # time.sleep(0.5)
 
-    # def use_decision_tree(self):
-    #     marking = self.package.type
-    #     if marking == Package_types.fragile:
-    #         marking = 0
-    #     elif marking == Package_types.priority:
-    #         marking = 1
-    #     tree = DecisionTree(marking, self.weekend, self.package.company.popularity,
-    #                         self.package.company.payment_delay, self.package.payed_upfront,
-    #                         self.package.company.shipping_type)
-    #     decision = tree.decision
-    #     return decision
-
     def use_astar(self):
         start_state = State(1,self.truck.x,self.truck.y) # sprawić aby paczka i shelf były wyszukiwane raz
         if self.truck.has_package:
@@ -67,7 +55,7 @@ class Environment:
         else:
             end_position = self.finding_fields.find_package()
         end_state = State(1,end_position.x, end_position.y)
-        self.astar.find_path(start_state,end_state)
+        self.astar.find_path(start_state,end_state,self.window)
     
     def update_truck(self):
         next_field_to_move = self.astar.path[0].state
diff --git a/Global_variables.py b/Global_variables.py
index 9129f8e..a27df53 100644
--- a/Global_variables.py
+++ b/Global_variables.py
@@ -10,11 +10,6 @@ class Global_variables(object):
     RECT_COLOR = (70, 77, 87)
     SHELF_COLOR = (143, 68, 33)
 
-    def __init__(self) -> None:
-        dim_x = 28
-        dim_y = 15
-        self.GRID = [["empty" for i in range(dim_x)] for j in range(dim_y)]
-
     def __new__(cls):
         if cls._instance is None:
             cls._instance = super(Global_variables, cls).__new__(cls)
diff --git a/Min_heap.py b/Min_heap.py
new file mode 100644
index 0000000..b69bbbf
--- /dev/null
+++ b/Min_heap.py
@@ -0,0 +1,75 @@
+from cgitb import small
+from heapq import heapify
+import math
+from multiprocessing.dummy import Array
+
+from Node import Node, State
+
+
+
+class Min_heap:
+    def __init__(self):
+        self.items = []
+
+    def parent(self,i):
+        return (i - 1) >> 1
+
+    def left(self,i):
+        return (i << 1) + 1
+
+    def right(self,i):
+        return (i << 1) + 2
+
+    def heapify(self, i):
+        l = self.left(i)
+        r = self.right(i)
+        if l < len(self.items) and self.items[l] < self.items[i]:
+            smallest = l
+        else:
+            smallest = i
+        if r < len(self.items) and self.items[r] < self.items[smallest]:
+            smallest = r
+        if smallest != i:
+            self.items[i], self.items[smallest] = self.items[smallest], self.items[i]
+            self.items[i].heap_index, self.items[smallest].heap_index = self.items[smallest].heap_index, self.items[i].heap_index
+            self.heapify(smallest)
+
+    def extract(self):
+        if len(self.items) < 1:
+            print("STOS PUSTY!")
+            return
+        min = self.items[0]
+        self.items[0] = self.items[len(self.items) - 1]
+        self.items.pop()
+        self.heapify(0)
+        return min
+
+    def decrese_key(self, index, item):
+        if item > self.items[index]:
+            print("Nowy klucz wiekszy od klucza aktualnego!")
+            return
+        self.items[index] = item
+        self.items[index].heap_index = index
+        while index > 0 and self.items[self.parent(index)] > self.items[index]:
+            self.items[index], self.items[self.parent(
+                index)] = self.items[self.parent(index)], self.items[index]
+            self.items[index].heap_index, self.items[self.parent(
+                index)].heap_index = self.items[self.parent(index)].heap_index, self.items[index].heap_index
+            index = self.parent(index)
+
+    def insert(self, item):
+        temp_node = Node(State(0,0,0),False)
+        temp_node.h_cost = math.inf
+        temp_node.heap_index = len(self.items) - 1
+        self.items.append(temp_node)
+        self.decrese_key(len(self.items) - 1, item)
+
+    def count(self):
+        return len(self.items)
+
+    def contains(self, item):
+        in_range = len(self.items) > item.heap_index
+        contains = False
+        if in_range:
+            contains = self.items[item.heap_index] is item
+        return in_range and contains
\ No newline at end of file
diff --git a/Moving_truck.py b/Moving_truck.py
index ea01939..fc4fe74 100644
--- a/Moving_truck.py
+++ b/Moving_truck.py
@@ -2,13 +2,14 @@ from Empty import Empty
 from Package import Package
 from Shelf import Shelf
 
-
+# TODO: DODAC OBSERWER ZAMIAST PRZEKAZYWANIE WSZYSTKICH BZDET
 class Moving_truck:
-    def __init__(self, window, enviroment_2d, truck, package_spawner):
+    def __init__(self, window, enviroment_2d, truck, package_spawner, a_star):
         self.enviroment_2d = enviroment_2d
         self.truck = truck
         self.window = window
         self.package_spawner = package_spawner
+        self.astar = a_star
 
     def move(self, x, y):
         truck_x = self.truck.x
@@ -30,6 +31,7 @@ class Moving_truck:
         self.truck.has_package = True
         self.truck.package_type = package.type
         self.truck.sector = package.sector
+        self.astar.reset_grid()
         self.move_without_swapping(truck_x, truck_y, package_x, package_y)
 
 
@@ -42,6 +44,7 @@ class Moving_truck:
                                       y].is_placed = True
         self.truck.has_package = False
         self.package_spawner.spawn_package()
+        self.astar.reset_grid()
 
     def swap_fields(self, x1, y1, x2, y2):
         self.enviroment_2d[x1][y1], self.enviroment_2d[x2][y2] = self.enviroment_2d[x2][y2], self.enviroment_2d[x1][y1]
diff --git a/Node.py b/Node.py
new file mode 100644
index 0000000..f3e1cdf
--- /dev/null
+++ b/Node.py
@@ -0,0 +1,64 @@
+
+import math
+
+
+class State:
+
+    def __init__(self, direction, x, y):
+        self.direction = direction  # kierunek w ktorym "patrzy wozek"
+        self.x = x
+        self.y = y
+
+    def get_direction(self):
+        return self.direction
+
+    def get_x(self):
+        return self.x
+
+    def get_y(self):
+        return self.y
+
+    def goal_test(self, goal):    # sprawdza czy osiagnelismy cel
+        if self.x == goal[0] and self.y == goal[1]:
+            return True
+        else:
+            return False
+
+
+class Node:
+    def __init__(self, state, walkable):
+        self.state = state
+        self.direction = state.direction
+        self.walkable = walkable
+        self.g_cost = 0
+        self.h_cost = 0
+        self.parent = None
+        self.heap_index = 0
+
+    def get_action(self):
+        return self.action
+
+    def get_direction(self):
+        return self.direction
+
+    def get_parent(self):
+        return self.parent
+
+    def f_cost(self):
+        if self.walkable:
+            return self.g_cost + self.h_cost
+        else:
+            # return 0
+            return math.inf
+    # if fringe[i].f_cost() < current_node.f_cost() or (fringe[i].f_cost() == current_node.f_cost() and fringe[i].h_cost < current_node.h_cost):
+    def __lt__(self, other):
+        if self.f_cost() == other.f_cost():
+            return self.h_cost < other.h_cost
+        return self.f_cost() < other.f_cost()
+    def __gt__(self,other):
+        if self.f_cost() == other.f_cost():
+            return self.h_cost > other.h_cost
+        return self.f_cost() > other.f_cost()
+    def __eq__(self,other):
+        return self.f_cost() == other.f_cost() and self.h_cost == other.h_cost
+
diff --git a/Program.py b/Program.py
index 9ee68bc..2ef8674 100644
--- a/Program.py
+++ b/Program.py
@@ -17,5 +17,5 @@ class Program:
             for event in pygame.event.get():                # integrating with keyboard
                 if event.type == QUIT:
                     running = False
-                self.environment.update_all_elements()
-                self.environment.draw_all_elements()
+            self.environment.update_all_elements()
+            self.environment.draw_all_elements()