#include "StdAfx.h"
#include "PathScheduler_Sorting.h"
#include <algorithm>

CPathScheduler_Sorting::CPathScheduler_Sorting(void)
{
}


CPathScheduler_Sorting::~CPathScheduler_Sorting(void)
{
}

int CPathScheduler_Sorting::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;

	switch(m_PathSchedulerParam.GetSortAxisType())
	{
	case 0: // x Axis
		if (m_PathSchedulerParam.GetSortOrderType()==0) // ascend
		{
			std::sort(vecPathData.begin(), vecPathData.end(), AscendAxisX);
		}
		else // descend
		{
			std::sort(vecPathData.begin(), vecPathData.end(), DescendAxisX);
		}
		break;

	case 1: // y Axis
		if (m_PathSchedulerParam.GetSortOrderType()==0) // ascend
		{
			std::sort(vecPathData.begin(), vecPathData.end(), AscendAxisY);
		}
		else // descend
		{
			std::sort(vecPathData.begin(), vecPathData.end(), DescendAxisY);
		}
		break;

	default:
		return 0;
		break;
	}

	// 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 = i;

		const SPathData *pPathData = &vecPathData[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;
			}
		}
	}

	return scheduleResult.GetPathSchedulerResultCount();
}