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


#include "MotionInfo_Axis.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);
}

CPathScheduler_Quad_Sorting::CPathScheduler_Quad_Sorting(void)
{
}


CPathScheduler_Quad_Sorting::~CPathScheduler_Quad_Sorting(void)
{
}

int CPathScheduler_Quad_Sorting::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.GetScanCount(), 
		m_PathSchedulerParam.GetGlassSizeX(),
		m_PathSchedulerParam.GetGlassSizeY(),
		m_PathSchedulerParam.GetScanMargin(),
		m_PathSchedulerParam.GetOriginDir(),
		m_PathSchedulerParam.GetBaseModuleDir(),
		m_vecScanRangeData);

	if (nRangeCount<1) return 0;


	int nLeftCollisionPosX = -1;
	int nRightCollisionPosX = -1;

	// set collision x pos
	switch(m_PathSchedulerParam.GetOriginDir())
	{
	case SchedulerDir_LeftTop: // left top
	case SchedulerDir_LeftBottom: // left bottom
		nLeftCollisionPosX	= (m_PathSchedulerParam.GetGlassSizeX() / 2) - m_PathSchedulerParam.GetCollisionDistX();
		nRightCollisionPosX	= (m_PathSchedulerParam.GetGlassSizeX() / 2) + m_PathSchedulerParam.GetCollisionDistX();
	case SchedulerDir_RightTop: // right top
	case SchedulerDir_RightBottom: // right bottom
		nLeftCollisionPosX	= (m_PathSchedulerParam.GetGlassSizeX() / 2) + m_PathSchedulerParam.GetCollisionDistX();
		nRightCollisionPosX	= (m_PathSchedulerParam.GetGlassSizeX() / 2) - m_PathSchedulerParam.GetCollisionDistX();
		break;
	}

	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]\n"));
	}
		
	int nTotalCount = 0;
	SPathData sStartPath; // ¸ðµâº° ½ºÄµ ½ÃÀÛÀ§Ä¡ ÀúÀå.
	for (int nModuleIdx=0; nModuleIdx<m_PathSchedulerParam.GetModuleCount(); nModuleIdx++)
	{
		if (int(vecStartPath.size()) <= nModuleIdx) continue;

		for (int nScanIdx = 0; nScanIdx<m_PathSchedulerParam.GetScanCount(); nScanIdx++)
		{
			SRangeData* pRangeData = GetScanRangeData(nModuleIdx, nScanIdx);

			if (pRangeData==NULL) continue;

			if (fp)
			{
				_ftprintf_s(fp, _T("%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);
			}

			// add range rect
			m_vecRangeRect.push_back(CRect(pRangeData->nLeft, pRangeData->nTop, pRangeData->nRight, pRangeData->nBottom));

			if (nScanIdx==0) // first scan ÀÇ ½ÃÀÛÀ§Ä¡´Â ÇöÀç ¸ðµâÀÇ À§Ä¡ÀÌ´Ù.
			{
				sStartPath = vecStartPath[nModuleIdx];
			}

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

			// °ÕÆ®¸® Ãæµ¹À§Ä¡ ÀúÀå.
			int nGantryIdx = GetGantryIndex(nModuleIdx);
			switch(nGantryIdx)
			{
			case 0:
				schedulerResult.SetCollisionPositionX(nLeftCollisionPosX);
				break;

			case 1:
				schedulerResult.SetCollisionPositionX(nRightCollisionPosX);
				break;
			}

			int nCount = CalculatePath(nModuleIdx, nScanIdx, sStartPath, vecPathData, *pRangeData, schedulerResult);
			nTotalCount += nCount;
			if (nCount>1)
			{
				// ´ÙÀ½ ½ºÄµÀÇ ½ÃÀÛÀ§Ä¡´Â ÀÌÀü ½ºÄµÀÇ Á¾·á À§Ä¡ÀÌ´Ù.
				const SSchedulerResult* pNode = schedulerResult.GetPathSchedulerResult(schedulerResult.GetPathSchedulerResultCount() - 1);
				if (pNode)
				{
					sStartPath.nPosX = (pRangeData->nScanEndX);
					sStartPath.nPosY = int(pNode->dPositionY * 1000);
				}
				m_vecPathSchedulerResult.push_back(schedulerResult);
			}
			else // ´ë»ó °áÇÔÀÌ ¾øÀ»¶§ xÃà À§Ä¡¸¸ º¯°æÇÑ´Ù.
			{
				sStartPath.nPosX = (pRangeData->nScanEndX); // 
			}



		}
	}

	if (fp)
		fclose(fp);

	return int(m_vecPathSchedulerResult.size());
}


int CPathScheduler_Quad_Sorting::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);
		}
	}

	// sort for scan direction
	if (rtRange.nScanDir==0)
	{
		std::sort(vecPathData.begin(), vecPathData.end(), ComparePosAscend);
	}
	else
	{
		std::sort(vecPathData.begin(), vecPathData.end(), ComparePosDescend);
	}

	// set result count
	scheduleResult.SetScheduleResultCount((int)vecPathData.size());

	FILE *fp = NULL;
	CString strFilename = _T("");
	strFilename.Format(_T("c:\\temp\\%d_%d.csv"), nModuleIdx, nScanIdx);
	_tfopen_s(&fp, strFilename, _T("w"));
	if (fp)
	{
		_ftprintf_s(fp, _T("[Left], [Right], [Top], [Bottom], [ScanStart], [ScanEnd], [ScanDir]\n"));
		_ftprintf_s(fp, _T("%d, %d, %d, %d, %d, %d, %d\n"), 
			rtRange.nLeft, 
			rtRange.nRight, 
			rtRange.nTop, 
			rtRange.nBottom, 
			rtRange.nScanStartX, 
			rtRange.nScanEndX, 
			rtRange.nScanDir);
		_ftprintf_s(fp, _T("[Index], [PosX], [PosY]\n"));
	}

	// calculate path
	for (int nIdx=0; nIdx<(int)vecPathData.size(); nIdx++)
	{
		SSchedulerResult sResult;
		sResult.nTotalIndex			= vecPathData[nIdx].nIndex;
		sResult.dPositionX			= double(vecPathData[nIdx].nPosX) / 1000.0;
		sResult.dPositionY			= double(vecPathData[nIdx].nPosY) / 1000.0;

		memcpy(sResult.pDataType, vecPathData[nIdx].pDataType, sizeof(int)*PATH_DATA_TYPE_COUNT);

		if (fp)
			_ftprintf_s(fp, _T("%d, %d, %d\n"), nIdx, vecPathData[nIdx].nPosX, vecPathData[nIdx].nPosY);

		scheduleResult.SetScheduleResult(nIdx, sResult);
	}

	if (fp)
		fclose(fp);

	return scheduleResult.GetPathSchedulerResultCount();
}



int CPathScheduler_Quad_Sorting::CalculateRangeRect(int nGantryCount, int nModuleCount, int nScanCount, int nGlassSizeX, int nGlassSizeY, int nScanMargin, int nOriginDir, int nBaseDir, VectorRangeData& vecRangeData)
{
	int nGlassDivX			= nGlassSizeX / nGantryCount;
	int nGantryModuleCount	= nModuleCount / nGantryCount;
	int nGlassDivModuleY	= nGlassSizeY / nGantryModuleCount;
	int nGlassDivScanY		= nGlassDivModuleY / nScanCount;

	SRangeData tmpRange;

	// x axis
	int nTotalModuleIdx = 0;
	for (int nGantryIdx=0; nGantryIdx<nGantryCount; nGantryIdx++)
	{
		// scan x range
		tmpRange.nLeft	= nGantryIdx * nGlassDivX;
		tmpRange.nRight	= tmpRange.nLeft + nGlassDivX;

		for (int nModuleIdx=0; nModuleIdx<nGantryModuleCount; nModuleIdx++)
		{
			// module y range
			int nModuleStart 	= nModuleIdx * nGlassDivModuleY;

			for (int nScanIdx = 0; nScanIdx<nScanCount; nScanIdx++)
			{
				// scan y  range
				tmpRange.nTop		= nModuleStart + (nScanIdx*nGlassDivScanY);
				tmpRange.nBottom	= tmpRange.nTop + nGlassDivScanY;

				// ½ºÄµ y ½ÃÀÛÀ§Ä¡ ±âº»°ªÀº ¿µ¿ªÀÇ Áß¾ÓÀÓ.
			//	tmpRange.nScanStartY	= tmpRange.nTop + ((tmpRange.nBottom - tmpRange.nTop) / 2);
				tmpRange.nModuleIdx		= nTotalModuleIdx;
				tmpRange.nScanIdx		= nScanIdx;
				tmpRange.nScanDir		= (nScanIdx % 2);

				if (tmpRange.nScanDir==0) // Á¤¹æÇâ ½ºÄµ
				{
					tmpRange.nScanStartX	= tmpRange.nLeft - nScanMargin;	//  ½ºÄµ ½ÃÀÛÀ§Ä¡				
					tmpRange.nScanEndX		= tmpRange.nRight + nScanMargin; // ½ºÄµ Á¾·áÀ§Ä¡
				}
				else // ¿ª¹æÇâ ½ºÄµ
				{
					tmpRange.nScanStartX	= tmpRange.nRight + nScanMargin; // ½ºÄµ ½ÃÀÛÀ§Ä¡
					tmpRange.nScanEndX		= tmpRange.nLeft - nScanMargin;	//  ½ºÄµ Á¾·áÀ§Ä¡				
				}

				vecRangeData.push_back(tmpRange);
			}

			nTotalModuleIdx++;
		}
	}

	// change range data (origin_dir / base_dir)

	// 	int nModuleIdx;		// module index
	// 	int nScanIdx;		// scan index
	// 	int nScanDir;		// scan dir 0/1
	// 
	// 	int nScanStartX;	// scan start x
	// 	int nScanStartY;	// scan start y
	// 	int nScanEndX;		// scan end x
	// 
	// 	int nLeft;			// range left
	// 	int nTop;			// range top
	// 	int nRight;			// range right
	// 	int nBottom;		// range bottom

	VectorRangeData vecResult;

	for (int nModuleIdx=0; nModuleIdx<nModuleCount; nModuleIdx++)
	{
		for (int nScanIdx=0; nScanIdx<nScanCount; nScanIdx++)
		{
			SRangeData *pData = GetScanRangeData(nModuleIdx, nScanIdx);
			if (pData==NULL) continue;

			SRangeData newData = *pData;

			if (IsChangeModuleIndex(nOriginDir, nBaseDir))
			{
				int nTmpValue= GetChangeModuleIndex(nModuleIdx, nOriginDir, nBaseDir, nGantryModuleCount, nModuleCount);
				newData.nModuleIdx = nTmpValue;
			}


			int bScanIdx = IsChangeScanIndex(nOriginDir, nBaseDir);
			int bScanDir = IsChangeScanDir(nOriginDir, nBaseDir);

			if (bScanDir==1 && bScanIdx==1) // 11
			{
				// change scan index
				int nTmpValue = GetChangeScanIndex(nScanIdx, nScanCount);
				newData.nScanIdx = nTmpValue;
			}
			else if (bScanDir==1 && bScanIdx==0) // 10
			{
				// change scan dir
				int nTmpValue = GetChangeScanDir(newData.nScanDir);
				newData.nScanDir = nTmpValue;

				// change scan pos
				swap_value(newData.nScanStartX,	newData.nScanEndX);
			}
			else if (bScanDir==0 && bScanIdx==1) // 01
			{
				// change scan index
				int nTmpValue = GetChangeScanIndex(nScanIdx, nScanCount);
				newData.nScanIdx = nTmpValue;

				// change scan dir
				nTmpValue = GetChangeScanDir(newData.nScanDir);
				newData.nScanDir = nTmpValue;

				// change scan pos
				swap_value(newData.nScanStartX,	newData.nScanEndX);
			}
			// 00
			

			vecResult.push_back(newData);
		}
	}
	
	// set member data
	vecRangeData = vecResult;

	return (int)vecRangeData.size();
}


SRangeData* CPathScheduler_Quad_Sorting::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_Sorting::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_Sorting::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_Sorting::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_Sorting::IsChangeModuleIndex(int nDir1, int nDir2)
{
	return (nDir1!=nDir2);
}

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

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

int CPathScheduler_Quad_Sorting::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_Sorting::GetChangeScanIndex(int nScanIndex, int nTotalScanCount)
{
	return (nTotalScanCount - 1) - nScanIndex;
}

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

int CPathScheduler_Quad_Sorting::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;
}