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

INCLUDE_game

void Pathfinding::Initialize(){
    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),float(y)});
				nodes[y * game->WORLD_SIZE.x + x].bObstacle = 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)]);
			}

		// Manually positio 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];
}

int Pathfinding::Solve_AStar(vf2d startPos,vf2d endPos){
    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)];
    
    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].bVisited = false;
            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 && nodeNeighbour->bObstacle == 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);
            }
        }	
    }

    int pathLength=0;
    if (nodeEnd != nullptr)
    {
        sNode *p = nodeEnd;
        while (p->parent != nullptr)
        {
            pathLength++;
            p = p->parent;
        }
    }
    return pathLength;
}