#include "CMap.h"

namespace { auto & map = BWEM::Map::Instance(); }

// gets the TilePosition of the center of an area
TilePosition CMap::getAreaAverage(const BWEM::Area* area)
{
	int xAverage = (area->TopLeft().x + area->BottomRight().x) / 2;
	int yAverage = (area->TopLeft().y + area->BottomRight().y) / 2;
	return TilePosition(xAverage, yAverage);
}

TilePosition CMap::getAreaAverage(const BWEM::Area area)
{
	int xAverage = (area.TopLeft().x + area.BottomRight().x) / 2;
	int yAverage = (area.TopLeft().y + area.BottomRight().y) / 2;
	return TilePosition(xAverage, yAverage);
}

TilePosition CMap::getChokepointCenter(const BWEM::Area* area, const BWEM::Area* neighborArea)
{
	TilePosition average = TilePosition(0, 0);
	for (auto a : area->ChokePointsByArea())
	{
		if (a.first->Id() == neighborArea->Id())
		{
			average = TilePosition(a.second->front().Center());
			break;
		}
	}
	return average;
}

// gets a walking path from one Area to another
// this is just the a* pathfinding algorithm
std::vector<const BWEM::Area*> CMap::getPath(const BWEM::Area* a1, const BWEM::Area* a2)
{
	std::vector<const BWEM::Area*> returnPath;
	std::vector<const BWEM::Area*> closedSet;
	std::vector<const BWEM::Area*> openSet;
	openSet.push_back(a1);

	std::map<const BWEM::Area*, const BWEM::Area*> cameFrom;
	std::map<int, int> gScore;

	for (const BWEM::Area& a : map.Areas())
	{
		gScore.insert(std::pair<int, int>(a.Id(), 10000));
	}

	gScore[a1->Id()] = 0;

	while (!openSet.empty())
	{
		const BWEM::Area* current;

		int score = 9999;
		for (auto o : openSet)
		{
			if (gScore[o->Id()] < score)
			{
				score = gScore[o->Id()];
				current = o;
			}
		}

		if (current->Id() == a2->Id()) // found the goal
		{
			std::vector<const BWEM::Area*> path;

			for (auto const& ent : cameFrom)
			{
				path.push_back(ent.first);
			}

			path.push_back(current);
			return reconstructPath(cameFrom, current);
		}

		// remove current from openSet
		int index = 0;
		for (size_t i = 0; i < openSet.size(); i++)
		{
			if (current == openSet.at(i))
			{
				index = i;
				break;
			}
		}
		openSet.erase(openSet.begin() + index);

		// add current to closedSet
		closedSet.push_back(current);

		for (auto n : current->AccessibleNeighbours())
		{
			if (std::find(closedSet.begin(), closedSet.end(), n) != closedSet.end()) // if the neighbor is in the closed set, ignore
			{
				continue;
			}

			TilePosition tp1 = CMap::getAreaAverage(current);
			TilePosition tp2 = CMap::getAreaAverage(n);

			int tentativeGScore = gScore[current->Id()] + 2;

			if (!(std::find(openSet.begin(), openSet.end(), n) != openSet.end()))
			{
				openSet.push_back(n);
			}
			else if (tentativeGScore >= gScore[n->Id()])
			{
				continue;
			}
			cameFrom[n] = current;
			//cameFrom.emplace(n, current);
			gScore[n->Id()] = tentativeGScore;
		}
	}

	return returnPath;
}

// used by getPath to format the final path
std::vector<const BWEM::Area*> CMap::reconstructPath(std::map<const BWEM::Area*, const BWEM::Area*> cameFrom, const BWEM::Area* current)
{
	std::vector<const BWEM::Area*> returnPath;
	returnPath.push_back(current);

	while (cameFrom.find(current) != cameFrom.end())
	{
		current = cameFrom[current];
		returnPath.push_back(current);
	}

	std::reverse(returnPath.begin(), returnPath.end());

	return returnPath;
}