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


CPathScheduler::CPathScheduler(void)
{
	Reset();
}


CPathScheduler::~CPathScheduler(void)
{
	Reset();
}

void CPathScheduler::Reset()
{
	m_nPointMin				= 20;
	m_nPointMax				= 500;

	m_PathSchedulerParam.Reset();

	m_vecPathSchedulerResult.clear();
}

CPathSchedulerResult*	CPathScheduler::GetPathSchedulerResult(int nIndex)
{
	if (nIndex<0 || nIndex>=(int)m_vecPathSchedulerResult.size()) return NULL;

	return &m_vecPathSchedulerResult[nIndex];
}


// getter
int	CPathScheduler::GetPathSchedulerResultCount() const
{ 
	return (int) m_vecPathSchedulerResult.size(); 
}

double CPathScheduler::GetTotalSchedulerTime(int nIndex) const
{
	if (nIndex<0 || nIndex>=(int)m_vecPathSchedulerResult.size()) return 0.0;

	return m_vecPathSchedulerResult[nIndex].GetTotalSchedulerTime();
}

double CPathScheduler::GetTotalSchedulerDistance(int nIndex) const
{
	if (nIndex<0 || nIndex>=(int)m_vecPathSchedulerResult.size()) return 0.0;

	return m_vecPathSchedulerResult[nIndex].GetTotalSchedulerDistance();
}

CPathSchedulerParm* CPathScheduler::GetPathSchedulerParam()
{ 
	return &m_PathSchedulerParam; 
}


int	CPathScheduler::GetRangeRectCount() const
{
	return (int)m_vecRangeRect.size();
}

CRect* CPathScheduler::GetRangeRect(int nIndex)
{
	if (nIndex<0 ||nIndex>=GetRangeRectCount()) return NULL;
	return &m_vecRangeRect[nIndex];
}

const CRect* CPathScheduler::GetRangeRect(int nIndex) const
{
	if (nIndex<0 ||nIndex>=GetRangeRectCount()) return NULL;
	return &m_vecRangeRect[nIndex];
}


// setter
void CPathScheduler::SetPointInfo(int nMin, int nMax)
{
	m_nPointMin = nMin;
	m_nPointMax = nMax;
}

void CPathScheduler::SetPathSchedulerParam(const CPathSchedulerParm& scheduleParam)			
{ 
	m_PathSchedulerParam = scheduleParam; 
}

BOOL CPathScheduler::CalculateDistanceSpeedTime(SSchedulerResult& pt1, const SSchedulerResult &pt2)
{
	double dx, dy, sx, sy, accelx, accely, decelx, decely;

	double dtx=0.0, dty=0.0;
	dx = fabs(pt1.dPositionX-pt2.dPositionX);
	dy = fabs(pt1.dPositionY-pt2.dPositionY);

	const SMotionInfo *pInfo = NULL;

	switch(m_PathSchedulerParam.GetMoveType())
	{
	case Motion_MaxAxisTime: // max pos
		if (dx==0.0 && dy==0.0)
		{
			pt1.nAxisType			= 1;
			pt1.dMoveTime			= 0.0;
			pt1.dMoveSpeed			= 0.0;
			pt1.dMoveDistance		= 0.0;
			pt1.dMoveAccel			= 0.0;
			pt1.dMoveDecel			= 0.0;
			pt1.dMotionDelayTime	= 0.0;
			return TRUE;
		}

		dtx = dty = 0.0;
		if ( (pInfo=m_PathSchedulerParam.GetMotionInfoX(dx))!=NULL)
		{
			dtx = pInfo->GetDurationTime(m_PathSchedulerParam.GetCurveType(), dx);
			sx = pInfo->dMoveSpeed;
			accelx = pInfo->dAccelTime;
			decelx = pInfo->dDecelTime;
		}

		if ( (pInfo=m_PathSchedulerParam.GetMotionInfoY(dy))!=NULL)
		{
			dty = pInfo->GetDurationTime(m_PathSchedulerParam.GetCurveType(), dy);
			sy = pInfo->dMoveSpeed;
			accely = pInfo->dAccelTime;
			decely = pInfo->dDecelTime;
		}

		if (dtx > dty)
		{
			pt1.nAxisType			= 0;
			pt1.dMoveTime			= dtx;
			pt1.dMoveSpeed			= sx;
			pt1.dMoveDistance		= dx;
			pt1.dMoveAccel			= accelx;
			pt1.dMoveDecel			= decelx;
		}
		else 
		{
			pt1.nAxisType			= 1;
			pt1.dMoveTime			= dty;
			pt1.dMoveSpeed			= sy;
			pt1.dMoveDistance		= dy;
			pt1.dMoveAccel			= accely;
     		pt1.dMoveDecel			= decely;
		}
		return TRUE;

	case Motion_XAxisTime: // x pos
		if (dx==0.0)
		{
			pt1.nAxisType			= 0;
			pt1.dMoveTime			= 0.0;
			pt1.dMoveSpeed			= 0.0;
			pt1.dMoveDistance		= 0.0;
			pt1.dMoveAccel			= 0.0;
			pt1.dMoveDecel			= 0.0;
			pt1.dMotionDelayTime	= 0.0;
			return TRUE;
		}

		if ( (pInfo=m_PathSchedulerParam.GetMotionInfoX(dx))!=NULL)
		{
			pt1.nAxisType		= 0;
			pt1.dMoveTime		= pInfo->GetDurationTime(m_PathSchedulerParam.GetCurveType(), dx);
			pt1.dMoveSpeed		= pInfo->dMoveSpeed;
			pt1.dMoveDistance	= dx;
			pt1.dMoveAccel		= pInfo->dAccelTime;
			pt1.dMoveDecel		= pInfo->dDecelTime;
		}
		return TRUE;

	case Motion_YAxisTime: // y pos
		if (dy==0.0)
		{
			pt1.nAxisType			= 1;
			pt1.dMoveTime			= 0.0;
			pt1.dMoveSpeed			= 0.0;
			pt1.dMoveDistance		= 0.0;
			pt1.dMoveAccel			= 0.0;
			pt1.dMoveDecel			= 0.0;
			pt1.dMotionDelayTime	= 0.0;
			return TRUE;
		}

		if ( (pInfo=m_PathSchedulerParam.GetMotionInfoY(dy))!=NULL)
		{
			pt1.nAxisType		= 1;
			pt1.dMoveTime		= pInfo->GetDurationTime(m_PathSchedulerParam.GetCurveType(), dy);
			pt1.dMoveSpeed		= pInfo->dMoveSpeed;
			pt1.dMoveDistance	= dy;
			pt1.dMoveAccel		= pInfo->dAccelTime;
			pt1.dMoveDecel		= pInfo->dDecelTime;
		}
		return TRUE;


	case Motion_XYAxisTime: // xy distance
		return FALSE;
	}

	return FALSE;
}


BOOL CPathScheduler::CalculateDistanceSpeedTime(SSchedulerResult& pt1, double dPosX, double dPosY)
{
	double dx, dy, sx, sy, accelx, accely, decelx, decely;

	double dtx=0.0, dty=0.0;

	const SMotionInfo *pInfo = NULL;

	switch(m_PathSchedulerParam.GetMoveType())
	{
	case Motion_MaxAxisTime: // max pos
		dtx = dty = 0.0;
		dx = fabs(pt1.dPositionX-dPosX);
		dy = fabs(pt1.dPositionY-dPosY);

		if ( (pInfo=m_PathSchedulerParam.GetMotionInfoX(dx))!=NULL)
		{
			dtx = pInfo->GetDurationTime(m_PathSchedulerParam.GetCurveType(), dx);
			sx = pInfo->dMoveSpeed;
			accelx = pInfo->dAccelTime;
			decelx = pInfo->dDecelTime;
		}

		if ( (pInfo=m_PathSchedulerParam.GetMotionInfoY(dy))!=NULL)
		{
			dty = pInfo->GetDurationTime(m_PathSchedulerParam.GetCurveType(), dy);
			sy = pInfo->dMoveSpeed;
			accely = pInfo->dAccelTime;
			decely = pInfo->dDecelTime;
		}

		if (dtx > dty)
		{
			pt1.nAxisType = 0;
			pt1.dMoveTime = dtx;
			pt1.dMoveSpeed = sx;
			pt1.dMoveDistance = dx;
			pt1.dMoveAccel = accelx;
			pt1.dMoveDecel = decelx;
		}
		else
		{
			pt1.nAxisType = 1;
			pt1.dMoveTime = dty;
			pt1.dMoveSpeed = sy;
			pt1.dMoveDistance = dy;
			pt1.dMoveAccel = accely;
			pt1.dMoveDecel = decely;
		}
		return TRUE;

	case Motion_XAxisTime: // x pos
		dx = fabs(pt1.dPositionX-dPosX);
		if ( (pInfo=m_PathSchedulerParam.GetMotionInfoX(dx))!=NULL)
		{
			pt1.nAxisType		= 0;
			pt1.dMoveTime		= pInfo->GetDurationTime(m_PathSchedulerParam.GetCurveType(), dx);
			pt1.dMoveSpeed		= pInfo->dMoveSpeed;
			pt1.dMoveDistance	= dx;
			pt1.dMoveAccel		= pInfo->dAccelTime;
			pt1.dMoveDecel		= pInfo->dDecelTime;
		}

		return TRUE;

	case Motion_YAxisTime: // y pos
		dy = fabs(pt1.dPositionY-dPosY);
		if ( (pInfo=m_PathSchedulerParam.GetMotionInfoY(dy))!=NULL)
		{
			pt1.nAxisType		= 1;
			pt1.dMoveTime		= pInfo->GetDurationTime(m_PathSchedulerParam.GetCurveType(), dy);
			pt1.dMoveSpeed		= pInfo->dMoveSpeed;
			pt1.dMoveDistance	= dy;
			pt1.dMoveAccel		= pInfo->dAccelTime;
			pt1.dMoveDecel		= pInfo->dDecelTime;
		}

		return TRUE;

	case Motion_XYAxisTime: // xy distance
		return FALSE;
	}


	return FALSE;
}


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

	// filtering path data
	VectorPathData vecPathData;
	for (constVectorPathDataIt it=vecTotalPathData.begin(); it!=vecTotalPathData.end(); it++)
	{
		if ( (*it == startPath) && rtRange.PtInRect(CPoint(it->nPosX,it->nPosY)) )
		{
			vecPathData.push_back(*it);
		}
	}

	// check path count
	int nPathCount = (int) vecPathData.size();
	if (nPathCount<1) 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;

		const SPathData *pPathData		= &vecPathData[i];
		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 (int) vecPathData.size();
}

int CPathScheduler::CalculatePath(const SPathData& startPath, const VectorPathData& vecTotalPathData, CPathSchedulerResult& scheduleResult)
{
	scheduleResult.Reset();
	
	// filtering 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;

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

		const SPathData *pPathData		= &vecPathData[i];
		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 (int) vecPathData.size();
}

int CPathScheduler::CalculatePath( const SPathData& startPath, const VectorPathData& vecFirstPathData, const VectorPathData& vecSecondPathData, CPathSchedulerResult& scheduleResult )
{
	return 1;
}

int CPathScheduler::PathScheduling( const SPathData* pPathData, int nPathDataCount, const SPathData* pStartPath, int nStartPathCount )
{
	VectorPathData vecPathData, vecStartPath;
	vecPathData.reserve(nPathDataCount);
	vecStartPath.reserve(nStartPathCount);

	for (int nIdx=0; nIdx<nPathDataCount; nIdx++)
	{
		vecPathData.push_back(pPathData[nIdx]);
	}

	for (int nIdx=0; nIdx<nStartPathCount; nIdx++)
	{
		vecStartPath.push_back(pStartPath[nIdx]);
	}

	return PathScheduling(vecPathData, vecStartPath);	
}

int CPathScheduler::PathScheduling( const SPathData* pPathData, int nPathDataCount, const SPathData& startPath )
{
	VectorPathData vecPathData;
	vecPathData.reserve(nPathDataCount);

	for (int nIdx=0; nIdx<nPathDataCount; nIdx++)
	{
		vecPathData.push_back(pPathData[nIdx]);
	}

	return PathScheduling(vecPathData, startPath);	
}


int	CPathScheduler::PathScheduling(const VectorPathData& vecPathData, const VectorPathData& vecStartPath)
{
	m_vecPathSchedulerResult.clear();

	for (constVectorPathDataIt it=vecStartPath.begin(); it!=vecStartPath.end(); it++)
	{
		CPathSchedulerResult scheduleResult(it->nIndex);

		CalculatePath(*it, vecPathData, scheduleResult);

		m_vecPathSchedulerResult.push_back(scheduleResult);
	}

	return (int) m_vecPathSchedulerResult.size();
}

int	CPathScheduler::PathScheduling(const VectorPathData& vecPathData, const SPathData& startPath)
{
	m_vecPathSchedulerResult.clear();

	CPathSchedulerResult scheduleResult(startPath.nIndex);

	CalculatePath(startPath, vecPathData, scheduleResult);

	m_vecPathSchedulerResult.push_back(scheduleResult);

	return (int) m_vecPathSchedulerResult.size();
}


BOOL CPathScheduler::LoadPathSchedulerParam(const CString& strFilename)
{
	return CPathSchedulerParm::LoadParm(strFilename, m_PathSchedulerParam);
}

BOOL CPathScheduler::SavePathSchedulerParam(const CString& strFilename)
{
	return CPathSchedulerParm::SaveParm(strFilename, m_PathSchedulerParam);
}

int CPathScheduler::PathScheduling( const VectorPathData& vecPathData, const VectorPathData& vecStackPathData, const VectorPathData& vecStartPath )
{
	m_vecPathSchedulerResult.clear();

	for (constVectorPathDataIt it=vecStartPath.begin(); it!=vecStartPath.end(); it++)
	{
		CPathSchedulerResult scheduleResult(it->nIndex);

		CalculatePath(*it, vecPathData, vecStackPathData, scheduleResult);

		m_vecPathSchedulerResult.push_back(scheduleResult);
	}

	return (int) m_vecPathSchedulerResult.size();
}