diff --git a/game/game_state.h b/game/game_state.h
index a37b473..3a7158f 100644
--- a/game/game_state.h
+++ b/game/game_state.h
@@ -40,6 +40,12 @@ typedef struct Game {
     struct {
         BzObjectPool *pathData;
     } pools;
+    struct {
+        bool path;
+        bool entityColliders;
+        bool mapColliders;
+        bool spatialGrid;
+    } debugDraw;
     f32 elapsed;
 } Game;
 
diff --git a/game/main.c b/game/main.c
index 9b15a88..30554f6 100644
--- a/game/main.c
+++ b/game/main.c
@@ -10,10 +10,15 @@
 
 #include "pathfinding.h"
 
+#include <time.h>
+
 ECS_COMPONENT_DECLARE(Game);
 
 ecs_world_t *ECS = NULL;
 
+static ecs_entity_t renderCollidersSystem;
+static ecs_entity_t renderDebugPathSystem;
+
 bool init(void *userData);
 void deinit(void *userData);
 
@@ -132,6 +137,9 @@ bool init(void *userData) {
 
     ECS_SYSTEM(ECS, renderColliders, EcsOnUpdate, Position, Size);
 
+    renderDebugPathSystem = renderDebugPath;
+    renderCollidersSystem = renderColliders;
+
     return true;
 }
 void deinit(void *userData) {
@@ -229,7 +237,6 @@ void render(float dt, void *userData) {
     BeginMode2D(game->camera);
 
     bzTileMapDraw(&game->map);
-    bzTileMapDrawColliders(&game->map);
 
     if (game->input.building) {
         Color placeColor = game->input.buildingCanPlace ?
@@ -244,8 +251,6 @@ void render(float dt, void *userData) {
                            game->input.buildingSize.sizeY * height, placeColor);
     }
 
-    bzSpatialGridDrawDebugGrid(game->entityGrid);
-
 
     Vector2 worldPos = GetScreenToWorld2D(GetMousePosition(), game->camera);
     int tileX = (int) worldPos.x / 16;
@@ -257,6 +262,7 @@ void render(float dt, void *userData) {
         heap = bzHeapCreate(PathNode, game->map.width * game->map.height);
     if (!ecs_has(ECS, game->entity, Path) && IsMouseButtonPressed(MOUSE_BUTTON_LEFT)) {
         Path path = {};
+        clock_t begin = clock();
         const Position *start = ecs_get(ECS, game->entity, Position);
         findPath(&(PathfindingDesc) {
                 .start=(TilePosition) {
@@ -272,9 +278,19 @@ void render(float dt, void *userData) {
         if (path.paths) {
             ecs_set_ptr(ECS, game->entity, Path, &path);
         }
+        clock_t end = clock();
+        double timeSpent = (double) (end - begin) / CLOCKS_PER_SEC;
+        timeSpent *= 1000;
+        bzLogInfo("A* took: %.3fms", timeSpent);
     }
 
     ecs_progress(ECS, dt);
+    ecs_enable(ECS, renderDebugPathSystem, game->debugDraw.path);
+    ecs_enable(ECS, renderCollidersSystem, game->debugDraw.entityColliders);
+    if (game->debugDraw.mapColliders)
+        bzTileMapDrawColliders(&game->map);
+    if (game->debugDraw.spatialGrid)
+        bzSpatialGridDrawDebugGrid(game->entityGrid);
 
     EndMode2D();
 }
@@ -303,6 +319,13 @@ void imguiRender(float dt, void *userData) {
         if (game->input.building)
             game->input.state = INPUT_PLACING;
 
+    }
+    if (igCollapsingHeader_TreeNodeFlags("DebugDraw", 0)) {
+        igCheckbox("map colliders", &game->debugDraw.mapColliders);
+        igCheckbox("entity colliders", &game->debugDraw.entityColliders);
+        igCheckbox("spatial grid", &game->debugDraw.spatialGrid);
+        igCheckbox("path", &game->debugDraw.path);
+
     }
     if (igCollapsingHeader_TreeNodeFlags("Entities", 0)) {
         igSliderFloat("Frame duration", &game->frameDuration, 0.0f, 1.0f, NULL, 0);
diff --git a/game/systems_entity.c b/game/systems_entity.c
index 78947fa..8b16c6c 100644
--- a/game/systems_entity.c
+++ b/game/systems_entity.c
@@ -173,9 +173,9 @@ void renderDebugPath(ecs_iter_t *it) {
         while (pathData) {
             for (i32 iPath = 0; iPath < pathData->numWaypoints; iPath++) {
                 Color color = RED;
-                if (first && iPath < path[i].curWaypoint - 1)
+                if (first && iPath < path[i].curWaypoint)
                     color = GREEN;
-                else if (first && iPath == path[i].curWaypoint - 1)
+                else if (first && iPath == path[i].curWaypoint)
                     color = ORANGE;
                 color.a = 180;