void PathFinder::solveConflicts(GoalTypes goalType, std::map<KeyType, Agent*>& agents)
{
	Agent* pFirstAgent = NULL;
	Agent* pCollidingAgent = NULL;
	std::map<FixedCoordinate, KeyType, FixedCoordinateCompare>::iterator keyIt;
	std::map<KeyType, Agent*>::iterator agentIt;

	switch (goalType) {
		case GoalType_Goal:
			while (!mCollidingAgents.empty())
			{
				pCollidingAgent = mCollidingAgents.getFirst();
				//pFirstAgent = agents[mAgentGoalPositions[pCollidingAgent->getPotentialInformation().getBestGoalPosition()]];
				keyIt = mAgentGoalPositions.find(pCollidingAgent->getPotentialInformation().getBestGoalPosition());
				assert(keyIt != mAgentGoalPositions.end());
				agentIt = agents.find(keyIt->second);
				assert(agentIt != agents.end());
				pFirstAgent = agentIt->second;
				solveGoalConflict(pFirstAgent, pCollidingAgent);
				mCollidingAgents.removeFirst();
			}
			break;

			
		case GoalType_NextGoal:
			while (!mCollidingAgents.empty())
			{
				pCollidingAgent = mCollidingAgents.getFirst();
				//pFirstAgent = agents[mAgentNextGoalPositions[pCollidingAgent->getPotentialInformation().getBestNextGoalPosition()]];
				keyIt = mAgentNextGoalPositions.find(pCollidingAgent->getPotentialInformation().getBestNextGoalPosition());
				assert(keyIt != mAgentNextGoalPositions.end());
				agentIt = agents.find(keyIt->second);
				assert(agentIt != agents.end());
				pFirstAgent = agentIt->second;
				solveNextGoalConflict(pFirstAgent, pCollidingAgent);
				mCollidingAgents.removeFirst();
			}
			break;
	}
}
void PathFinder::printMap()
{
	KeyType agentMap[MAP_WIDTH][MAP_HEIGHT];
	KeyType enemyMap[MAP_WIDTH][MAP_HEIGHT];

	for (int i = 0; i < MAP_WIDTH; i++)
	{
		for (int j = 0; j < MAP_HEIGHT; j++)
		{
			agentMap[i][j] = INVALID_KEY;
		}
	}

	for (int i = 0; i < MAP_WIDTH; i++)
	{
		for (int j = 0; j < MAP_HEIGHT; j++)
		{
			enemyMap[i][j] = INVALID_KEY;
		}
	}

	// Iterate through all the agents and put the key in the local map
	std::map<KeyType, Agent*>* agents = mpAgentHandler->checkOutAllAgents();
	std::map<KeyType, Agent*>::iterator agentIt;
	for (agentIt = agents->begin(); agentIt != agents->end(); ++agentIt)
	{
		Coordinate position = agentIt->second->getPosition();
		agentMap[static_cast<int>(position.x+0.5f)][static_cast<int>(position.y+0.5f)] = agentIt->first;
	}
	mpAgentHandler->checkInAllAgents();

	// Iterate through all the enemies
	std::map<KeyType, Enemy*>* enemies = mpEnemyHandler->checkOutAllEnemies();
	std::map<KeyType, Enemy*>::iterator enemyIt;
	for (enemyIt = enemies->begin(); enemyIt != enemies->end(); ++enemyIt)
	{
		FixedCoordinate position = enemyIt->second->getPosition();
		enemyMap[position.x][position.y] = enemyIt->first;
	}
	mpEnemyHandler->checkInAllEnemies();

	// Print the map
	std::list<std::wostream*>::iterator it;
	for (it = mpOutStreams->begin(); it != mpOutStreams->end(); ++it)
	{
		printLine(**it, true);
		std::wstring text;
		std::wstringstream ss;
		std::wostream& out = **it;

		for (int height = 0; height < MAP_HEIGHT; height++)
		{
			// Print the id
			for (int width = 0; width < MAP_WIDTH; width++)
			{
				ss.clear();
				text = L"";
				// Check the agent and enemy map
				if (agentMap[width][height] != INVALID_KEY)
				{
					ss << agentMap[width][height];
				}
				else if (enemyMap[width][height] != INVALID_KEY)
				{
					ss << enemyMap[width][height];
				}
				ss >> text;

				out << L"|" << std::setw(5) << text;
			}
			out << L"|" << std::endl;

			// Print additional information
			for (int width = 0; width < MAP_WIDTH; width++)
			{
				// Check the agent and enemy map
				if (agentMap[width][height] != INVALID_KEY)
				{
					// get the goal position
					Agent* pAgent = mpAgentHandler->checkOutAgent(agentMap[width][height]);
					FixedCoordinate goal = pAgent->getPotentialInformation().getBestGoalPosition();
					FixedCoordinate nextGoal = pAgent->getPotentialInformation().getBestNextGoalPosition();
					mpAgentHandler->checkInAgent(agentMap[width][height]);

					// current position
					FixedCoordinate curPos;
					curPos.x = width;
					curPos.y = height;

					text = getArrow(curPos, goal);
					text += L" ";
					text += getArrow(goal, nextGoal);
				}
				else if (enemyMap[width][height] != INVALID_KEY)
				{
					text = L"XXXXX";
				}
				else
				{
					text = L"";
				}

				out << L"|" << std::setw(5) << text;
			}
			out << L"|" << std::endl;
			out.setf(std::ios::fixed, std::ios::floatfield);
			out.precision(2);
			
			// Print the potential info
			for (int width = 0; width < MAP_WIDTH; width++)
			{
				out << L"|" << std::setw(5) << mEnemyPotentialMap[width][height] << text;
			}

			
			out << L"|" << std::endl;
			
			if (height < MAP_HEIGHT - 1)
			{
				printLine(**it);
			}
		}

		printLine(**it, false, true);

		out << "\n\n";
	}
}