#include "npc.h"

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

#include <gint/display.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 *npcRPG;
// uint32_t nbNPC = 0;

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;
    free(npc->xpath);
    free(npc->ypath);
    npc->xpath = malloc(4);
    npc->ypath = malloc(4);
    if(npc->xpath == NULL || npc->ypath == NULL)
        return 1;
    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->path_length++;
    npc->xpath[npc->path_length - 1] = x - npc->x;
    npc->ypath[npc->path_length - 1] = y - npc->y;
    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 spos,
                        int16_t dest, NPC *npc) {
    if(npc_clear_path(npc))
        goto as_recons_fail;

    int16_t next = came_from[dest];

    unsigned int i;

    for(i = 0; i < 64; i++) {
        if(npc_append_path((next % w) * T_WIDTH, (next / h) * T_HEIGHT, npc)) {
            goto as_recons_fail;
        }

        next = came_from[next];
        if(next == spos) {
            if(npc_append_path((spos % w) * T_WIDTH, (spos / h) * T_HEIGHT,
                               npc))
                goto as_recons_fail;
            break;
        }
    }

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

    free(came_from);

    npc->hasPath = true;

    return 0;

as_recons_fail:

    free(came_from);

    return 1;
}

// Returns non zero error code on failure
// 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;
    int32_t h = full_map->h;
    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 = malloc(w * h);
    for(i = 0; i < w * h; i++)
        visited[i] = 1;
    visited[spos] = 0;

    int16_t *came_from = malloc(w * h * 2);
    for(i = 0; i < w * h; i++)
        came_from[i] = -1;

    uint8_t *gscore = malloc(w * h * 2);
    for(i = 0; i < w * h; i++)
        gscore[i] = 255;
    gscore[spos] = 0;

    uint8_t *fscore = malloc(w * h * 2);
    for(i = 0; i < w * h; i++)
        fscore[i] = 255;
    fscore[spos] = length(dest_x - x, dest_y - y);

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

    for(int iter = 0; iter < 64; iter++) {
        bscore = 255;
        // Cheapest known tile
        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 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) {
    for(uint32_t u = 0; u < game->map_level->nbNPC; u++) {
        update_npc(&game->map_level->npcs[u]);
    }
}

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

void npc_draw(Game *game) {
    Player *pl = &game->player;
    size_t i;
    const bopti_image_t *npc_sprites[FACES] = {
        &tiny_npc_male, &tiny_npc_female, &tiny_npc_milkman, &tiny_npc_police};

    for(uint32_t u = 0; u < game->map_level->nbNPC; u++) {
        NPC *Data = &game->map_level->npcs[u];

/* Render the path if in debug*/
#if DEBUGMODE
        if(!Data->hasPath)
            continue; /* this NPC has a trajectory */
        int NbPoints = Data->path_length + 1;
        for(int v = 0; v < NbPoints; v++) {

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

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

        int16_t delX = ((int16_t)((Data->curx>>PRECISION) * PXSIZE)) - (int16_t)pl->wx;
        int16_t delY = ((int16_t)((Data->cury>>PRECISION) * PXSIZE)) - (int16_t)pl->wy;
        bopti_image_t *face = npc_sprites[Data->face];
        dimage(pl->px - P_WIDTH / 2 + delX, pl->py - P_HEIGHT / 2 + delY, face);
    }
}