#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <curses.h>
#include <math.h>
#include <unistd.h>

#include "path.h"
#include "map.h"
#include "structs.h"
#include "priority_queue.h"
#include "error.h"
#include "config.h"

/* TODO: make it move the map maybe to show the path */
/* TODO: figure out input when automatic = 1 */
int anim(Map map, size_t width, size_t height, Position start, Position end, Position *cur, char **visited, PositionPQ *frontier) {
    static char automatic = 0;
    while (1) {
        draw_map(map, width, height, start, end, cur, NULL, visited, frontier);
        mvprintw(height+2 + map_offset_y, map_offset_x, "cur: %zu %zu", cur->x, cur->y);
        if (automatic) { usleep(ANIM_DELAY_USEC); return 0; }
        switch (getch()) {
            case 'h': map_offset_x -= 2; break;
            case 'l': map_offset_x += 2; break;
            case 'j': map_offset_y += 1; break;
            case 'k': map_offset_y -= 1; break;
            case 'a': automatic = 1; break;
            case 'q': return -1;
            default:  return 0;
        }
    }
    return 0;
}
/* BLOODY FUCK IT WORKS */
Path breadth_first_search_path(int dirs, Map map, size_t width, size_t height, Position start, Position end, char **visited, char should_anim) {
    /* The function to use to find neighbours */
    unsigned int (*neighbours)(Position[], size_t[], Position, size_t, size_t, char**) = 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;

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

    visited_clear(visited, width, height);

    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);
            if (should_anim) {
                clear();
            }
            return path; /* Found path */
        }

        Position na[dirs];
        unsigned int nc = neighbours(na, NULL, 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) {
                clear();
                path_free(path, height);
                return NULL;
            }
        }
    }

    if (should_anim) {
        clear();
    }

    path_free(path, height);
    return NULL;
}

Path dijkstra_path(int dirs, Map map, size_t width, size_t height, Position start, Position end, char **visited, char should_anim) {
    /* The function to use to find neighbours */
    unsigned int (*neighbours)(Position[], size_t[], Position, size_t, size_t, char**) = NULL;

    switch (dirs) {
        case 4: neighbours = &neighbours_4dir; break;
        case 8: neighbours = &neighbours_8dir; break;
        default: error("Tried to call dijkstra_path with wrong direction amount\n");
    }

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

    visited_clear(visited, width, height);
    size_t cost_so_far[height][width];
    memset(cost_so_far, 0xFF, sizeof(size_t) * height * width);
    cost_so_far[start.y][start.x] = 0;

    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;

            size_t new_cost = cost_so_far[cur.y][cur.x] + costs[i];
            if (new_cost < cost_so_far[na[i].y][na[i].x]) {
                cost_so_far[na[i].y][na[i].x] = new_cost;
                path[na[i].y][na[i].x].parent = cur;
                ppq_insert(&frontier, na[i], new_cost);
            }
        }

        if (should_anim) {
            if (anim(map, width, height, start, end, &cur, visited, frontier) == -1) {
                clear();
                path_free(path, height);
                return NULL;
            }
        }
    }

    path_free(path, height);
    return NULL;
}

Path astar_path(int dirs, Map map, size_t width, size_t height, Position start, Position end, char **visited, char should_anim) {
    /* The function to use to find neighbours */
    unsigned int (*neighbours)(Position[], size_t[], Position, size_t, size_t, char**) = NULL;
    /* The heuristic function */
    size_t (*heuristic)(Position, Position) = NULL;

    /* TODO: maybe a sort of a demo with different heuristics would be cool idk */
    switch (dirs) {
        case 4: neighbours = &neighbours_4dir; heuristic = &manhattan_distance; break;
        case 8: neighbours = &neighbours_8dir; heuristic = &diagonal_distance; break;
        default: error("Tried to call dijkstra_path with wrong direction amount\n");
    }

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

    visited_clear(visited, width, height);
    size_t cost_so_far[height][width];
    memset(cost_so_far, 0xFF, sizeof(size_t) * height * width);
    cost_so_far[start.y][start.x] = 0;

    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;

            size_t new_cost = cost_so_far[cur.y][cur.x] + costs[i];
            if (new_cost < cost_so_far[na[i].y][na[i].x]) {
                cost_so_far[na[i].y][na[i].x] = new_cost;
                path[na[i].y][na[i].x].parent = cur;
                size_t priority = new_cost + heuristic(na[i], end);
                ppq_insert(&frontier, na[i], priority);
            }
        }

        if (should_anim) {
            if (anim(map, width, height, start, end, &cur, visited, frontier) == -1) {
                clear();
                path_free(path, height);
                return NULL;
            }
        }
    }

    path_free(path, height);
    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) * COST_ORTHOGONAL;
    } else {
        d += (b.x - a.x) * COST_ORTHOGONAL;
    }
    if (a.y > b.y) {
        d += (a.y - b.y) * COST_ORTHOGONAL;
    } else {
        d += (b.y - a.y) * COST_ORTHOGONAL;
    }
    return d;
}

/* FIXME: something faster, maybe? or at least better written. */
size_t diagonal_distance(Position a, Position b) {
    size_t one = 0;
    size_t two = 0;
    if (a.x > b.x) {
        one = (a.x - b.x) * COST_ORTHOGONAL;
    } else {
        one = (b.x - a.x) * COST_ORTHOGONAL;
    }
    if (a.y > b.y) {
        two = (a.y - b.y) * COST_ORTHOGONAL;
    } else {
        two = (b.y - a.y) * COST_ORTHOGONAL;
    }
    return (size_t) hypot(one, two);
}

size_t path_length(Path path, Position start, Position goal) {
    size_t length = 0;
    if (path != NULL) {
        Position cur = goal;
        while (cur.x != start.x || cur.y != start.y) {
            if (cur.x - path[cur.y][cur.x].parent.x == 0 || cur.y - path[cur.y][cur.x].parent.y == 0) {
                length += COST_ORTHOGONAL;
            } else {
                length += COST_DIAGONAL;
            }
            cur = path[cur.y][cur.x].parent;
        }
    }
    return length;
}

void path_free(Path path, size_t height) {
    if (path == NULL) return;
    for (size_t i = 0; i < height; i++) {
        free(path[i]);
    }
    free(path);
}

char **visited_new(size_t width, size_t height) {
    char **visited = malloc(sizeof(char*) * height);
    if (visited == NULL) return NULL;

    for (size_t row = 0; row < height; row++) {
        visited[row] = malloc(sizeof(char) * width);
        if (visited[row] == NULL) return NULL;
        memset(visited[row], 0x00, width*sizeof(char));
    }

    return visited;
}

void visited_clear(char **visited, size_t width, size_t height) {
    for (size_t row = 0; row < height; row++) {
        memset(visited[row], 0x00, sizeof(char) * width);
    }
}

void visited_free(char **visited, size_t height) {
    if (visited == NULL) return;
    for (size_t row = 0; row < height; row++) {
        free(visited[row]);
    }
    free(visited);
    return;
}