#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, char visited[height][width]) {
    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);

    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) {
            break;
        }

        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;
}