diff --git a/CMakeLists.txt b/CMakeLists.txt
index 784785a..f13922e 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -14,6 +14,8 @@ add_executable(PixelDefense
         game/systems/systems.h
 
         game/utils/building_types.h
+        game/utils/pathfinding.c
+        game/utils/pathfinding.h
 
         game/buildings.c
         game/buildings.h
diff --git a/assets/maps/test.tmj b/assets/maps/test.tmj
index 0363fcc..8adee82 100644
--- a/assets/maps/test.tmj
+++ b/assets/maps/test.tmj
@@ -13,22 +13,22 @@
             28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29,
             55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56,
             1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2,
-            28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29,
-            55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56,
-            1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2,
-            28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29,
-            55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 191, 273, 273, 273, 273, 273, 273, 193, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56,
-            1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 191, 273, 247, 2, 3, 1, 2, 3, 1, 245, 273, 273, 193, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2,
-            28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 191, 247, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 245, 273, 273, 273, 193, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29,
-            55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 191, 247, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 245, 273, 273, 193, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56,
-            1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 191, 247, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 245, 273, 273, 193, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2,
-            28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 191, 247, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 245, 193, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29,
-            55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 191, 247, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 245, 273, 193, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56,
-            1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 217, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 245, 193, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2,
-            28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 191, 247, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 245, 193, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29,
-            55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 217, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 245, 193, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56,
-            1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 191, 247, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 245, 193, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2,
-            28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 217, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 245, 193, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29,
+            28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 191, 273, 192, 273, 273, 192, 273, 273, 193, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29,
+            55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 217, 56, 217, 55, 56, 217, 55, 56, 245, 273, 193, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56,
+            1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 217, 2, 217, 1, 2, 217, 1, 2, 3, 1, 245, 193, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2,
+            28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 217, 29, 217, 28, 29, 217, 28, 29, 30, 28, 29, 217, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29,
+            55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 191, 273, 273, 273, 273, 273, 273, 193, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 217, 56, 217, 55, 56, 217, 55, 190, 57, 55, 56, 217, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56,
+            1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 191, 273, 247, 2, 3, 1, 2, 3, 1, 245, 273, 273, 193, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 191, 247, 2, 244, 1, 2, 217, 1, 217, 3, 1, 2, 217, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2,
+            28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 191, 247, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 245, 273, 273, 273, 193, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 217, 28, 29, 30, 28, 29, 217, 28, 217, 30, 28, 29, 217, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29,
+            55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 191, 247, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 245, 273, 273, 193, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 217, 55, 56, 191, 193, 56, 217, 55, 217, 57, 55, 56, 217, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56,
+            1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 191, 247, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 245, 273, 273, 193, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 217, 1, 272, 247, 217, 2, 217, 1, 217, 3, 1, 2, 217, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2,
+            28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 191, 247, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 245, 193, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 217, 28, 29, 30, 217, 29, 217, 28, 217, 30, 28, 29, 217, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29,
+            55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 191, 247, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 245, 273, 193, 55, 56, 57, 55, 56, 57, 55, 56, 217, 55, 56, 57, 217, 56, 217, 55, 217, 57, 55, 56, 217, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56,
+            1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 217, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 245, 193, 2, 3, 1, 2, 3, 1, 2, 218, 273, 274, 3, 217, 2, 217, 1, 217, 3, 1, 2, 217, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2,
+            28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 191, 247, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 245, 193, 30, 28, 29, 30, 28, 29, 217, 28, 29, 30, 217, 29, 244, 28, 217, 30, 28, 29, 217, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29,
+            55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 217, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 245, 193, 55, 56, 57, 55, 56, 217, 55, 56, 57, 217, 56, 57, 55, 217, 57, 55, 56, 217, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56,
+            1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 191, 247, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 245, 193, 2, 3, 1, 2, 217, 1, 2, 3, 217, 2, 3, 1, 217, 3, 1, 2, 217, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2,
+            28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 217, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 245, 193, 30, 28, 29, 245, 273, 273, 273, 246, 273, 273, 273, 246, 273, 273, 273, 247, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29,
             55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 217, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 245, 193, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56, 57, 55, 56,
             1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 191, 247, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 217, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2,
             28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 217, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 245, 193, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29, 30, 28, 29,
@@ -384,8 +384,8 @@
                  "type":"",
                  "visible":true,
                  "width":10,
-                 "x":1155.52083333333,
-                 "y":473.416666666667
+                 "x":1155.52,
+                 "y":473.417
                 }],
          "opacity":1,
          "type":"objectgroup",
diff --git a/engine/CMakeLists.txt b/engine/CMakeLists.txt
index 8b32253..ed6be43 100644
--- a/engine/CMakeLists.txt
+++ b/engine/CMakeLists.txt
@@ -32,6 +32,7 @@ set(BreezeSources
         breeze/core/memory.c
         breeze/core/module_system.c
 
+        breeze/utils/heap.c
         breeze/map/map.c
         breeze/map/tileset.c
 
@@ -47,7 +48,6 @@ set(BreezeHeaders
         breeze/map/map.h
         breeze/map/tileset.h
 
-        breeze/utils/heap.c
         breeze/utils/heap.h
         breeze/utils/string.h
         breeze/utils/tokenizer.h
diff --git a/engine/breeze/utils/heap.c b/engine/breeze/utils/heap.c
index 4e77230..5ae0bd3 100644
--- a/engine/breeze/utils/heap.c
+++ b/engine/breeze/utils/heap.c
@@ -36,6 +36,10 @@ void *_bzHeapNew(i32 startCapacity, i32 stride, i32 weightOffset) {
 void _bzHeapFree(void *heap) {
     bzFree(HEAP_HEAD(heap));
 }
+void _bzHeapReset(void *heap) {
+    BzHeap *head = HEAP_HEAD(heap);
+    head->size = 0;
+}
 
 i32 _bzHeapSize(void *heap) {
     return HEAP_HEAD(heap)->size;
diff --git a/engine/breeze/utils/heap.h b/engine/breeze/utils/heap.h
index c8f752e..7ce13a9 100644
--- a/engine/breeze/utils/heap.h
+++ b/engine/breeze/utils/heap.h
@@ -5,6 +5,7 @@
 
 void *_bzHeapNew(i32 startCapacity, i32 stride, i32 weightOffset);
 void _bzHeapFree(void *heap);
+void _bzHeapReset(void *heap);
 
 i32 _bzHeapSize(void *heap);
 bool _bzHeapIsEmpty(void *heap);
@@ -14,14 +15,15 @@ i32 _bzHeapPushTmpIdx(void *heap);
 
 #define bzHeapNew(T, n) (T *) ((T *)_bzHeapNew((n), sizeof(T), offsetof(T, weight)))
 #define bzHeapFree(heap) _bzHeapFree((void *) (heap))
+#define bzHeapReset(heap) _bzHeapReset((void *) (heap))
 
 #define bzHeapSize(heap) _bzHeapSize((void *) (heap))
 #define bzHeapIsEmpty(heap) _bzHeapIsEmpty((void *) (heap))
 #define bzHeapPop(heap) ((heap)[_bzHeapPop((void *) (heap))])
-#define bzHeapPush(heap, item) do {             \
-    void *h = (void *) (heap);                  \
-    (heap)[_bzHeapPushTmpIdx(h)] = (item);      \
-    _bzHeapPush(h);                             \
+#define bzHeapPush(heap, ...) do {                  \
+    void *h = (void *) (heap);                      \
+    (heap)[_bzHeapPushTmpIdx(h)] = (__VA_ARGS__);   \
+    _bzHeapPush(h);                                 \
 } while(0)
 
 
diff --git a/game/main.c b/game/main.c
index f087411..c3fab9b 100644
--- a/game/main.c
+++ b/game/main.c
@@ -8,6 +8,7 @@
 #include "map_layers.h"
 #include "buildings.h"
 
+#include "utils/pathfinding.h"
 
 Game *GAME = NULL;
 
@@ -152,6 +153,22 @@ void render(float dt, Game *game) {
     bzTileMapDraw(&game->map);
     bzTileMapDrawColliders(&game->map);
 
+
+    Vector2 worldPos = GetScreenToWorld2D(GetMousePosition(), game->camera);
+    int tileX = (int) worldPos.x / 16;
+    int tileY = (int) worldPos.y / 16;
+
+
+    static PathNode *heap = NULL;
+    if (!heap)
+        heap = bzHeapNew(PathNode, game->map.width * game->map.height);
+    findPath(&(PathfindingDesc) {
+        .start=(TilePosition){57, 24},
+        .target=(TilePosition){tileX, tileY},
+        .map=&game->map,
+        .heap=heap,
+    });
+
     ecs_progress(ECS, dt);
 
     EndMode2D();
diff --git a/game/utils/pathfinding.c b/game/utils/pathfinding.c
new file mode 100644
index 0000000..fe0d434
--- /dev/null
+++ b/game/utils/pathfinding.c
@@ -0,0 +1,105 @@
+#include "pathfinding.h"
+
+static i32 dst(TilePosition a, TilePosition b) {
+    //i32 dX = a.x - b.x;
+    //i32 dY = a.y - b.y;
+    //return dX * dX + dY * dY;
+    int dstX = a.x - b.x;
+    int dstY = a.y - b.y;
+    if (dstX < 0) dstX = -dstX;
+    if (dstY < 0) dstY = -dstY;
+
+    if (dstX > dstY)
+        return 14 * dstY + 10 * (dstX - dstY);
+    return 14 * dstX + 10 * (dstY - dstX);
+}
+
+bool findPath(const PathfindingDesc *desc) {
+    BZ_ASSERT(desc->map);
+    BzTileMap *map = desc->map;
+    typedef struct Visited {
+        bool visited : 1;
+        i8 x : 3;
+        i8 y : 3;
+    } Visited;
+    Visited visited[map->width * map->height] = {};
+    bzMemSet(visited, 0, sizeof(visited));
+
+    PathNode *heap = desc->heap;
+    if (!heap) heap = bzHeapNew(PathNode, map->width * map->height);
+    else bzHeapReset(heap);
+
+    i32 toTargetCost = dst(desc->start, desc->target);
+    bzHeapPush(heap, (PathNode) {toTargetCost, 0, toTargetCost, desc->start});
+
+    bool foundPath = false;
+
+    while (!bzHeapIsEmpty(heap)) {
+        PathNode node = bzHeapPop(heap);
+        if (node.pos.x == desc->target.x &&
+            node.pos.y == desc->target.y) {
+            // Found path
+            foundPath = true;
+            break;
+        }
+
+        visited[node.pos.y * map->width + node.pos.x].visited = true;
+
+        for (int y = node.pos.y - 1; y <= node.pos.y + 1; y++) {
+            for (int x = node.pos.x - 1; x <= node.pos.x + 1; x++) {
+                if (x == node.pos.x && y == node.pos.y)
+                    continue;
+                if (y < 0 || y >= map->height ||
+                    x < 0 || x >= map->width)
+                    continue;
+                if (bzTileMapGetCollider(map, x, y).shapes[0].type != BZ_TILE_SHAPE_NONE)
+                    continue;
+                Visited *curVisited = &visited[y * map->width + x];
+                if (curVisited->visited)
+                    continue;
+                curVisited->visited = true;
+
+                TilePosition curPos = {x, y};
+                i32 gCost = node.gCost + dst(node.pos, curPos);
+                //if (gCost >= node.gCost) continue;
+                toTargetCost = dst(curPos, desc->target);
+
+                curVisited->x = (i8) (curPos.x - node.pos.x);
+                curVisited->y = (i8) (curPos.y - node.pos.y);
+
+                bzHeapPush(heap, (PathNode) {
+                    .weight = gCost + toTargetCost,
+                    .gCost = gCost,
+                    .hCost = toTargetCost,
+                    .pos = (TilePosition) {x, y}
+                });
+
+            }
+        }
+    }
+
+    DrawRectangle(desc->start.x * 16, desc->start.y * 16, 16, 16, BLUE);
+    Color color = RED;
+    if (foundPath) {
+        color = GREEN;
+        TilePosition pos = desc->target;
+        int count = 0;
+        while (pos.x != desc->start.x || pos.y != desc->start.y) {
+            Visited visit = visited[pos.y * map->width + pos.x];
+            BZ_ASSERT(visit.x != 0 && visit.y != 0);
+            pos.x -= visit.x;
+            pos.y -= visit.y;
+            DrawRectangle(pos.x * 16, pos.y * 16, 16, 16, GREEN);
+            count++;
+        }
+        bzLogInfo("Path length: %d", count);
+    }
+    DrawRectangle(desc->target.x * 16, desc->target.y * 16, 16, 16, color);
+
+    if (!desc->heap) {
+        bzHeapFree(heap);
+        heap = NULL;
+    }
+
+    return foundPath;
+}
\ No newline at end of file
diff --git a/game/utils/pathfinding.h b/game/utils/pathfinding.h
new file mode 100644
index 0000000..7c0813f
--- /dev/null
+++ b/game/utils/pathfinding.h
@@ -0,0 +1,29 @@
+#ifndef PIXELDEFENSE_PATHFINDING_H
+#define PIXELDEFENSE_PATHFINDING_H
+
+#include <breeze.h>
+
+#include "../components.h"
+
+typedef struct PathMove {
+    i8 x;
+    i8 y;
+} PathMove;
+
+typedef struct PathNode {
+    i32 weight; // g + h
+    i32 gCost; // from start cost
+    i32 hCost; // to target cost
+    TilePosition pos;
+} PathNode;
+
+typedef struct PathfindingDesc {
+    TilePosition start;
+    TilePosition target;
+    BzTileMap *map;
+    PathNode *heap;
+} PathfindingDesc;
+
+bool findPath(const PathfindingDesc *desc);
+
+#endif //PIXELDEFENSE_PATHFINDING_H
diff --git a/tiled/test.tmx b/tiled/test.tmx
index 35303c8..8bde69b 100644
--- a/tiled/test.tmx
+++ b/tiled/test.tmx
@@ -18,22 +18,22 @@
 28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,
 55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,
 1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,
-28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,
-55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,
-1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,
-28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,
-55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,191,273,273,273,273,273,273,193,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,
-1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,191,273,247,2,3,1,2,3,1,245,273,273,193,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,
-28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,191,247,30,28,29,30,28,29,30,28,29,30,28,245,273,273,273,193,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,
-55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,191,247,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,245,273,273,193,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,
-1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,191,247,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,245,273,273,193,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,
-28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,191,247,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,245,193,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,
-55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,191,247,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,245,273,193,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,
-1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,217,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,245,193,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,
-28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,191,247,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,245,193,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,
-55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,217,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,245,193,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,
-1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,191,247,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,245,193,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,
-28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,217,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,245,193,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,
+28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,191,273,192,273,273,192,273,273,193,28,29,30,28,29,30,28,29,30,28,29,30,28,29,
+55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,217,56,217,55,56,217,55,56,245,273,193,57,55,56,57,55,56,57,55,56,57,55,56,
+1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,217,2,217,1,2,217,1,2,3,1,245,193,1,2,3,1,2,3,1,2,3,1,2,
+28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,217,29,217,28,29,217,28,29,30,28,29,217,28,29,30,28,29,30,28,29,30,28,29,
+55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,191,273,273,273,273,273,273,193,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,217,56,217,55,56,217,55,190,57,55,56,217,55,56,57,55,56,57,55,56,57,55,56,
+1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,191,273,247,2,3,1,2,3,1,245,273,273,193,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,191,247,2,244,1,2,217,1,217,3,1,2,217,1,2,3,1,2,3,1,2,3,1,2,
+28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,191,247,30,28,29,30,28,29,30,28,29,30,28,245,273,273,273,193,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,217,28,29,30,28,29,217,28,217,30,28,29,217,28,29,30,28,29,30,28,29,30,28,29,
+55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,191,247,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,245,273,273,193,55,56,57,55,56,57,55,56,57,55,56,57,55,56,217,55,56,191,193,56,217,55,217,57,55,56,217,55,56,57,55,56,57,55,56,57,55,56,
+1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,191,247,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,245,273,273,193,1,2,3,1,2,3,1,2,3,1,2,217,1,272,247,217,2,217,1,217,3,1,2,217,1,2,3,1,2,3,1,2,3,1,2,
+28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,191,247,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,245,193,29,30,28,29,30,28,29,30,28,29,217,28,29,30,217,29,217,28,217,30,28,29,217,28,29,30,28,29,30,28,29,30,28,29,
+55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,191,247,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,245,273,193,55,56,57,55,56,57,55,56,217,55,56,57,217,56,217,55,217,57,55,56,217,55,56,57,55,56,57,55,56,57,55,56,
+1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,217,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,245,193,2,3,1,2,3,1,2,218,273,274,3,217,2,217,1,217,3,1,2,217,1,2,3,1,2,3,1,2,3,1,2,
+28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,191,247,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,245,193,30,28,29,30,28,29,217,28,29,30,217,29,244,28,217,30,28,29,217,28,29,30,28,29,30,28,29,30,28,29,
+55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,217,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,245,193,55,56,57,55,56,217,55,56,57,217,56,57,55,217,57,55,56,217,55,56,57,55,56,57,55,56,57,55,56,
+1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,191,247,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,245,193,2,3,1,2,217,1,2,3,217,2,3,1,217,3,1,2,217,1,2,3,1,2,3,1,2,3,1,2,
+28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,217,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,245,193,30,28,29,245,273,273,273,246,273,273,273,246,273,273,273,247,28,29,30,28,29,30,28,29,30,28,29,
 55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,217,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,245,193,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,57,55,56,
 1,2,3,1,2,3,1,2,3,1,2,3,1,2,191,247,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,217,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,3,1,2,
 28,29,30,28,29,30,28,29,30,28,29,30,28,29,217,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,245,193,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,30,28,29,