//!
/**
 **************************************************************************************************************************************************************
 * A* path-finding implementation
 **************************************************************************************************************************************************************
 * @file   AStar.h
 * @author Guillaume Blanchard
 **************************************************************************************************************************************************************/

/**************************************************************************************************************************************************************
 * Includes
 **************************************************************************************************************************************************************/

// BotStrike
#include "AStar.h"

namespace BS {

/**************************************************************************************************************************************************************
 * Implementation
 **************************************************************************************************************************************************************/

/**
 * Constructor
 */
CAStar::CAStar(): m_TargetX(0), m_TargetY(0), m_TargetDX(0), m_TargetDY(0), m_CloseNumber(0), m_Arena(NULL)
{
}

/**
 * Initialize the A* data structure
 * @param	arena	The arena data information.
 */
void CAStar::Initialize(const BS::CArena* arena)
{
	m_TargetX = 0;
	m_TargetY = 0;
	m_TargetDX = 0;
	m_TargetDY = 0;
	m_CloseNumber = 0;
	m_Arena = arena;
	
	// Initialize the open list
	m_OpenList.SetCapacity(m_Arena->width * m_Arena->height);
	m_OpenList.Clear();

	// Initialize the cells
	m_NodeList.resize(m_Arena->width * m_Arena->height);
	loop(y, m_Arena->height)
		loop(x, m_Arena->width)
			m_NodeList[y * m_Arena->width + x].Reset(x, y);
}

/**
 * Compare two nodes and check if the first one is bigger than second one.
 * @param	n1		The first node.
 * @param	n2		The second node.
 * @return			TRUE if 'n1' is bigger than 'n2'.
 */
bool CAStar::IsBigger(const CNode* n1, const CNode* n2)
{
	return n1->ComputeCost() > n2->ComputeCost();
}

/**
 * Check if a given coordinate point to a valid cell node
 * @param	x		The x coordinate.
 * @param	y		The x coordinate.
 * @return			TRUE if the coordinate point to a valid cell.
 */
bool CAStar::IsValidNode(i32 x, i32 y) const
{
	return !(x < 0 || y < 0 || x >= m_Arena->width || y >= m_Arena->height);
}

/**
 * Get the lowset (cheaper) node from the open list.
 * @return			The lower node.
 */
CNode* CAStar::PickLowestNode()
{
	CNode* node = m_OpenList.GetRoot().Addr;
	m_OpenList.Remove();
	return node;
}

/**
 * Compute distance-to-goal cost using manathan algorithm [cost = dx + dy]
 * @param	node	The node to check.
 * @return			The computed distance-to-goal cost.
 */
f32 CAStar::DistanceToGoalManathan(CNode* node)
{
	return scast<f32>(abs(node->X - Instance().m_TargetX) + abs(node->Y - Instance().m_TargetY));
}

/**
 * Compute distance-to-goal cost in step unit.
 * While diagonal move have same cost than horizontal/vetical ones, just get the maximum value between dx and dy [cost = max(dx, dy)]
 * @param	node	The node to check.
 * @return			The computed distance-to-goal cost.
 */
f32 CAStar::DistanceToGoal(CNode* node)
{
	return scast<f32>(noe::Max(abs(node->X - Instance().m_TargetX), abs(node->Y - Instance().m_TargetY)));
}

/**
 * Compute distance-to-goal cost with some trick to incite bot to go behind its target.
 * @param	node	The node to check.
 * @return			The computed distance-to-goal cost.
 */
f32 CAStar::DistanceToBehindGoal(CNode* node)
{
	i32 dist = scast<i32>(DistanceToGoal(node));
	
	// try to get behind ennemy
	i32 dx = node->X - Instance().m_TargetX;
	i32 dy = node->Y - Instance().m_TargetY;
	if((dx > 0 && Instance().m_TargetDX < 0) || (dx < 0 && Instance().m_TargetDX > 0))
		dist <<= 1;
	if((dy > 0 && Instance().m_TargetDY > 0) || (dy < 0 && Instance().m_TargetDY < 0))
		dist >>= 1;
	if((dx > 0 && Instance().m_TargetDX < 0) || (dx < 0 && Instance().m_TargetDX > 0))
		dist <<= 1;
	if((dy > 0 && Instance().m_TargetDY > 0) || (dy < 0 && Instance().m_TargetDY < 0))
		dist >>= 1;

	return scast<f32>(dist);
}

/**
 * Add a node to the open list. Or replace it if new path is cheaper than previous one.
 * @param	node	The node to add.
 * @param	parent	The node parent.
 */
void CAStar::AddToOpenList(CNode* node, CNode* parent)
{
	if(node->Flag == CNode::FLAG_FREE)
	{
		node->Parent = parent;
		node->CostToStart = parent ? parent->CostToStart + 10.f : 0.f;
		node->CostToGoal = 10.f * m_CostToGoal(node);
		node->Flag = CNode::FLAG_OPEN;
		m_OpenList.Insert(node);
	}
	else if(node->Flag == CNode::FLAG_OPEN)
	{
		f32 cost = parent ? parent->CostToStart + 10.f : 0.f;
		if(cost < node->CostToStart)
		{
			node->Parent = parent;
			node->CostToStart = cost;
		}
	}
	// else if(node->Flag == CNode::FLAG_CLOSE) // do nothing
}

/**
 * Add a node to the close list.
 * @param	node	The node to add.
 */
void CAStar::AddToCloseList(CNode* node)
{
	if(node->Flag != CNode::FLAG_CLOSE)
	{
		node->Flag = CNode::FLAG_CLOSE;
		m_CloseNumber++;
	}
}

/**
 * Process path-finding.
 * @param	path		Result path.
 * @param	startX		Start position.
 * @param	startY		Start position.
 * @param	targetX		Target position.
 * @param	targetY		Target position.
 * @param	targetDX	Target orientation.
 * @param	targetDY	Target orientation.
 * @param	dist		How close to the target must be the path.
 * @param	func		Cost-to-goal computation function pointer.
 */
BOOL CAStar::FindPath(std::vector<CNode*>& path, i32 startX, i32 startY, i32 targetX, i32 targetY, i32 targetDX, i32 targetDY, i32 dist, CostToGoalFunc func)
{
	static int dir[8][2] = { {-1,-1}, {0,-1}, {1,-1}, {1,0}, {1,1}, {0,1}, {-1,1}, {-1,0} };

	m_TargetX = targetX;
	m_TargetY = targetY;
	m_TargetDX = targetDX;
	m_TargetDY = targetDY;
	m_CostToGoal = func;

	i32 x = startX;
	i32 y = startY;
	
	CNode* node = &m_NodeList[y * m_Arena->width + x];
	AddToOpenList(node, NULL);

	path.clear();

	for( ;; )
	{
		if(m_OpenList.IsEmpty())
			return FALSE;

		node = PickLowestNode();

		if((node->X == targetX) && (node->Y == targetY))
			x = 0;

		if(DistanceToGoal(node) < dist)
			break;
	
		loop(i, 8)
		{
			x = node->X + dir[i][0];
			y = node->Y + dir[i][1];
			if(IsValidNode(x, y))
			{
				if((m_Arena->cell(x, y) & BS::ECell::Box)
				|| (m_Arena->cell(x, y) & BS::ECell::Blue)
				|| (m_Arena->cell(x, y) & BS::ECell::Red)
				|| (m_Arena->cell(x, y) & BS::ECell::Grenade))
					continue;

				CNode* next = &m_NodeList[y * m_Arena->width + x];
				AddToOpenList(next, node); // add to open list
			}
		}

		AddToCloseList(node); // finish with this node: put it into the close list

		if(m_OpenList.GetSize() + m_CloseNumber >= scast<i32>(m_NodeList.size())) // no more free cells
			return FALSE;
	}

	// Compute the result path
	while(node->Parent != NULL)
	{
		path.push_back(node);
		node = node->Parent;
	}
	
	return TRUE;
}

/**************************************************************************************************************************************************************/

} // namespace BS