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

CPathScheduler_Dual_Active_TSP::CPathScheduler_Dual_Active_TSP(void)
{
}


CPathScheduler_Dual_Active_TSP::~CPathScheduler_Dual_Active_TSP(void)
{
}

int CPathScheduler_Dual_Active_TSP::CalculatePath( const SPathData& startPath, const VectorPathData& vecTotalPathData, const CRect& rtRange, CPathSchedulerResult& scheduleResult )
{
	bool bShortPath= GetPathSchedulerParam()->GetUseShortPath();
	
	if (bShortPath == TRUE)
	{
		//½ºÄÉÁ층 20210111
		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();

	}
	else
	{
	//½ºÄÉÁ층 20210111
	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);
		pCurResult->nPointIndex = 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();

		pCurResult->nTotalIndex = vecPathData[i].nIndex;
		pCurResult->dPositionX = double(vecPathData[i].nPosX) / 1000.0;
		pCurResult->dPositionY = double(vecPathData[i].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();
	 
}
	

}