/*/////////////////////////////////////////////////////////////////////
	File Name: AStar.cpp
	Maintainer: Ryan Thorlakson
	Creation Date: 2-25-06
	Description: This code was pulled from my senior game project,
		Last Stand. It implements a highly optimized A* pathfinding
		algorithm used by the grunt enemies in our game. It makes use 
		of a binary heap for open list sorting and a data structure
		for tiles maintained between calls with a shifting base ID.
		It will not compile on its own as it requires other parts
		of the project.
/////////////////////////////////////////////////////////////////////*/



/*/////////////////////////////////////////////////////////////////////
	Template Name: BinaryHeap
	Maintainer: Ryan Thorlakson
	Description: Allows for O(lg n) insertion of elements and O(lg n)
			removal of extreme element.
/////////////////////////////////////////////////////////////////////*/
template<class T> class BinaryHeap
{
public:

	//The return of this function can be 0, negative, or positive.
	//0: values are equal
	//positive: t1 should be placed higher on the binary heap (closer to pop out)
	//negative: t2 should be placed higher on the binary heap (closer to pop out)
	typedef int (*BinaryHeapSortFn)(const T& t1, const T& t2);

	typedef bool (*BinaryHeapIDFn)(const T& t1, const T& t2);

	typedef void (*BinaryHeapMoveFn)(const T& t, int newPos);
	
	BinaryHeap(BinaryHeapSortFn sortFn, BinaryHeapIDFn IDFn, BinaryHeapMoveFn moveFn)
	{
		m_SortFn = sortFn;
		m_IDFn = IDFn;
		m_MoveFn = moveFn;
		
		//add dummy to front of vector as position 0 for alignment, never used
		m_Heap.push_back(T());
	}

	/*/////////////////////////////////////////////////////////////////////
	Function Name: BinaryHeap::Push
	Maintainer: Ryan Thorlakson
	Description: Pushes an element onto the binary heap, automatically sorts
	/////////////////////////////////////////////////////////////////////*/
	void Push(const T &data)
	{
		m_Heap.push_back(data);
		
		ImproveElement((int)m_Heap.size() - 1);
	}

	/*/////////////////////////////////////////////////////////////////////
	Function Name: BinaryHeap::Pop
	Maintainer: Ryan Thorlakson
	Description: Pops the extreme element 
	/////////////////////////////////////////////////////////////////////*/
	T Pop(void)
	{
		T ret = m_Heap[1];
		m_Heap[1] = m_Heap.back();
		m_Heap.pop_back();

		int heapMax = (int)m_Heap.size() - 1;

		if(heapMax == 0)
			return ret;

		int childIndex = 1, parentIndex;
		while(1)
		{
			parentIndex = childIndex;

			const T &parent = m_Heap[parentIndex];

			const int childPos1 = parentIndex * 2;
			const int childPos2 = childPos1 + 1;

			//check first child if it exists
			if(childPos1 <= heapMax)
			{
				const T &child = m_Heap[childPos1];
				const int dif = m_SortFn(child, parent);
				if(dif > 0)
					childIndex = childPos1;
			}

			//check second child if it exists
			if(childPos2 <= heapMax)
			{
				const T &child = m_Heap[childPos2];
				const int dif = m_SortFn(child, m_Heap[childIndex]);
				if(dif > 0)
					childIndex = childPos2;
			}

			if(parentIndex == childIndex)
				break;

			swap(m_Heap[parentIndex], m_Heap[childIndex]);
			m_MoveFn(m_Heap[parentIndex], parentIndex);
		}

		m_MoveFn(m_Heap[childIndex], childIndex);

		return ret;
	}

	/*/////////////////////////////////////////////////////////////////////
	Function Name: BinaryHeap::GetHeapIndex
	Maintainer: Ryan Thorlakson
	Description: Pops the extreme element 
	Return value: -1 if the object is not found in the heap, otherwise the
			index in the heap's array.
	/////////////////////////////////////////////////////////////////////*/
	int GetHeapIndex(T &dummyCompare)
	{
		for(int i = 1; i < (int)m_Heap.size(); ++i)
		{
			if(m_IDFn(dummyCompare, m_Heap[i]))
			{
				return i;
			}
		}
		return -1;
	}

	/*/////////////////////////////////////////////////////////////////////
	Function Name: BinaryHeap::GetByIndex
	Maintainer: Ryan Thorlakson
	Description: Gets a binary heap element by its index in the heap's array
	/////////////////////////////////////////////////////////////////////*/
	T &GetByIndex(int index)
	{
		return m_Heap[index];
	}

	/*/////////////////////////////////////////////////////////////////////
	Function Name: BinaryHeap::ImproveElement
	Maintainer: Ryan Thorlakson
	Description: Checks a binary heap element to ensure it is in the
			proper position, and moves it if not.
	/////////////////////////////////////////////////////////////////////*/
	void ImproveElement(int index)
	{
		T *child = &m_Heap[index];
		while(index != 1)
		{
			const int parentPos = index / 2;
			T &parent = m_Heap[parentPos];
			const int dif = m_SortFn(*child, parent);
			//if child's score is lower,
			if(dif > 0)
			{
				m_MoveFn(parent, index);
				swap(*child, parent);
			}
			else
			{
				break;
			}
			index = parentPos;
			child = &parent;
		}

		m_MoveFn(*child, index);
	}

	/*/////////////////////////////////////////////////////////////////////
	Function Name: BinaryHeap::Size
	Maintainer: Ryan Thorlakson
	Description: Returns number of elements on the binary heap, excluding
			the beginning dummy element
	/////////////////////////////////////////////////////////////////////*/
	int Size()
	{
		return (int)m_Heap.size() - 1;
	}

private:
	
	BinaryHeapSortFn m_SortFn;
	BinaryHeapIDFn m_IDFn;
	BinaryHeapMoveFn m_MoveFn;

	std::vector<T> m_Heap;
};

/*/////////////////////////////////////////////////////////////////////
	Function Name: SortAStarRef
	Maintainer: Ryan Thorlakson
	Description: Used by the AStar algorithm to sort binary heap elements
/////////////////////////////////////////////////////////////////////*/
int SortAStarRef(const AStarRef &n1, const AStarRef &n2)
{
	//Lower scores should be placed higher on the binary heap
	return n2.m_Score - n1.m_Score;
}

/*/////////////////////////////////////////////////////////////////////
	Function Name: IDAStarRef
	Maintainer: Ryan Thorlakson
	Description: Used by the AStar algorithm to identify binary heap elements
/////////////////////////////////////////////////////////////////////*/
bool IDAStarRef(const AStarRef &n1, const AStarRef &n2)
{
	return n1.m_pNode == n2.m_pNode;
}

/*/////////////////////////////////////////////////////////////////////
	Function Name: MoveAStarRef
	Maintainer: Ryan Thorlakson
	Description: Used by the AStar algorithm to udpate external data 
			structures when moving binary heap elements
/////////////////////////////////////////////////////////////////////*/
void MoveAStarRef(const AStarRef &n, int newPos)
{
    n.m_pNode->heapPos = newPos;        
}

/*/////////////////////////////////////////////////////////////////////
	Function Name: AIManager::AStar
	Maintainer: Ryan Thorlakson
	Description: Finds an A* path from the position to the goal
/////////////////////////////////////////////////////////////////////*/
void AIManager::AStar(int xPos, int yPos, int xGoal, int yGoal, std::list<Enemy::DIRECTION> &path)
{
	const int STRUCTURE_PENALTY = 10;

	StructureManager &SM = GI.m_GameLogic->GetStructureManager();

	using Enemy::DIRECTION;
	using std::list;

	BinaryHeap<AStarRef> openList(SortAStarRef, IDAStarRef, MoveAStarRef);

	const int OPEN = m_AStarBase + 1;
	const int CLOSED = m_AStarBase + 2;

	int startID = XYToID(xPos, yPos);
	int goalID = XYToID(xGoal, yGoal);

	int heuristic = abs(xPos - xGoal) * 10 + abs(yPos - yGoal) * 10;

	AITile &startTile = tileMap[xPos][yPos];
	startTile.m_Node = AStarNode(startID, 0, 0, heuristic, Enemy::N);
	startTile.m_OpenOrClosed = OPEN;
	openList.Push(AStarRef(0, &startTile.m_Node));

	AStarNode *goalNode = 0;

	while(openList.Size() != 0)
	{
		//Get lowest cost open list tile
		AStarRef openRef = openList.Pop();
		AStarNode &node = *openRef.m_pNode;

		int x, y;
		IDToXY(node.tileID, x, y);
		
		//Add evaluated node to closed list
		AITile &curTile = tileMap[x][y];
		curTile.m_OpenOrClosed = CLOSED;

		if(node.tileID == goalID) //Found the goal?
		{
			goalNode = &node;
			break;
		}

		//Check all 8 directions
		static DIRECTION dirList[8] = {Enemy::N, Enemy::NE, Enemy::E, Enemy::SE, Enemy::S, Enemy::SW, Enemy::W, Enemy::NW};
		for(int i = 0; i < 8; ++i)
		{
			DIRECTION curDir = dirList[i];

			int dx, dy;
			Enemy::DirToXY(curDir, dx, dy);
			int nx = x + dx;
			int ny = y + dy;

			if(OutOfBounds(nx, ny))
				continue;

			AITile &nTile = tileMap[nx][ny];
			
			//Already on closed list?
			if(nTile.m_OpenOrClosed == CLOSED)
				continue;

			AStarNode &nNode = nTile.m_Node;

			int nID = XYToID(nx, ny);

			//Passable tile?
			if(Enemy::Passable(x, y, curDir, true))
			{
				Structure *structure = SM.GetStructure(nx, ny);

				//Get movement cost
				int moveCost;
				if(!structure)
				{
					if(Enemy::IsDiagonal(curDir))
						moveCost = 14; // approx root(2) * 10
					else
						moveCost = 10;
				}
				else //Structure here, moving straight
				{
					moveCost = 10 * STRUCTURE_PENALTY;
				}

				//Manhattan distance
				int heuristic = abs(nx - xGoal) * 10 + abs(ny - yGoal) * 10;

				//Already on open list?
				if(nTile.m_OpenOrClosed == OPEN)
				{
					//Check if this is a better path to the tile than previous path
					const int ID = nNode.heapPos;

					AStarNode &openNode = *openList.GetByIndex(ID).m_pNode;
					if(node.cost + moveCost < openNode.cost)
					{
						openNode.cost = node.cost + moveCost;
						openNode.score = openNode.cost + openNode.heuristic;
						openNode.parent = &node;
						openNode.dirTo = curDir;

						openList.ImproveElement(ID);
					}

					continue;
				}					

				//Add to open list
				int nScore = node.cost + moveCost;
				nTile.m_Node = AStarNode(nID, &node, nScore, heuristic, curDir); 
				openList.Push(AStarRef(nScore, &nTile.m_Node));
				nTile.m_OpenOrClosed = OPEN;
			}
		}
	}

	m_AStarBase += 3;

	if(!goalNode) //Couldn't find a path
		return;

	//Add directions to goal to path
	path.clear();
	while(goalNode->parent)
	{
		path.push_front(goalNode->dirTo);
		goalNode = goalNode->parent;
	}
}