#include <mazemaker.h>
#include <string.h>
#include "pq.h"
#include "set.h"

DEFINE_PQ(maze_path_t, PathPQ, unsigned int);

static void path_copy(maze_path_t const* src, maze_path_t* dst) {
    dst->length = src->length;
    dst->nodes = calloc(dst->length, sizeof(maze_point_t));
    memcpy(dst->nodes, src->nodes, src->length * sizeof(maze_point_t));
}

static void path_copy_append(maze_path_t const* src, maze_path_t* dst, maze_point_t pt) {
    dst->length = src->length + 1;
    dst->nodes = calloc(dst->length, sizeof(maze_point_t));
    memcpy(dst->nodes, src->nodes, src->length * sizeof(maze_point_t));
    dst->nodes[dst->length - 1] = pt;
}

int mazemaker_solve(mazegrid_t const* maze, maze_path_t* path) {
    int code = 0;
    struct node_set* set = node_set_new(maze->width, maze->height);
    NEW_PQ(PathPQ, pq);

    // put starting point in the queue
    maze_path_t start;
    start.length = 1;
    start.nodes = malloc(sizeof(maze_point_t));
    start.nodes[0].x = 0;
    start.nodes[0].y = 0;

    PUSH_PQ(pq, PathPQ, start, 0);
    while (!EMPTY_PQ(pq)) {
        maze_path_t top;
        unsigned int dist;
        size_t x, y;
        POP_PQ(pq, PathPQ, top, dist);

        x = top.nodes[top.length - 1].x;
        y = top.nodes[top.length - 1].y;
        node_set_add(set, x, y);

        // check to see if we've reached the end 
        if ((x == maze->width - 1) && (y == maze->height - 1)) {
            path_copy(&top, path);
            mazemaker_path_free(&top);
            code = 1;
            break;
        }

        // otherwise check for unexplored paths in all directions...
        
        // ... up
        if ((y < maze->height) && (mazegrid_get_edge(maze, x, y, EDGE_UP) != 0)) {
            if (!node_set_contains(set, x, y + 1)) {
                maze_path_t newpath;
                maze_point_t pt = { .x = x, .y = y + 1 };
                path_copy_append(&top, &newpath, pt);
                PUSH_PQ(pq, PathPQ, newpath, dist + 1);
            }
        }
        
        // ... right
        if ((x < maze->width) && (mazegrid_get_edge(maze, x, y, EDGE_RIGHT) != 0)) {
            if (!node_set_contains(set, x + 1, y)) {
                maze_path_t newpath;
                maze_point_t pt = { .x = x + 1, .y = y };
                path_copy_append(&top, &newpath, pt);
                PUSH_PQ(pq, PathPQ, newpath, dist + 1);
            }
        }

        // ... down
        if ((y > 0) && (mazegrid_get_edge(maze, x, y, EDGE_DOWN) != 0)) {
            if (!node_set_contains(set, x, y - 1)) {
                maze_path_t newpath;
                maze_point_t pt = { .x = x, .y = y - 1};
                path_copy_append(&top, &newpath, pt);
                PUSH_PQ(pq, PathPQ, newpath, dist + 1);
            }
        }

        // ... left
        if ((x > 0) && (mazegrid_get_edge(maze, x, y, EDGE_LEFT) != 0)) {
            if (!node_set_contains(set, x - 1, y)) {
                maze_path_t newpath;
                maze_point_t pt = { .x = x - 1, .y = y };
                path_copy_append(&top, &newpath, pt);
                PUSH_PQ(pq, PathPQ, newpath, dist + 1);
            }
        }

        // delete old path
        mazemaker_path_free(&top);
    }

    // If no path is found, we will run out of new nodes to explore, the queue
    // will go empty, and the loop will terminate. The default return code of 0
    // indicates that no solution has been put in the path parameter.

    if (!EMPTY_PQ(pq)) {
        maze_path_t top;
        unsigned int dist;
        FOREACH_PQ(pq, top, dist) {
            free(top.nodes);
        }
        FOREACH_PQ_END
    }
    FREE_PQ(pq);
    node_set_free(set);
    return code;
}

void mazemaker_path_free(maze_path_t* path) {
    path->length = 0;
    free(path->nodes);
}