Start working in Dijkstra

1 files changed, 41 insertions, 5 deletions

diff --git a/path.c b/path.c
index ac03b8b..a462a25 100644
--- a/path.c
+++ b/path.c
@@ -71,8 +71,17 @@ Path breadth_first_search_path(int dirs, Map map, size_t width, size_t height, P
     return NULL;
 }
 
-/* FIXME: Rewrite this shit */
-Path astar_path_4dir(Map map, size_t width, size_t height, Position start, Position end) {
+/* FIXME: THIS IS NOT YET DIJKSTRA!!! don't use for now (WIP) */
+Path dijkstra_path(int dirs, Map map, size_t width, size_t height, Position start, Position end, char visited[height][width], char should_anim) {
+    return NULL;
+    /* The function to use to find neighbours */
+    unsigned int (*neighbours)(Position[], size_t[], Position, size_t, size_t, char[height][width]) = NULL;
+    switch (dirs) {
+        case 4: neighbours = &neighbours_4dir; break;
+        case 8: neighbours = &neighbours_8dir; break;
+        default: error("Tried to call breadth_first_search_path with wrong direction amount\n");
+    }
+    
     Path path = malloc(sizeof(PathNode)*height);
     if (path == NULL) return NULL;
 
@@ -82,14 +91,41 @@ Path astar_path_4dir(Map map, size_t width, size_t height, Position start, Posit
         memset(path[row], 0, width * sizeof(PathNode));
     }
 
-    size_t costs[height][width];
-    memset(costs, 0xFF, height*width*sizeof(size_t));
+    PositionPQ *frontier = ppq_new(start, 0);
 
-    (void)start, (void)end, (void)map;
+    memset(visited, 0, height * width * sizeof(char));
+
+    while (frontier != NULL) {
+        Position cur = ppq_pop(&frontier);
+        visited[cur.y][cur.x] = 1;
+
+        if (cur.x == end.x && cur.y == end.y) {
+            ppq_free(frontier);
+            return path; /* Found path */
+        }
+
+        Position na[dirs];
+        size_t costs[dirs];
+        unsigned int nc = neighbours(na, costs, cur, width, height, visited);
+
+        for (unsigned int i = 0; i < nc; i++) {
+            /* The Russian constitution doesn't allow walking on walls */
+            if (map[na[i].y][na[i].x] == WALL) continue;
+
+            if (ppq_insert(&frontier, na[i], 0) != PPQ_INSERT_ALREADY) {
+                path[na[i].y][na[i].x].parent = cur;
+            }
+        }
+        
+        if (should_anim) {
+            if (anim(map, width, height, start, end, &cur, visited, frontier) == -1) return NULL;
+        }
+    }
 
     return NULL;
 }
 
+
 /* FIXME: I feel like there's a better way to do this, but not sure */
 size_t manhattan_distance(Position a, Position b) {
     size_t d = 0;