1 files changed, 78 insertions, 0 deletions
diff --git a/path.c b/path.c
new file mode 100644
index 0000000..19fdf0b
--- /dev/null
+++ b/path.c
@@ -0,0 +1,78 @@
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+
+#include "path.h"
+#include "map.h"
+#include "structs.h"
+#include "priority_queue.h"
+
+/* BLOODY FUCK IT WORKS */
+Path breadth_first_search_path_4dir(Map map, size_t width, size_t height, Position start, Position end) {
+    (void) map, (void)end;
+    Path path = malloc(sizeof(PathNode)*height);
+    if (path == NULL) return NULL;
+
+    for (size_t row = 0; row < height; row++) {
+        path[row] = malloc(sizeof(PathNode) * width);
+        if (path[row] == NULL) return NULL;
+        memset(path[row], 0, width * sizeof(PathNode));
+    }
+
+    PositionPQ *frontier = ppq_new(start, 0);
+    char visited[height][width];
+    memset(visited, 0, height * width * sizeof(char));
+
+    while (frontier != NULL) {
+        Position cur = ppq_pop(&frontier);
+        visited[cur.y][cur.x] = 1;
+
+        Position na[4];
+        unsigned int nc = neighbours(na, 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;
+
+            ppq_insert(&frontier, na[i], 0);
+            path[na[i].y][na[i].x].parent = cur;
+        }
+    }
+
+    return path;
+}
+
+/* FIXME: Rewrite this shit */
+Path astar_path_4dir(Map map, size_t width, size_t height, Position start, Position end) {
+    Path path = malloc(sizeof(PathNode)*height);
+    if (path == NULL) return NULL;
+
+    for (size_t row = 0; row < height; row++) {
+        path[row] = malloc(sizeof(PathNode) * width);
+        if (path[row] == NULL) return NULL;
+        memset(path[row], 0, width * sizeof(PathNode));
+    }
+
+    size_t costs[height][width];
+    memset(costs, 0xFF, height*width*sizeof(size_t));
+
+    (void)start, (void)end, (void)map;
+
+    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;
+    if (a.x > b.x) {
+        d += a.x - b.x;
+    } else {
+        d += b.x - a.x;
+    }
+    if (a.y > b.y) {
+        d += a.y - b.y;
+    } else {
+        d += b.y - a.y;
+    }
+    return d;
+}