diff --git a/heuristicfn.py b/heuristicfn.py
index 04d8486..2b90daa 100644
--- a/heuristicfn.py
+++ b/heuristicfn.py
@@ -1,2 +1,3 @@
 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
+    return abs(startx - goalx) + abs(starty - 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 12c9020..2c9e7a5 100644
--- a/main.py
+++ b/main.py
@@ -24,6 +24,7 @@ FPS = 10
 FIELDCOUNT = 16
 FIELDWIDTH = 50
 
+
 class Agent:
     def __init__(self, rect, direction):
         self.rect = rect
@@ -42,10 +43,10 @@ def randomize_map():        # tworzenie mapy z losowymi polami
                 temp_priority.append(1)
             else:
                 prob = random.uniform(0, 100)
-                if 0 <= prob <= 10:
+                if 0 <= prob <= 12:
                     field_array_2.append(COBBLE)
                     temp_priority.append(3)
-                elif 10 < prob <= 20:
+                elif 12 < prob <= 24:
                     field_array_2.append(SAND)
                     temp_priority.append(2)
                 else:
@@ -57,37 +58,49 @@ def randomize_map():        # tworzenie mapy z losowymi polami
     return field_array_1, field_priority
 
 
-def draw_window(agent, fields):
+def draw_window(agent, fields, flip):
+    if flip:
+        direction = pygame.transform.flip(AGENT, True, False)
+    else:
+        direction = pygame.transform.flip(AGENT, False, False)
     for i in range(16):
         for j in range(16):
             window.blit(fields[i][j], (i * 50, j * 50))
-    window.blit(AGENT, (agent.rect.x, agent.rect.y))      # wyswietlanie agenta
+    window.blit(direction, (agent.rect.x, agent.rect.y))  # wyswietlanie agenta
     pygame.display.update()
 
 
 def main():
     clock = pygame.time.Clock()
     run = True
-    agent = GarbageTruck(0, 0, pygame.Rect(0, 0, 50, 50), 0)   # tworzenie pola dla agenta
+    x, y = [0, 0]
+    agent = GarbageTruck(0, 0, pygame.Rect(x, y, 50, 50), 0)   # tworzenie pola dla agenta
     fields, priority_array = randomize_map()
     print(priority_array)
-    print(priority_array[4][8])
+    final_x, final_y = [100, 300]
     while run:
         clock.tick(FPS)
         for event in pygame.event.get():
             if event.type == pygame.QUIT:
                 run = False
         # keys_pressed = pygame.key.get_pressed()
-        draw_window(agent, fields)
-        steps = astar(State(None, None, 0, 0, 'E', priority_array[0][0], heuristicfn(0,0,300,100)), 300, 100, priority_array)
+        draw_window(agent, fields, False)   # false = kierunek east (domyslny), true = west
+        steps = astar(State(None, None, x, y, 'E', priority_array[0][0], heuristicfn(x, y, final_x, final_y)), final_x, final_y, priority_array)
         for interm in steps:
             if interm.action == 'LEFT':
                 agent.turn_left()
+                draw_window(agent, fields, True)
             elif interm.action == 'RIGHT':
                 agent.turn_right()
+                draw_window(agent, fields, False)
             elif interm.action == 'FORWARD':
                 agent.forward()
-            draw_window(agent, fields)
+                if agent.orientation == 0:
+                    draw_window(agent, fields, False)
+                elif agent.orientation == 2:
+                    draw_window(agent, fields, True)
+                else:
+                    draw_window(agent, fields, False)
             time.sleep(0.5)
     
         while True: