#include "pathfinding.h"
#include "game.h"
#include "util.h"
#include "comp/physical.h"
#include "comp/fighter.h"

#include <stdlib.h>
#include <string.h>

void pfg_all2one_free(pfg_all2one_t *paths)
{
    free(paths->direction);
    free(paths->distance);
}

void pfg_path_free(pfg_path_t *grid_path)
{
    free(grid_path->points);
}

//---
// BFS algorithm
//---

struct point {
    uint8_t x, y;
    int16_t cost;
    uint8_t dir;
};

static int compare_point(void const *_p1, void const *_p2)
{
    struct point const *p1=_p1, *p2=_p2;
    return p1->cost - p2->cost;
}

pfg_all2one_t pfg_dijkstra(map_t const *map, ivec2 center, uint8_t *occupation)
{
    pfg_all2one_t paths = { .map = map, .x = center.x, .y = center.y };
    int size = map->width * map->height;

    #define idx(x, y) ((y) * map->width + (x))

    /* These correspond to the opposites of the directions of <geometry.h> */
    static int dx_array[4] = {  0, -1,  0, +1 };
    static int dy_array[4] = { +1,  0, -1,  0 };

    /* Distance and shortest path information for each node */
    paths.direction = malloc(size * sizeof *paths.direction);
    paths.distance = malloc(size * sizeof *paths.distance);
    /* Queue of nodes to check */
    pqueue_t queue = pqueue_alloc(size, sizeof(struct point), compare_point);

    if(!paths.direction || !paths.distance || !queue.array) {
        pfg_all2one_free(&paths);
        paths.direction = NULL;
        paths.distance = NULL;
        pqueue_destroy(&queue);
        return paths;
    }

    /* Initialize directions and distance */
    memset(paths.direction, 0xff, size * sizeof *paths.direction);
    memset(paths.distance, 0xff, size * sizeof *paths.distance);
    /* Initialize queue */
    struct point point = { center.x, center.y, 0, UP };
    pqueue_add(&queue, &point);

    /* Explore nodes listed in the queue */
    while(!pqueue_empty(&queue)) {
        /* Pop a node from the queue; ignore it if we've visited it before */
        pqueue_pop(&queue, &point);
        int point_i = idx(point.x, point.y);
        if(paths.direction[point_i] != -1)
            continue;

        paths.direction[point_i] = point.dir;

        for(int dir = 0; dir < 4; dir++) {
            int dx = dx_array[dir];
            int dy = dy_array[dir];

            map_cell_t *cell = map_cell(map, point.x+dx, point.y+dy);
            if(!cell) continue;

            if(map->tileset->tiles[cell->base].solid)
                continue;
            if(cell->decor && map->tileset->tiles[cell->decor].solid)
                continue;

            int next_i = idx(point.x+dx, point.y+dy);
            int occ = occupation ? occupation[next_i] : 0;

            /* The direction to the center is the opposite of [dx;dy], which
               is [dir] due to how [dx_array] and [dy_array] are laid out */
            struct point next = { .x=point.x+dx, .y=point.y+dy };
            next.cost = paths.distance[point_i] + 1 + 2*occ;

            if(paths.distance[next_i]>=0 && paths.distance[next_i]<=next.cost)
                continue;
            paths.distance[next_i] = next.cost;

            next.dir = dir;
            pqueue_add(&queue, &next);
        }
    }

    pqueue_destroy(&queue);
    return paths;
    #undef idx
}

//---
// Path generation from the BFS
//---

pfg_path_t pfg_inwards(pfg_all2one_t const *field, ivec2 p)
{
    #define idx(x, y) ((y) * field->map->width + (x))

    pfg_path_t path = { field->map, field->distance[idx(p.x, p.y)], NULL };
    if(path.length < 0 || !field->distance) return path;

    path.points = malloc((path.length + 1) * sizeof *path.points);
    if(!path.points) return path;

    for(int i = 0; i <= path.length; i++) {
        path.points[i] = p;

        vec2 dir = fdir(field->direction[idx(p.x,p.y)]);
        p.x += ffloor(dir.x);
        p.y += ffloor(dir.y);
    }

    return path;
    #undef idx
}

pfg_path_t pfg_outwards(pfg_all2one_t const *field, ivec2 p)
{
    pfg_path_t path = pfg_inwards(field, p);
    if(!path.points) return path;

    /* Invert all steps along the path */
    for(int i = 0; i <= path.length / 2; i++) {
        int j=i, k=path.length-i;

        ivec2 tmp = path.points[j];
        path.points[j] = path.points[k];
        path.points[k] = tmp;
    }

    return path;
}

//---
// Raycasting tools
//---

bool raycast_clear(map_t const *map, vec2 start, vec2 end)
{
    vec2 u = { end.x - start.x, end.y - start.y };
    if(u.x == 0 && u.y == 0) return true;

    fixed_t inv_ux = u.x ? fdiv(fix(1), u.x) : 0;
    fixed_t inv_uy = u.y ? fdiv(fix(1), u.y) : 0;

    /* Current point is [start + t*u]; when t = 1, we've reached [end] */
    fixed_t t = fix(0);

    while(t < fix(1)) {
        fixed_t x = start.x + fmul(t, u.x);
        fixed_t y = start.y + fmul(t, u.y);

        /* Re-check current cell to avoid diagonal clips, where we change tiles
           diagonally in a single step (which happens quite often when snapping
           to points with integer or half-integer coordinates as things align
           perfectly) */
        int current_x = ffloor(x-(u.x < 0));
        int current_y = ffloor(y-(u.y < 0));

        map_cell_t *cell = map_cell(map, current_x, current_y);
        if(!cell)
            return false;
        if(map->tileset->tiles[cell->base].solid)
            return false;
        if(cell->decor && map->tileset->tiles[cell->decor].solid)
            return false;

        /* Distance to the next horizontal, and vertical line */
        fixed_t dist_y = (u.y >= 0) ? fix(1) - fdec(y) : -(fdec(y-1) + 1);
        fixed_t dist_x = (u.x >= 0) ? fix(1) - fdec(x) : -(fdec(x-1) + 1);

        /* Increase in t that would make us hit a horizontal line */
        fixed_t dty = fmul(dist_y, inv_uy);
        fixed_t dtx = fmul(dist_x, inv_ux);

        int next_x = current_x;
        int next_y = current_y;

        /* Move to the next point */
        if(!u.x || (u.y && dty <= dtx)) {
            /* Make sure we don't get stuck, at all costs */
            t += dty + (dty == 0);
            next_y += (u.y >= 0 ? 1 : -1);
        }
        else {
            t += dtx + (dtx == 0);
            next_x += (u.x >= 0 ? 1 : -1);
        }

        if(t > fix(1)) break;
        cell = map_cell(map, next_x, next_y);
        if(!cell)
            return false;
        if(map->tileset->tiles[cell->base].solid)
            return false;
        if(cell->decor && map->tileset->tiles[cell->decor].solid)
            return false;
    }

    return true;
}

bool raycast_clear_hitbox(map_t const *map, vec2 start, vec2 end,
    rect hitbox)
{
    vec2 dir = { end.x - start.x, end.y - start.y };

    /* Opposite corners */
    vec2 p1, p2;

    /* Select the corners of the hitbox */
    if(((dir.x >= 0) ^ (dir.y >= 0)) == 0) {
        p1 = (vec2){ hitbox.r, hitbox.t };
        p2 = (vec2){ hitbox.l, hitbox.b };
    }
    else {
        p1 = (vec2){ hitbox.l, hitbox.t };
        p2 = (vec2){ hitbox.r, hitbox.b };
    }

    vec2 s1 = { start.x + p1.x, start.y + p1.y };
    vec2 s2 = { start.x + p2.x, start.y + p2.y };
    vec2 e1 = { end.x + p1.x, end.y + p1.y };
    vec2 e2 = { end.x + p2.x, end.y + p2.y };

    return raycast_clear(map, s1, e1) && raycast_clear(map, s2, e2);
}

//---
// General pathfinding
//---

void pfc_path_free(pfc_path_t *path)
{
    free(path->points);
}

pfc_path_t pfc_shortcut_full(pfg_path_t const *grid, vec2 start,
    vec2 end, rect hitbox)
{
    pfc_path_t path = { grid->map, 0, NULL };
    if(!grid->points) return path;

    /* Allocate enough points to avoid doing the calculation twice */
    path.points = malloc((grid->length + 3) * sizeof *path.points);
    if(!path.points) return path;

    int current = -1;
    path.points[path.length] = start;

    /* Find the furthest point on the integer path which can be reached in a
       straight line, then start again from there */
    while(current <= grid->length) {
        vec2 p1 = (current == -1) ? start :
            vec_i2f_center(grid->points[current]);
        bool made_progress = false;

        for(int next = grid->length + 1; next > current; next--) {
            vec2 p2 = (next == grid->length + 1) ? end :
                vec_i2f_center(grid->points[next]);

            if(raycast_clear_hitbox(path.map, p1, p2, hitbox)) {
                path.points[++path.length] = p2;
                current = next;
                made_progress = true;
                break;
            }
        }

        /* This should never happen because you can always move from start to
           the first grid point, and from the last grid point to the end; plus,
           grid points are trivially adjacent. But for robustness, check */
        if(!made_progress) return path;
    }

    return path;
}

vec2 pfc_shortcut_one(pfg_path_t const *grid, vec2 start, vec2 end,
    rect hitbox)
{
    vec2 target = start;
    if(!grid->points) return target;

    /* Find a point that can be reached directly by bisecting the path  */
    for(int next = grid->length + 1; next >= 0; next /= 2) {
        vec2 p2 = (next == grid->length + 1) ? end :
            vec_i2f_center(grid->points[next]);

        if(raycast_clear_hitbox(grid->map, start, p2, hitbox)) {
            target = p2;
            break;
        }
        if(next == 0)
            break;
    }

    return target;
}

//---
// Auto-aiming
//---

vec2 pathfinding_autoaim(game_t const *game, entity_t *src, map_t const *map)
{
    physical_t *src_p = getcomp(src, physical);
    if(!src_p)
        return (vec2){ 0, 0 };

    vec2 src_pos = { src_p->x, src_p->y };
    vec2 src_facing = fdir(src_p->facing);

    vec2 current_best_dir = { 0, 0 };
    fixed_t best_dist2 = -1;

    for(int i = 0; i < game->entity_count; i++) {
        entity_t *e = game->entities[i];
        physical_t *p = getcomp(e, physical);
        fighter_t *f = getcomp(e, fighter);
        if(!p || !f || !f->enemy)
            continue;

        vec2 relpos = { p->x - src_pos.x, p->y - src_pos.y };
        if(vec_dot(relpos, vec_rotate_60(src_facing)) < 0)
            continue;
        if(vec_dot(relpos, vec_rotate_m60(src_facing)) < 0)
            continue;

        fixed_t dist2 = fmul(relpos.x, relpos.x) + fmul(relpos.y, relpos.y);
        if(dist2 < best_dist2 || best_dist2 < 0) {
            /* TODO: Prune based on raycasts */
            (void)map;

            best_dist2 = dist2;
            current_best_dir = fnormalize(relpos);
        }
    }

    return current_best_dir;
}