#include "StdAfx.h"
#include "AlignFinder_Corner.h"
#include "CHImageControls/CHImageProcess.h"
#include "CHImageControls/CHDataFitting.h"


CAlignFinder_Corner::CAlignFinder_Corner(int nIndex) : CAlignFinder(nIndex)
{

}

CAlignFinder_Corner::~CAlignFinder_Corner(void)
{

}

SAlignFindResult CAlignFinder_Corner::FindAlign(CCHImageData *pImageData, const SAlignFindParam& findParam, CCHImageData *pTempImage)
{
	m_pTempImage = pTempImage;

	m_findParam = findParam;

	return FindAlign(pImageData, pTempImage);
}

SAlignFindResult CAlignFinder_Corner::FindAlign(CCHImageData *pImageData, CCHImageData *pTempImage)
{
	m_pTempImage = pTempImage;

	if (pImageData==NULL) return SAlignFindResult();

	if (pImageData->GetImageExist()==FALSE) return SAlignFindResult();

	if (pImageData->GetChannels()!=1) return SAlignFindResult();

	return FindAlign((BYTE*)pImageData->GetImageBuffer(), pImageData->GetWidth(), pImageData->GetHeight());
}


SAlignFindResult CAlignFinder_Corner::FindAlign(BYTE* pImage, int nWidth, int nHeight, const SAlignFindParam& findParam)
{
	m_pTempImage = NULL;

	m_findParam = findParam;

	return FindAlign(pImage, nWidth, nHeight);
}


SAlignFindResult CAlignFinder_Corner::FindAlign(BYTE* pImage, int nWidth, int nHeight)
{
	SAlignFindResult findResult;
	if (pImage==NULL) return findResult;

	ResetImages();

	findResult.nResultCode = 0;

	// step1. Áß°£°á°ú À̹ÌÁö »ý¼º
	if (m_SourceImage.CreateImage(nWidth, nHeight, 8, 1)==FALSE) return findResult;
	memcpy(m_SourceImage.GetImageBuffer(), pImage, sizeof(BYTE)*nWidth*nHeight);



// 	CString str = _T("");
// 
// 	str.Format(_T("%d"), m_nIndex);
// 	AfxMessageBox(str);

// 	// step3. matching
// 	TRACE(_T("[A2E]CAlignFinder::FindAlign FindAlignMatching\n"));
// 	if (FindAlignMatching(findResult)==1)
// 	{
// 		return findResult;
// 	}

	// step2. edge find
	if (FindAlignCorner(findResult)==1) 
	{
		// draw result
		m_ResultImage.CopyImageFrom(&m_SourceImage);

		int nBoxWidth = 30;
		CPoint pt1((int)findResult.dPosX, 0);
		CPoint pt2((int)findResult.dPosX, m_ResultImage.GetHeight());
		m_ResultImage.DrawLine(pt1, pt2, RGB(0,255,0), 2);


		pt1 = CPoint(0, (int)findResult.dPosY);
		pt2 = CPoint(m_ResultImage.GetWidth(), (int)findResult.dPosY);
		m_ResultImage.DrawLine(pt1, pt2, RGB(0,255,0), 2);
		return findResult;
	}

	return findResult;
}


int CAlignFinder_Corner::FindAlignCorner(SAlignFindResult& findResult)
{
	findResult.nResultProcess = AlignProcess_Edge;

	// image check
	if (m_SourceImage.GetImageExist()==FALSE)
	{
		findResult.nResultCode = AlignEdge_None;
		return 0;
	}

	// sobel kernel size
	int nSobelKernelSize = (m_findParam.nCorner_SobelKernelSize >= 4) ? 5: 3;
	
	// MAKE EDGE IMAGE
	if (CCHImageProcess::ImageSobel(&m_SourceImage, &m_EdgeImage, 1, 1, nSobelKernelSize)!=1)
	{
		findResult.nResultCode = AlignEdge_BinaryFail;
		return -1;
	}

	// step2. sobel image 
	CCHImageData xImage, yImage;
	if (CCHImageProcess::ImageSobel(&m_SourceImage, &xImage, &yImage, nSobelKernelSize)!=1)
	{
		findResult.nResultCode = AlignEdge_EdgeFail;
		return -1;
	}
		
	CString strXImage = _T("");
	CString strYImage = _T("");

	strXImage.Format(_T("c:\\temp\\x_edge_image_%d.bmp"), m_nIndex);
	strYImage.Format(_T("c:\\temp\\y_edge_image_%d.bmp"), m_nIndex);
 	xImage.SaveImage(strXImage);
	yImage.SaveImage(strYImage);

	// step3. x projection
	VectorDouble vecX1, vecX2;
	if (ProjectionXDirection(&xImage, vecX1, vecX2, m_findParam.nCorner_EdgeKernelSize)!=1)
	{
		findResult.nResultCode = AlignEdge_LowScore;
		return -2;
	}

	strXImage.Format(_T("c:\\temp\\x_array_data_%d.csv"), m_nIndex);
	PrintArrayValue(strXImage, vecX2);

	// step3. y projection
	VectorDouble vecY1, vecY2;
	if (ProjectionYDirection(&yImage, vecY1, vecY2, m_findParam.nCorner_EdgeKernelSize)!=1)
	{
		findResult.nResultCode = AlignEdge_LowScore;
		return -3;
	}
	strYImage.Format(_T("c:\\temp\\y_array_data_%d.csv"), m_nIndex);
	PrintArrayValue(strYImage, vecY2);

	// step4. find cross point
	if (CalculateCrossPoint(m_findParam.nCorner_DirType, vecX2, vecY2, m_findParam.nCorner_EdgeThreshold, m_findParam.nCorner_IgnorePixel, findResult)!=1)
	{
		findResult.nResultCode = AlignEdge_EdgeFail;
		return -4;
	}

	findResult.nResultCode = AlignEdge_Success;

	return 1;
}

int CAlignFinder_Corner::ProjectionXDirection(CCHImageData *pImageData, VectorDouble& vecArray, VectorDouble& vecResult, int nKernelSize)
{
	if (pImageData==NULL) return 0;

	if (pImageData->GetImageExist()==FALSE) return 0;

	if (pImageData->GetChannels()!=1) return 0;

	BYTE* pBuff = (BYTE*)pImageData->GetImageBuffer();
	int nWidth = pImageData->GetWidth();
	int nHeight = pImageData->GetHeight();
	int nStep = pImageData->GetWidthStep();

	BYTE *pXBuf = NULL;
	double dTemp = 0;
	for (int i=0; i<nWidth; i++)
	{
		dTemp = 0.0;
		pXBuf = pBuff + i;
		for (int j=0; j<nHeight; j++)
		{
			dTemp += (*pXBuf);
			pXBuf += nStep;
		}
		dTemp /= nHeight;
		vecArray.push_back(dTemp);
	}

	// start
	for (int i=0; i<nKernelSize; i++)
	{
		vecResult.push_back(0.0);
	}

	double dLeft, dRight;
	for (int i=nKernelSize; i<nWidth-nKernelSize; i++)
	{
		dLeft = dRight = 0.0;
		for (int k=1; k<m_findParam.nKernelSize+1; k++)
		{
			dLeft += vecArray[i-k];
			dRight += vecArray[i+k];
		}
		vecResult.push_back(fabs(dLeft-dRight));
	}

	// end
	for (int i=0; i<nKernelSize; i++)
	{
		vecResult.push_back(0.0);
	}


	return 1;
}

int CAlignFinder_Corner::ProjectionYDirection(CCHImageData *pImageData, VectorDouble& vecArray, VectorDouble& vecResult, int nKernelSize)
{
	if (pImageData==NULL) return 0;

	if (pImageData->GetImageExist()==FALSE) return 0;

	if (pImageData->GetChannels()!=1) return 0;

	BYTE* pBuff = (BYTE*)pImageData->GetImageBuffer();
	int nWidth = pImageData->GetWidth();
	int nHeight = pImageData->GetHeight();
	int nStep = pImageData->GetWidthStep();

	BYTE *pYBuf = NULL;
	double dTemp = 0;
	for (int i=0; i<nHeight; i++)
	{
		dTemp = 0.0;
		pYBuf = pBuff + (i*nStep);
		for (int j=0; j<nWidth; j++)
		{
			dTemp += (*pYBuf);
			pYBuf++;
		}
		dTemp /= nWidth;
		vecArray.push_back(dTemp);
	}

	// start
	for (int i=0; i<nKernelSize; i++)
	{
		vecResult.push_back(0.0);
	}

	double dLeft, dRight;
	for (int i=nKernelSize; i<nHeight-nKernelSize; i++)
	{
		dLeft = dRight = 0.0;
		for (int k=1; k<nKernelSize+1; k++)
		{
			dLeft += vecArray[i-k];
			dRight += vecArray[i+k];
		}
		vecResult.push_back(fabs(dLeft-dRight));
	}

	// end
	for (int i=0; i<nKernelSize; i++)
	{
		vecResult.push_back(0.0);
	}

	return 1;
}

int FindRightPos(const VectorDouble& vecData, int nThreshold, int nMargin, VectorDouble& vecXPos, VectorDouble& vecYPos)
{
	vecXPos.resize(3);
	vecYPos.resize(3);

	bool bFind = false;
	int nMaxPos = 0;
	for (int i=int(vecData.size()-nMargin-1); i>-1; i--)
	{
		if (!bFind)
		{
			if (vecData[i] >= nThreshold)
			{
				nMaxPos = i;
				vecYPos[1] = vecData[i];
				bFind = true;
			}
		}
		else
		{
			if (vecData[i] <= nThreshold)
			{
				break;
			}
			else
			{
				if (vecData[i] > vecYPos[1])
				{
					nMaxPos = i;
					vecYPos[1] = vecData[i];
				}
			}
		}
	}

	if (bFind) 
	{
		vecXPos[0]	= nMaxPos - 1;
		vecXPos[1]	= nMaxPos;
		vecXPos[2]	= nMaxPos + 1;


		vecYPos[0]	= vecData[nMaxPos - 1];
		vecYPos[2]	= vecData[nMaxPos + 1];

		return 1;
	}

	return 0;
}



int FindLeftPos(const VectorDouble& vecData, int nThreshold, int nMargin, VectorDouble& vecXPos, VectorDouble& vecYPos)
{
	vecXPos.resize(3);
	vecYPos.resize(3);

	bool bFind = false;
	int nMaxPos = 0;
	for (int i=nMargin; i<(int)vecData.size(); i++)
	{
		if (!bFind)
		{
			if (vecData[i] >= nThreshold)
			{
				nMaxPos = i;
				vecYPos[1] = vecData[i];
				bFind = true;
			}
		}
		else
		{
			if (vecData[i] <= nThreshold)
			{
				break;
			}
			else
			{
				if (vecData[i] > vecYPos[1])
				{
					nMaxPos = i;
					vecYPos[1] = vecData[i];
				}
			}
		}
	}

	if (bFind) 
	{
		vecXPos[0]	= nMaxPos - 1;
		vecXPos[1]	= nMaxPos;
		vecXPos[2]	= nMaxPos + 1;
		

		vecYPos[0]	= vecData[nMaxPos - 1];
		vecYPos[2]	= vecData[nMaxPos + 1];

		return 1;
	}

	return 0;
}

double CalculateSubPixel(VectorDouble& vecXData, VectorDouble& vecYData)
{
	VectorDouble vecResult;
	if (CCHDataFitting::GaussianFitting(vecXData, vecYData, vecResult)!=1)
	{
		return -1.0;
	}

	return vecResult[1];
}

int CAlignFinder_Corner::CalculateCrossPoint(int nDirType, const VectorDouble& vecX, const VectorDouble& vecY, int nThreshold, int nIgnore, SAlignFindResult& sFindResult)
{
	int nReturnCode = 0;

	VectorDouble vecXPos, vecYPos;
	vecXPos.clear();
	vecYPos.clear();

	switch(nDirType)
	{
	case 0: // left top

		// find x pos
		nReturnCode = FindLeftPos(vecX, nThreshold, nIgnore, vecXPos, vecYPos);
		if (nReturnCode==0) return 0;
		// cal sub pixel x
		sFindResult.dPosX = CalculateSubPixel(vecXPos, vecYPos);

		vecXPos.clear(); vecYPos.clear();

		// find y pos
		nReturnCode = FindLeftPos(vecY, nThreshold, nIgnore, vecXPos, vecYPos);
	if (nReturnCode==0) return 0;
		// cal cub pixel y
		sFindResult.dPosY = CalculateSubPixel(vecXPos, vecYPos);
		
		sFindResult.nResultCode = 1;
		return 1;

		break;

	case 1: // left bottom

		// find x pos
		nReturnCode = FindLeftPos(vecX, nThreshold, nIgnore, vecXPos, vecYPos);
		if (nReturnCode==0) return 0;
	// cal sub pixel x
	sFindResult.dPosX = CalculateSubPixel(vecXPos, vecYPos);

		vecXPos.clear(); vecYPos.clear();

		// find y pos
		nReturnCode = FindRightPos(vecY, nThreshold, nIgnore, vecXPos, vecYPos);
		if (nReturnCode==0) return 0;
		// cal cub pixel y
		sFindResult.dPosY = CalculateSubPixel(vecXPos, vecYPos);

		sFindResult.nResultCode = 1;
		return 1;

		break;

	case 2: // right top

		// find x pos
		nReturnCode = FindRightPos(vecX, nThreshold, nIgnore, vecXPos, vecYPos);
	if (nReturnCode==0) return 0;
		// cal sub pixel x
		sFindResult.dPosX = CalculateSubPixel(vecXPos, vecYPos);

		vecXPos.clear(); vecYPos.clear();

		// find y pos
		nReturnCode = FindLeftPos(vecY, nThreshold, nIgnore, vecXPos, vecYPos);
		if (nReturnCode==0) return 0;
	// cal cub pixel y
	sFindResult.dPosY = CalculateSubPixel(vecXPos, vecYPos);

	sFindResult.nResultCode = 1;
		return 1;

		break;

	case 3: // right bottom

		// find x pos
		nReturnCode = FindRightPos(vecX, nThreshold, nIgnore, vecXPos, vecYPos);
		if (nReturnCode==0) return 0;
		// cal sub pixel x
		sFindResult.dPosX = CalculateSubPixel(vecXPos, vecYPos);
	
		vecXPos.clear(); vecYPos.clear();

		// find y pos
		nReturnCode = FindRightPos(vecY, nThreshold, nIgnore, vecXPos, vecYPos);
		if (nReturnCode==0) return 0;
		// cal cub pixel y
		sFindResult.dPosY = CalculateSubPixel(vecXPos, vecYPos);

		sFindResult.nResultCode = 1;
	return 1;

		break;
	}
	
	return 0;
}


void CAlignFinder_Corner::PrintArrayValue(const CString& strFilename, const VectorDouble& vecArray)
{
	FILE *fp = NULL;
	_tfopen_s(&fp, strFilename, _T("w"));

	if (fp==NULL) return;

	for (int i=0; i<(int)vecArray.size(); i++)
	{
		_ftprintf_s(fp, _T("%d, %.3lf\n"), i, vecArray[i]);	
	}

	fclose(fp);
}