// AStar.cpp: implementation of the CAStar class.
//
//////////////////////////////////////////////////////////////////////

#include "AStar.h"
#include <math.h>
//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////


CAStar::CAStar()
{
	pASNode=NULL;
}

int CAStar::GetNodeIndex(int pathIndex)
{
	return PathList[PathList.size()-pathIndex-1];
}

bool CAStar::FindPath(int FromIndex,int ToIndex)
{
	OpenList.clear();
	PathList.clear();
	int i=0;
	//打開起始點
	pASNode[FromIndex].bIsOpen=true;
	pASNode[FromIndex].G=0;
	OpenList.push_back(FromIndex);
	//計算所有的H值
	for(i=0;i<NodeCount;i++)
	{
		pASNode[i].H=sqrt(pow(pASNode[i].x-pASNode[ToIndex].x,2)+
					      pow(pASNode[i].y-pASNode[ToIndex].y,2));
	}
	while(1)
	{
		bool bFoundPath=false;
		int NotClosedCount=0;
		for(i=0;i<OpenList.size();i++)
		{
			//遍歷所有打開并且沒有關閉的點,
			if(!pASNode[OpenList[i]].bIsClose)
			{
				//如果打開的點是目標點,則退出循環,到達
				if(OpenList[i]==ToIndex)
					bFoundPath=true;//找到目標點
				NotClosedCount++;
			}
		}

		//如果所有打開的點都已被關閉,則退出,不可達
		if(NotClosedCount==0)
			return false;
		//從打開列表中找到一個F值最小的點
		int MinFIndex=-1;
		int MinF=99999999;
		for(i=0;i<OpenList.size();i++)
		{
			//遍歷所有打開并且沒有關閉的點,尋找最小的F值
			int tempI=OpenList[i];
			if(!pASNode[tempI].bIsClose)
			{
				int tempF=pASNode[tempI].G+pASNode[ToIndex].H;
				if(tempF<MinF)
				{
					MinF=tempF;
					MinFIndex=tempI;
				}
			}
		}
		//從打開列表中找到一個F值最小的點,把他的鄰居全打開,把自己關閉,設置parent
		//并查看鄰居,看自己做鄰居的父結點是否能夠獲得更小的G值		
		for(int j=0;j<pASNode[MinFIndex].NeighborCount;j++)
		{
			SASNode *pTempChild=&pASNode[pASNode[MinFIndex].Neighbor[j]];
			if(pTempChild->bIsOpen)//如果是打開的鄰居,看自己是否可以做它的父結點
			{
				if(pTempChild->G>pASNode[MinFIndex].G+pASNode[MinFIndex].NeighborDistance[j])
				{
					pTempChild->G=pASNode[MinFIndex].G+pASNode[MinFIndex].NeighborDistance[j];
					pTempChild->ParentIndex=MinFIndex;
				}
			}
			else
			{
				pTempChild->bIsOpen=true;
				OpenList.push_back(pASNode[MinFIndex].Neighbor[j]);//加入打開列表
				pTempChild->ParentIndex=MinFIndex;
				pTempChild->G=pASNode[MinFIndex].G+pASNode[MinFIndex].NeighborDistance[j];
			}
		}
		pASNode[MinFIndex].bIsClose=true;
		if(bFoundPath)
			break;
		//繼續循環
	}
	//如果運行到此處,則說明已經到達了目標,則從目標逆推得到路徑
	int tempIndex=ToIndex;
	PathList.push_back(tempIndex);
	while(tempIndex!=FromIndex)
	{
		tempIndex=pASNode[tempIndex].ParentIndex;
		PathList.push_back(tempIndex);
	}
	//路徑已經存儲完畢
	PathPointCount=PathList.size();
	return true;
}

int CAStar::GetNodeCount()
{
	return PathPointCount;
}

void CAStar::InitNodeByWayPoint(CWayPoint *pWayPoint)
{
	if(pASNode)
	{
		delete[] pASNode;
		pASNode=NULL;
	}
	NodeCount=pWayPoint->GetNodeCount();
	pASNode=new SASNode[NodeCount];
	for(int i=0;i<NodeCount;i++)
	{
		SWPNode* wpNode=pWayPoint->GetWPNode(i);
		pASNode[i].x=wpNode->x;
		pASNode[i].y=wpNode->y;
		pASNode[i].bIsClose=false;
		pASNode[i].bIsOpen=false;
		pASNode[i].F=0;
		pASNode[i].G=0;
		pASNode[i].ParentIndex=-1;
		pASNode[i].NeighborCount=0;
		for(int j=0;j<wpNode->NeighborCount;j++)
		{
			pASNode[i].NeighborCount=wpNode->NeighborCount;
			pASNode[i].Neighbor[j]=pWayPoint->GetIndexFromID(wpNode->Neighbor[j]);
			pASNode[i].NeighborDistance[j]=wpNode->NeighborDistance[j];
		}
	}
}

CAStar::~CAStar()
{
	delete[] pASNode;
	pASNode=NULL;
}