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

#include "GreedyTSP.h"
#include "AnnealingTSP.h"
#include "DynamicTSP.h"


inline bool ComparePosAscend(SPathData& a, SPathData& b)
{
	return (a.nPosX < b.nPosX);
}

inline bool ComparePosDescend(SPathData& a, SPathData& b)
{
	return (a.nPosX > b.nPosX);
}


inline bool ComparePosX(SPathData& a, SPathData& b)
{
	return (a.nPosX < b.nPosX);
}


inline bool ComparePosY(SPathData& a, SPathData& b)
{
	return (a.nPosY < b.nPosY);
}

inline int GetDiffCountX(const VectorPathData& vecPathData, int nCenterPos)
{
	int nLeft = 0;
	int nRight = 0;
	for (constVectorPathDataIt it=vecPathData.begin(); it!=vecPathData.end(); it++)
	{
		if (it->nPosX < nCenterPos)
		{
			nLeft++;
		}
		else
		{
			nRight++;
		}
	}

	return abs(nLeft-nRight);
}


inline int GetDiffCountY(const VectorPathData& vecPathData, int nCenterPos)
{
	int nTop = 0;
	int nBottom = 0;
	for (constVectorPathDataIt it=vecPathData.begin(); it!=vecPathData.end(); it++)
	{
		if (it->nPosY < nCenterPos)
		{
			nTop++;
		}
		else
		{
			nBottom++;
		}
	}

	return abs(nTop-nBottom);
}

inline int GetCollisionPosX(const VectorPathData& vecPathData, int nGlassSizeX, int nCollisionDist)
{
	VectorPathData vecSortData = vecPathData;

	// sort
	std::sort(vecSortData.begin(), vecSortData.end(), ComparePosX);
	int nSize = (int)vecSortData.size();
	int nCenterPosX = vecSortData[nSize/2].nPosX;
	int nCollisionDistX = nCollisionDist;

	// À§Ä¡°¡ ³Ê¹« À۰ųª Ŭ¶§?
	if ( nCenterPosX<(nCollisionDistX*2) || nCenterPosX>=(nGlassSizeX-(nCollisionDistX*2)) ) 
	{
		int nLeftPos = (nCollisionDistX*2);
		int nRightPos = nGlassSizeX-(nCollisionDistX*2);

		// Á¿ìÀÇ Å©±â Â÷ÀÌ°¡ ÀÛÀº À§Ä¡·Î ÇÒ´çÇÏÀÚ!
		nCenterPosX =  (GetDiffCountX(vecPathData, nLeftPos) < GetDiffCountX(vecPathData, nRightPos)) ? nLeftPos: nRightPos;
	}

	return nCenterPosX;
}


inline int GetCollisionPosY(const VectorPathData& vecPathData, int nGlassSizeY, int nCollisionDist)
{
	VectorPathData vecSortData = vecPathData;

	// sort
	std::sort(vecSortData.begin(), vecSortData.end(), ComparePosY);
	int nSize = (int)vecSortData.size();
	int nCenterPosY = vecSortData[nSize/2].nPosY;
	int nCollisionDistY = nCollisionDist;

	// À§Ä¡°¡ ³Ê¹« À۰ųª Ŭ¶§?
	if ( nCenterPosY<(nCollisionDistY*2) || nCenterPosY>=(nGlassSizeY-(nCollisionDistY*2)) ) 
	{
		int nTopPos = (nCollisionDistY*2);
		int nBottomPos = nGlassSizeY-(nCollisionDistY*2);

		// Á¿ìÀÇ Å©±â Â÷ÀÌ°¡ ÀÛÀº À§Ä¡·Î ÇÒ´çÇÏÀÚ!
		nCenterPosY =  (GetDiffCountY(vecPathData, nTopPos) < GetDiffCountY(vecPathData, nBottomPos)) ? nTopPos: nBottomPos;
	}

	return nCenterPosY;
}


CPathScheduler_Quad_Active_TSP::CPathScheduler_Quad_Active_TSP(void)
{
}


CPathScheduler_Quad_Active_TSP::~CPathScheduler_Quad_Active_TSP(void)
{
}



int CPathScheduler_Quad_Active_TSP::PathScheduling( const VectorPathData& vecPathData, const VectorPathData& vecStartPath )
{

	m_vecRangeRect.clear();
	m_vecScanRangeData.clear();
	m_vecPathSchedulerResult.clear();

	int nGantryCount = 2;

	int nRangeCount = CalculateRangeRect(
		nGantryCount, 
		m_PathSchedulerParam.GetModuleCount(), 
		m_PathSchedulerParam.GetGlassSizeX(),
		m_PathSchedulerParam.GetGlassSizeY(),
		m_PathSchedulerParam.GetCollisionDistX(),
		m_PathSchedulerParam.GetCollisionDistY(),
		m_PathSchedulerParam.GetOriginDir(),
		m_PathSchedulerParam.GetBaseModuleDir(),
		vecPathData,
		m_vecScanRangeData);

	if (nRangeCount<1) return 0;

	FILE *fp = NULL;
	CString strFilename = _T("");
	strFilename.Format(_T("c:\\temp\\range_info.csv"));
	_tfopen_s(&fp, strFilename, _T("w"));
	if (fp)
	{
		_ftprintf_s(fp, _T("[ModuleIdx], [ScanIdx], [Left], [Right], [Top], [Bottom], [ScanDir], [StartX], [EndX], [ColliX], [ColliY]\n"));
	}

	m_vecRangeRect.clear();

	const int nScanCount = 2;
	int nTotalCount = 0;
	SPathData sStartPath; // ¸ðµâº° ½ºÄµ ½ÃÀÛÀ§Ä¡ ÀúÀå.
	for (int nModuleIdx=0; nModuleIdx<m_PathSchedulerParam.GetModuleCount(); nModuleIdx++)
	{
		if (int(vecStartPath.size()) <= nModuleIdx) continue;

		// ¿µ¿ªº° ½ºÄÉÁ층 ¼öÇà.
		CPathSchedulerResult schedulerResult(nModuleIdx, 0);

		for (int nScanIdx=0; nScanIdx<nScanCount; nScanIdx++)
		{
			SRangeData* pRangeData = GetScanRangeData(nModuleIdx, nScanIdx); // ½ºÄµ¼ö´Â 1ȸÀÓ.
			if (pRangeData==NULL) continue;

			CRect rect = CRect(pRangeData->nLeft, pRangeData->nTop, pRangeData->nRight, pRangeData->nBottom);
			rect.NormalizeRect();
			m_vecRangeRect.push_back(rect);

			if (fp)
			{
				_ftprintf_s(fp, _T("%d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n"), 
					pRangeData->nModuleIdx,
					pRangeData->nScanIdx, 
					pRangeData->nLeft, 
					pRangeData->nRight, 
					pRangeData->nTop, 
					pRangeData->nBottom,
					pRangeData->nScanDir,
					pRangeData->nScanStartX,
					pRangeData->nScanEndX,
					pRangeData->nColliPosX,
					pRangeData->nColliPosY);
			}

			// ù¹ø° ½ºÄµÀÇ ½ÃÀÛÀ§Ä¡´Â ÇöÀç¸ðµâÀÇ À§Ä¡ÀÌ´Ù.
			if (nScanIdx==0)
			{
				sStartPath = vecStartPath[nModuleIdx];
			}
			
			// Àӽà °á°ú
			CPathSchedulerResult tmpResult(nModuleIdx, 0);
			
			// °æ·Î °è»ê
			int nCount = CalculatePath(nModuleIdx, nScanIdx, sStartPath, vecPathData, *pRangeData, tmpResult);
			nTotalCount += nCount;

			// ¸ðµâÀÇ Ãæµ¹À§Ä¡ ÀúÀå
			if (pRangeData->nColliPosX!=-1)
			{
				schedulerResult.SetCollisionPositionX(pRangeData->nColliPosX);
			}

			if (pRangeData->nColliPosY!=-1)
			{
				int nLastPos = schedulerResult.GetCollisionPositionY();
				if ( (schedulerResult.GetModuleIndex()%2) == 0 )
				{
					max(nLastPos, pRangeData->nColliPosY);
				}
				else
				{
					min(nLastPos, pRangeData->nColliPosY);
				}
				schedulerResult.SetCollisionPositionY(nLastPos);
			}

			// ¸ðµâ °á°ú¿¡ Ãß°¡
			schedulerResult.AddTailScheduleResult(tmpResult);

			int nTmpIndex = tmpResult.GetPathSchedulerResultCount()-1;
			const SSchedulerResult* pLastPoint = tmpResult.GetPathSchedulerResult(nTmpIndex);
			if (pLastPoint)
			{
				sStartPath.nPosX = (int)(pLastPoint->dPositionX*1000.0);
				sStartPath.nPosY = (int)(pLastPoint->dPositionY*1000.0);
			}
		}
		
		// °á°ú ÀúÀå.
		m_vecPathSchedulerResult.push_back(schedulerResult);
	}

	if (fp) fclose(fp);

	return int(m_vecPathSchedulerResult.size());
}


int CPathScheduler_Quad_Active_TSP::CalculatePath(int nModuleIdx, int nScanIdx, const SPathData& startPath, const VectorPathData& vecTotalPathData, const SRangeData& rtRange, CPathSchedulerResult& scheduleResult )
{
	scheduleResult.Reset();

	CRect rtTemp(rtRange.nLeft, rtRange.nTop, rtRange.nRight, rtRange.nBottom);

	// get path data
	VectorPathData vecPathData;
	for (constVectorPathDataIt it=vecTotalPathData.begin(); it!=vecTotalPathData.end(); it++)
	{
		if ( rtTemp.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 scheduleResult.GetPathSchedulerResultCount();
}

int CPathScheduler_Quad_Active_TSP::CalculateRangeRect(int nGantryCount, int nModuleCount, int nGlassSizeX, int nGlassSizeY, 
	int nDistX, int nDistY, int nOriginDir, int nBaseDir, const VectorPathData& vecPathData, VectorRangeData& vecRangeData)
{
	// get x pos
	int nResultPosX = GetCollisionPosX(vecPathData, m_PathSchedulerParam.GetGlassSizeX(), m_PathSchedulerParam.GetCollisionDistX());

	const int nXPosCount = 5;
	int nColliPosX[nXPosCount]	= {0, 0, 0, 0, 0};
	nColliPosX[0]		= 0;
	nColliPosX[1]		= nResultPosX - m_PathSchedulerParam.GetCollisionDistX();
	nColliPosX[2]		= nResultPosX;
	nColliPosX[3]		= nResultPosX + m_PathSchedulerParam.GetCollisionDistX();
	nColliPosX[4]		= m_PathSchedulerParam.GetGlassSizeX();

	const int nYPosCount = 3;
	int nColliPosY[nYPosCount]	= {0, 0, 0};
	nColliPosY[0]	= 0;
	nColliPosY[1]	= m_PathSchedulerParam.GetGlassSizeY()/2;
	nColliPosY[2]	= m_PathSchedulerParam.GetGlassSizeY();

	BOOL bChangeModuleIdx = FALSE;
	if (m_PathSchedulerParam.GetBaseModuleDir()==SchedulerDir_LeftBottom && m_PathSchedulerParam.GetBaseModuleDir()==SchedulerDir_RightBottom)
	{
		bChangeModuleIdx = TRUE;
	}

	// left
	int nScanIdx = 1;
	for (int nXIdx=0; nXIdx<2; nXIdx++)
	{
		VectorPathData vecTemp;
		for (constVectorPathDataIt it=vecPathData.begin(); it!=vecPathData.end(); it++)
		{
			if (it->nPosX >= nColliPosX[nXIdx] && it->nPosX < nColliPosX[nXIdx+1])
			{
				vecTemp.push_back(*it);
			}
		}

		nColliPosY[1] = GetCollisionPosY(vecTemp, m_PathSchedulerParam.GetGlassSizeY(), 0);

		// yÃà Ãæµ¹¿µ¿ª
		if (nColliPosY[1] < m_PathSchedulerParam.GetCollisionDistY())
		{
			nColliPosY[1] = m_PathSchedulerParam.GetCollisionDistY();
		}
		else if (nColliPosY[1] > (m_PathSchedulerParam.GetGlassSizeY()-m_PathSchedulerParam.GetCollisionDistY()))
		{
			nColliPosY[1] = m_PathSchedulerParam.GetGlassSizeY()-m_PathSchedulerParam.GetCollisionDistY();
		}

		int nModuleIdx = 3;
		for (int nYIdx=0; nYIdx<nYPosCount-1; nYIdx++)
		{
			SRangeData range;
			range.nModuleIdx = nModuleIdx--;
			range.nScanIdx	= nScanIdx;
			range.nScanDir	= 0;

			range.nLeft		= nColliPosX[nXIdx];
			range.nRight	= nColliPosX[nXIdx+1];

			range.nTop		= nColliPosY[nYIdx];
			range.nBottom	= nColliPosY[nYIdx+1];

			range.nColliPosX = nResultPosX - m_PathSchedulerParam.GetCollisionDistX();

			if ( (nModuleIdx%2) == 0 )
				range.nColliPosY = nColliPosY[1] + m_PathSchedulerParam.GetCollisionDistY();
			else
				range.nColliPosY = nColliPosY[1] - m_PathSchedulerParam.GetCollisionDistY();

			vecRangeData.push_back(range);
		}
		nScanIdx--;
	}

	// right
	nScanIdx = 1;
	for (int nXIdx=2; nXIdx<nXPosCount-1; nXIdx++)
	{
		VectorPathData vecTemp;
		for (constVectorPathDataIt it=vecPathData.begin(); it!=vecPathData.end(); it++)
		{
			if (it->nPosX >= nColliPosX[nXIdx] && it->nPosX < nColliPosX[nXIdx+1])
			{
				vecTemp.push_back(*it);
			}
		}

		nColliPosY[1] = GetCollisionPosY(vecTemp, m_PathSchedulerParam.GetGlassSizeY(), 0);

		// yÃà Ãæµ¹¿µ¿ª
		if (nColliPosY[1] < m_PathSchedulerParam.GetCollisionDistY())
		{
			nColliPosY[1] = m_PathSchedulerParam.GetCollisionDistY();
		}
		else if (nColliPosY[1] > (m_PathSchedulerParam.GetGlassSizeY()-m_PathSchedulerParam.GetCollisionDistY()))
		{
			nColliPosY[1] = m_PathSchedulerParam.GetGlassSizeY()-m_PathSchedulerParam.GetCollisionDistY();
		}

		int nModuleIdx = 1;
		for (int nYIdx=0; nYIdx<nYPosCount-1; nYIdx++)
		{
			SRangeData range;
			range.nModuleIdx = nModuleIdx--;
			range.nScanIdx	= nScanIdx;
			range.nScanDir	= 0;

			range.nLeft		= nColliPosX[nXIdx];
			range.nRight	= nColliPosX[nXIdx+1];

			range.nTop		= nColliPosY[nYIdx];
			range.nBottom	= nColliPosY[nYIdx+1];

			range.nColliPosX = nResultPosX + m_PathSchedulerParam.GetCollisionDistX();

			if ( (nModuleIdx%2) == 0 )
				range.nColliPosY = nColliPosY[1] + m_PathSchedulerParam.GetCollisionDistY();
			else
				range.nColliPosY = nColliPosY[1] - m_PathSchedulerParam.GetCollisionDistY();

			vecRangeData.push_back(range);
		}

		nScanIdx--;
	}


	return (int)vecRangeData.size();
}

SRangeData* CPathScheduler_Quad_Active_TSP::GetScanRangeData( int nModuleIdx, int nScanIdx )
{
	for (VectorRangeDataIt it=m_vecScanRangeData.begin(); it!=m_vecScanRangeData.end(); it++)
	{
		if (it->nModuleIdx==nModuleIdx && it->nScanIdx==nScanIdx)
		{
			return &(*it);
		}
	}
	return NULL;
}

const SRangeData* CPathScheduler_Quad_Active_TSP::GetScanRangeData( int nModuleIdx, int nScanIdx ) const
{
	for (constVectorRangeDataIt it=m_vecScanRangeData.begin(); it!=m_vecScanRangeData.end(); it++)
	{
		if (it->nModuleIdx==nModuleIdx && it->nScanIdx==nScanIdx)
		{
			return &(*it);
		}
	}
	return NULL;
}



int CPathScheduler_Quad_Active_TSP::GetDirectionX(int nDir1, int nDir2)
{
	//SchedulerDir_LeftTop=0, SchedulerDir_RightTop, SchedulerDir_LeftBottom, SchedulerDir_RightBottom

	// Dir1 == Left
	if (nDir1==SchedulerDir_LeftTop || nDir1==SchedulerDir_LeftBottom)
	{
		if (nDir2==SchedulerDir_LeftTop || nDir2==SchedulerDir_LeftBottom)
		{
			return 1;
		}
		else 
		{
			return -1;
		}
	}

	// Dir1 == Right
	if (nDir2==SchedulerDir_RightTop || nDir2==SchedulerDir_RightBottom)
	{
		return 1;
	}

	return -1;
}

int CPathScheduler_Quad_Active_TSP::GetDirectionY(int nDir1, int nDir2)
{
	//SchedulerDir_LeftTop=0, SchedulerDir_RightTop, SchedulerDir_LeftBottom, SchedulerDir_RightBottom

	// Dir1 == Top
	if (nDir1==SchedulerDir_LeftTop || nDir1==SchedulerDir_RightTop)
	{
		if (nDir2==SchedulerDir_LeftTop || nDir2==SchedulerDir_RightTop)
		{
			return 1;
		}
		else 
		{
			return -1;
		}
	}

	// Dir1 == Bottom
	if (nDir2==SchedulerDir_LeftBottom || nDir2==SchedulerDir_RightBottom)
	{
		return 1;
	}

	return -1;
}


int CPathScheduler_Quad_Active_TSP::IsChangeModuleIndex(int nDir1, int nDir2)
{
	return (nDir1!=nDir2);
}

int CPathScheduler_Quad_Active_TSP::IsChangeScanIndex(int nDir1, int nDir2)
{
	if (GetDirectionY(nDir1, nDir2)==-1) return 1;
	return 0;
}

int CPathScheduler_Quad_Active_TSP::IsChangeScanDir(int nDir1, int nDir2)
{
	if (GetDirectionX(nDir1, nDir2)==-1) return 1;
	return 0;
}

int CPathScheduler_Quad_Active_TSP::GetChangeModuleIndex(int nModuleIndex, int nOriginDir, int nBaseDir, int nGantryModuleCount, int nTotalModuleCount)
{
	int nTmpValue = nModuleIndex;

	if (GetDirectionX(nOriginDir, nBaseDir)==-1) // x ¿ª¹æÇâ
	{
		if (nTmpValue<nGantryModuleCount)
		{
			nTmpValue = nTmpValue + nGantryModuleCount;
		}
		else
		{
			nTmpValue = nTmpValue - nGantryModuleCount;
		}		
	}

	if (GetDirectionY(nOriginDir, nBaseDir)==-1) // y ¿ª¹æÇâ
	{
		if (nTmpValue<nGantryModuleCount)
		{
			nTmpValue = (nGantryModuleCount - 1) - nTmpValue;
		}
		else
		{
			nTmpValue = nTotalModuleCount + (nGantryModuleCount - 1) - nTmpValue;
		}	
	}

	return nTmpValue;
}

int CPathScheduler_Quad_Active_TSP::GetChangeScanIndex(int nScanIndex, int nTotalScanCount)
{
	return (nTotalScanCount - 1) - nScanIndex;
}

int CPathScheduler_Quad_Active_TSP::GetChangeScanDir(int nScanDir)
{
	return !nScanDir;
}

int CPathScheduler_Quad_Active_TSP::GetGantryIndex( int nModuleIdx )
{
	int nGantryCount = 2;
	int nGantryModuleCount	= m_PathSchedulerParam.GetModuleCount() / nGantryCount;

	if (nModuleIdx<nGantryModuleCount)
	{
		return 0;
	}

	if (nModuleIdx>nGantryModuleCount && nModuleIdx<m_PathSchedulerParam.GetModuleCount())
	{
		return 1;
	}

	return -1;
}