#include "StdAfx.h"
#include "PathScheduler_Quad_Active_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);
}


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_Sorting::CPathScheduler_Quad_Active_Sorting(void)
{
}

CPathScheduler_Quad_Active_Sorting::~CPathScheduler_Quad_Active_Sorting(void)
{
}

int CPathScheduler_Quad_Active_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.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();

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

		SRangeData* pRangeData = GetScanRangeData(nModuleIdx, 0); // ½ºÄµ¼ö´Â 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);
		}

		// ½ÃÀÛÀ§Ä¡´Â ÇöÀç¸ðµâÀÇ À§Ä¡ÀÌ´Ù.
		sStartPath = vecStartPath[nModuleIdx];
		
		// ¿µ¿ªº° ½ºÄÉÁ층 ¼öÇà.
		CPathSchedulerResult schedulerResult(nModuleIdx, 0);

		// °æ·Î °è»ê
		int nCount = CalculatePath(nModuleIdx, 0, sStartPath, vecPathData, *pRangeData, schedulerResult);
		nTotalCount += nCount;

		// ¸ðµâÀÇ Ãæµ¹À§Ä¡ ÀúÀå
		schedulerResult.SetCollisionPositionX(pRangeData->nColliPosX);
		schedulerResult.SetCollisionPositionY(pRangeData->nColliPosY);

		// °á°ú ÀúÀå.
		m_vecPathSchedulerResult.push_back(schedulerResult);
	}

	if (fp) fclose(fp);

	return int(m_vecPathSchedulerResult.size());
}


int CPathScheduler_Quad_Active_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

	SSchedulerResult sStartPos;
	sStartPos.dPositionX = double(startPath.nPosX / 1000.0);
	sStartPos.dPositionY = double(startPath.nPosY / 1000.0);


	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;
		sResult.dAutoFocusTime		= m_PathSchedulerParam.GetAutoFocusTime();
		sResult.dImageGrabTime		= m_PathSchedulerParam.GetImageGrabTime();
		sResult.dMotionDelayTime	= m_PathSchedulerParam.GetMotionDelayTime();

		memcpy(sResult.pDataType, vecPathData[nIdx].pDataType, sizeof(int)*PATH_DATA_TYPE_COUNT);
		
		// exist start pos
		if (!CalculateDistanceSpeedTime(sResult, sStartPos))
		{
			continue;
		}

		// save cur_pos to start_pos
		sStartPos = sResult;
	
		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_Active_Sorting::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());

	// get left or right
	VectorPathData vecLeft, vecRight;
	for (constVectorPathDataIt it=vecPathData.begin(); it!=vecPathData.end(); it++)
	{
		if (it->nPosX > nResultPosX)
		{
			vecRight.push_back(*it);
		}
		else
		{
			vecLeft.push_back(*it);
		}
	}

	// get y pos
	int nResultLeftY = GetCollisionPosY(vecLeft, m_PathSchedulerParam.GetGlassSizeY(), 0);

	// get y pos
	int nResultRightY = GetCollisionPosY(vecRight, m_PathSchedulerParam.GetGlassSizeY(), 0);

	
	SRangeData range;
	range.nScanIdx = 0;
	range.nScanDir = 0;

	// left top
	range.nTop				= 0;
	range.nBottom			= nResultLeftY;
	range.nLeft				= 0;
	range.nRight			= nResultPosX;
	
	range.nModuleIdx		= 0;
	range.nScanStartX		= range.nLeft;	//  ½ºÄµ ½ÃÀÛÀ§Ä¡				
	range.nScanEndX			= range.nRight; // ½ºÄµ Á¾·áÀ§Ä¡
	range.nColliPosX		= range.nRight - m_PathSchedulerParam.GetCollisionDistX();
	range.nColliPosY		= range.nBottom - m_PathSchedulerParam.GetCollisionDistY();
	vecRangeData.push_back(range);

	// left bottom
	range.nTop				= nResultLeftY;
	range.nBottom			= m_PathSchedulerParam.GetGlassSizeY();
	range.nLeft				= 0;
	range.nRight			= nResultPosX;
	
	range.nModuleIdx		= 1;
	range.nScanStartX		= range.nLeft;	//  ½ºÄµ ½ÃÀÛÀ§Ä¡				
	range.nScanEndX			= range.nRight; // ½ºÄµ Á¾·áÀ§Ä¡
	range.nColliPosX		= range.nRight - m_PathSchedulerParam.GetCollisionDistX();
	range.nColliPosY		= range.nTop + m_PathSchedulerParam.GetCollisionDistY();
	vecRangeData.push_back(range);

	// right top
	range.nTop				= 0;
	range.nBottom			= nResultRightY;
	range.nLeft				= nResultPosX;
	range.nRight			= m_PathSchedulerParam.GetGlassSizeX();

	range.nModuleIdx		= 2;
	range.nScanStartX		= range.nLeft;	//  ½ºÄµ ½ÃÀÛÀ§Ä¡				
	range.nScanEndX			= range.nRight; // ½ºÄµ Á¾·áÀ§Ä¡
	range.nColliPosX		= range.nLeft + m_PathSchedulerParam.GetCollisionDistX();
	range.nColliPosY		= range.nBottom - m_PathSchedulerParam.GetCollisionDistY();
	vecRangeData.push_back(range);

	// right bottom
	range.nTop				= nResultRightY;
	range.nBottom			= m_PathSchedulerParam.GetGlassSizeY();
	range.nLeft				= nResultPosX;
	range.nRight			= m_PathSchedulerParam.GetGlassSizeX();

	range.nModuleIdx		= 3;
	range.nScanStartX		= range.nLeft;	//  ½ºÄµ ½ÃÀÛÀ§Ä¡				
	range.nScanEndX			= range.nRight; // ½ºÄµ Á¾·áÀ§Ä¡
	range.nColliPosX		= range.nLeft + m_PathSchedulerParam.GetCollisionDistX();
	range.nColliPosY		= range.nTop + m_PathSchedulerParam.GetCollisionDistY();
	vecRangeData.push_back(range);

	// set info
	int nGlassDivX			= nGlassSizeX / nGantryCount;
	int nGantryModuleCount	= nModuleCount / nGantryCount;
	int nGlassDivModuleY	= nGlassSizeY / nGantryModuleCount;
	int nGlassDivScanY		= nGlassDivModuleY;
	int nScanCount			= 1;
	int nScanIdx			= 0;

	VectorRangeData vecResult;
	
	for (int nModuleIdx=0; nModuleIdx<nModuleCount; nModuleIdx++)
	{
		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 bScanDir = IsChangeScanDir(nOriginDir, nBaseDir);
		if (bScanDir==1)	// 10
		{
			// change scan dir
			int nTmpValue = GetChangeScanDir(newData.nScanDir);
 			newData.nScanDir = nTmpValue;

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


		vecResult.push_back(newData);
	}

	// set member data
	vecRangeData = vecResult;

	return (int)vecRangeData.size();
}


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

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

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

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

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

int CPathScheduler_Quad_Active_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;
}