#include "StdAfx.h"
#include "PathScheduler_Dual_TSP.h"
#include "DynamicTSP.h"
#include "AnnealingTSP.h"

CPathScheduler_Dual_TSP::CPathScheduler_Dual_TSP(void)
{
}


CPathScheduler_Dual_TSP::~CPathScheduler_Dual_TSP(void)
{
}

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

	// get path data
	VectorPathData vecPathData;
	for (constVectorPathDataIt it=vecTotalPathData.begin(); it!=vecTotalPathData.end(); it++)
	{
		if ( rtRange.PtInRect( CPoint(it->nPosX,it->nPosY )) )
		{
			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 (int) vecPathData.size();
}