#include "StdAfx.h"
#include "MosisBuffer/MosisMatch.h"
#include "FilterInspect.h"
#include <math.h>

//#define		SAVE_IMAGE
//#define		SAVE_IMAGE_C2C
#define		FILTER_PATH "D:\\Image\\Filter"
#define		FILTER_C2C_PATH "D:\\Image\\FilterC2C"

CFilterInspect::CFilterInspect(void)
{
}

CFilterInspect::~CFilterInspect(void)
{
}

#define Rect_Grow(rect, dx, dy) {rect.left-= dx, rect.right+= dx, rect.top-= dy, rect.bottom+= dy;}

BOOL CFilterInspect::FilteringTiltC2CVert(LPBYTE pOrg,LPBYTE pFirst,LPBYTE pSecond,CSize szFrame,int nLowThres,int nHighThres,int nSupress,int nDefectIdx)
{
#ifdef SAVE_IMAGE_C2C
	CreateDirectory(FILTER_C2C_PATH,NULL);
#endif

	if(pOrg == NULL || pFirst == NULL || pSecond == NULL)
		return TRUE;

	int			nSource,nFirstVal,nSecondVal;
	BOOL		bSuccess;
	const int	nBigSize = 64;
	const int	nMatMargin = 8;
	const int	nMaxYValue = nMatMargin/2;
	CRect		rectBig(0,0,nBigSize*2,nBigSize*2);
	CRect		rectSmall(0,0,(nBigSize-nMatMargin)*2,(nBigSize-nMatMargin)*2);
	LPBYTE		pBuffer;
	double		dNearX,dFarX,dNearY,dFarY;
	double		dSection = (double)nHighThres/255.;

	stMosisMatchParam	matchParam;
	CMatchPt			matchResult;
	CMosisMatch			match;

	int			nDefectBuf = nBigSize*szFrame.cx+nBigSize;
	pBuffer = pOrg-nDefectBuf;
	CCropBuffer			bigBuf(CMosisBuffer(pBuffer,szFrame.cx,szFrame.cy),rectBig);

	int			nDefectSmallBuf = (nBigSize-nMatMargin)*szFrame.cx+(nBigSize-nMatMargin);
	pBuffer = pFirst-nDefectSmallBuf;
	CCropBuffer			firstBuf(CMosisBuffer(pBuffer,szFrame.cx,szFrame.cy),rectSmall);

	pBuffer = pSecond-nDefectSmallBuf;
	CCropBuffer			secondBuf(CMosisBuffer(pBuffer,szFrame.cx,szFrame.cy),rectSmall);

	matchParam.m_Method= stMosisMatchParam::CV_TM_CCOEFF_NORMED;
	matchParam.m_Acceptance= 0.3;
	matchParam.m_Pyramid= 0;
	matchParam.m_bInterpol= TRUE;
	
	match.SetBigImage(bigBuf,rectBig);	
	match.SetSmallImage(firstBuf,rectSmall);

	dNearX = dFarX = dNearY = dFarY = 0.;
	bSuccess= match.DoTemplateMatch(matchResult, matchParam);
	if(bSuccess == TRUE)
	{
		dNearX = (nMatMargin-matchResult.m_X);
		dNearY = (nMatMargin-matchResult.m_Y);
		if(abs(dNearY) >= nMaxYValue)
			dNearY = 0.;
		if(abs(dNearX >= nMatMargin))
			dNearX = 0.;
	}
	else
	{
#ifdef SAVE_IMAGE_C2C		
		CString		strFile;		
		strFile.Format("%s\\[%]C2C_FilterFail_First_s[%.3f].bmp",FILTER_C2C_PATH,nDefectIdx,matchResult.m_Score);
		CBufferAttach		attchOrg(strFile);
		attchOrg.AttachToFile(bigBuf);
		pBuffer = pFirst-nDefectBuf;
		CCropBuffer			buffer(CMosisBuffer(pBuffer,szFrame.cx,szFrame.cy),rectBig);	
		attchOrg.AttachToFile(buffer);
#endif
		return TRUE;
	}

	match.SetSmallImage(secondBuf,rectSmall);
	bSuccess= match.DoTemplateMatch(matchResult, matchParam);
	if(bSuccess == TRUE)
	{
		dFarX = (nMatMargin-matchResult.m_X);	
		dFarY = (nMatMargin-matchResult.m_Y);	
		if(abs(dFarY) >= nMaxYValue)
			dFarY = 0.;
		if(abs(dFarX >= nMatMargin))
			dFarX = 0.;
	}
	else
	{
#ifdef SAVE_IMAGE_C2C		
		CString		strFile;		
		strFile.Format("%s\\[%d]C2C_FilterFail_Second_s[%.3f].bmp",FILTER_C2C_PATH,nDefectIdx,matchResult.m_Score);
		CBufferAttach		attchOrg(strFile);
		attchOrg.AttachToFile(bigBuf);
		pBuffer = pSecond-nDefectBuf;
		CCropBuffer			buffer(CMosisBuffer(pBuffer,szFrame.cx,szFrame.cy),rectBig);	
		attchOrg.AttachToFile(buffer);
#endif
		return TRUE;
	}

	nSource = GetArea2x2Value(pOrg,szFrame,0,0,nSupress);
	nFirstVal = GetArea2x2Value(pFirst,szFrame,dNearX,dNearY,nSupress);
	nSecondVal = GetArea2x2Value(pSecond,szFrame,dFarX,dFarY,nSupress);
	
	int			nThres = nLowThres + (int)(dSection*(double)nSource);

	if(abs(nSource-nFirstVal) >= nThres && abs(nSource-nSecondVal) >= nThres)
	{
	/*
#ifdef SAVE_IMAGE_C2C		
		CString		strFile;		
		strFile.Format("%s\\[%d]C2C_FilterFail_T[%d]_sc[%.3f]_org[%d]_f[%d,%.3f,%.3f]_s[%d,%.3f,%.3f].bmp",FILTER_C2C_PATH,nDefectIdx,nThres,matchResult.m_Score,nSource,nFirstVal,dNearX,dNearY,nSecondVal,dFarX,dFarY);
		CBufferAttach		attchOrg(strFile);
		attchOrg.AttachToFile(bigBuf);
		pBuffer = pFirst-nDefectBuf;
		CCropBuffer			buffer(CMosisBuffer(pBuffer,szFrame.cx,szFrame.cy),rectBig);	
		attchOrg.AttachToFile(buffer);
		pBuffer = pSecond-nDefectBuf;
		CCropBuffer			buffer2(CMosisBuffer(pBuffer,szFrame.cx,szFrame.cy),rectBig);	
		attchOrg.AttachToFile(buffer2);
#endif
		*/
		return TRUE;			
	}		

#ifdef SAVE_IMAGE_C2C		
	CString		strFile;		
	strFile.Format("%s\\C2C_FilterSuccess_Final_T[%d]_sc[%.3f]_org[%d]_f[%d,%.3f,%.3f]_s[%d,%.3f,%.3f].bmp",FILTER_C2C_PATH,nThres,matchResult.m_Score,nSource,nFirstVal,dNearX,dNearY,nSecondVal,dFarX,dFarY);
	CBufferAttach		attchOrg(strFile);
	attchOrg.AttachToFile(bigBuf);
	pBuffer = pFirst-nDefectBuf;
	CCropBuffer			buffer(CMosisBuffer(pBuffer,szFrame.cx,szFrame.cy),rectBig);	
	attchOrg.AttachToFile(buffer);
	pBuffer = pSecond-nDefectBuf;
	CCropBuffer			buffer2(CMosisBuffer(pBuffer,szFrame.cx,szFrame.cy),rectBig);	
	attchOrg.AttachToFile(buffer2);
#endif

	return FALSE;
}

BOOL CFilterInspect::FilteringTiltVertImpl(BOOL b2by2, LPBYTE lpBuffer,CSize szFrame,int nThres,double dScanPitch,CPoint pointDefect,CRect rectIns,int nSupress)
{
#ifdef SAVE_IMAGE
	CreateDirectory(FILTER_PATH,NULL);
#endif

	if(lpBuffer == NULL)
		return TRUE;


	double		dNearX,dFarX,dNearY,dFarY;
	CRect		rectDefect,rectTgt[2];		
	int			nSource,nFirstVal,nSecondVal;
	double		yPitch = dScanPitch;	
	BOOL		bSuccess;
	int			nScanPitch = (int)(yPitch);
	int			nScanPitch2= (int)(yPitch*2);
	int			nBigSize = 32;
	int			nMatMargin = 8;	

	stMosisMatchParam	matchParam;
	CMatchPt	matchResult, matchResultFar;

	CMosisMatch			match;
	CMosisBuffer		bigBuf(lpBuffer,szFrame.cx,szFrame.cy);			

	matchParam.m_Method= stMosisMatchParam::CV_TM_CCOEFF_NORMED;
	matchParam.m_Acceptance= 0.3;
	matchParam.m_Pyramid= 0;
	matchParam.m_bInterpol= TRUE;

	rectDefect = CRect(pointDefect.x,pointDefect.y,pointDefect.x,pointDefect.y);
	rectTgt[0] = rectTgt[1] = rectDefect;
	Rect_Grow(rectDefect,nBigSize,nBigSize);
	if(rectDefect.top < 0)
	{
		rectDefect.top = 0;
		rectDefect.bottom = nBigSize*2;
	}
	match.SetBigImage(bigBuf,rectDefect);		

#if defined(MOSIS_OLD_TILT_BUG)
	if((rectIns.bottom-pointDefect.y) < (int)yPitch)
	{			
		rectTgt[0].OffsetRect(0,-nScanPitch);
		rectTgt[1].OffsetRect(0,-nScanPitch*2);
		dNearY = pointDefect.y-yPitch;
		dFarY = pointDefect.y-yPitch*2;
	}
	else if((pointDefect.y-rectIns.top) < (int)yPitch)
	{			
		rectTgt[0].OffsetRect(0,nScanPitch);
		rectTgt[1].OffsetRect(0,nScanPitch*2);
		dNearY = pointDefect.y+yPitch;
		dFarY = pointDefect.y+yPitch*2;
	}
	else
	{
		rectTgt[0].OffsetRect(0,-nScanPitch);
		rectTgt[1].OffsetRect(0,nScanPitch);
		dNearY = pointDefect.y-yPitch;
		dFarY = pointDefect.y+yPitch;
	}
#else
	if((rectIns.bottom-pointDefect.y) < (int)yPitch)
	{			
		rectTgt[0].OffsetRect(0,-nScanPitch- 1);
		rectTgt[1].OffsetRect(0,-nScanPitch2- 1);
		dNearY = (int)(pointDefect.y-yPitch);
		dFarY = (int)(pointDefect.y-yPitch*2);
	}
	else if((pointDefect.y-rectIns.top) < (int)yPitch)
	{			
		rectTgt[0].OffsetRect(0,nScanPitch);
		rectTgt[1].OffsetRect(0,nScanPitch2);
		dNearY = (int)(pointDefect.y+yPitch);
		dFarY = (int)(pointDefect.y+yPitch*2);
	}
	else
	{
		rectTgt[0].OffsetRect(0,-nScanPitch-1);
		rectTgt[1].OffsetRect(0,nScanPitch);
		dNearY = (int)(pointDefect.y-yPitch);
		dFarY = (int)(pointDefect.y+yPitch);
	}
#endif

	Rect_Grow(rectTgt[0],nBigSize-nMatMargin,nBigSize-nMatMargin);
	Rect_Grow(rectTgt[1],nBigSize-nMatMargin,nBigSize-nMatMargin);

	dNearX = dFarX = pointDefect.x;		
	match.SetSmallImage(bigBuf,rectTgt[0]);

	bSuccess= match.DoTemplateMatch(matchResult, matchParam);
	if(bSuccess == TRUE)
	{
		dNearX += (nMatMargin-matchResult.m_X);
		dNearY += (nMatMargin-matchResult.m_Y);
	}
	else
	{
#ifdef SAVE_IMAGE		
		CString		strFile;		
		strFile.Format("%s\\FilterFail_Org[%d,%d]_Tgt[%d,%d]_s[%.3f].bmp",FILTER_PATH,pointDefect.x,pointDefect.y,rectTgt[0].left,rectTgt[0].top,matchResult.m_Score);
		CCropBuffer			buffer;
		buffer.DoCrop(bigBuf,rectDefect);
		CBufferAttach		attchOrg(strFile);
		attchOrg.AttachToFile(buffer);
		rectTgt[0].left = rectDefect.left;
		rectTgt[0].right = rectDefect.right;
		buffer.DoCrop(bigBuf,rectTgt[0]);
		attchOrg.AttachToFile(buffer);
#endif
		return TRUE;
	}

	match.SetSmallImage(bigBuf,rectTgt[1]);
	bSuccess= match.DoTemplateMatch(matchResult, matchParam);
	if(bSuccess == TRUE)
	{
		dFarX += (nMatMargin-matchResult.m_X);
		dFarY += (nMatMargin-matchResult.m_Y);
	}
	else
	{
#ifdef SAVE_IMAGE		
		CString		strFile;		
		strFile.Format("%s\\FilterFail_Org[%d,%d]_Tgt[%d,%d]_s[%.3f].bmp",FILTER_PATH,pointDefect.x,pointDefect.y,rectTgt[0].left,rectTgt[0].top,matchResult.m_Score);
		CCropBuffer			buffer;
		buffer.DoCrop(bigBuf,rectDefect);
		CBufferAttach		attchOrg(strFile);
		attchOrg.AttachToFile(buffer);
		rectTgt[1].left = rectDefect.left;
		rectTgt[1].right = rectDefect.right;
		buffer.DoCrop(bigBuf,rectTgt[1]);
		attchOrg.AttachToFile(buffer);
#endif
		return TRUE;
	}

	if(b2by2)
	{
		nSource = GetArea2x2Value(lpBuffer,szFrame,pointDefect.x,pointDefect.y,nSupress);
		nFirstVal = GetArea2x2Value(lpBuffer,szFrame,dNearX,dNearY,nSupress);
		nSecondVal = GetArea2x2Value(lpBuffer,szFrame,dFarX,dFarY,nSupress);
	}else
	{
		nSource = GetPixelValue(lpBuffer,szFrame,pointDefect.x,pointDefect.y,nSupress);
		nFirstVal = GetPixelValue(lpBuffer,szFrame,dNearX,dNearY,nSupress);
		nSecondVal = GetPixelValue(lpBuffer,szFrame,dFarX,dFarY,nSupress);
	}
// 	for(int y= 0; y < 3; y++)
// 	{
// 		for(int x= 0; x < 2; x++)
// 		{
// 			nSource = GetPixelValue(lpBuffer,szFrame,pointDefect.x+ x,pointDefect.y+ y,nSupress);
// 			nFirstVal = GetPixelValue(lpBuffer,szFrame,dNearX+ x,dNearY+ y,nSupress);
// 			nSecondVal = GetPixelValue(lpBuffer,szFrame,dFarX+ x,dFarY+ y,nSupress);
// 			if(abs(nSource-nFirstVal) >= nThres && abs(nSource-nSecondVal) >= nThres)
// 			{
// 				return TRUE;
// 			}
// 		}
// 	}

	if(abs(nSource-nFirstVal) >= nThres && abs(nSource-nSecondVal) >= nThres)
	{
#ifdef SAVE_IMAGE		
		CString		strFile;		
		strFile.Format("%s\\FilterFail_O[%d]_T1[%d,%.3f]_T2[%d,%.3f].bmp",FILTER_PATH,nSource,nFirstVal,dNearX,nSecondVal,dFarX);
		CCropBuffer			buffer;
		buffer.DoCrop(bigBuf,rectDefect);
		CBufferAttach		attchOrg(strFile);
		attchOrg.AttachToFile(buffer);
		rectTgt[0].left = rectTgt[1].left = rectDefect.left;
		rectTgt[0].right = rectTgt[1].right = rectDefect.right;
		buffer.DoCrop(bigBuf,rectTgt[0]);
		attchOrg.AttachToFile(buffer);
		buffer.DoCrop(bigBuf,rectTgt[1]);
		attchOrg.AttachToFile(buffer);
#endif

		return TRUE;			
	}		

#ifdef SAVE_IMAGE		
	CString		strFile;		
	strFile.Format("%s\\FilterSuccess__O[%d]_T1[%d,%.3f]_T2[%d,%.3f].bmp",FILTER_PATH,nSource,nFirstVal,dNearX,nSecondVal,dFarX);
	CCropBuffer			buffer;
	buffer.DoCrop(bigBuf,rectDefect);
	CBufferAttach		attchOrg(strFile);
	attchOrg.AttachToFile(buffer);
	rectTgt[0].left = rectTgt[1].left = rectDefect.left;
	rectTgt[0].right = rectTgt[1].right = rectDefect.right;
	buffer.DoCrop(bigBuf,rectTgt[0]);
	attchOrg.AttachToFile(buffer);
	buffer.DoCrop(bigBuf,rectTgt[1]);
	attchOrg.AttachToFile(buffer);
#endif

	return FALSE;
}

int	CFilterInspect::GetArea2x2Value(LPBYTE lpImg,CSize szFrame,int DefectX,int DefectY,int nSupress)
{
	LPBYTE		lpBuffer = lpImg;
	int			nValue,nBright;

	nValue = lpBuffer[DefectY * szFrame.cx + DefectX];
	if(nSupress < nValue) nValue = nSupress;
	nBright = nValue;
	nValue = lpBuffer[DefectY * szFrame.cx + DefectX+1];
	if(nSupress < nValue) nValue = nSupress;
	nBright += nValue;
	nValue = lpBuffer[(DefectY+1) * szFrame.cx + DefectX];
	if(nSupress < nValue) nValue = nSupress;
	nBright += nValue;
	nValue = lpBuffer[(DefectY+1) * szFrame.cx + DefectX+1];
	if(nSupress < nValue) nValue = nSupress;
	nBright += nValue;

	return nBright/4;
}

int	CFilterInspect::GetPixelValue(LPBYTE lpImg,CSize szFrame,int DefectX,int DefectY,int nSupress)
{
	return lpImg[DefectY * szFrame.cx + DefectX];

}

int CFilterInspect::GetPixelValue(LPBYTE lpImg, CSize szFrame, double TgtX, double TgtY, int nSupress)
{
	LPBYTE		lpBuffer = lpImg;
	float		dLP;
	float		dTgtBuf[5],dXBuf[2][5];
	int			iX;
	int			nTgtX,nTgtY;
	float		fValue = 0.,fSource = 0.,fTgtVal = 0.;
	float		fSupress = (float)nSupress;

	nTgtX = (int)TgtX;
	nTgtY = (int)TgtY;
	dLP = (float)(abs(TgtY)-abs(nTgtY));

	for(iX=nTgtX-1;iX<=nTgtX+3;iX++)
	{
		dTgtBuf[0] = lpBuffer[(nTgtY-1) * szFrame.cx + iX];
		dTgtBuf[1] = lpBuffer[(nTgtY) * szFrame.cx + iX];
		dTgtBuf[2] = lpBuffer[(nTgtY+1) * szFrame.cx + iX];
		dTgtBuf[3] = lpBuffer[(nTgtY+2) * szFrame.cx + iX];
//		dTgtBuf[4] = lpBuffer[(nTgtY+3) * szFrame.cx + iX];

		dXBuf[0][iX-(nTgtX-1)] = catmullRomSpline(dLP,dTgtBuf[0],dTgtBuf[1],dTgtBuf[2],dTgtBuf[3]);
		if(fSupress < dXBuf[0][iX-(nTgtX-1)]) dXBuf[0][iX-(nTgtX-1)] = fSupress;
// 		dXBuf[1][iX-(nTgtX-1)] = catmullRomSpline(dLP,dTgtBuf[1],dTgtBuf[2],dTgtBuf[3],dTgtBuf[4]);
// 		if(fSupress < dXBuf[1][iX-(nTgtX-1)]) dXBuf[1][iX-(nTgtX-1)] = fSupress;
	}

	dLP = (float)(TgtX-nTgtX);
	fValue = catmullRomSpline(dLP,dXBuf[0][0],dXBuf[0][1],dXBuf[0][2],dXBuf[0][3]);
	if(fSupress < fValue) fValue = fSupress;
	fTgtVal = fValue;
// 	fValue = catmullRomSpline(dLP,dXBuf[0][1],dXBuf[0][2],dXBuf[0][3],dXBuf[0][4]);
// 	if(fSupress < fValue) fValue = fSupress;
// 	fTgtVal += fValue;
// 	fValue = catmullRomSpline(dLP,dXBuf[1][0],dXBuf[1][1],dXBuf[1][2],dXBuf[1][3]);
// 	if(fSupress < fValue) fValue = fSupress;
// 	fTgtVal += fValue;
// 	fValue = catmullRomSpline(dLP,dXBuf[1][1],dXBuf[1][2],dXBuf[1][3],dXBuf[1][4]);
// 	if(fSupress < fValue) fValue = fSupress;
// 	fTgtVal += fValue;	

	return (int)(fValue);
}

int CFilterInspect::GetArea2x2Value(LPBYTE lpImg,CSize szFrame,double TgtX,double TgtY,int nSupress)
{
	LPBYTE		lpBuffer = lpImg;
	float		dLP;
	float		dTgtBuf[5],dXBuf[2][5];
	int			iX;
	int			nTgtX,nTgtY;
	float		fValue = 0.,fSource = 0.,fTgtVal = 0.;
	float		fSupress = (float)nSupress;

	nTgtX = (int)TgtX;
	nTgtY = (int)TgtY;
	dLP = (float)(abs(TgtY)-abs(nTgtY));

	for(iX=nTgtX-1;iX<=nTgtX+3;iX++)
	{			
		dTgtBuf[0] = lpBuffer[(nTgtY-1) * szFrame.cx + iX];
		dTgtBuf[1] = lpBuffer[(nTgtY) * szFrame.cx + iX];
		dTgtBuf[2] = lpBuffer[(nTgtY+1) * szFrame.cx + iX];
		dTgtBuf[3] = lpBuffer[(nTgtY+2) * szFrame.cx + iX];
		dTgtBuf[4] = lpBuffer[(nTgtY+3) * szFrame.cx + iX];

		dXBuf[0][iX-(nTgtX-1)] = catmullRomSpline(dLP,dTgtBuf[0],dTgtBuf[1],dTgtBuf[2],dTgtBuf[3]);
		if(fSupress < dXBuf[0][iX-(nTgtX-1)]) dXBuf[0][iX-(nTgtX-1)] = fSupress;
		dXBuf[1][iX-(nTgtX-1)] = catmullRomSpline(dLP,dTgtBuf[1],dTgtBuf[2],dTgtBuf[3],dTgtBuf[4]);
		if(fSupress < dXBuf[1][iX-(nTgtX-1)]) dXBuf[1][iX-(nTgtX-1)] = fSupress;
	}

	dLP = (float)(TgtX-nTgtX);
	fValue = catmullRomSpline(dLP,dXBuf[0][0],dXBuf[0][1],dXBuf[0][2],dXBuf[0][3]);
	if(fSupress < fValue) fValue = fSupress;
	fTgtVal = fValue;
	fValue = catmullRomSpline(dLP,dXBuf[0][1],dXBuf[0][2],dXBuf[0][3],dXBuf[0][4]);
	if(fSupress < fValue) fValue = fSupress;
	fTgtVal += fValue;
	fValue = catmullRomSpline(dLP,dXBuf[1][0],dXBuf[1][1],dXBuf[1][2],dXBuf[1][3]);
	if(fSupress < fValue) fValue = fSupress;
	fTgtVal += fValue;
	fValue = catmullRomSpline(dLP,dXBuf[1][1],dXBuf[1][2],dXBuf[1][3],dXBuf[1][4]);
	if(fSupress < fValue) fValue = fSupress;
	fTgtVal += fValue;	

	return (int)(fTgtVal/4.);
}

#define M11  0.0    
#define M12  1.0   
#define M13  0.0   
#define M14  0.0   
#define M21 -0.5   
#define M22  0.0   
#define M23  0.5   
#define M24  0.0   
#define M31  1.0   
#define M32 -2.5   
#define M33  2.0   
#define M34 -0.5   
#define M41 -0.5   
#define M42  1.5   
#define M43 -1.5   
#define M44  0.5  

float CFilterInspect::catmullRomSpline(float x, float v0,float v1, float v2,float v3) 
{   
	double c1,c2,c3,c4;   

	c1 = M12*v1;   
	c2 = M21*v0 + M23*v2;   
	c3 = M31*v0 + M32*v1 + M33*v2 + M34*v3;   
	c4 = M41*v0 + M42*v1 + M43*v2 + M44*v3;   

	return(float)(((c4*x + c3)*x +c2)*x + c1);   
}