#include "StdAfx.h"
#include "PathFinder.h"
#include "Map/Map.h"
#include "ClientApp.h"
#include "ObjectManager.h"

CPathFinder::CPathFinder()
:m_PathType(Smooth)
,m_pkOpen(NULL)
,m_pkClosed(NULL)
,m_pkBest(NULL)
{
}

CPathFinder::~CPathFinder()
{
	ClearNodes();
}

void CPathFinder::ClearNodes()
{
	AStarNode* pkTemp = NULL;

	if(m_pkOpen)
	{
		while(m_pkOpen)
		{
			pkTemp = m_pkOpen->GetNextNode();

			delete m_pkOpen;

			m_pkOpen = pkTemp;
		}
	}
	m_pkOpen = NULL;

	if(m_pkClosed)
	{
		while(m_pkClosed)
		{
			pkTemp = m_pkClosed->GetNextNode();

			delete m_pkClosed;

			m_pkClosed = pkTemp;
		}
	}
	m_pkClosed = NULL;
}

int CPathFinder::GetDir(NiPoint3 k0, NiPoint3 k1)
{
	if(k0.x == k1.x && k0.y < k1.y)			// 上
		return 0;
	else if(k0.x < k1.x && k0.y < k1.y)		// 右上
		return 1;
	else if(k0.x < k1.x && k0.y == k1.y)	// 右
		return 2;
	else if(k0.x < k1.x && k0.y > k1.y)		// 右下
		return 3;
	else if(k0.x == k1.x && k0.y > k1.y)	// 下
		return 4;
	else if(k0.x > k1.x && k0.y > k1.y)		// 左下
		return 5;
	else if(k0.x > k1.x && k0.y == k1.y)	// 左
		return 6;
	else
		return 7;
}

void CPathFinder::CheckDestValid(int iSX, int iSY, int& iDX, int& iDY)
{
	if(!CMap::Get()->GetGridInfo((float)iDX, (float)iDY))
	{
		int iDir = GetDir(NiPoint3((float)iDX, (float)iDY, 0.0f), NiPoint3((float)iSX, (float)iSY, 0.0f));
		switch(iDir)
		{
		case 0:
			iDY++;
			break;
		case 1:
			iDX++; iDY++;
			break;
		case 2:
			iDX++;
			break;
		case 3:
			iDX++; iDY--;
			break;
		case 4:
			iDY--;
			break;
		case 5:
			iDX--; iDY--;
			break;
		case 6:
			iDX--;
			break;
		case 7:
			iDX--; iDY++;
			break;

		default:
			NIASSERT(0);
			break;
		}

		CheckDestValid(iSX, iSY, iDX, iDY);
	}
	else
	{
		return;
	}
}

bool Walkable(NiPoint3 kCheckPoint, AStarNode* pkNode)
{
	NiPoint3 kNextPoint = NiPoint3((float)pkNode->GetNodeX(), (float)pkNode->GetNodeY(), 0.0f);
	
	if(pkNode->GetNodeX() - kCheckPoint.x == 0)
	{
		;
	}
	else if(pkNode->GetNodeY() - kCheckPoint.y == 0)
	{
		;
	}
	else
	{
		float k = ((float)(pkNode->GetNodeX() - kCheckPoint.x)) / ((float)(pkNode->GetNodeY() - kCheckPoint.y));

		int iStepX = abs(pkNode->GetNodeX() - (int)kCheckPoint.x);
		int iStepY = abs(pkNode->GetNodeY() - (int)kCheckPoint.y);
		if(iStepX >= iStepY)
		{
			int iDir = (pkNode->GetNodeX() - (int)kCheckPoint.x) / abs(pkNode->GetNodeX() - (int)kCheckPoint.x);
			int i = 0;
			while(kCheckPoint.x + i * iDir != pkNode->GetNodeX())
			{
				if(!CMap::Get()->GetGridInfo((float)kCheckPoint.x + i * iDir, (float)kCheckPoint.y + i * iDir / k))
					return false;
				i++;
			}
		}
		else
		{
			int iDir = (pkNode->GetNodeY() - (int)kCheckPoint.y) / abs(pkNode->GetNodeY() - (int)kCheckPoint.y);
			int i = 0;
			while(kCheckPoint.y + i * iDir != pkNode->GetNodeY())
			{
				if(!CMap::Get()->GetGridInfo((float)kCheckPoint.x + i * iDir * k, (float)kCheckPoint.y + i * iDir))
					return false;
				i++;
			}
		}
	}
	return true;
}

bool CPathFinder::GeneratePathWP(int iSX, int iSY, int iSZ, int iDX, int iDY, int iDZ)
{
	//CheckDestValid(iSX, iSY, iDX, iDY);
	int iRetval = 0;
	float fLastTime = NiGetCurrentTimeInSec();
	CPathNode* pkPath = CPathNodeManager::Get()->GetNearestPathNode(NiPoint3((float)iSX, (float)iSY, (float)iSZ));
	if (!pkPath)
		return false;

	StepInitializeWP((int)pkPath->m_kTranslate.x, (int)pkPath->m_kTranslate.y, (int)pkPath->m_kTranslate.z, iDX, iDY, iDZ);
	m_pkOpen->SetPathNode(pkPath);
	int i = 0;
	if(pkPath)
	{
		while(iRetval == 0)
		{
			iRetval = StepWP(pkPath);
			//强制退出
			if((NiGetCurrentTimeInSec() - fLastTime) > 1.2f)
			{
				assert(0);
				iRetval = -1;
			}
			i++;

			// 超过300强制退出
			//if(i > 300)
			//{
			//	iRetval = 1;
			//}
		}
	}

	ULOG("计算寻路点时间 %.1f", NiGetCurrentTimeInSec() - fLastTime);
	if(iRetval == -1 || !m_pkBest)
	{
		m_pkBest = NULL;
		m_PathPoint.push_back(NiPoint3((float)iDX, (float)iDY, (float)iDZ));
		return false;
	}

	ProcessPathNormal();

	// 暂时这么写
	if(m_PathPoint.size() == 0)
	{
		m_PathPoint.push_back(NiPoint3((float)iDX, (float)iDY, (float)iDZ));
	}

	return true;
}

bool CPathFinder::GeneratePath(int iSX, int iSY,int iSZ, int iDX, int iDY, int iDZ)
{
	// Check
	CheckDestValid(iSX, iSY, iDX, iDY);

	StepInitialize(iSX, iSY, iDX, iDY);

	int iRetval = 0;
	float fLastTime = NiGetCurrentTimeInSec();
	while(iRetval == 0)
	{
		iRetval = Step();
		//强制退出
		if(NiGetCurrentTimeInSec() - fLastTime > 1.2f)
			iRetval = -1;
	}

	if(iRetval == -1 || !m_pkBest)
	{
		m_pkBest = NULL;
		m_PathPoint.push_back(NiPoint3((float)iDX, (float)iDY, CMap::Get()->GetHeight((float)iDX, (float)iDY)));
		return false;
	}

	ProcessPathSmooth();
	
	return true;
}

void CPathFinder::ProcessPath()
{
	switch(m_PathType)
	{
	case Normal:
		ProcessPathNormal();
		break;
	case Straight:
		ProcessPathStraight();
		break;
	case Smooth:
		ProcessPathSmooth();
		break;
	}
}

void CPathFinder::ProcessPathNormal()
{
	m_PathPoint.clear();

	AStarNode* pkNode = m_pkBest;
	while(pkNode)
	{
		if(pkNode->GetNodeX() == m_iSX && pkNode->GetNodeY() == m_iSY)
		{
			break;
		}
		else
		{
			m_PathPoint.push_front(NiPoint3((float)pkNode->GetNodeX(), (float)pkNode->GetNodeY(), CMap::Get()->GetHeight((float)pkNode->GetNodeX(), (float)pkNode->GetNodeY())));
		}
		pkNode = pkNode->GetParentNode();
	}
}

void CPathFinder::ProcessPathStraight()
{
	ProcessPathNormal();

	if(m_PathPoint.size() > 2)
	{
		deque<NiPoint3> kTemp;
		for(unsigned int ui = 0; ui < m_PathPoint.size(); ui++)
		{
			kTemp.push_back(m_PathPoint[ui]);
		}
		m_PathPoint.clear();

		int iDir = GetDir(NiPoint3((float)m_iSX, (float)m_iSY, 0.0f), kTemp[0]);
		int iTestDir = -1;

		for(unsigned int ui = 0; ui < kTemp.size() - 1; ui++)
		{
			iTestDir = GetDir(kTemp[ui], kTemp[ui + 1]);
			if(iDir == iTestDir)
				continue;

			iDir = iTestDir;
			m_PathPoint.push_back(kTemp[ui]);
		}
		m_PathPoint.push_back(kTemp[kTemp.size() - 1]);
	}
}

void CPathFinder::ProcessPathSmooth()
{
	m_PathPoint.clear();
	NiPoint3 kCheckPoint = NiPoint3((float)m_pkBest->GetNodeX(), (float)m_pkBest->GetNodeY(), 0.0f);
	AStarNode* pkNode = m_pkBest->GetParentNode();
	while(pkNode != NULL && pkNode->GetParentNode() != NULL)
	{
		if(Walkable(kCheckPoint, pkNode->GetParentNode()))
		{
			pkNode = pkNode->GetParentNode();
		}
		else
		{
			kCheckPoint = NiPoint3((float)pkNode->GetNodeX(), (float)pkNode->GetNodeY(), 0.0f);
			m_PathPoint.push_front(NiPoint3((float)pkNode->GetNodeX(), (float)pkNode->GetNodeY(), CMap::Get()->GetHeight((float)pkNode->GetNodeX(), (float)pkNode->GetNodeY())));
			pkNode = pkNode->GetParentNode();
		}
	}
	m_PathPoint.push_back(NiPoint3((float)m_iDX, (float)m_iDY, CMap::Get()->GetHeight((float)m_iDX, (float)m_iDY)));
}

void CPathFinder::StepInitializeWP(int iSX, int iSY, int iSZ, int iDX, int iDY, int iDZ)
{
	ClearNodes();

	m_iSX = iSX; m_iSY = iSY; m_iDX = iDX; m_iDY = iDY; m_iSZ = iSZ; m_iDZ = iDZ;

	AStarNode* pkTemp = new AStarNode(iSX, iSY, iSZ);
	pkTemp->SetGValue(0.0f);
	pkTemp->SetHValue(float((iSX - iDX) + (iSY - iDY)));
	pkTemp->UpdateFValue();

	m_pkOpen = pkTemp;	
}

void CPathFinder::StepInitialize(int iSX, int iSY, int iDX, int iDY)
{
	ClearNodes();

	m_iSX = iSX; m_iSY = iSY; m_iDX = iDX; m_iDY = iDY;

	AStarNode* pkTemp = new AStarNode(iSX, iSY);
	pkTemp->SetGValue(0.0f);
	pkTemp->SetHValue(float(abs(iSX - iDX) + abs(iSY - iDY)));
	pkTemp->UpdateFValue();

	m_pkOpen = pkTemp;
}

int CPathFinder::StepWP(CPathNode* pkPathNode)
{
	if(!(m_pkBest = GetBest()))
		return -1;

	if(m_pkBest->GetNodeX() == m_iDX && m_pkBest->GetNodeY() == m_iDY)
		return 1;

	CreateChildrenWP(m_pkBest, pkPathNode);

	return 0;
}

int CPathFinder::Step()
{
	if(!(m_pkBest = GetBest()))
		return -1;

	if(m_pkBest->GetNodeX() == m_iDX && m_pkBest->GetNodeY() == m_iDY)
		return 1;

	CreateChildren(m_pkBest);

	return 0;
}
bool CPathFinder::GetPathPoint(std::deque<NiPoint3>& PathPoint)
{
	if (m_PathPoint.size())
	{
		for(unsigned int ui = 0; ui < m_PathPoint.size(); ui++)
		{
			PathPoint.push_back(m_PathPoint[ui]);
		}
		return true;
	}
	return false;
}
NiPoint3 CPathFinder::GetNextPathPoint()
{
	NiPoint3 kPathPoint;
	if(m_PathPoint.size())
	{
		kPathPoint = m_PathPoint[0];
		m_PathPoint.pop_front();
	}
	else
	{
		kPathPoint = ObjectMgr->GetLocalPlayer()->GetPosition();
	}

	return kPathPoint;
}

void CPathFinder::ClearPathPoint()
{
	m_PathPoint.clear();
}

bool CPathFinder::IsPathPointEmpty()
{
	return m_PathPoint.empty();
}

AStarNode* CPathFinder::GetBest()
{
	if(!m_pkOpen)
		return NULL;

	AStarNode* pkTemp = m_pkOpen;
	AStarNode* pkTemp2 = m_pkClosed;

	m_pkOpen = pkTemp->GetNextNode();

	m_pkClosed = pkTemp;
	m_pkClosed->SetNextNode(pkTemp2);

	return pkTemp;
}

void CPathFinder::CreateChildrenWP(AStarNode* pkNode, CPathNode* pkPathNode)
{
	AStarNode kTemp;
	int iX = pkNode->GetNodeX();
	int iY = pkNode->GetNodeY();
	int iZ = pkNode->GetNodeZ();

	//CPathNode* pkPath = CPathNodeManager::Get()->GetNearestPathNode(NiPoint3((float)iX, (float)iY, (float)iZ));
	CPathNode* pkPath = pkNode->GetPathNode();
	if(!pkPath)
		return;
	for(unsigned int ui = 0; ui < pkPath->m_uiLinkCount; ui++)
	{
		//通过ID 获取PathNode
		float fLength = -FLT_MAX;
		CPathNode* pkNodeIndex = CPathNodeManager::Get()->GetLinkPathNode(pkPath, ui, fLength);
		kTemp.SetNodeX((int)pkNodeIndex->m_kTranslate.x);
		kTemp.SetNodeY((int)pkNodeIndex->m_kTranslate.y);
		kTemp.SetNodeZ((int)pkNodeIndex->m_kTranslate.z);

		LinkChildWP(pkNode, &kTemp, fLength, pkNodeIndex);
	}
}

void CPathFinder::CreateChildren(AStarNode* pkNode)
{
	AStarNode kTemp;
	int iX = pkNode->GetNodeX();
	int iY = pkNode->GetNodeY();

	for(int y = -1; y < 2; y++)
	{
		for(int x = -1; x < 2; x++)
		{
			kTemp.SetNodeX(iX + x);
			kTemp.SetNodeY(iY + y);
			if(x == 0 && y == 0)
				continue;

			LinkChild(pkNode, &kTemp);
		}
	}
}

void CPathFinder::LinkChildWP(AStarNode* pkNode, AStarNode* pkTemp, float fLength, CPathNode* pkPath)
{
	int iX = pkTemp->GetNodeX();
	int iY = pkTemp->GetNodeY();
	int iZ = pkTemp->GetNodeZ();

	float fG = pkNode->GetG() + fLength;//(NiPoint3(iX, iY, iZ) - NiPoint3(pkNode->GetNodeX(), pkNode->GetNodeY(), pkNode->GetNodeZ())).Length();

	AStarNode* pkCheck = NULL;
	if(pkCheck = CheckList(m_pkOpen, iX, iY))
	{
		pkNode->AddChild(pkCheck);

		if(fG < pkCheck->GetG())
		{
			pkCheck->SetParentNode(pkNode);
			pkCheck->SetGValue(fG);
			pkCheck->UpdateFValue();
		}
	}
	else if(pkCheck = CheckList(m_pkClosed, iX, iY))
	{
		pkNode->AddChild(pkCheck);

		if(fG < pkCheck->GetG())
		{
			pkCheck->SetParentNode(pkNode);
			pkCheck->SetGValue(fG);
			pkCheck->UpdateFValue();
		}
	}
	else
	{
		AStarNode* pkNewNode = new AStarNode(iX, iY);

		pkNewNode->SetParentNode(pkNode);
		pkNewNode->SetGValue(fG);
		pkNewNode->SetHValue(float((iX - m_iDX) + (iY - m_iDY)));
		pkNewNode->UpdateFValue();
		pkNewNode->SetPathNode(pkPath);

		AddToOpen(pkNewNode);

		pkNode->AddChild(pkNewNode);
	}
}

void CPathFinder::LinkChild(AStarNode* pkNode, AStarNode* pkTemp)
{
	int iX = pkTemp->GetNodeX();
	int iY = pkTemp->GetNodeY();

	// 即使无法达到，也需要加入该点，否则会进入死循环。。。。。
	if(!CMap::Get()->GetGridInfo((float)iX, (float)iY))
		return;

	float fG = pkNode->GetG() + (2.0f - CMap::Get()->GetGridInfo((float)iX, (float)iY));

	AStarNode* pkCheck = NULL;
	if(pkCheck = CheckList(m_pkOpen, iX, iY))
	{
		pkNode->AddChild(pkCheck);

		if(fG < pkCheck->GetG())
		{
			pkCheck->SetParentNode(pkNode);
			pkCheck->SetGValue(fG);
			pkCheck->UpdateFValue();
		}
	}
	else if(pkCheck = CheckList(m_pkClosed, iX, iY))
	{
		pkNode->AddChild(pkCheck);

		if(fG < pkCheck->GetG())
		{
			pkCheck->SetParentNode(pkNode);
			pkCheck->SetGValue(fG);
			pkCheck->UpdateFValue();
		}
	}
	else
	{
		AStarNode* pkNewNode = new AStarNode(iX, iY);

		pkNewNode->SetParentNode(pkNode);
		pkNewNode->SetGValue(fG);
		pkNewNode->SetHValue(float((iX - m_iDX) + (iY - m_iDY)));
		pkNewNode->UpdateFValue();

		AddToOpen(pkNewNode);

		pkNode->AddChild(pkNewNode);
	}
}

AStarNode* CPathFinder::CheckList(AStarNode* pkNode, int iX, int iY)
{
	while(pkNode)
	{
		if(pkNode->GetNodeX() == iX && pkNode->GetNodeY() == iY)
			return pkNode;

		pkNode = pkNode->GetNextNode();
	}

	return NULL;
}

void CPathFinder::AddToOpen(AStarNode* pkAddNode)
{
	AStarNode* pkNode = m_pkOpen;
	AStarNode* pkPrev = NULL;

	if(!m_pkOpen)
	{
		m_pkOpen = pkAddNode;
		m_pkOpen->SetNextNode(NULL);

		return;
	}

	while(pkNode)
	{
		if(pkAddNode->GetF() > pkNode->GetF())
		{
			pkPrev = pkNode;
			pkNode = pkNode->GetNextNode();
		}
		else
		{
			if(pkPrev)
			{
				pkPrev->SetNextNode(pkAddNode);
				pkAddNode->SetNextNode(pkNode);
			}
			else
			{
				AStarNode* pkTemp = m_pkOpen;

				m_pkOpen = pkAddNode;
				m_pkOpen->SetNextNode(pkTemp);
			}

			return;
		}
	}

	pkPrev->SetNextNode(pkAddNode);
}

//
//CAStar::CAStar()
//:m_pkOpen(NULL)
//,m_pkClosed(NULL)
//,m_pkBest(NULL)
//{
//}
//
//CAStar::~CAStar()
//{
//	ClearNodes();
//}
//
//void CAStar::ClearNodes()
//{
//	AStarNode* pkTemp = NULL;
//
//	if(m_pkOpen)
//	{
//		while(m_pkOpen)
//		{
//			pkTemp = m_pkOpen->m_pkNext;
//
//			delete m_pkOpen;
//
//			m_pkOpen = pkTemp;
//		}
//	}
//
//	if(m_pkClosed)
//	{
//		while(m_pkClosed)
//		{
//			pkTemp = m_pkClosed->m_pkNext;
//
//			delete m_pkClosed;
//
//			m_pkClosed = pkTemp;
//		}
//	}
//}
//
//bool CAStar::GeneratePath(int iSX, int iSY, int iDX, int iDY)
//{
//	StepInitialize(iSX, iSY, iDX, iDY);
//
//	int iRetval = 0;
//	while(iRetval == 0)
//	{
//		iRetval = Step();
//	}
//
//	if(iRetval == -1 || !m_pkBest)
//	{
//		m_pkBest = NULL;
//		return false;
//	}
//
//	return true;
//}
//
//void CAStar::StepInitialize(int iSX, int iSY, int iDX, int iDY)
//{
//	ClearNodes();
//
//	m_iSX = iSX; m_iSY = iSY; m_iDX = iDX; m_iDY = iDY;
//
//	AStarNode* pkTemp = new AStarNode(iSX, iSY);
//	pkTemp->m_fG = 0;
//	pkTemp->m_fH = float(abs(iSX - iDX) + abs(iSY - iDY));
//	pkTemp->m_fF = pkTemp->m_fG + pkTemp->m_fH;
//
//	m_pkOpen = pkTemp;
//}
//
//int CAStar::Step()
//{
//	if(!(m_pkBest = GetBest()))
//		return -1;
//
//	if(m_pkBest->m_iX == m_iDX && m_pkBest->m_iY == m_iDY)
//		return 1;
//
//	CreateChildren(m_pkBest);
//
//	return 0;
//}
//
//AStarNode* CAStar::GetBest()
//{
//	if(!m_pkOpen)
//		return NULL;
//
//	AStarNode* pkTemp = m_pkOpen;
//	AStarNode* pkTemp2 = m_pkClosed;
//
//	m_pkOpen = pkTemp->m_pkNext;
//
//	m_pkClosed = pkTemp;
//	m_pkClosed->m_pkNext = pkTemp2;
//
//	return pkTemp;
//}
//
//void CAStar::CreateChildren(AStarNode* pkNode)
//{
//	AStarNode kTemp;
//	int iX = pkNode->m_iX;
//	int iY = pkNode->m_iY;
//	int iCost[3][3] = { 14, 10, 14,
//						10,  0, 10,
//						14, 10, 14 };
//	for(int y = -1; y < 2; y++)
//	{
//		for(int x = -1; x < 2; x++)
//		{
//			kTemp.m_iX = iX + x;
//			kTemp.m_iY = iY + y;
//			kTemp.m_iCost = iCost[x + 1][y + 1];
//
//			if(x == 0 && y == 0)
//				continue;
//
//			LinkChild(pkNode, &kTemp);
//		}
//	}
//}
//
//void CAStar::LinkChild(AStarNode* pkNode, AStarNode* pkTemp)
//{
//	int iX = pkTemp->m_iX;
//	int iY = pkTemp->m_iY;
//	float fG = pkNode->m_fG + (1.5f - CMap::Get()->GetGridInfo((float)iX, (float)iY)) * pkTemp->m_iCost;
//
//	AStarNode* pkCheck = NULL;
//	if(pkCheck = CheckList(m_pkOpen, iX, iY))
//	{
//		pkNode->AddChild(pkCheck);
//
//		if(fG < pkCheck->m_fG)
//		{
//			pkCheck->m_pkParent = pkNode;
//			pkCheck->m_fG = fG;
//			pkCheck->m_fF = pkCheck->m_fG + pkCheck->m_fH;
//		}
//	}
//	else if(pkCheck = CheckList(m_pkClosed, iX, iY))
//	{
//		pkNode->AddChild(pkCheck);
//
//		if(fG < pkCheck->m_fG)
//		{
//			pkCheck->m_pkParent = pkNode;
//			pkCheck->m_fG = fG;
//			pkCheck->m_fF = pkCheck->m_fG + pkCheck->m_fH;
//		}
//	}
//	else
//	{
//		AStarNode* pkNewNode = new AStarNode(iX, iY);
//
//		pkNewNode->m_pkParent = pkNode;
//		pkNewNode->m_fG = fG;
//		pkNewNode->m_fH = float(abs(iX - m_iDX) + abs(iY - m_iDY));
//		pkNewNode->m_fF = pkNewNode->m_fG + pkNewNode->m_fH;
//
//		AddToOpen(pkNewNode);
//
//		pkNode->AddChild(pkNewNode);
//	}
//}
//
//AStarNode* CAStar::CheckList(AStarNode* pkNode, int iX, int iY)
//{
//	while(pkNode)
//	{
//		if(pkNode->m_iX == iX && pkNode->m_iY == iY)
//			return pkNode;
//
//		pkNode = pkNode->m_pkNext;
//	}
//
//	return NULL;
//}
//
//void CAStar::AddToOpen(AStarNode* pkAddNode)
//{
//	AStarNode* pkNode = m_pkOpen;
//	AStarNode* pkPrev = NULL;
//
//	if(!m_pkOpen)
//	{
//		m_pkOpen = pkAddNode;
//		m_pkOpen->m_pkNext = NULL;
//
//		return;
//	}
//
//	while(pkNode)
//	{
//		if(pkAddNode->m_fF > pkNode->m_fF)
//		{
//			pkPrev = pkNode;
//			pkNode = pkNode->m_pkNext;
//		}
//		else
//		{
//			if(pkPrev)
//			{
//				pkPrev->m_pkNext = pkAddNode;
//				pkAddNode->m_pkNext = pkNode;
//			}
//			else
//			{
//				AStarNode* pkTemp = m_pkOpen;
//
//				m_pkOpen = pkAddNode;
//				m_pkOpen->m_pkNext = pkTemp;
//			}
//
//			return;
//		}
//	}
//
//	pkPrev->m_pkNext = pkAddNode;
//}