#include "StdAfx.h"
#include "CHPathScheduler/PathScheduler_TSP.h"
#include "CHPathScheduler/DynamicTSP.h"
#include "CHPathScheduler/AnnealingTSP.h"

CPathScheduler_TSP::CPathScheduler_TSP(void)
{
}

CPathScheduler_TSP::~CPathScheduler_TSP(void)
{

}

int CPathScheduler_TSP::CalculatePath(const SPathData& startPath, const VectorPathData& vecTotalPathData, CPathSchedulerResult& scheduleResult)
{
	scheduleResult.Reset();

	// get path data
	VectorPathData vecPathData;
	for (constVectorPathDataIt it=vecTotalPathData.begin(); it!=vecTotalPathData.end(); it++)
	{
		if (*it == startPath )
		{
			vecPathData.push_back(*it);
		}
	}

	// check path count
	int nPathCount = (int) vecPathData.size();
	if (nPathCount<1) return 0;

	// new TSP
	CGreedyTSP *pTSP = NULL;
	if (nPathCount<=m_nPointMin)
	{
		pTSP = new CDynamicTSP();
	}
	else if (nPathCount<=m_nPointMax)
	{
		pTSP = new CAnnealingTSP();
	}
	else
	{
		pTSP = new CGreedyTSP();
	}
	if (pTSP==NULL) return 0;

	// set path data
	pTSP->SetPathData(vecPathData, startPath);

	// cal tsp
	if (pTSP->CalculateTSP()<0.1) 
	{
		delete pTSP;
		return 0;
	}

	// check path count
	nPathCount = pTSP->GetPathCount();
	if (nPathCount<1) 
	{
		delete pTSP;
		return 0;
	}

	// resize result count
	scheduleResult.SetScheduleResultCount(nPathCount);

	// ½ºÄÉÁì ½ÃÀÛÀ§Ä¡..
	BOOL bStartPos = FALSE;
	SSchedulerResult sStartPos;
	if (startPath.nPosX!=NO_PATH_DATA && startPath.nPosY!=NO_PATH_DATA)
	{
		sStartPos.nTotalIndex		= -1;
		sStartPos.nPointIndex		= -1;
		sStartPos.dPositionX		= startPath.nPosX / 1000.0;
		sStartPos.dPositionY		= startPath.nPosY / 1000.0;
		sStartPos.dAutoFocusTime	= m_PathSchedulerParam.GetAutoFocusTime();
		sStartPos.dImageGrabTime	= m_PathSchedulerParam.GetImageGrabTime();
		sStartPos.dMotionDelayTime	= m_PathSchedulerParam.GetMotionDelayTime();
		bStartPos = TRUE;
	}

	for (int i=0; i<nPathCount; i++)
	{
		SSchedulerResult *pCurResult = scheduleResult.GetPathSchedulerResult(i);
		if (pCurResult==NULL) continue;

		pCurResult->nPointIndex = pTSP->GetPathResult(i);
		if (pCurResult->nPointIndex<0) continue;

		const SPathData *pPathData = pTSP->GetPathData(pCurResult->nPointIndex);
		if (pPathData==NULL) continue;
		pCurResult->nTotalIndex			= pPathData->nIndex;
		pCurResult->dPositionX			= double(pPathData->nPosX) / 1000.0;
		pCurResult->dPositionY			= double(pPathData->nPosY) / 1000.0;
		pCurResult->dAutoFocusTime		= m_PathSchedulerParam.GetAutoFocusTime();
		pCurResult->dImageGrabTime		= m_PathSchedulerParam.GetImageGrabTime();
		pCurResult->dMotionDelayTime	= m_PathSchedulerParam.GetMotionDelayTime();

		memcpy(pCurResult->pDataType, pPathData->pDataType, sizeof(int)*PATH_DATA_TYPE_COUNT);

		if (i==0) // first point
		{
			// exist start pos
			if (bStartPos)
			{
				if (!CalculateDistanceSpeedTime(*pCurResult, sStartPos))
				{
					continue;
				}
			}
			else if(!CalculateDistanceSpeedTime(*pCurResult, *pCurResult))
			{
				continue;
			}
		}
		else // from second point
		{
			SSchedulerResult *pPrevResult = scheduleResult.GetPathSchedulerResult(i-1);
			if (!CalculateDistanceSpeedTime(*pCurResult, *pPrevResult))
			{
				continue;
			}
		}
	}

	delete pTSP;

	return scheduleResult.GetPathSchedulerResultCount();
}

int CPathScheduler_TSP::CalculatePath( const SPathData& startPath, const VectorPathData& vecFirstPathData, const VectorPathData& vecSecondPathData, CPathSchedulerResult& scheduleResult )
{
	scheduleResult.Reset();

	// get path data
	VectorPathData vecPathData;
	for (constVectorPathDataIt it=vecFirstPathData.begin(); it!=vecFirstPathData.end(); it++)
	{
		if (*it == startPath )
		{
			vecPathData.push_back(*it);
		}
	}

	// Second filtering path data
	VectorPathData vecSecondPath;
	for (constVectorPathDataIt it=vecSecondPathData.begin(); it!=vecSecondPathData.end(); it++)
	{
		if (*it == startPath )
		{
			vecSecondPath.push_back(*it);
		}
	}

	// check path count
	int nPathCount = (int) vecPathData.size();
	if (nPathCount<1) return 0;

	int nSecondPathCount = (int) vecSecondPath.size();
	if ( nSecondPathCount<1)	return 0;

	// new TSP
	CGreedyTSP *pTSP = NULL;
	if (nPathCount <=m_nPointMin)
	{
		pTSP = new CDynamicTSP();
	}
	else if (nPathCount <=m_nPointMax)
	{
		pTSP = new CAnnealingTSP();
	}
	else
	{
		pTSP = new CGreedyTSP();
	}
	if (pTSP==NULL) return 0;

	// set path data
	pTSP->SetPathData(vecPathData, startPath);

	// cal tsp
	if (pTSP->CalculateTSP()<0.1) 
	{
		delete pTSP;
		return 0;
	}

	// check path count
	nPathCount = pTSP->GetPathCount();
	if (nPathCount<1) 
	{
		delete pTSP;
		return 0;
	}

	// new Second TSP
	CGreedyTSP *pSecondTSP = NULL;
	if (nSecondPathCount <=m_nPointMin)
	{
		pSecondTSP = new CDynamicTSP();
	}
	else if (nSecondPathCount <=m_nPointMax)
	{
		pSecondTSP = new CAnnealingTSP();
	}
	else
	{
		pSecondTSP = new CGreedyTSP();
	}
	if (pSecondTSP==NULL) return 0;

	// set path data
	pSecondTSP->SetPathData(vecSecondPath, startPath);

	// cal tsp
	if (pSecondTSP->CalculateTSP()<0.1) 
	{
		delete pSecondTSP;
		return 0;
	}

	// check path count
	nSecondPathCount = pSecondTSP->GetPathCount();
	if (nSecondPathCount<1) 
	{
		delete pSecondTSP;
		return 0;
	}

	// resize result count
	scheduleResult.SetScheduleResultCount(nPathCount+nSecondPathCount);

	// ½ºÄÉÁì ½ÃÀÛÀ§Ä¡..
	BOOL bStartPos = FALSE;
	SSchedulerResult sStartPos;
	if (startPath.nPosX!=NO_PATH_DATA && startPath.nPosY!=NO_PATH_DATA)
	{
		sStartPos.nTotalIndex		= -1;
		sStartPos.nPointIndex		= -1;
		sStartPos.dPositionX		= startPath.nPosX / 1000.0;
		sStartPos.dPositionY		= startPath.nPosY / 1000.0;
		sStartPos.dAutoFocusTime	= m_PathSchedulerParam.GetAutoFocusTime();
		sStartPos.dImageGrabTime	= m_PathSchedulerParam.GetImageGrabTime();
		sStartPos.dMotionDelayTime	= m_PathSchedulerParam.GetMotionDelayTime();
		bStartPos = TRUE;
	}

	for (int i=0; i<nPathCount; i++)
	{
		SSchedulerResult *pCurResult = scheduleResult.GetPathSchedulerResult(i);
		if (pCurResult==NULL) continue;

		pCurResult->nPointIndex = pTSP->GetPathResult(i);
		if (pCurResult->nPointIndex<0) continue;

		const SPathData *pPathData = pTSP->GetPathData(pCurResult->nPointIndex);
		if (pPathData==NULL) continue;
		pCurResult->nTotalIndex			= pPathData->nIndex;
		pCurResult->dPositionX			= double(pPathData->nPosX) / 1000.0;
		pCurResult->dPositionY			= double(pPathData->nPosY) / 1000.0;
		pCurResult->dAutoFocusTime		= m_PathSchedulerParam.GetAutoFocusTime();
		pCurResult->dImageGrabTime		= m_PathSchedulerParam.GetImageGrabTime();
		pCurResult->dMotionDelayTime	= m_PathSchedulerParam.GetMotionDelayTime();

		memcpy(pCurResult->pDataType, pPathData->pDataType, sizeof(int)*PATH_DATA_TYPE_COUNT);

		if (i==0) // first point
		{
			// exist start pos
			if (bStartPos)
			{
				if (!CalculateDistanceSpeedTime(*pCurResult, sStartPos))
				{
					continue;
				}
			}
			else if(!CalculateDistanceSpeedTime(*pCurResult, *pCurResult))
			{
				continue;
			}
		}
		else // from second point
		{
			SSchedulerResult *pPrevResult = scheduleResult.GetPathSchedulerResult(i-1);
			if (!CalculateDistanceSpeedTime(*pCurResult, *pPrevResult))
			{
				continue;
			}
		}
	}

	delete pTSP;

	// Second½ºÄÉÁì ½ÃÀÛÀ§Ä¡..
	SSchedulerResult *pSecondStartResult = scheduleResult.GetPathSchedulerResult(nPathCount-1);
	if (pSecondStartResult==NULL) return 0;

	BOOL bSecondStartPos = FALSE;
	SSchedulerResult sSecondStartPos;
	if (pSecondStartResult->dPositionX !=NO_PATH_DATA && pSecondStartResult->dPositionY !=NO_PATH_DATA)
	{
		sSecondStartPos.nTotalIndex		= -1;
		sSecondStartPos.nPointIndex		= -1;
		sSecondStartPos.dPositionX		= pSecondStartResult->dPositionX;
		sSecondStartPos.dPositionY		= pSecondStartResult->dPositionY;
		sSecondStartPos.dAutoFocusTime	= m_PathSchedulerParam.GetAutoFocusTime();
		sSecondStartPos.dImageGrabTime	= m_PathSchedulerParam.GetImageGrabTime();
		sSecondStartPos.dMotionDelayTime	= m_PathSchedulerParam.GetMotionDelayTime();
		bSecondStartPos = TRUE;
	}

	for (int i=0; i<nSecondPathCount; i++)
	{
		SSchedulerResult *pCurResult = scheduleResult.GetPathSchedulerResult(i+nPathCount);
		if (pCurResult==NULL) continue;

		pCurResult->nPointIndex = pSecondTSP->GetPathResult(i);
		if (pCurResult->nPointIndex<0) continue;

		const SPathData *pPathData = pSecondTSP->GetPathData(pCurResult->nPointIndex);
		if (pPathData==NULL) continue;
		pCurResult->nTotalIndex			= pPathData->nIndex;
		pCurResult->dPositionX			= double(pPathData->nPosX) / 1000.0;
		pCurResult->dPositionY			= double(pPathData->nPosY) / 1000.0;
		pCurResult->dAutoFocusTime		= m_PathSchedulerParam.GetAutoFocusTime();
		pCurResult->dImageGrabTime		= m_PathSchedulerParam.GetImageGrabTime();
		pCurResult->dMotionDelayTime	= m_PathSchedulerParam.GetMotionDelayTime();

		memcpy(pCurResult->pDataType, pPathData->pDataType, sizeof(int)*PATH_DATA_TYPE_COUNT);

		if (i==0) // first point
		{
			// exist start pos
			if (bStartPos)
			{
				if (!CalculateDistanceSpeedTime(*pCurResult, sStartPos))
				{
					continue;
				}
			}
			else if(!CalculateDistanceSpeedTime(*pCurResult, *pCurResult))
			{
				continue;
			}
		}
		else // from second point
		{
			SSchedulerResult *pPrevResult = scheduleResult.GetPathSchedulerResult(i-1 + nPathCount);
			if (!CalculateDistanceSpeedTime(*pCurResult, *pPrevResult))
			{
				continue;
			}
		}
	}

	delete pSecondTSP;

	return scheduleResult.GetPathSchedulerResultCount();
}