#include "StdAfx.h"
#include "PathFinder.h"

#include "NaviField.h"
#include <math.h>


#pragma warning( disable: 4258 )
#pragma warning( disable: 4288 )

static
const unsigned short gCostTable[8] =
{
	10, 10, 10, 10, 14, 14, 14, 14
};

static inline
float GetPosOnCatmullRomSpline( float* cp, float t )
{
	float t2 = t * t;
	float t3 = t2 * t;

	return 0.5f * (
		( 2.0f*cp[1] ) +
		( -cp[0] + cp[2] ) * t +
		( 2.0f*cp[0] - 5.0f*cp[1] + 4.0f*cp[2] - cp[3] ) * t2 +
		( -cp[0] + 3.0f*cp[1]- 3.0f*cp[2] + cp[3] ) * t3 );
}

cPathFinder::cPathFinder( unsigned int capacity )
: mCellCount( 0 )
, mLineCount( 0 )
, mCells( 0 )
, mNodes( 0 )
, mOpenQueue( capacity )
, mGoalX( 0 )
, mGoalY( 0 )
{
}

cPathFinder::~cPathFinder()
{
	Clear();
}

void cPathFinder::Clear()
{
	if( mCells )
	{
		delete [] mCells;
		mCells = 0;
	}

	if( mNodes )
	{
		for( unsigned int i = 0, iend = mLineCount * mLineCount; i < iend; ++i )
		{
			delete mNodes[i];
		}
		delete [] mNodes;
		mNodes = 0;
	}
}

void cPathFinder::Init( const cNaviField* naviField )
{
	assert( naviField );

	Clear();

	/// ±×¸®µå Å©±â¸¦ °Ë»ç
	unsigned int cellCount = naviField->GetCellCount();

	if( CheckCellCount( cellCount ) == false )
	{
		assert( 0 && "invalid navifield grid size" );
		return;
	}

	/// ±×¸®µå Å©±â ¹× ´ÜÀ§ ¼³Á¤
	mCellCount = cellCount;
	mLineCount = mCellCount + 1;

	/// ³ëµå ¹è¿­ »ý¼º ¹× ÃʱâÈ­
	unsigned int numVerts = mLineCount * mLineCount;
	mNodes = new cPathNode*[numVerts];
	mCells = new cPathNode**[mLineCount];
	::memset( mNodes, 0, sizeof(cPathNode*) * numVerts );

	for( unsigned int xi = 1; xi < mCellCount; ++xi )
	{
		for( unsigned int yi = 1; yi < mCellCount; ++yi )
		{
			if( naviField->GetValue( xi, yi ) == 0 )
			{
				mNodes[yi * mLineCount + xi] = new cPathNode;
			}
		}
	}
	for( unsigned int i = 0; i < mLineCount; ++i )
	{
		mCells[i] = &mNodes[i * mLineCount];
	}
	for( unsigned int xi = 1; xi < mCellCount; ++xi )
	{
		for( unsigned int yi = 1; yi < mCellCount; ++yi )
		{
			cPathNode* n = mCells[yi][xi];

			if( n )
			{
				n->mXIndex = (unsigned short)xi;
				n->mYIndex = (unsigned short)yi;
				n->mParent = 0;
				n->mTotalCost = 0;
				n->mCostFromStart = 0;
				n->mCostToGoal = 0;
				n->mClosed = false;

				n->mChild[0] = mCells[yi-1][xi  ];
				n->mChild[1] = mCells[yi  ][xi+1];
				n->mChild[2] = mCells[yi+1][xi  ];
				n->mChild[3] = mCells[yi  ][xi-1];
				n->mChild[4] = mCells[yi-1][xi+1];
				n->mChild[5] = mCells[yi+1][xi+1];
				n->mChild[6] = mCells[yi+1][xi-1];
				n->mChild[7] = mCells[yi-1][xi-1];
			}
		}
	}
}

bool cPathFinder::CheckCellCount( unsigned int cellCount )
{
	return ( cellCount % 512 == 0 );
}

unsigned short cPathFinder::CalcHeuristic( unsigned short x, unsigned short y )
{
	int dx = x - mGoalX;
	int dy = y - mGoalY;

	return (unsigned short)(::sqrtf(float(dx*dx + dy*dy)) * 5.0f);
}

bool cPathFinder::FindPath( NiPoint2* pathArray, unsigned int* pathCount, unsigned int maxCount, const NiPoint2& s, const NiPoint2& g )
{
	unsigned short sx = (unsigned short)((s.x + 50.0f) / 100.0f);
	unsigned short sy = (unsigned short)((s.y + 50.0f) / 100.0f);
	unsigned short gx = (unsigned short)((g.x + 50.0f) / 100.0f);
	unsigned short gy = (unsigned short)((g.y + 50.0f) / 100.0f);

	return FindPath( pathArray, pathCount, maxCount, s, g, sx, sy, gx, gy );
}

bool cPathFinder::FindPath( NiPoint2* pathArray, unsigned int* pathCount, unsigned int maxCount, const NiPoint2& s, const NiPoint2& goal, unsigned short sx, unsigned short sy, unsigned short gx, unsigned short gy )
{
	maxCount;
	assert( pathArray );
	assert( pathCount );
	assert( maxCount >= 4 && maxCount <= MAX_PATH_COUNT );
	assert( mNodes );

	if( sx == 0 || sx >= mLineCount || sy == 0 || sy >= mLineCount )
		return false;
	if( gx == 0 || gx >= mLineCount || gy == 0 || gy >= mLineCount )
		return false;

	/// ¸ñÇ¥Á¡ÀÌ À̵¿ ºÒ°¡ÀÎÁö °Ë»ç
	if( mCells[gy][gx] == 0 )
		return false;

	/// ÀÏÁ÷¼± °Ë»ç
	{
		unsigned int i = 1;
		unsigned int iend = (unsigned int)((goal-s).Length() / 100.0f);
		unsigned int x;
		unsigned int y;
		float t;
		NiPoint2 pos;

		if( iend == 0 )
		{
			x = (unsigned int)((goal.x + 50.0f) / 100.0f);
			y = (unsigned int)((goal.y + 50.0f) / 100.0f);

			if( mCells[y][x] )
			{
				pathArray[0].x = goal.x;
				pathArray[0].y = goal.y;
				*pathCount = 1;
				return true;
			}
		}
		else
		{
			for( ; i <= iend; ++i )
			{
				t = float(i) / float(iend);
				pos = (1.0f-t)*s + t*goal;
				x = (unsigned int)((pos.x + 50.0f) / 100.0f);
				y = (unsigned int)((pos.y + 50.0f) / 100.0f);

				if( mCells[y][x] == 0 )
					break;
			}

			if( i > iend )
			{
				pathArray[0].x = goal.x;
				pathArray[0].y = goal.y;
				*pathCount = 1;
				return true;
			}
		}
	}

	/// ±æã±â Áغñ
	mGoalX = gx;
	mGoalY = gy;
	mOpenQueue.Clear();

	for( unsigned int x = 1; x < mCellCount; ++x )
	{
		for( unsigned int y = 1; y < mCellCount; ++y )
		{
			cPathNode* n = mCells[y][x];

			if( n )
			{
				n->mParent = 0;
				n->mClosed = false;
			}
		}
	}

	/// ¿­¸° Å¥¿¡ ½ÃÀÛ ³ëµå¸¦ Ãß°¡
	bool failed = false;
	cPathNode* node = 0;
	cPathNode* start = mCells[sy][sx];

	if( start == 0 )
	{
		cPathNode* n = start = &mTempNode;
		n->mXIndex = (unsigned short)sx;
		n->mYIndex = (unsigned short)sy;
		n->mParent = 0;
		n->mCostFromStart = 0;
		n->mTotalCost = n->mCostToGoal = CalcHeuristic( sx, sy );
		n->mClosed = false;

		n->mChild[0] = mCells[sy-1][sx  ];
		n->mChild[1] = mCells[sy  ][sx+1];
		n->mChild[2] = mCells[sy+1][sx  ];
		n->mChild[3] = mCells[sy  ][sx-1];
		n->mChild[4] = mCells[sy-1][sx+1];
		n->mChild[5] = mCells[sy+1][sx+1];
		n->mChild[6] = mCells[sy+1][sx-1];
		n->mChild[7] = mCells[sy-1][sx-1];
	}
	else
	{
		cPathNode* n = start;
		n->mParent = 0;
		n->mCostFromStart = 0;
		n->mTotalCost = n->mCostToGoal = CalcHeuristic( sx, sy );
		n->mClosed = false;
	}

	mOpenQueue.Push( start );

	while( !failed )
	{
		if( mOpenQueue.GetCount() == 0 )
		{
			failed = true;
			break;
		}

		/// ÃÖ¿ì¼± ¼øÀ§ ³ëµå¸¦ °¡Á®¿È
		node = (cPathNode*)mOpenQueue.Front();
		mOpenQueue.PopFront();

		if( node->mClosed )
			continue;
		else
			node->mClosed = true;

		/// ÇöÀç ³ëµå°¡ ¸ñÇ¥¶ó¸é ±æã±â ¼º°ø!
		if( node->mXIndex == gx && node->mYIndex == gy )
			break;

		/// ÀÎÁ¢ ³ëµåµé(Àڽĵé)¿¡ ´ëÇØ
		for( unsigned int i = 0; i < 8; ++i )
		{
			/// ÀÚ½ÄÀÌ ÀÌ¹Ì ´ÝÇôÀְųª Áö³ª°¥ ¼ö ¾ø´ÂÁö °Ë»ç
			cPathNode* child = node->mChild[i];

			if( child == 0 || child->mClosed )
				continue;

			/// ºñ¿ëÀ» °è»ê
			unsigned short g = node->mCostFromStart + gCostTable[i];

			if( child->mParent == 0 )
			{
				/// ¿­¸° ¸ñ·Ï¿¡ ¾øÀ¸¸é
				unsigned short h = CalcHeuristic( child->mXIndex, child->mYIndex );

				child->mParent = node;
				child->mTotalCost = g + h;
				child->mCostFromStart = g;
				child->mCostToGoal = h;

				/// ¿­¸° ¸ñ·Ï¿¡ Ãß°¡
				if( mOpenQueue.Push( child ) == false )
				{
					failed = true;
					break;
				}
			}
			else if( g < child->mCostFromStart )
			{
				/// ¿­¸° ¸ñ·Ï¿¡ ÀÌ¹Ì ÀÖ°í, ºñ¿ëÀÌ ÀÌÀüº¸´Ù ´õ ½Î¸é
				child->mParent = node;
				child->mTotalCost = g + child->mCostToGoal;
				child->mCostFromStart = g;

				/// ¿­¸° ¸ñ·Ï¿¡ Ãß°¡
				if( mOpenQueue.Push( child ) == false )
				{
					failed = true;
					break;
				}
			}
		}
	}

	if( failed )
	{
		return false;
	}
	if( node == 0 || node == start )
	{
		return false;
	}

	/// ±æã±â ¿Ï·á ÀÛ¾÷
	node->mNext = 0;
	cPathNode* temp = node;
	cPathNode* parent = temp->mParent;

	while( parent )
	{
		parent->mNext = temp;
		temp = parent;
		parent = temp->mParent;
	}

	/// ±æã±â À̵¿ °æ·Î¸¦ º¸°£
	static NiPoint2 posArray0[MAX_PATH_COUNT];
	static NiPoint2 posArray1[MAX_PATH_COUNT];
	static float valArray[MAX_PATH_COUNT];
	unsigned int c = 0;

	cPathNode* checkPoint = start;
	cPathNode* smoothTemp = 0;
	for( temp = start; c < MAX_PATH_COUNT; )
	{
		//*
		if( smoothTemp )
		{
			NiPoint2 sPos(checkPoint->mXIndex * 100.0f, checkPoint->mYIndex * 100.0f);
			NiPoint2 ePos(smoothTemp->mXIndex * 100.0f, smoothTemp->mYIndex * 100.0f);

			unsigned int i = 1;
			unsigned int iend = (unsigned int)((ePos-sPos).Length() / 100.0f);
			unsigned int x;
			unsigned int y;
			float t;
			NiPoint2 pos;

			if( iend == 0 )
			{
				x = (unsigned int)((ePos.x + 50.0f) / 100.0f);
				y = (unsigned int)((ePos.y + 50.0f) / 100.0f);

				if( mCells[y][x] )
				{
					temp = temp->mNext;
					if( temp == 0 )
						break;

					smoothTemp = temp->mNext;
					continue;
				}
			}
			else
			{
				for( ; i <= iend; ++i )
				{
					t = float(i) / float(iend);
					pos = (1.0f-t)*sPos + t*ePos;
					x = (unsigned int)((pos.x + 50.0f) / 100.0f);
					y = (unsigned int)((pos.y + 50.0f) / 100.0f);

					if( mCells[y][x] == 0 )
						break;
				}

				///
				if( i > iend )
				{
					temp = temp->mNext;
					if( temp == 0 )
						break;

					smoothTemp = temp->mNext;
					continue;
				}
				else
				{
					checkPoint = temp;
				}
			}
		}

		//*/
		posArray0[c].x = temp->mXIndex * 100.0f;
		posArray0[c].y = temp->mYIndex * 100.0f;
		++c;

		temp = temp->mNext;
		if( temp == 0 )
			break;

		smoothTemp = temp->mNext;
	}
	{
		float x = node->mXIndex * 100.0f;
		float y = node->mYIndex * 100.0f;

		if( posArray0[c-1].x != x || posArray0[c-1].y != y )
		{
			posArray0[c].x = x;
			posArray0[c].y = y;
			++c;
		}
	}


	{
		unsigned int iend = c;

		unsigned int c = 0;
		unsigned int i = 1;

		for( ; i < iend; ++i, ++c )
		{
			pathArray[c] = posArray0[i];
		}

		if( failed == false && c > 0 && (pathArray[c-1].x != goal.x || pathArray[c-1].y != goal.y) )
		{
			pathArray[c-1].x = goal.x;
			pathArray[c-1].y = goal.y;
		}

		*pathCount = c;
		assert( c <= maxCount );
	}

	/*
	///
	unsigned int iend = c;

	if( iend < 4 )
	{
	unsigned int c = 0;
	unsigned int i = 1;

	for( ; i < iend; ++i, ++c )
	{
	pathArray[c] = posArray0[i];
	}

	if( failed == false && c > 0 && (pathArray[c-1].x != goal.x || pathArray[c-1].y != goal.y) )
	{
	pathArray[c-1].x = goal.x;
	pathArray[c-1].y = goal.y;
	}

	*pathCount = c;
	assert( c <= maxCount );
	}
	else
	{
	float cx[4];
	float cy[4];
	unsigned int c = 0;
	unsigned int i = 0;

	for( ; i < iend-3; ++i )
	{
	cx[0] = posArray0[i  ].x;
	cx[1] = posArray0[i+1].x;
	cx[2] = posArray0[i+2].x;
	cx[3] = posArray0[i+3].x;
	cy[0] = posArray0[i  ].y;
	cy[1] = posArray0[i+1].y;
	cy[2] = posArray0[i+2].y;
	cy[3] = posArray0[i+3].y;

	posArray1[c].x = GetPosOnCatmullRomSpline( cx, 0.0f );
	posArray1[c].y = GetPosOnCatmullRomSpline( cy, 0.0f );
	if( ++c >= MAX_PATH_COUNT )
	break;

	posArray1[c].x = GetPosOnCatmullRomSpline( cx, 0.25f );
	posArray1[c].y = GetPosOnCatmullRomSpline( cy, 0.25f );
	if( ++c >= MAX_PATH_COUNT )
	break;

	posArray1[c].x = GetPosOnCatmullRomSpline( cx, 0.5f );
	posArray1[c].y = GetPosOnCatmullRomSpline( cy, 0.5f );
	if( ++c >= MAX_PATH_COUNT )
	break;

	posArray1[c].x = GetPosOnCatmullRomSpline( cx, 0.75f );
	posArray1[c].y = GetPosOnCatmullRomSpline( cy, 0.75f );
	if( ++c >= MAX_PATH_COUNT )
	break;
	}

	for( ++i; i < iend && c < MAX_PATH_COUNT; ++i, ++c )
	{
	posArray1[c] = posArray0[i];
	}

	if( failed == false && c > 0 && c < MAX_PATH_COUNT && (posArray1[c-1].x != goal.x || posArray1[c-1].y != goal.y) )
	{
	posArray1[c-1].x = goal.x;
	posArray1[c-1].y = goal.y;
	}

	/// °æ·Î ÃÖ¼ÒÈ­
	assert( c <= MAX_PATH_COUNT );
	iend = c;
	i = 1;

	for( ; i < iend; ++i )
	{
	NiPoint2& pos0 = posArray1[i-1];
	NiPoint2& pos1 = posArray1[i];
	float dx = pos1.x - pos0.x;

	if( dx )
	valArray[i] = (pos1.y - pos0.y) / dx;
	else
	valArray[i] = 10000.0f;
	}

	///
	iend = iend-1;
	c = 1;
	i = 1;

	for( ; i < iend; ++i )
	{
	if( NiAbs(valArray[i] - valArray[i+1]) > 0.06f )
	{
	pathArray[c] = posArray1[i];

	if( ++c >= maxCount-1 )
	break;
	}
	}
	pathArray[0] = posArray1[0];
	pathArray[c] = posArray1[i];
	++c;

	*pathCount = c;
	assert( c <= maxCount );
	}
	//*/
	return true;
}

bool cPathFinder::IsPossible( NiPoint2 start, NiPoint2 goal )
{
	unsigned int i = 1;
	unsigned int iend = (unsigned int)((goal-start).Length() / 100.0f );
	unsigned int x, y;
	float t;
	NiPoint2 pos;

	if( iend == 0 )
	{
		x = (unsigned int)((goal.x + 50.0f) / 100.0f);
		y = (unsigned int)((goal.y + 50.0f) / 100.0f);

		if( mCells[y][x] )
			return true;
	}
	else
	{
		for( ; i <= iend; ++i )
		{
			t = float(i) / float(iend);
			pos = (1.0f-t)*start + t*goal;
			x = (unsigned int)((pos.x + 50.0f) / 100.0f);
			y = (unsigned int)((pos.y + 50.0f) / 100.0f);

			if( mCells[y][x] == 0 )
				break;
		}

		///
		if( i > iend )
			return true;
	}

	return false;
}