#include "npc.h"

#include "config.h"
#include "dialogs.h"
#include "game.h"
#include "map.h"

#include <gint/display.h>
#include <gint/hardware.h>
#include <gint/keyboard.h> /*debug*/
#include <math.h>
#include <stdint.h>
#include <stdlib.h>

extern bopti_image_t tiny_npc_male;
extern bopti_image_t tiny_npc_female;
extern bopti_image_t tiny_npc_milkman;
extern bopti_image_t tiny_npc_police;

NPC npc_stack[NPC_STACK_SIZE];
uint32_t npc_count;

NPC *npc_create() {
    if(npc_count == NPC_STACK_SIZE)
        return NULL;

    return &npc_stack[npc_count++];
}

void npc_remove(NPC *npc) {
    uint32_t pos = (uint32_t)npc - (uint32_t)npc_stack;

    if(pos >= NPC_STACK_SIZE)
        return;

    if(npc->owns_path) {
        free(npc->xpath);
        free(npc->ypath);
    }

    if(pos == NPC_STACK_SIZE) {
        if(npc_count)
            npc_count--;
        return;
    }
    uint32_t move_size = sizeof(NPC) * (npc_count - pos);
    if(move_size + pos > NPC_STACK_SIZE)
        move_size = NPC_STACK_SIZE - pos;

    memmove(npc, &npc_stack[++pos], move_size);
    if(npc_count)
        npc_count--;
}

void npc_remove_pos(uint32_t pos) { npc_remove(&npc_stack[pos]); }

float length(float x, float y) { return sqrtf(x * x + y * y); }

int npc_clear_path(NPC *npc) {
    npc->currentPoint = 0;
    npc->hasPath = 0;
    npc->path_length = 0;

    if(npc->owns_path) {
        free(npc->xpath);
        free(npc->ypath);
    }

    npc->xpath = malloc(4);
    npc->ypath = malloc(4);
    if(npc->xpath == NULL || npc->ypath == NULL)
        return 1;
    npc->owns_path = true;
    return 0;
}

int npc_append_path(uint16_t x, uint16_t y, NPC *npc) {
    npc->xpath = realloc(npc->xpath, npc->path_length * 2 + 2);
    npc->ypath = realloc(npc->ypath, npc->path_length * 2 + 2);
    if(npc->xpath == NULL || npc->ypath == NULL)
        return 1;
    npc->xpath[npc->path_length] = x - npc->x;
    npc->ypath[npc->path_length] = y - npc->y;
    npc->path_length++;
    return 0;
}

void as_clean(uint8_t *visited, uint8_t *gscore, uint8_t *fscore) {
    free(visited);
    free(gscore);
    free(fscore);
}

int as_reconstruct_path(int16_t *came_from, int w, int h, int16_t start,
                        int16_t dest, bool is_alloc, NPC *npc) {
    if(npc_clear_path(npc))
        goto as_recons_fail;

    int16_t prev = came_from[dest];

    uint32_t i;
    int x, y;

    for(i = 0; i < PATHFIND_MAX_ITER; i++) {
        x = ((prev%w)*T_WIDTH + T_WIDTH/2) << PRECISION;
        y = ((prev/w)*T_HEIGHT + T_HEIGHT/2) << PRECISION;
        if(npc_append_path(x,y,npc)) {
            goto as_recons_fail;
        }
        prev = came_from[prev];
        if(prev == start){
            x = ((prev%w)*T_WIDTH + T_WIDTH/2) << PRECISION;
            y = ((prev/w)*T_HEIGHT + T_HEIGHT/2) << PRECISION;
            if(npc_append_path(x,y,npc))
                goto as_recons_fail;
            break;
        }
        if(prev == -1)
            goto as_recons_fail;
    }

    uint16_t tx, ty;

    // Flip the path because it started from the end

    /*for(i = 0; i < npc->path_length / 2; i++) {
        tx = npc->xpath[i];
        ty = npc->ypath[i];
        npc->xpath[i] = npc->xpath[npc->path_length - i - 1];
        npc->ypath[i] = npc->ypath[npc->path_length - i - 1];
        npc->ypath[npc->path_length - i - 1] = tx;
        npc->ypath[npc->path_length - i - 1] = ty;
    }*/

    if(is_alloc)
        free(came_from);

    npc->hasPath = true;

    return 0;

as_recons_fail:

    if(is_alloc)
        free(came_from);

    return 1;
}

uint32_t xyram = 0xe500e000 + 32;

/* Custom a* implemetation
 * Unoptimized, may become an issue */
int npc_pathfind(int32_t dest_x, int32_t dest_y, Map *full_map, NPC *npc) {
    int32_t i, j;

    int32_t w = full_map->w/T_WIDTH/PXSIZE;
    int32_t h = full_map->h/T_HEIGHT/PXSIZE;
    int32_t x = (npc->curx >> PRECISION) / T_WIDTH;
    int32_t y = (npc->cury >> PRECISION) / T_HEIGHT;
    dest_x /= T_WIDTH;
    dest_y /= T_HEIGHT;
    int32_t spos = y * w + x;

    uint8_t *map = full_map->walkable;

    /*if(dest_x < 0 || dest_x > w || dest_y < 0 || dest_x > h)
        return 2;*/
    if(map[spos])
        return 2;
    if(map[dest_y * w + dest_x])
        return 2;

    npc_clear_path(npc);

    uint8_t *visited;
    int16_t *came_from;
    uint8_t *gscore;
    uint8_t *fscore;

    bool is_alloc;

    if(1 || isSH3() || w * h * 5 > 1024 * 15) {
        is_alloc = true;

        visited = malloc(w * h);
        came_from = malloc(w * h * 2);
        gscore = malloc(w * h * 2);
        fscore = malloc(w * h * 2);
        if(!visited || !came_from || !gscore || !fscore) {
            as_clean(visited, gscore, fscore);
            free(came_from);
            return 4;
        }

        for(i = 0; i < w * h; i++)
            visited[i] = 1;
        for(i = 0; i < w * h; i++)
            came_from[i] = -1;
        for(i = 0; i < w * h; i++)
            gscore[i] = 255;
        for(i = 0; i < w * h; i++)
            fscore[i] = 255;
    } else {
        is_alloc = false;

        visited = (void *)xyram;
        gscore = (void *)(xyram + w * h);
        fscore = (void *)(xyram + w * h * 2);
        came_from = (void *)(xyram + w * h * 3);
        for(i = 0; i < w * h; i++)
            visited[i] = 1;
        for(i = 0; i < w * h; i++)
            came_from[i] = -1;
        for(i = 0; i < w * h; i++)
            gscore[i] = 255;
        for(i = 0; i < w * h; i++)
            fscore[i] = 255;
    }

    fscore[spos] = length(dest_x - x, dest_y - y);
    visited[spos] = 0;
    gscore[spos] = 0;

    uint8_t bscore;
    int32_t bx = x;
    int32_t by = y;

    for(int iter = 0; iter < PATHFIND_MAX_ITER; iter++) {
        bscore = 255;
        /* Cheapest known tile */
        /* Could be improved with a priority queue*/
        for(i = 0; i <= w * h; i++) {
            if(visited[i])
                continue;
            if(map[i] == 1)
                continue;
            if(fscore[i] > bscore)
                continue;
            bx = i % w;
            by = i / w;
            bscore = fscore[i];
        }
        if(bx == dest_x && by == dest_y) {
            as_clean(visited, gscore, fscore);
            return 0; /*as_reconstruct_path(came_from, w, h, spos,
                                        dest_y * w + dest_x, npc)*/
        }

        visited[by * w + bx] = 1;

        int att_score;

        for(i = bx - 1; i < bx + 2; i++) {
            if(i > w)
                break;
            for(j = by - 1; j < by + 2; j++) {
                if(j > h)
                    break;
                if(map[j * w + i] == 1)
                    continue;
                if(i == bx && j == by)
                    continue;
                att_score = gscore[by * w + bx] + round(length(bx - i, by - j));
                if(att_score < gscore[j * w + i]) {
                    came_from[j * w + i] = by * w + bx;
                    gscore[j * w + i] = att_score;
                    fscore[j * w + i] =
                        att_score + round(length(dest_x - i, dest_y - j));
                    if(visited[j * w + i])
                        visited[j * w + i] = 0;
                }
            }
        }
    }

    as_clean(visited, gscore, fscore);

    free(came_from);
    return 3;
}

// Refactoring to make adding complexity cleaner
void update_npcs(Game *game) {
    uint32_t i;
    for(i = 0; i < game->map_level->nbNPC; i++) {
        update_npc(&game->map_level->npcs[i]);
    }
    for(i = 0; i < npc_count; i++) {
        update_npc(&npc_stack[i]);
        /*Temp debug*/
        game->mana = npc_pathfind(game->player.x, game->player.y, game->map_level,
                     &npc_stack[i]);
    }
}

void update_npc(NPC *npc) {
    /* if the NPC has no path or is paused, skip it */
    if(!npc->hasPath || npc->paused == true)
        return;

    float vecX = (float)(npc->xpath[npc->currentPoint] + npc->x) -
                 (npc->curx >> PRECISION);
    float vecY = (float)(npc->ypath[npc->currentPoint] + npc->y) -
                 (npc->cury >> PRECISION);
    float vecN = length(vecX, vecY);

    if(vecN > 0.5f) {
        vecX /= vecN * 2.0;
        vecY /= vecN * 2.0;
    } else {
        npc->currentPoint++;
        npc->currentPoint = npc->currentPoint % npc->path_length;
    }

    npc->curx += vecX * (float)(1 << PRECISION);
    npc->cury += vecY * (float)(1 << PRECISION);
}

bopti_image_t *npc_sprites[FACES] = {&tiny_npc_male, &tiny_npc_female,
                                     &tiny_npc_milkman, &tiny_npc_police};

void npc_draw_single(NPC *npc, Game *game) {

    Player *pl = &game->player;
    /* Render the path if in debug and it has one*/
#if DEBUGMODE
    if(npc->hasPath) {
        int NbPoints = npc->path_length + 1;
        for(int v = 0; v < NbPoints; v++) {

            int16_t deltaX1 =
                ((int16_t)(npc->x + npc->xpath[v % NbPoints]) * PXSIZE) -
                (int16_t)pl->x;
            int16_t deltaY1 =
                ((int16_t)(npc->y + npc->ypath[v % NbPoints]) * PXSIZE) -
                (int16_t)pl->y;
            int16_t deltaX2 =
                ((int16_t)(npc->x + npc->xpath[(v + 1) % NbPoints]) * PXSIZE) -
                (int16_t)pl->x;
            int16_t deltaY2 =
                ((int16_t)(npc->y + npc->ypath[(v + 1) % NbPoints]) * PXSIZE) -
                (int16_t)pl->y;

            dline(pl->px + deltaX1, pl->py + deltaY1, pl->px + deltaX2,
                  pl->py + deltaY2, PATH_COLOR);
        }
    }
#endif /* DEBUGMODE */

    int16_t delX = ((npc->curx * PXSIZE) >> PRECISION) - (int16_t)pl->x;
    int16_t delY = ((npc->cury * PXSIZE) >> PRECISION) - (int16_t)pl->y;
    game->npc_animation.image = npc_sprites[npc->face];
    unsigned char frame = game->npc_animation.frame;
    if(npc->paused || !npc->hasPath)
        game->npc_animation.frame = 0;
    animation_draw(&game->npc_animation, pl->px - P_WIDTH / 2 + delX,
                   pl->py - P_HEIGHT / 2 + delY);
    game->npc_animation.frame = frame;
}

void npc_draw(Game *game) {
    uint32_t u;

    for(u = 0; u < game->map_level->nbNPC; u++) {
        npc_draw_single(&game->map_level->npcs[u], game);
    }
    for(u = 0; u < npc_count; u++) {
        npc_draw_single(&npc_stack[u], game);
    }
}

void npc_reload(GUNUSED Game *game) { npc_count = 0; }