#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"

Path (*path_func)(int, Map, size_t **, size_t, size_t, Position, Position, char **, char) = &astar_path;

char anim_automatic = 0;

/* TODO: make it move the map maybe to show the path */
/* TODO: figure out input when automatic = 1. timeout() seems useful */
int anim(Map map, size_t width, size_t height, Position start, Position end, Position *cur, char **visited, PositionPQ *frontier) {
    while (1) {
        draw_map(map, width, height, start, end, cur, NULL, visited, frontier);
        set_message("cur: %zu %zu", cur->x, cur->y); print_message(height);
        if (anim_automatic) { wrefresh(stdscr); 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 'H': clear(); map_offset_x += 20; break;
            case 'L': clear(); map_offset_x -= 20; break;
            case 'J': clear(); map_offset_y -= 10; break;
            case 'K': clear(); map_offset_y += 10; break;

            case 'z': clear(); map_offset_x = 2; map_offset_y = 1; break; /* Move to top left corner */
            case 'x': clear(); map_offset_x = - width * 2 + COLS - 2; map_offset_y = - height + LINES - 2; break; /* Move to bottom right corner */

            case 'g':
                      switch (getch()) {
                          case 's':
                              clear();
                              map_offset_x = -start.x * 2 + COLS/2;
                              map_offset_y = -start.y + LINES/2;
                              break;
                          case 'e':
                              clear();
                              map_offset_x = -end.x * 2 + COLS/2;
                              map_offset_y = -end.y + LINES/2;
                              break;
                      }
                      break;

            /* TODO: Add a binding to do a bmp */

            case 'a': anim_automatic = 1; break;
            case 'q': clear_message(); return -1;
            default:  return 0;
        }
    }
    return 0;
}

/* BLOODY FUCK IT WORKS */
//Path breadth_first_search_path(int dirs, Map map, size_t **cell_costs, size_t width, size_t height, Position start, Position end, char **visited, char should_anim) {
//    (void)cell_costs; /* BFS doesn't support costs */
//    anim_automatic = 0;
//
//    /* The function to use to find neighbours */
//    unsigned int (*neighbours)(Position[], size_t[], Position, size_t, size_t, char**, size_t**) = 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 = path_new(width, height);
//    if (path == NULL) error("Path was NULL\n");
//
//    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, NULL);
//
//        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] = 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 **cell_costs, size_t width, size_t height, Position start, Position end, char **visited, char should_anim) {
    anim_automatic = 0;

    /* The function to use to find neighbours */
    unsigned int (*neighbours)(Position[], size_t[], Position, size_t, size_t, char**, size_t**) = 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 = path_new(width, height);
    if (path == NULL) error("Path was NULL\n");

    PositionPQ *frontier = ppq_new(start, 0);

    visited_clear(visited, width, height);
    size_t **cost_so_far = cost_new(width, height);
    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);
            cost_free(cost_so_far, height);
            return path; /* Found path */
        }

        Position na[dirs];
        size_t costs[dirs];
        unsigned int nc = neighbours(na, costs, cur, width, height, visited, cell_costs);

        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] = 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);
                ppq_free(frontier);
                cost_free(cost_so_far, height);
                return NULL;
            }
        }
    }

    path_free(path, height);
    ppq_free(frontier);
    cost_free(cost_so_far, height);
    return NULL;
}

Path astar_path(int dirs, Map map, size_t **cell_costs, size_t width, size_t height, Position start, Position end, char **visited, char should_anim) {
    anim_automatic = 0;

    /* The function to use to find neighbours */
    unsigned int (*neighbours)(Position[], size_t[], Position, size_t, size_t, char**, size_t**) = 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 astar_path with wrong direction amount\n");
    }

    Path path = path_new(width, height);
    if (path == NULL) error("Path was NULL\n");

    PositionPQ *frontier = ppq_new(start, 0);

    visited_clear(visited, width, height);
    size_t **cost_so_far = cost_new(width, height);
    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);
            cost_free(cost_so_far, height);
            return path; /* Found path */
        }

        Position na[dirs];
        size_t costs[dirs];
        unsigned int nc = neighbours(na, costs, cur, width, height, visited, cell_costs);

        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] = 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);
                ppq_free(frontier);
                cost_free(cost_so_far, height);
                return NULL;
            }
        }
    }

    path_free(path, height);
    ppq_free(frontier);
    cost_free(cost_so_far, 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].x == 0 || cur.y - path[cur.y][cur.x].y == 0) {
                length += COST_ORTHOGONAL;
            } else {
                length += COST_DIAGONAL;
            }
            cur = path[cur.y][cur.x];
        }
    }
    return length;
}

Path path_new(size_t width, size_t height) {
    Path path = malloc(sizeof(Position*)*height);
    if (path == NULL) return NULL;

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

    return path;
}

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

size_t visited_count(char **visited, size_t width, size_t height) {
    size_t count = 0;
    for (size_t row = 0; row < height; row++) {
        for (size_t col = 0; col < width; col++) {
            if (visited[row][col]) count++;
        }
    }
    return count;
}

size_t **cost_new(size_t width, size_t height) {
    size_t **cost = malloc(sizeof(size_t*) * height);
    if (cost == NULL) return NULL;

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

    return cost;
}

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