diff --git a/astar.py b/astar.py
index 182ccac..8e73f5a 100644
--- a/astar.py
+++ b/astar.py
@@ -12,6 +12,7 @@ def astar(obiekty, start, cel):
     sasiedzi = [(0, 1), (0, -1), (1, 0), (-1, 0)]
     close_set = set()
     came_from = {}
+    hscore = {start: heurystyka(obiekty, start, cel)}
     gscore = {start: 0}
     fscore = {start: heurystyka(obiekty, start, cel)}
     oheap = []
@@ -20,12 +21,14 @@ def astar(obiekty, start, cel):
     while oheap:
         current = heapq.heappop(oheap)[1]
         if current == cel:
+            obiekty["plansza"][current[0], current[1]].setKolor((255, 0, 0))
             data = []
             while current in came_from:
                 data.append(current)
                 current = came_from[current]
             return data[::-1]
         close_set.add(current)
+        obiekty["plansza"][current[0], current[1]].setKolor((255, 0, 0))
         for i, j in sasiedzi:
             sasiad = current[0] + i, current[1] + j
 
@@ -35,12 +38,15 @@ def astar(obiekty, start, cel):
                 continue
             elif 6 <= sasiad[0] <= 7 and 10 <= sasiad[1] <= 11:
                 continue
-            tentative_g_score = gscore[current] + heurystyka(obiekty, current, sasiad)
-            if sasiad in close_set and tentative_g_score >= gscore.get(sasiad, 0):
+            tentative_h_score = heurystyka(obiekty, sasiad, cel) + heurystyka(obiekty, current, sasiad)
+
+            if sasiad in close_set and tentative_h_score > hscore.get(current, 0):
                 continue
-            if tentative_g_score < gscore.get(sasiad, 0) or sasiad not in [i[1] for i in oheap]:
+            if tentative_h_score <= hscore.get(current, 0) or sasiad not in [i[1] for i in oheap]:
                 came_from[sasiad] = current
-                gscore[sasiad] = tentative_g_score
-                fscore[sasiad] = tentative_g_score + heurystyka(obiekty, sasiad, cel)
+                hscore[sasiad] = tentative_h_score
+                fscore[sasiad] = tentative_h_score + gscore[current]
+                gscore[sasiad] = gscore[current] + heurystyka(obiekty, current, sasiad)
                 heapq.heappush(oheap, (fscore[sasiad], sasiad))
+                obiekty["plansza"][sasiad[0], sasiad[1]].setKolor((0, 255, 0))
     return False