Implement breadth-first-search + fix the priority queue + some other stuff

9 files changed, 225 insertions, 89 deletions

diff --git a/Makefile b/Makefile
index 0b0fb84..47226b3 100644
--- a/Makefile
+++ b/Makefile
@@ -1,11 +1,11 @@
 CC=gcc
 EXECUTABLE=astar
 
-astar: main.c map.c stack.c config.h priority_queue.h priority_queue.c
-	$(CC) -Wall -Wpedantic -Wextra -Werror -O3 -o $(EXECUTABLE) priority_queue.c map.c stack.c main.c -lncurses
+astar: main.c map.c stack.c config.h priority_queue.h priority_queue.c path.c
+	$(CC) -Wall -Wpedantic -Wextra -Werror -O3 -o $(EXECUTABLE) priority_queue.c map.c stack.c path.c main.c -lncurses
 
-debug: main.c map.c stack.c config.h priority_queue.h priority_queue.c
-	$(CC) -Wall -g -o $(EXECUTABLE) priority_queue.c map.c stack.c main.c -lncurses
+debug: main.c map.c stack.c config.h priority_queue.h priority_queue.c path.c
+	$(CC) -Wall -g -o $(EXECUTABLE) priority_queue.c map.c stack.c path.c main.c -lncurses
 
 clean: 
 	rm ./$(EXECUTABLE)
diff --git a/main.c b/main.c
index 1142134..747254d 100644
--- a/main.c
+++ b/main.c
@@ -3,16 +3,20 @@
 #include <stdlib.h>
 #include <stdio.h>
 #include <time.h>
+
 #include "map.h"
 #include "structs.h"
 #include "config.h"
 #include "error.h"
 #include "priority_queue.h"
+#include "path.h"
 
 /* So, TODO for now:
-   - Implement Dijkstra and greedy-best-first search algorithms
+   - Implement Dijkstra algorithm
    - Implement the A* algorithm
       - Implement it with 4 and 8 directions
+   - Add ability to see visited squares
+   - MORE MAPS FOR THE MAP PEOPLE
    - Implement adding maps from files (with rle, preferably)
    - Implement controls (to change maps, move start/goal, etc.)
    - Clean up unused `#include`s */
@@ -28,47 +32,56 @@ void initialize_colors(void) {
     start_color();
     use_default_colors();
     init_pair(EMPTY_COLOR, COLOR_BLACK, -1);
-    init_pair(GOAL_COLOR, COLOR_RED, -1);
+    init_pair(GOAL_COLOR, -1, COLOR_RED);
     init_pair(WALL_COLOR, COLOR_WHITE, COLOR_WHITE); /* Using white as bg makes
                                                         them seem solid */
-    init_pair(START_COLOR, COLOR_RED, -1);
+    init_pair(START_COLOR, -1, COLOR_RED);
+    init_pair(PATH_COLOR, COLOR_RED, COLOR_RED);
 }
 
-int main(void) {
-    signal(SIGINT, sigint_handler);
-
+void init_ncurses(void) {
     initscr();     /* Initialize the ncurses screen */
     cbreak();      /* Process input one char at a time */
     curs_set(0);   /* Hide the cursor */
     noecho();      /* Don't echo characters */
     initialize_colors();
+}
 
-    /* FIXME: shitty. sometimes leaves enough space on the right for a bigger map */
-    /* size_t height = LINES/2 - DRAW_MAP_OFFSET_Y,
-           width = COLS/4 - 1;
-    Map map = rbt_maze_map(width, height, (unsigned int) time(NULL));
-    Position fake_start_position_to_pass_into_draw_map = {0, 0};
-    Position fake_goal_position_to_pass_into_draw_map = {width*2-2, height*2-2}; */
+int main(void) {
+    signal(SIGINT, sigint_handler);
 
-    size_t width, height;
-    Position start_pos, end_pos;
-    Map map = file_plaintext_map("maps/test", &width, &height, &start_pos, &end_pos);
+    init_ncurses();
 
+    size_t mwidth = 20;
+    size_t mheight = 10;
+    Map map = rbt_maze_map(mwidth, mheight, (unsigned int) time(NULL));
+    Position start_pos = {0, 0};
+    Position end_pos = {mwidth*2-2, mheight*2-2};
+    size_t height = mheight * 2 - 1;
+    size_t width = mwidth * 2 - 1; 
     int offset_x = 0, offset_y = 0;
+
+    //size_t width, height;
+    //Position start_pos, end_pos;
+    //Map map = file_plaintext_map("maps/test", &width, &height, &start_pos, &end_pos);
+    //print_map_out(map, width, height);
+    draw_map(map, width, height, offset_x, offset_y, start_pos, end_pos, NULL);
+    
+    Path path = breadth_first_search_path_4dir(map, width, height, start_pos, end_pos);
+
     while (1) {
-        draw_map(map, width, height, offset_x, offset_y, start_pos, end_pos);
+        draw_map(map, width, height, offset_x, offset_y, start_pos, end_pos, path);
         char c = getch();
         switch (c) {
             case 'h': offset_x -= 2; break;
             case 'l': offset_x += 2; break;
             case 'j': offset_y += 1; break;
             case 'k': offset_y -= 1; break;
-          /*
             case 'n': 
-                 FIXME: free it before generating a new one 
-                map = rbt_maze_map(width, height, (unsigned int) time(NULL));
+                //FIXME: free it all before generating a new one 
+                map = rbt_maze_map(mwidth, mheight, (unsigned int) time(NULL));
+                path = breadth_first_search_path_4dir(map, width, height, start_pos, end_pos);
                 break;
-            */
             case 'q': endwin(); return 0;          
         }
     }
diff --git a/map.c b/map.c
index 9d0a264..0c0bf86 100644
--- a/map.c
+++ b/map.c
@@ -2,10 +2,12 @@
 #include <stdlib.h>
 #include <curses.h>
 #include <limits.h>
+
 #include "map.h"
 #include "config.h"
 #include "stack.h"
 #include "error.h"
+#include "path.h"
 
 Map empty_map(size_t width, size_t height) {
     Map map = malloc(sizeof(MapTile*) * height);
@@ -143,7 +145,8 @@ Map file_plaintext_map(char *filename, size_t *width, size_t *height, Position *
 /* FIXME: I don't think we need DRAW_MAP_OFFSET_{X,Y} anymore
  * Although, at the same time, it does make less magic values, so I guess it's fine */
 /* TODO: Maybe add an option to render with ▄? that doubles the resolution, but requires us to use ncursesw, probably. */
-void draw_map(Map map, size_t width, size_t height, int offset_x, int offset_y, Position start, Position goal) {
+void draw_map(Map map, size_t width, size_t height, int offset_x, int offset_y, Position start, Position goal, Path path) {
+    (void)path;
     /* I think it flickers less when we do that */
     wnoutrefresh(stdscr);
 
@@ -198,6 +201,17 @@ void draw_map(Map map, size_t width, size_t height, int offset_x, int offset_y,
         }
     }
 
+    /* Draw path */
+    if (path != NULL) {
+        attron(COLOR_PAIR(PATH_COLOR));
+        Position cur = goal;
+        while (cur.x != start.x || cur.y != start.y) {
+            mvaddch(cur.y + DRAW_MAP_OFFSET_Y + offset_y, cur.x*2 + DRAW_MAP_OFFSET_X + offset_x, ' ');
+            mvaddch(cur.y + DRAW_MAP_OFFSET_Y + offset_y, cur.x*2 + DRAW_MAP_OFFSET_X + 1 + offset_x, ' ');
+            cur = path[cur.y][cur.x].parent;
+        }
+    }
+
     /* Draw the start */
     attron(A_BOLD);
     attron(COLOR_PAIR(START_COLOR));
@@ -215,3 +229,15 @@ void draw_map(Map map, size_t width, size_t height, int offset_x, int offset_y,
     doupdate();
 }
 
+void print_map_out(Map map, size_t width, size_t height) {
+    for (size_t i = 0; i < height; i++) {
+        for (size_t j = 0; j < width; j++) {
+            switch(map[i][j]) {
+                case EMPTY: putchar(EMPTY_CHAR); break;
+                case WALL: putchar(WALL_CHAR); break;
+            }
+        }
+        putchar('\n');
+    }
+}
+
diff --git a/map.h b/map.h
index a4cc971..7e17479 100644
--- a/map.h
+++ b/map.h
@@ -3,24 +3,8 @@
 
 #include <stddef.h>
 #include "structs.h"
+#include "path.h"
 
-enum MapTile_e {
-    EMPTY = 0,
-    WALL,
-};
-
-typedef enum MapTile_e MapTile;
-
-enum Colors_e {
-    EMPTY_COLOR = 1,
-    GOAL_COLOR = 2,
-    WALL_COLOR = 3,
-    START_COLOR = 4,
-};
-
-/* A map is a 2D array of MapTiles. 
- * Use as map[row][column] */
-typedef MapTile** Map;
 
 /* Returns an empty map of given size */
 Map empty_map(size_t width, size_t height);
@@ -50,7 +34,10 @@ Map rbt_maze_map(size_t width, size_t height, unsigned int seed);
  * ..@....... */
 Map file_plaintext_map(char *filename, size_t *width, size_t *height, Position *start_pos, Position *end_pos);
 
-/* Draw the map. Bet you didn't expect that.  */
-void draw_map(Map map, size_t width, size_t height, int offset_x, int offset_y, Position start, Position goal);
+/* Draw the map. Bet you didn't expect that. 
+ * path could be NULL to draw a map with no path */
+void draw_map(Map map, size_t width, size_t height, int offset_x, int offset_y, Position start, Position goal, Path path);
+
+void print_map_out(Map map, size_t width, size_t height);
 
 #endif /*MAP_H_ */
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;
+}
diff --git a/path.h b/path.h
new file mode 100644
index 0000000..9de376e
--- /dev/null
+++ b/path.h
@@ -0,0 +1,12 @@
+#ifndef ASTAR_H_
+#define ASTAR_H_
+
+#include "structs.h"
+#include "map.h"
+
+Path breadth_first_search_path_4dir(Map map, size_t width, size_t height, Position start, Position end);
+
+Path astar_path_4dir(Map map, size_t width, size_t height, Position start, Position end);
+size_t manhattan_distance(Position a, Position b);
+
+#endif /* ASTAR_H_ */
diff --git a/priority_queue.c b/priority_queue.c
index 79377c3..c8bd8b0 100644
--- a/priority_queue.c
+++ b/priority_queue.c
@@ -12,54 +12,46 @@ PositionPQ *ppq_new(Position pos, size_t priority) {
     return ppq;
 }
 
-/* It works, but I don't trust it.
- * Left the `printf`s as they were when I was debugging this shit, I have a
- * feeling I'll need them later. */
-void ppq_insert(PositionPQ *ppq, Position pos, size_t priority) {
-    /* printf("Inserting %li %li with prio %li\n", pos.x, pos.y, priority); */
-    while (ppq->next != NULL) {
-        if (ppq->priority > priority) {
-            printf(" inserting in middle\n");
-            PositionPQ *n = ppq_new(pos, priority);
-            n->prev = ppq->prev;
-            n->next = ppq;
-            if (ppq->prev != NULL) {
-                ppq->prev->next = n;
-            }
-            ppq->prev = n;
-            return;
-        } else {
-            ppq = ppq->next;
-        }
-    }
-
-    /* Reached end node */
+void ppq_insert(PositionPQ **ppq, Position pos, size_t priority) {
+    PositionPQ *start = *ppq;
 
-    if (priority > ppq->priority) { /* Insert after */
-        /* printf(" reached end, inserting after\n"); */
-        PositionPQ *n = ppq_new(pos, priority);
-        n->prev = ppq;
-        ppq->next = n;
+    if ((*ppq) == NULL) {
+        (*ppq) = ppq_new(pos, priority);
         return;
-    } else { /* Insert before */
-        /* printf(" reached end, inserting before\n"); */
-        PositionPQ *n = ppq_new(pos, priority);
-        n->prev = ppq->prev;
-        n->next = ppq;
-        if (ppq->prev != NULL) {
-            ppq->prev->next = n;
-        }
-        ppq->prev = n;
+    }
+    
+    PositionPQ *n = ppq_new(pos, priority);
+    if (start->priority > priority) {
+        n->next = start;
+        start = n;
         return;
     }
+
+    PositionPQ *temp = *ppq;
+
+    while(temp->next != NULL && temp->next->priority <= priority)
+        temp = temp->next;
+
+    n->next = temp->next;
+    temp->next = n;
+
+    return;
 }
 
-Position ppq_pop(PositionPQ *ppq) {
-    Position pos = ppq->pos;
-    ppq->next->prev = NULL;
-    PositionPQ *next = ppq->next;
-    free(ppq);
-    ppq = next;
+Position ppq_pop(PositionPQ **ppq) {
+    if (*ppq == NULL) {
+        error("Tried to pop a NULL PositionPQ\n");
+    }
+    Position pos = (*ppq)->pos;
+    if ((*ppq)->next != NULL) { /* If there's a next node */
+        (*ppq)->next->prev = NULL;
+        PositionPQ *next = (*ppq)->next;
+        free((*ppq));
+        (*ppq) = next;
+    } else { /* No next node */
+        free((*ppq));
+        (*ppq) = NULL;
+    }
     return pos;
 }
 
@@ -69,6 +61,10 @@ void ppq_reprioritize(PositionPQ *ppq, Position pos, size_t priority) {
 }
 
 void ppq_print(PositionPQ *ppq) {
+    if (ppq == NULL) {
+        printf("ppq is NULL\n");
+        return;
+    }
     int i = 0;
     while (ppq->next != NULL) {
         printf("%i - %li: %li, %li\n", i++, ppq->priority, ppq->pos.x, ppq->pos.y);
diff --git a/priority_queue.h b/priority_queue.h
index fd51e96..1f933e7 100644
--- a/priority_queue.h
+++ b/priority_queue.h
@@ -19,10 +19,10 @@ typedef struct PositionPQNode_s PositionPQ;
 PositionPQ *ppq_new(Position pos, size_t priority);
 
 /* Insert a pos with priority into a given PositionPQ */
-void ppq_insert(PositionPQ *ppq, Position pos, size_t priority);
+void ppq_insert(PositionPQ **ppq, Position pos, size_t priority);
 
 /* Remove and return the position with the lowest priority */
-Position ppq_pop(PositionPQ *ppq);
+Position ppq_pop(PositionPQ **ppq);
 
 /* Change the priority of a given pos, moving it to a different place in the
  * linked list ("POTENTIALLY NOT NEEDED" since we don't use different weights */
diff --git a/structs.h b/structs.h
index 5221829..8e5f28e 100644
--- a/structs.h
+++ b/structs.h
@@ -7,7 +7,31 @@ struct Position_s {
     size_t x;
     size_t y;
 };
-
 typedef struct Position_s Position;
 
+enum MapTile_e {
+    EMPTY = 0,
+    WALL,
+};
+
+typedef enum MapTile_e MapTile;
+
+enum Colors_e {
+    EMPTY_COLOR = 1,
+    GOAL_COLOR = 2,
+    WALL_COLOR = 3,
+    START_COLOR = 4,
+    PATH_COLOR = 5,
+};
+
+/* A map is a 2D array of MapTiles. 
+ * Use as map[row][column] */
+typedef MapTile** Map;
+
+struct PathNode_s {
+    Position parent;
+};
+typedef struct PathNode_s PathNode;
+typedef PathNode** Path;
+
 #endif /* STRUCTS_H_ */