#include <iostream>
#include <SDL2/SDL.h>
#include <set>
#include <queue>
#include <algorithm>
#include <array>

#include "Tilemap.h"

void drawProgress(Cell* start, Cell* goal, std::vector<Cell*>& resolved, std::vector<Cell*>& unresolved, std::vector<Cell*> path);
void drawPath(std::vector<Cell*>& path);
int heuristic(Cell *a, Cell *b);
void recreatePath(Cell *end, std::vector<Cell *> &path);
SDL_Rect getRect(Cell *cell);

int windowWidth = 800;
int windowHeight = 600;
int cellWidth = 10;
int cellHeight = 10;
SDL_Renderer* renderer;

// I'd love to animate this still, but while-loops within while-loops often turn out lame
void findPath(Cell* start, Cell* goal, std::vector<Cell*>& path) {
    std::vector<Cell*> unresolved_nodes;
    std::vector<Cell*> resolved_nodes;

    unresolved_nodes.push_back(start);
    Cell* current = *unresolved_nodes.begin();
    current->g = 0;
    current->h = heuristic(start, goal);
    current->f = current->g + current->h;

    while (!unresolved_nodes.empty()) {
        size_t bestIndex = 0;

        // Find the node with the best f-value from the available nodes
        for (size_t i = 0; i < unresolved_nodes.size(); i++) {
            if (unresolved_nodes[i]->f <= unresolved_nodes[bestIndex]->f)
                bestIndex = i;
        }

        current = unresolved_nodes[bestIndex];

        if (current == goal) {
            std::cout << "Found the path!" << std::endl;
            recreatePath(goal, path);
        }

        // Remove the node from the list of nodes to process
        auto it = std::remove(unresolved_nodes.begin(), unresolved_nodes.end(), current);
        unresolved_nodes.erase(it, unresolved_nodes.end());
        resolved_nodes.push_back(current);

        for(auto const& neighbor : current->neighbors) {

            // If the node is not in the set of resolved nodes
            if(std::find(resolved_nodes.begin(), resolved_nodes.end(), neighbor) == resolved_nodes.end()) {
                int tentative_g = current->g + 1;

                bool new_path = false;

                // Check if the node has been processed before and make sure it is not a wall
                if(std::find(unresolved_nodes.begin(), unresolved_nodes.end(), neighbor) != unresolved_nodes.end()
                    || current->value == 1) {

                    if(tentative_g < neighbor->g)
                        neighbor->g = tentative_g;
                } else {
                    new_path = true;

                    neighbor->g = tentative_g;
                    unresolved_nodes.push_back(neighbor);
                }

                if(new_path) {
                    neighbor->h = heuristic(neighbor, goal);
                    neighbor->f = neighbor->g + neighbor->h;
                    neighbor->previous = current;
                }
            }
        }
        
        drawProgress(start, goal, resolved_nodes, unresolved_nodes, path);
    }
}

std::vector<std::vector<Cell*>> collisionMap;

int main() {
    SDL_Window *window = nullptr;
    window = SDL_CreateWindow("Pathfinding", SDL_WINDOWPOS_UNDEFINED, SDL_WINDOWPOS_UNDEFINED, windowWidth,
                              windowHeight, SDL_WINDOW_SHOWN);

    renderer = SDL_CreateRenderer(window, -1, SDL_RENDERER_PRESENTVSYNC);

    SDL_Event event;

    Tilemap tilemap(renderer);
    bool loadedCorrectly = tilemap.loadFromFile("resources/maps/normalrooms/floor_1/room_1.json");

    if (!loadedCorrectly) {
        std::cout << "Unable to load tilemap. Verify file path to ensure it is set correctly" << std::endl;
    }

    collisionMap = tilemap.getCollisionMap();

    auto start = collisionMap[0][0];
    auto end = collisionMap[collisionMap.size() - 1][collisionMap[0].size() - 1];

    std::vector<Cell*> path;

    while (true) {
        SDL_SetRenderDrawColor(renderer, 0, 0, 0, 255);
        SDL_PollEvent(&event);

        if (event.type == SDL_QUIT)
            break;

        SDL_RenderClear(renderer);

        drawPath(path);

        if(path.empty()) {
            findPath(start, end, path);
        }

        SDL_RenderPresent(renderer);
    }

    return 0;
}

void drawProgress(Cell* start, Cell* goal, std::vector<Cell*>& resolved, std::vector<Cell*>& unresolved, std::vector<Cell*> path) {
    for(auto const& open : unresolved) {
        SDL_SetRenderDrawColor(renderer, 0, 150, 150, 255);
        SDL_Rect rect = getRect(open);
        SDL_RenderFillRect(renderer, &rect);
    }

    for(auto const& closed : resolved) {
        SDL_SetRenderDrawColor(renderer, 150, 0, 0, 255);
        SDL_Rect rect = getRect(closed);
        SDL_RenderFillRect(renderer, &rect);
    }

    for(auto const& edge : path) {
        SDL_SetRenderDrawColor(renderer, 50, 100, 150, 255);
        SDL_Rect rect = getRect(edge);
        SDL_RenderFillRect(renderer, &rect);
    }

    SDL_SetRenderDrawColor(renderer, 255, 0, 0, 255);
    SDL_Rect startRect = getRect(start);
    SDL_RenderFillRect(renderer, &startRect);

    SDL_SetRenderDrawColor(renderer, 0, 255, 0, 255);
    SDL_Rect goalRect = getRect(goal);
    SDL_RenderFillRect(renderer, &goalRect);
}

int heuristic(Cell *a, Cell *b) {
    int deltaX = b->x - a->x;
    int deltaY = b->y - a->x;

    // We use a heuristic that returns the euclidean distance from point A to B
    return (int)std::sqrt((deltaX * deltaX + deltaY * deltaY));;
}

void recreatePath(Cell *end, std::vector<Cell *> &path) {
    Cell *node = end;
    path.push_back(node);

    while (node->previous != nullptr) {
        path.push_back(node->previous);
        node = node->previous;
    }
}

void drawPath(std::vector<Cell*>& path) {
    for(auto const& edge : path) {
        SDL_SetRenderDrawColor(renderer, 200, 100, 150, 255);
        SDL_Rect rect = getRect(edge);
        SDL_RenderFillRect(renderer, &rect);
    }
}

SDL_Rect getRect(Cell *cell) {
    return SDL_Rect{cell->x * cellWidth, cell->y * cellHeight, cellWidth - 1, cellHeight - 1};
}