#include "Pathfinding.h"
#include "DEFINES.h"
#include "Crawler.h"

INCLUDE_game

void Pathfinding::Initialize(){
    if(nodes!=nullptr){
        delete[] nodes;
    }
    nodes = new sNode[game->WORLD_SIZE.x * game->WORLD_SIZE.y];
		for (int x = 0; x < game->WORLD_SIZE.x; x++)
			for (int y = 0; y < game->WORLD_SIZE.y; y++)
			{
				nodes[y * game->WORLD_SIZE.x + x].x = x; // ...because we give each node its own coordinates
				nodes[y * game->WORLD_SIZE.x + x].y = y;
                geom2d::rect<int>tile=game->GetTileCollision(game->GetCurrentLevel(),{float(x*24),float(y*24)});
				nodes[y * game->WORLD_SIZE.x + x].bObstacle = tile.pos!=game->NO_COLLISION.pos||tile.size!=game->NO_COLLISION.size;
                tile=game->GetTileCollision(game->GetCurrentLevel(),{float(x*24),float(y*24)},true);
                nodes[y * game->WORLD_SIZE.x + x].bObstacleUpper = tile.pos!=game->NO_COLLISION.pos||tile.size!=game->NO_COLLISION.size;
				nodes[y * game->WORLD_SIZE.x + x].parent = nullptr;
				nodes[y * game->WORLD_SIZE.x + x].bVisited = false;
			}

		for (int x = 0; x < game->WORLD_SIZE.x; x++)
			for (int y = 0; y < game->WORLD_SIZE.y; y++)
			{
				if(y>0)
					nodes[y*game->WORLD_SIZE.x + x].vecNeighbours.push_back(&nodes[(y - 1) * game->WORLD_SIZE.x + (x + 0)]);
				if(y<game->WORLD_SIZE.y-1)
					nodes[y*game->WORLD_SIZE.x + x].vecNeighbours.push_back(&nodes[(y + 1) * game->WORLD_SIZE.x + (x + 0)]);
				if (x>0)
					nodes[y*game->WORLD_SIZE.x + x].vecNeighbours.push_back(&nodes[(y + 0) * game->WORLD_SIZE.x + (x - 1)]);
				if(x<game->WORLD_SIZE.x-1)
					nodes[y*game->WORLD_SIZE.x + x].vecNeighbours.push_back(&nodes[(y + 0) * game->WORLD_SIZE.x + (x + 1)]);
                if (y>0 && x>0)
                    nodes[y*game->WORLD_SIZE.x + x].vecNeighbours.push_back(&nodes[(y - 1) * game->WORLD_SIZE.x + (x - 1)]);
                if (y<game->WORLD_SIZE.y-1 && x>0)
                    nodes[y*game->WORLD_SIZE.x + x].vecNeighbours.push_back(&nodes[(y + 1) * game->WORLD_SIZE.x + (x - 1)]);
                if (y>0 && x<game->WORLD_SIZE.x-1)
                    nodes[y*game->WORLD_SIZE.x + x].vecNeighbours.push_back(&nodes[(y - 1) * game->WORLD_SIZE.x + (x + 1)]);
                if (y<game->WORLD_SIZE.y - 1 && x<game->WORLD_SIZE.x-1)
                    nodes[y*game->WORLD_SIZE.x + x].vecNeighbours.push_back(&nodes[(y + 1) * game->WORLD_SIZE.x + (x + 1)]);
			}

		// Manually position the start and end markers so they are not nullptr
		nodeStart = &nodes[(game->WORLD_SIZE.y / 2) * game->WORLD_SIZE.x + 1];
		nodeEnd = &nodes[(game->WORLD_SIZE.y / 2) * game->WORLD_SIZE.x + game->WORLD_SIZE.x-2];
}

std::vector<vf2d> Pathfinding::Solve_AStar(vf2d startPos,vf2d endPos,float maxRange,bool upperLevel){
    float dist=sqrt(pow(endPos.x-startPos.x,2)+pow(endPos.y-startPos.y,2));
    if(dist>maxRange*24)return {};

    nodeStart=&nodes[int(startPos.y/24)*game->WORLD_SIZE.x+int(startPos.x/24)];
    nodeEnd=&nodes[int(endPos.y/24)*game->WORLD_SIZE.x+int(endPos.x/24)];

    
    geom2d::rect<int>posPerimeter{{int(std::min(startPos.x,endPos.x)),int(std::min(startPos.y,endPos.y))},{int(abs(endPos.x-startPos.x)),int(abs(endPos.y-startPos.y))}};
    posPerimeter.pos={int(std::clamp(posPerimeter.pos.x-maxRange*24,0.f,game->WORLD_SIZE.x*24.f)),int(std::clamp(posPerimeter.pos.y-maxRange*24,0.f,game->WORLD_SIZE.y*24.f))};
    posPerimeter.size={int(std::clamp(posPerimeter.size.x+maxRange*24*2,0.f,game->WORLD_SIZE.x*24.f-posPerimeter.pos.x)),int(std::clamp(posPerimeter.size.y+maxRange*24*2,0.f,game->WORLD_SIZE.y*24.f-posPerimeter.pos.y))};
    
    for (int x = 0; x < game->WORLD_SIZE.x; x++){
        for (int y = 0; y < game->WORLD_SIZE.y; y++){
            if(geom2d::overlaps(posPerimeter,vi2d{x*24,y*24})){
                nodes[y*game->WORLD_SIZE.x + x].bVisited = false;
            } else {
                nodes[y*game->WORLD_SIZE.x + x].bVisited = true;
            }
            nodes[y*game->WORLD_SIZE.x + x].fGlobalGoal = INFINITY;
            nodes[y*game->WORLD_SIZE.x + x].fLocalGoal = INFINITY;
            nodes[y*game->WORLD_SIZE.x + x].parent = nullptr;	// No parents
        }
    }

    auto distance = [](sNode* a, sNode* b) // For convenience
    {
        return sqrtf((a->x - b->x)*(a->x - b->x) + (a->y - b->y)*(a->y - b->y));
    };

    auto heuristic = [distance](sNode* a, sNode* b)
    {
        return distance(a, b);
    };

    sNode *nodeCurrent = nodeStart;
    nodeStart->fLocalGoal = 0.0f;
    nodeStart->fGlobalGoal = heuristic(nodeStart, nodeEnd);

    std::list<sNode*> listNotTestedNodes;
    listNotTestedNodes.push_back(nodeStart);

    while (!listNotTestedNodes.empty() && nodeCurrent != nodeEnd)
    {
        listNotTestedNodes.sort([](const sNode* lhs, const sNode* rhs){ return lhs->fGlobalGoal < rhs->fGlobalGoal; } );
        
        while(!listNotTestedNodes.empty() && listNotTestedNodes.front()->bVisited)
            listNotTestedNodes.pop_front();
        if (listNotTestedNodes.empty())
            break;

        nodeCurrent = listNotTestedNodes.front();
        nodeCurrent->bVisited = true;
        for (auto nodeNeighbour : nodeCurrent->vecNeighbours)
        {
            if (!nodeNeighbour->bVisited && ((!upperLevel && nodeNeighbour->bObstacle == 0)||(upperLevel && nodeNeighbour->bObstacleUpper==0)))
                listNotTestedNodes.push_back(nodeNeighbour);

            float fPossiblyLowerGoal = nodeCurrent->fLocalGoal + distance(nodeCurrent, nodeNeighbour);

            if (fPossiblyLowerGoal < nodeNeighbour->fLocalGoal)
            {
                nodeNeighbour->parent = nodeCurrent;
                nodeNeighbour->fLocalGoal = fPossiblyLowerGoal;
                nodeNeighbour->fGlobalGoal = nodeNeighbour->fLocalGoal + heuristic(nodeNeighbour, nodeEnd);
            }
        }	
    } 

    if (nodeEnd != nullptr)
    {
        int pathLength=INFINITE;
        sNode *p = nodeEnd;
        std::vector<vf2d>finalPath;
        while (p->parent != nullptr)
        {
            if(pathLength==INFINITE){
                pathLength=1;
            } else {
                pathLength++;
            }
            finalPath.push_back({float((*p).x),float((*p).y)});
            p = p->parent;
        }
        std::reverse(finalPath.begin(),finalPath.end());
        return finalPath;
    } else {
        return {};
    }
}