#include <algolib.c>
#include <math.h>
#include <stdbool.h>
#include <stdio.h>
#include <vectorlib.c>

#include <SDL2/SDL.h>
#include <SDL2/SDL2_gfxPrimitives.h>
#include <SDL2/SDL_render.h>
#include <SDL2/SDL_ttf.h>
#include <SDL2/SDL_video.h>

#define PROJECT_NAME "algorithms-a* 1"

#define grid_cols 50
#define grid_rows 50

#define obj_n grid_cols* grid_rows

// im not moving these objects and functions out to algolib.c as its only used here.
// may move it later
typedef struct ITEM {
    int i;
    int j;

    int f;
    int g;
    int h;

    struct ITEM* neighbors[8];
    struct ITEM* previous;

    bool wall;
} ITEM;

void removeFromArr(ITEM* arr[], ITEM* elt)
{
    for (int i = obj_n - 1; i >= 0; i--) {
        if (arr[i] == elt) {
            for (int j = i; j < obj_n; j++) {
                if (arr[j + 1]) {
                    arr[j] = arr[j + 1];
                } else {
                    arr[j] = NULL;
                    break;
                }
            }
        }
    }
}

void pushIntoArr(ITEM* arr[], ITEM* elt)
{
    for (int i = obj_n - 1; i >= 0; i--) {
        if (arr[i]) {
            arr[i + 1] = elt;
            break;
        } else if (i == 0) {
            arr[i] = elt;
            break;
        }
    }
}

bool arrContains(ITEM* arr[], ITEM* node)
{
    for (int j = 0; j < obj_n; j++) {
        if (node == arr[j])
            return true;
    }
    return false;
}

void pushIntoArrNei(ITEM* arr[], ITEM* elt)
{
    for (int i = 8 - 1; i >= 0; i--) {
        if (arr[i]) {
            arr[i + 1] = elt;
            break;
        } else if (i == 0) {
            arr[i] = elt;
            break;
        }
    }
}

void addNeighbors(ITEM* item, ITEM* items[])
{
    int i = item->i;
    int j = item->j;
    for (int i = 0; i < 8; i++) {
        item->neighbors[i] = NULL;
    }
    if (i < grid_cols - 1) {
        pushIntoArrNei(item->neighbors, items[j * grid_cols + i + 1]);
    }
    if (i > 0) {
        pushIntoArrNei(item->neighbors, items[j * grid_cols + i - 1]);
    }
    if (j < grid_rows - 1) {
        pushIntoArrNei(item->neighbors, items[(j + 1) * grid_cols + i]);
    }
    if (j > 0) {
        pushIntoArrNei(item->neighbors, items[(j - 1) * grid_cols + i]);
    }
    if (i > 0 && j > 0) {
        pushIntoArrNei(item->neighbors, items[(j - 1) * grid_cols + (i - 1)]);
    }
    if (i < grid_cols - 1 && j > 0) {
        pushIntoArrNei(item->neighbors, items[(j - 1) * grid_cols + (i + 1)]);
    }
    if (i > 0 && j < grid_rows - 1) {
        pushIntoArrNei(item->neighbors, items[(j + 1) * grid_cols + (i - 1)]);
    }
    if (i < grid_cols - 1 && j < grid_rows - 1) {
        pushIntoArrNei(item->neighbors, items[(j + 1) * grid_cols + (i + 1)]);
    }
}

typedef struct OBJECT {
    ITEM* items[obj_n];
    ITEM* openSet[obj_n];
    ITEM* closedSet[obj_n];
    ITEM* start;
    ITEM* end;

    int w;
    int h;
} OBJECT;

#include <init.h>

void init_stuffs(int w, int h, STUFFS* stuff)
{
    stuff->obj.w = stuff->width / grid_cols;
    stuff->obj.h = stuff->height / grid_rows;

    for (int j = 0; j < grid_rows; j++) {
        for (int i = 0; i < grid_cols; i++) {
            int index = j * grid_cols + i;
            stuff->obj.items[index] = (ITEM*)malloc(sizeof(ITEM));
            stuff->obj.items[index]->i = i;
            stuff->obj.items[index]->j = j;
            stuff->obj.items[index]->f = 0;
            stuff->obj.items[index]->g = 0;
            stuff->obj.items[index]->h = 0;
            stuff->obj.items[index]->previous = NULL;
            stuff->obj.items[index]->wall = false;

            if (RandomFloat(0, 1) < 0.4) {
                stuff->obj.items[index]->wall = true;
            }

            stuff->obj.openSet[index] = NULL;
            stuff->obj.closedSet[index] = NULL;
        }
    }

    for (int i = 0; i < obj_n; i++) {
        addNeighbors(stuff->obj.items[i], stuff->obj.items);
    }

    stuff->obj.start = stuff->obj.items[0];
    stuff->obj.end = stuff->obj.items[obj_n - 1];
    stuff->obj.start->wall = false;
    stuff->obj.end->wall = false;

    stuff->obj.openSet[0] = stuff->obj.start;
}

void draw_node(ITEM* node, STUFFS* stuff)
{
    SDL_RenderFillRect(stuff->renderer, &(SDL_Rect) { node->i * stuff->obj.w, node->j * stuff->obj.h, stuff->obj.w - 1, stuff->obj.h - 1 });
}

float dist(float x, float y, float x2, float y2)
{
    return sqrt((pow(x, 2) + pow(y, 2) + pow(x2, 2) + pow(y2, 2)));
}

int heuristic(ITEM* a, ITEM* b)
{
    if (a && b) {
        // really slow
        // return (int)dist((float)a->i, (float)a->j, (float)b->i, (float)b->j);

        return fabsf((float)a->i - (float)b->i) + fabsf((float)a->j - (float)b->j);
    }
    return 0;
}

void draw(SDL_Window* window, STUFFS* stuff)
{
    static bool done = false;
    if (done == false) {
        SDL_SetRenderDrawColor(stuff->renderer, 0, 0, 0, 255);
        // SDL_RenderClear(renderer);
        SDL_RenderFillRect(stuff->renderer, &(SDL_Rect) { 0, 0, stuff->width, stuff->height });
        // for (int i=0; i<obj_n; i++){
        //     show(renderer,stuff,stuff->obj.objs[i]);
        // }

        ITEM* path[obj_n] = { NULL };
        ITEM* temp = NULL;

        if (stuff->obj.openSet[0]) {
            int winner = 0;
            for (int i = 0; i < obj_n; i++) {
                if (!stuff->obj.openSet[i])
                    break;

                if (stuff->obj.openSet[i]->f < stuff->obj.openSet[winner]->f) {
                    winner = i;
                }
            }

            ITEM* current = stuff->obj.openSet[winner];

            if (current == stuff->obj.end) {
                SDL_Log("DONE!\n");
                done = true;
                return;
            }

            removeFromArr(stuff->obj.openSet, current);
            pushIntoArr(stuff->obj.closedSet, current);

            // foreach neighbor
            ITEM** neighbors = current->neighbors;
            for (int i = 0; i < 8; i++) {
                ITEM* neighbor = neighbors[i];

                if (neighbor) {
                    if (!arrContains(stuff->obj.closedSet, neighbor) && !neighbor->wall) {
                        int tempG = current->g + 1;

                        bool newPath = false;
                        if (arrContains(stuff->obj.openSet, neighbor)) {
                            if (tempG < neighbor->g) {
                                neighbor->g = tempG;
                                newPath = true;
                            }
                        } else {
                            neighbor->g = tempG;
                            newPath = true;
                            pushIntoArr(stuff->obj.openSet, neighbor);
                        }

                        if (newPath) {
                            neighbor->h = heuristic(neighbor, stuff->obj.end);
                            neighbor->f = neighbor->g + neighbor->h;
                            neighbor->previous = current;
                        }
                    }
                }
            }

            temp = current;
        } else {
            SDL_Log("no solution\n");
            done = true;
            return;
        }

        SDL_SetRenderDrawColor(stuff->renderer, 255, 255, 255, 255);
        for (int j = 0; j < grid_rows; j++) {
            for (int i = 0; i < grid_cols; i++) {
                draw_node((stuff->obj.items[j * grid_cols + i]), stuff);
            }
        }

        // debug

        SDL_SetRenderDrawColor(stuff->renderer, 255, 0, 0, 255);
        for (int i = 0; i < obj_n; i++) {
            if (stuff->obj.closedSet[i]) {
                draw_node(stuff->obj.closedSet[i], stuff);
            }
        }

        SDL_SetRenderDrawColor(stuff->renderer, 0, 255, 0, 255);
        for (int i = 0; i < obj_n; i++) {
            if (stuff->obj.openSet[i]) {
                draw_node(stuff->obj.openSet[i], stuff);
            }
        }

        if (temp) {
            pushIntoArr(path, temp);
            while (temp->previous) {
                pushIntoArr(path, temp->previous);
                temp = temp->previous;
            }
        }

        SDL_SetRenderDrawColor(stuff->renderer, 0, 0, 255, 255);
        for (int i = 0; i < obj_n; i++) {
            if (path[i]) {
                draw_node(path[i], stuff);
            }
        }

        SDL_SetRenderDrawColor(stuff->renderer, 0, 0, 0, 255);
        for (int j = 0; j < grid_rows; j++) {
            for (int i = 0; i < grid_cols; i++) {
                if (stuff->obj.items[j * grid_cols + i]->wall)
                    draw_node((stuff->obj.items[j * grid_cols + i]), stuff);
            }
        }

        SDL_RenderPresent(stuff->renderer);
    }
}

void on_end(STUFFS* stuff)
{
    for (int i = 0; i < obj_n; i++) {
        free(stuff->obj.items[i]);
    }
}

#include <init.c>

/* Challanges
 *
 * draw a line over the path
 *
 * make it so 2 sqares on either corner of a square is also a wall
 *  0x
 *  x0
 */