// EdgeProc.cpp: implementation of the CEdgeProc class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "EdgeProc.h"
#include "SISBuffer.h"
#include <math.h>
#include "RANSAC_LineFittingAlgorithm.h"

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////
CEdgeProc::CEdgeProc()
{
	m_btBackground = 0;
	m_btWhite = 255;	
	m_subthick = 3;
}

CEdgeProc::~CEdgeProc()
{

}

BOOL CEdgeProc::CannyEdgeProcessing(LPBYTE pImageBuf,CRect &rectIns,enGradientMask enMask,int nThres,CSISBuffer &ptgt)
{
	if (!pImageBuf || ptgt.IsValidBuffer() == FALSE) return false;	
	
	if (enMask == GM_Sobel)
	{
		m_nGX[0][0] = -1;	m_nGX[0][1] = 0;	m_nGX[0][2] =  1; 
		m_nGX[1][0] = -2;	m_nGX[1][1] = 0;	m_nGX[1][2] =  2; 
		m_nGX[2][0] = -1;	m_nGX[2][1] = 0;	m_nGX[2][2] =  1; 
		
		m_nGY[0][0] = 1;	m_nGY[0][1] = 2;	m_nGY[0][2] = 1; 
		m_nGY[1][0] = 0;	m_nGY[1][1] = 0;	m_nGY[1][2] = 0;
		m_nGY[2][0] = -1;	m_nGY[2][1] = -2;	m_nGY[2][2] = -1; 
	}
	else if (enMask == GM_Prewitt)
	{
		m_nGX[0][0] = -1;	m_nGX[0][1] = 0;	m_nGX[0][2] = 1; 
		m_nGX[1][0] = -1;	m_nGX[1][1] = 0;	m_nGX[1][2] = 1; 
		m_nGX[2][0] = -1;	m_nGX[2][1] = 0;	m_nGX[2][2] = 1;
		
		m_nGY[0][0] = 1;	m_nGY[0][1] = 1;	m_nGY[0][2] = 1; 
		m_nGY[1][0] = 0;	m_nGY[1][1] = 0;	m_nGY[1][2] = 0; 
		m_nGY[2][0] = -1;	m_nGY[2][1] = -1;	m_nGY[2][2] = -1; 
	}
	else if (enMask == GM_Roberts)
	{
		m_nGX[0][0] = -1;	m_nGX[0][1] = 0;	m_nGX[0][2] = 0; 
		m_nGX[1][0] = 0;	m_nGX[1][1] = 1;	m_nGX[1][2] = 0; 
		m_nGX[2][0] = 0;	m_nGX[2][1] = 0;	m_nGX[2][2] = 0; 
		
		m_nGY[0][0] = 0;	m_nGY[0][1] = 0;	m_nGY[0][2] = -1; 
		m_nGY[1][0] = 0;	m_nGY[1][1] = 1;	m_nGY[1][2] = 0; 
		m_nGY[2][0] = 0;	m_nGY[2][1] = 0;	m_nGY[2][2] = 0; 
	}
	else return false;
	
	CSize szImageBuf = CSize(rectIns.Width(),rectIns.Height());	
	register int x, y, xp, yp, posy, posyp;
	int nSumX, nSumY, nEdgeDirection, nLeftPixel, nRightPixel;
	int p1, p2, p3, p4, p5, p6, p7, p8;

	int *img_mag = new int[szImageBuf.cx * szImageBuf.cy];
	int *img_dir = new int[szImageBuf.cx * szImageBuf.cy];
	float ORIENT;

	/**************************************************************
	*	    	SOBEL GRADIENT APPROXIMATION
	***************************************************************/
	int nMin = 100000, nMax = -100000;
	for (y = 1; y < szImageBuf.cy - 1; y++)
	{
		posy = y * szImageBuf.cx;
		for (x = 1; x < szImageBuf.cx - 1; x++)
		{
			nSumX = 0;
			nSumY = 0;

			/* Convolution starts here */
			/****************************
			* X,Y gradient approximation
			*****************************/
			for (yp = -1; yp <= 1; yp++)
			{
				posyp = (y + yp) * szImageBuf.cx;
				for (xp = -1; xp <= 1; xp++)
				{
					nSumX += (int)(pImageBuf[posyp + x + xp] * m_nGX[yp + 1][xp + 1]);
					nSumY += (int)(pImageBuf[posyp + x + xp] * m_nGY[yp + 1][xp + 1]);
				}
			}

			img_mag[posy + x] = (int)sqrtf((float)nSumX * nSumX + (float)nSumY * nSumY);
			
			if (img_mag[posy + x] < nMin)
				nMin = img_mag[posy + x];
			if (img_mag[posy + x] > nMax)
				nMax = img_mag[posy + x];		

			/****************************
			* Magnitude orientation
			****************************/
			if (nSumX == 0)	// Cannot divide by zero
			{
			    if (nSumY == 0) ORIENT = 0.0;
			    else if (nSumY < 0)
				{
				   nSumY = -nSumY;
				   ORIENT = 90.0;
				}
			    else ORIENT = 90.0;
			}
			else if (nSumX < 0 && nSumY > 0) // Can`t take invtan of angle in 2nd Quad
			{
				nSumX = -nSumX;
				ORIENT = 180 - (float)atan((float)nSumY / (float)nSumX) * (float)(180 / 3.1415926535);
			}
			else if (nSumX > 0 && nSumY < 0)	// Can`t take invtan of angle in 4th Quad
			{
				nSumY = -nSumY;
				ORIENT = 180 - (float)atan((float)nSumY / (float)nSumX) * (float)(180 / 3.1415926535);
			}
			else	// else angle is in 1st or 3rd Quad
				ORIENT = (float)atan((float)nSumY / (float)nSumX) * (float)(180 / 3.1415926535);
			
			/*******************************************************************
			* Find edgeDirection by assigning ORIENT a value of either 0, 45, 90
			* or 135 degrees, depeding on which value ORIENT is closest to 
			********************************************************************/
			if (ORIENT < 22.5)			img_dir[posy + x] = 0;
			else if (ORIENT < 67.5)		img_dir[posy + x] = 45;
			else if (ORIENT < 112.5)	img_dir[posy + x] = 90;
			else if (ORIENT < 157.5)	img_dir[posy + x] = 135;
			else						img_dir[posy + x] = 0;
		}
	}
// 	
// 	CStdioFile file;
// 	file.Open("C:\\Data.txt", CFile::modeCreate | CFile::modeWrite);
// 	for (y = 1; y < szImageBuf.cy - 1; y++)
// 	{
// 		CString str;
// 		posy = y * szImageBuf.cx;
// 		for (x = 1; x < szImageBuf.cx - 1; x++)
// 		{
// 			CString str2;
// 			str2.Format("(%d,%d)=(%d,%d)", x, y, img_mag[posy + x], img_dir[posy + x]);
// 			str += str2;
// 		}
// 		str += "\n";
// 		file.WriteString(str);
// 	}
// 	file.Close();

	int nMag, nMinMax = nMax - nMin;
	if (nMinMax == 0) nMinMax = 1;
	BYTE btResult = m_btBackground;
	for (y = 1; y < szImageBuf.cy - 1; y++)
	{
		posy = y * szImageBuf.cx;
		for (x = 1; x < szImageBuf.cx - 1; x++)
		{
			/************************************************
			* Obtain value of 2 adjacent pixels in edge
			* direction.
			*************************************************/
			nMag = img_mag[posy + x];
			nEdgeDirection = img_dir[posy + x];
			if (nEdgeDirection == 0)
			{
				nLeftPixel  = img_mag[y * szImageBuf.cx + x - 1];
				nRightPixel = img_mag[y * szImageBuf.cx + x + 1];
			}
			else if (nEdgeDirection == 45) 
			{
				nLeftPixel  = img_mag[(y + 1) * szImageBuf.cx + x - 1];
				nRightPixel = img_mag[(y - 1) * szImageBuf.cx + x + 1];
			}
			else if (nEdgeDirection == 90)
			{
				nLeftPixel  = img_mag[(y - 1) * szImageBuf.cx + x];
				nRightPixel = img_mag[(y + 1) * szImageBuf.cx + x];
			}
			else
			{
				nLeftPixel  = img_mag[(y - 1) * szImageBuf.cx + x - 1];
				nRightPixel = img_mag[(y + 1) * szImageBuf.cx + x + 1];
			}

            /*********************************************************
			* Compare current magnitude to both adjacent pixel values. 
			* And if it is less than either of the 2 adjacent values -
			* suppress it and make a nonedge
			**********************************************************/
			if (nMag < nLeftPixel || nMag < nRightPixel) btResult = m_btBackground;
			else
			{
				/***************
				*  Hysteresis
				***************/				
				// determine vaues of neighboring pixels
				p1 = img_mag[(y - 1) * szImageBuf.cx + x - 1];
				p2 = img_mag[(y - 1) * szImageBuf.cx + x];
				p3 = img_mag[(y - 1) * szImageBuf.cx + x + 1];
				p4 = img_mag[y * szImageBuf.cx + x - 1];
				p5 = img_mag[y * szImageBuf.cx + x + 1];
				p6 = img_mag[(y + 1) * szImageBuf.cx + x - 1];
				p7 = img_mag[(y + 1) * szImageBuf.cx + x];
				p8 = img_mag[(y + 1) * szImageBuf.cx + x + 1];

				// Check to see if neighboring pixel values are edges
				if (p1 > nThres || p2 > nThres
					|| p3 > nThres || p4 > nThres
					|| p5 > nThres || p6 > nThres
					|| p7 > nThres || p8 > nThres)
					btResult = m_btWhite;
				else
					btResult = m_btBackground;
			}

			ptgt.SetPixel(x,y,btResult);
			//pImageBuf[y*szImageBuf.cx+x] = btResult;
		}
	}		
	
	delete[] img_mag;
	delete[] img_dir;

	return TRUE;
}

float CEdgeProc::catmullRomSpline(float x, float v0,float v1, float v2,float v3) 
{   
#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 

	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);   
}  



void CEdgeProc::ThresholdProcessing(LPBYTE pImg,CSize szImg,int nThres,int nDir)
{

	float fNewThresValue = float(nThres);	

	// threshold and invert
	int nIndex;	
	int nFinThres = (int)(fNewThresValue);
	for (int i=0; i<szImg.cy; i++)
	{
		for (int j=0; j<szImg.cx; j++)
		{
			nIndex = (i*szImg.cx)+j;		
			if(nDir == 1)
			{
				if (pImg[nIndex] > nFinThres)
				{
					pImg[nIndex] = 255; 
				}
				else
				{
					pImg[nIndex] = 0;				
				}
			}
			else
			{
				if (pImg[nIndex] < nFinThres)
				{
					pImg[nIndex] = 255; 
				}
				else
				{
					pImg[nIndex] = 0;				
				}
			}
		}
	}	
}

int CEdgeProc::MaximizeDiscriminantFunction(double * p)
{
	double mi_255 = 0;
	int k;
	for (k=0; k<256; k++) 
		mi_255 += k*p[k];

	int index = 0;
	double max = 0;
	double mi_k = 0;
	double w_k = 0;
	double value;
	for (k=0; k<256; k++) 
	{
		mi_k += k*p[k];
		w_k += p[k];
		value = ((w_k == 0) || (w_k == 1))? -1 : ((mi_255*w_k - mi_k)*(mi_255*w_k - mi_k))/(w_k*(1-w_k));
		if (value >= max) 
		{
			index = k;
			max = value;
		}
	}

	return index;
}

void CEdgeProc::imCalcHistogram(LPBYTE pOrg, int nWidth,int nHeight, unsigned long* histo, int cumulative,CRect rect)
{
	int i,j;

	memset(histo, 0, 256 * sizeof(unsigned long));

	for (i = rect.top; i < rect.bottom; i++)
	{
		for(j=rect.left;j<rect.right;j++)
			histo[*(pOrg+i*nWidth+j)]++;
	}

	if (cumulative)
	{
		/* make cumulative histogram */
		for (i = 1; i < 256; i++)
			histo[i] += histo[i-1];
	}
}

unsigned char CEdgeProc::OtsuoBinary(CSISBuffer &pOrg,CRect rect,int nOpt)
{
	unsigned long histo[256];
	imCalcHistogram(pOrg.GetDataAddress(0,0), pOrg.GetDataWidth(),pOrg.GetHeight(), histo, nOpt,rect);

	double totalPixels = pOrg.GetDataWidth()*pOrg.GetHeight();
	double p[256];
	for (int i=0; i<256; i++) 
		p[i] = histo[i]/totalPixels;

	return (unsigned char)MaximizeDiscriminantFunction(p);
}

float CEdgeProc::AdaptiveThreshold(BYTE* pImageData, int nWidth, int nHeight, int nStep, int nThresValue)
{
	if (pImageData==NULL) return 0;

	float fThresValue =(float) nThresValue;
	float fNewThresValue = 0;

	for (int i=0; i<1000; i++)
	{
		fNewThresValue = GetThresholdValue(pImageData, nWidth, nHeight, nStep, (int)fThresValue);

		if (fNewThresValue==fThresValue) break;

		fThresValue = fNewThresValue;
	}

	return fNewThresValue;
}

float CEdgeProc::GetThresholdValue(BYTE* pImageData, int nWidth, int nHeight, int nStep, int nThresValue)
{
	if (pImageData==NULL) return 0;

	ULONG fore = 0;
	int fore_cnt = 0;
	ULONG back = 0;
	int back_cnt = 0;

	int nIndex;
	for (int i=0; i<nHeight; i++)
	{
		for (int j=0; j<nWidth; j++)
		{
			nIndex = (i*nStep)+j;

			if (pImageData[nIndex]==0) continue;

			if (pImageData[nIndex] > nThresValue)
			{
				fore += pImageData[nIndex];
				fore_cnt++;
			}
			else
			{
				back += pImageData[nIndex];
				back_cnt++;
			}
		}
	}

	float fore_avg = 0.0f;
	if (fore_cnt!=0) fore_avg = float(fore) / float(fore_cnt);

	float back_avg = 0.0f;
	if (back_cnt!=0) back_avg = float(back) / float(back_cnt);

	if (fore_cnt==0 || back_cnt==0) 
	{
		if (nThresValue>=128)
			return float(nThresValue)/2.0f;
		else
			return float(nThresValue)*2.0f;
	}

	return (fore_avg+back_avg)/2.0f;
}

BOOL CEdgeProc::VSobelBoundary(LPBYTE lpInImg, LPBYTE lpOutImg, int nWidth, int nBufWidth, int nHeight,int nThres)
{
	if (!lpInImg || !lpOutImg)
		return FALSE;

	int MaskBox[3][3] = {{-1, 0, 1}, {-1, 0, 1}, {-1, 0, 1}};
	int nHeightm1 = nHeight - 1;
	int nWidthm1 = nWidth - 1;
	int mr, mc, newValue, i, j, min, max;
	int* pTmpImg;
	float constVal1, constVal2;
	pTmpImg = new int[nHeight * nBufWidth];	// Á¤¼ö°ªÀ» °®´Â À̹ÌÁö µ¿Àû ¸Þ¸ð¸® ÇÒ´ç
	ZeroMemory(pTmpImg, nHeight * nBufWidth * sizeof(int));

	for (i = 1; i < nHeightm1; i++)
	{
		for (j= 1; j < nWidthm1; j++)
		{
			newValue = 0;
			for (mr = 0; mr < 3; mr++)
			{
				for (mc = 0; mc < 3; mc++)
					newValue += (MaskBox[mr][mc] * lpInImg[(i + mr - 1) * nBufWidth + (j + mc - 1)]);
			}

			// °ªÀ» ¾ç¼ö·Î º¯È¯
			if (newValue < 0)
				newValue = -newValue;
			pTmpImg[i * nBufWidth + j] = newValue;
		}
	}

	// µð½ºÇ÷¹À̸¦ À§ÇØ 0¿¡¼­ 255»çÀÌ·Î °ªÀÇ ¹üÀ§¸¦ ¸ÅÇÎ
	// À̸¦ À§ÇØ ¸ÕÀú ÃÖ´ë, ÃÖ¼Ò°ªÀ» ãÀº ÈÄ À̸¦ ÀÌ¿ëÇÏ¿© ¸ÅÇÎÇÑ´Ù.
	min = 0x0fffffff;
	max = -(0x0fffffff);

	for (i = 1; i < nHeightm1; i++)
	{
		for (j = 1; j < nWidthm1; j++)
		{
			newValue = pTmpImg[i * nBufWidth + j];
			if (newValue < min)
				min = newValue;
			if (newValue > max)
				max = newValue;
		}
	}

	// º¯È¯½Ã »ó¼ö°ªÀ» ¹Ì¸® °è»ê
	constVal1 = (float)(255.0 / (max - min));
	constVal2 = (float)(-255.0 * min / (max - min));

	for (i = 1; i < nHeightm1; i++)
	{
		for (j = 1; j < nWidthm1; j++)
		{
			// [min, max] »çÀÌÀÇ °ªÀ» [0, 255] °ªÀ¸·Î º¯È¯
			newValue = pTmpImg[i * nBufWidth + j];
			newValue = (int)(constVal1 * newValue + constVal2);

			lpOutImg[i * nBufWidth + j] = (BYTE)newValue;
		}
	}

	// Find Max Sum
	for (i = 1; i < nWidthm1; i++)
	{
		for (j = 1; j < nHeightm1; j++)
		{
			if (lpOutImg[j * nBufWidth + i] > nThres)
				lpOutImg[j * nBufWidth + i] = 255;
			else
				lpOutImg[j * nBufWidth + i] = 0;
		}
	}

	delete [] pTmpImg;

	return TRUE;
}

BOOL CEdgeProc::HSobelBoundary(LPBYTE lpInImg, LPBYTE lpOutImg, int nWidth, int nBufWidth, int nHeight,int nThres)
{
	if (!lpInImg || !lpOutImg)
		return FALSE;

	int MaskBox[3][3] = {{-1, -1, -1}, {0, 0, 0}, {1, 1, 1}};
	int nHeightm1 = nHeight - 1;
	int nWidthm1 = nWidth - 1;
	int mr, mc, newValue, i, j, min, max;
	int* pTmpImg;
	float constVal1, constVal2;
	pTmpImg = new int[nHeight * nBufWidth];	// Á¤¼ö°ªÀ» °®´Â À̹ÌÁö µ¿Àû ¸Þ¸ð¸® ÇÒ´ç
	ZeroMemory(pTmpImg, nHeight * nBufWidth * sizeof(int));

	for (i = 1; i < nHeightm1; i++)
	{
		for (j= 1; j < nWidthm1; j++)
		{
			newValue = 0;
			for (mr = 0; mr < 3; mr++)
			{
				for (mc = 0; mc < 3; mc++)
					newValue += (MaskBox[mr][mc] * lpInImg[(i + mr - 1) * nBufWidth + (j + mc - 1)]);
			}

			// °ªÀ» ¾ç¼ö·Î º¯È¯
			if (newValue < 0)
				newValue = -newValue;
			pTmpImg[i * nBufWidth + j] = newValue;
		}
	}

	// µð½ºÇ÷¹À̸¦ À§ÇØ 0¿¡¼­ 255»çÀÌ·Î °ªÀÇ ¹üÀ§¸¦ ¸ÅÇÎ
	// À̸¦ À§ÇØ ¸ÕÀú ÃÖ´ë, ÃÖ¼Ò°ªÀ» ãÀº ÈÄ À̸¦ ÀÌ¿ëÇÏ¿© ¸ÅÇÎÇÑ´Ù.
	min = 0x0fffffff;
	max = -(0x0fffffff);

	for (i = 1; i < nHeightm1; i++)
	{
		for (j = 1; j < nWidthm1; j++)
		{
			newValue = pTmpImg[i * nBufWidth + j];
			if (newValue < min)
				min = newValue;
			if (newValue > max)
				max = newValue;
		}
	}

	// º¯È¯½Ã »ó¼ö°ªÀ» ¹Ì¸® °è»ê
	constVal1 = (float)(255.0 / (max - min));
	constVal2 = (float)(-255.0 * min / (max - min));

	for (i = 1; i < nHeightm1; i++)
	{
		for (j = 1; j < nWidthm1; j++)
		{
			// [min, max] »çÀÌÀÇ °ªÀ» [0, 255] °ªÀ¸·Î º¯È¯
			newValue = pTmpImg[i * nBufWidth + j];
			newValue = (int)(constVal1 * newValue + constVal2);

			lpOutImg[i * nBufWidth + j] = (BYTE)newValue;
		}
	}

	// Find Max Sum
	for (i = 1; i < nWidthm1; i++)
	{
		for (j = 1; j < nHeightm1; j++)
		{
			if (lpOutImg[j * nBufWidth + i] > nThres)
				lpOutImg[j * nBufWidth + i] = 255;
			else
				lpOutImg[j * nBufWidth + i] = 0;
		}
	}

	delete [] pTmpImg;

	return TRUE;
}

void CEdgeProc::MakeIntegralImage(BYTE *image,int width,int height,int datawidth,int *intImage)
{
	int		x,y,offset,linesum;

	intImage[0] = image[0];

	for(x=1;x<width;++x)
	{
		intImage[x] = intImage[x-1] + image[x];
	}

	image += datawidth;
	for(y=1,offset=y*datawidth;y<height;++y,offset+=datawidth)
	{
		linesum = 0;
		for(x=0;x<width;++x)
		{
			linesum += image[x];
			intImage[offset+x] = intImage[offset-datawidth+x] + linesum;
		}
		image += datawidth;
	}
}

void CEdgeProc::ThresholdByIntegralImage(BYTE *image,int width,int height,int datawidth,int wsz,BYTE *matrix)
{
	std::vector<int> intImage(width*height);
	MakeIntegralImage(image,width,height,datawidth,&intImage[0]);

	const int winArea = wsz*wsz; 
	/*const int wsz = 10;*/

	int		x,y,offset,top,bottom,left,right;
	int		sum1,sum2,sum3,graySum;

	for(y=0,offset=0;y<height;y++,offset+=datawidth)
	{
		top = y-(wsz>>1);
		if(top < 0) top = 0;
		else if(top > height-wsz) top = height-wsz;

		bottom = top + wsz - 1;
		// y-range = [top,bottom];
		for(x=0;x<width;x++)
		{
			left = x-(wsz>>1);
			if(left < 0) left=0;
			else if(left > width-wsz) left = width-wsz;
			right = left+wsz-1;
			// xrange = [left,right];

			sum1 = (left>0 && top>0)?intImage[(top-1)*width+left-1]:0;
			sum2 = (left>0)?intImage[bottom*width+left-1]:0;
			sum3 = (top>0)?intImage[(top-1)*width+right]:0;

			graySum = intImage[bottom*width+right] - sum3-sum2+sum1;

			// Threshold T = (window_mean-3);
			if((image[offset+x]+3)*winArea<=graySum)
			{
				matrix[offset+x] = 0xFF;
			}
			else
			{
				matrix[offset+x] = 0x00;
			}
		}
	}
}

void CEdgeProc::Adaptive_Binarization(BYTE *gray,int width,int height,int datawidth,int w,double k,BYTE *bimage,CRect rect)
{
	int		whalf=w>>1;
	int		diff,sqdiff;

	int	*intimage = new int[width*height];
	int	*intsqimg = new int[width*height];
	ZeroMemory(intimage,width*height);
	ZeroMemory(intsqimg,width*height);

	int		i,j,x,y,a;
	int		linesum=0,linesqsum=0;

	intimage[0]=gray[0];
	for(x=1;x<width;++x)
	{
		a=gray[x];
		intimage[x] = intimage[x-1] + a;
		intsqimg[x] = intimage[x-1] + a*a;
	}

	for(y=1;y<height;++y)
	{
		linesum = linesqsum = 0;
		for(x=0;x<width;++x)
		{
			a= gray[y*datawidth+x];
			linesum += a;
			linesqsum += a*a;
			intimage[y*width+x]=intimage[(y-1)*width+x]+linesum;
			intsqimg[y*width+x]=intsqimg[(y-1)*width+x]+linesqsum;
		}
	}

	int			xmin,ymin,xmax,ymax,area;
	double		mean,std,threshold;
	int			diagsum,idiagsum,sqdiagsum,sqidiagsum;

	for(j=rect.top;j<rect.bottom;j++)
	{
		for(i=rect.left;i<rect.right;i++)
		{
			xmin = MAXIMUM(0,i-whalf);
			ymin = MAXIMUM(0,j-whalf);
			xmax = MINIMUM(width-1,i+whalf);
			ymax = MINIMUM(height-1,j+whalf);
			area = (xmax-xmin+1)*(ymax-ymin+1);

			if(!xmin && !ymin)		// origin
			{
				diff =	intimage[ymax*width+xmax];
				sqdiff = intsqimg[ymax*width+xmax];
			}
			else if(!xmin && ymin)		// first column
			{	
				diff = intimage[ymax*width+xmax] - intimage[(ymin-1)*width+xmax];
				sqdiff = intsqimg[ymax*width+xmax] - intsqimg[(ymin-1)*width+xmax];
			}
			else if(xmin && !ymin)		// first row
			{
				diff = intimage[ymax*width+xmax] - intimage[ymax*width+(xmin-1)];
				sqdiff = intsqimg[ymax*width+xmax] - intsqimg[ymax*width+(xmin-1)];
			}
			else
			{
				diagsum = intimage[ymax*width+xmax] + intimage[(ymin-1)*width+(xmin-1)];
				idiagsum = intimage[(ymin-1)*width+xmax] + intimage[ymax*width+(xmin-1)];

				diff = diagsum - idiagsum;

				sqdiagsum = intsqimg[ymax*width+xmax] + intsqimg[(ymin-1)*width+(xmin-1)];
				sqidiagsum = intsqimg[(ymin-1)*width+xmax] + intsqimg[ymax*width+(xmin-1)];

				sqdiff = sqdiagsum - sqidiagsum;
			}

			// threshold = window_mean *(1 + factor * (std_dev/128.-1));
			// 128 = max_allowed_std_deviation in the gray image;
			mean = double(diff)/double(area);
			std = sqrt((sqdiff-double(diff)*diff/double(area))/double(area-1));
			threshold = mean*(1.0+k*((std/128.0)-1.));
			if(gray[j*datawidth+i] > threshold)
				bimage[j*datawidth+i] = 0;
			else
				bimage[j*datawidth+i] = 255;
		}
	}

	delete[] intimage,intimage=NULL;
	delete[] intsqimg,intsqimg=NULL;
}

void CEdgeProc::LocalHistogramEqualization(BYTE *image,int width,int height,int wsize,BYTE *out)
{
	int		hwsize=wsize >> 1;
	wsize = (hwsize<<1)+1;
	int topstop = height-wsize;
	int leftstop=width-wsize;

	for(int y=0,offset=0;y<height;y++,offset+=width)
	{
		int top = y-hwsize;
		top = top<0?0:top>topstop?topstop:top;

		BYTE	*imgrow = &image[offset];
		BYTE *outrow=&out[offset];

		for(int x=0;x<width;x++)
		{
			int left = x-hwsize;
			left = left<0?0:left>leftstop?leftstop:left;

			int histo[256];
			memset(histo,0,sizeof(histo));
			for(int yy=0,woffset=top*width+left;yy<wsize;yy++,woffset+=width)
			{
				BYTE *winrow=&image[woffset];
				for(int xx=0;xx<wsize;xx++)
				{
					histo[winrow[xx]]++;
				}
			}

			int level = imgrow[x];
			int csum = 0;
			if(level<128)
			{
				for(int k=0;k<=level;k++)
					csum += histo[k];
			}
			else
			{
				csum = wsize*wsize;
				for(int k=level+1;k<256;k++)
					csum -= histo[k];
			}

			int a=int((255.*csum)/(wsize*wsize));
			outrow[x] = (a&~255) == 0?a:a<0?0:255;
		}
	}
}

// transpose a point
BOOL CEdgeProc::MovePoint(CPoint iPos, CPoint *oPos, CPoint B_P) 
{
	oPos->x = iPos.x + B_P.x;
	oPos->y = iPos.y + B_P.y;
	return TRUE;
}

// rotate a point based by (0,0)
BOOL CEdgeProc::RotatePoint(CPoint iPos, CPoint *oPos, double sinRad, double cosRad) 
{
	// rotate transform(Rad)
	oPos->x = (int)((double)iPos.x*cosRad + (double)iPos.y*(-sinRad));
	oPos->y = (int)((double)iPos.x*sinRad + (double)iPos.y*cosRad);

	return TRUE;
}

BOOL CEdgeProc::RotatePoint(CvPoint2D32f iPos, double &dXPos,double &dYPos, double sinRad, double cosRad)
{
	// rotate transform(Rad)

	dXPos = ((double)iPos.x*cosRad + (double)iPos.y*(-sinRad));
	dYPos = ((double)iPos.x*sinRad + (double)iPos.y*cosRad);

	return TRUE;
}

BOOL CEdgeProc::RotatePoint(CPoint iPos, double &dXPos,double &dYPos, double sinRad, double cosRad) 
{
	// rotate transform(Rad)
	dXPos = ((double)iPos.x*cosRad + (double)iPos.y*(-sinRad));
	dYPos = ((double)iPos.x*sinRad + (double)iPos.y*cosRad);

	return TRUE;
}

//rotate a point based by Base Point
BOOL CEdgeProc::RotatePoint(CPoint iPos, CPoint *oPos, CPoint B_P, double sinRad, double cosRad) 
{
	CPoint		 MovePos1, MovePos2;
	CPoint		 RotatePos;
	CPoint		 inverseB_P;

	inverseB_P.x = -B_P.x;
	inverseB_P.y = -B_P.y;

	MovePoint(iPos, &MovePos1, inverseB_P);
	if(!RotatePoint(MovePos1 , &RotatePos, sinRad, cosRad)) return FALSE;
	MovePoint(RotatePos, &MovePos2, B_P);

	oPos->x = MovePos2.x;
	oPos->y = MovePos2.y;

	return TRUE;
}

BOOL CEdgeProc::RotatePoint(CvPoint2D32f  iPos, double &dXPos,double &dYPos, CvPoint2D32f  B_P, double sinRad, double cosRad)
{
	CvPoint2D32f		 MovePos1;	
	CvPoint2D32f		 inverseB_P;
	double				rotateX,rotateY;

	inverseB_P.x = -B_P.x;
	inverseB_P.y = -B_P.y;

	MovePos1.x = iPos.x + inverseB_P.x;
	MovePos1.y = iPos.y + inverseB_P.y;	
	if(!RotatePoint(MovePos1 , rotateX,rotateY, sinRad, cosRad)) return FALSE;
	dXPos = rotateX+B_P.x;
	dYPos = rotateY+B_P.y;	

	return TRUE;
}

BOOL CEdgeProc::RotatePoint(CPoint iPos, double &dXPos,double &dYPos, CPoint B_P, double sinRad, double cosRad) 
{
	CPoint		 MovePos1;	
	CPoint		 inverseB_P;
	double		 rotateX,rotateY;

	inverseB_P.x = -B_P.x;
	inverseB_P.y = -B_P.y;

	MovePos1.x = iPos.x + inverseB_P.x;
	MovePos1.y = iPos.y + inverseB_P.y;	
	if(!RotatePoint(MovePos1 , rotateX,rotateY, sinRad, cosRad)) return FALSE;
	dXPos = rotateX+B_P.x;
	dYPos = rotateY+B_P.y;	

	return TRUE;
}

double CEdgeProc::GetTheta(sPoint *pLinedata,int nLineCnt,int nRange)
{
	if(pLinedata == NULL || nLineCnt <= 0)
		return 0.;

	CLineFitting		lineFit;
	double				dTheta = 0.;
	double				dCost,dSlope;
	sPoint				first,second;
	sLine				line;	

	dCost = lineFit.ransac_line_fitting(pLinedata, nLineCnt, line, nRange);	

	first.x = pLinedata[0].x+((double)nLineCnt*line.mx);
	first.y = pLinedata[0].y+((double)nLineCnt*line.my);	
	second.x = pLinedata[0].x-((double)nLineCnt*line.mx);
	second.y = pLinedata[0].y-((double)nLineCnt*line.my);				

	dSlope = (second.y-first.y)/(second.x-first.x);	
	dTheta = 90+M_DEGREE(atan(dSlope));		

	return dTheta;
}

BOOL CEdgeProc::CvFindMark(CSISBuffer lpOrg,IplImage *IpMark,CRect rcIns,TEMP_RESULT &result, double  dRate)
{
	if(!IpMark)
		return FALSE;

	if(rcIns.Width() > lpOrg.GetWidth() || rcIns.top < 0 || rcIns.top > rcIns.bottom || rcIns.bottom < 0 || rcIns.bottom > lpOrg.GetHeight())
		return FALSE;
	if(rcIns.left >= rcIns.right || rcIns.left < 0 || rcIns.right >= lpOrg.GetWidth())
		return FALSE;
	
	IplImage *scr = cvCreateImageHeader(cvSize(lpOrg.GetWidth(),lpOrg.GetHeight()),8,1);
	cvSetData(scr,lpOrg.GetDataAddress(),lpOrg.GetWidth());	

	int nDiv= 1;
	CvRect roi = cvRect(rcIns.left,rcIns.top,rcIns.Width(),rcIns.Height());
	CvRect roi2;

	char cMaxPath[100] ={0,};
	
	nDiv = 2;

	IplImage *sScr  = cvCreateImage(cvSize(roi.width/nDiv,roi.height/nDiv),8,1);
	IplImage *sTemp = cvCreateImage(cvSize(IpMark->width/nDiv,IpMark->height/nDiv),8,1);
	//cvSaveImage("D:\\test_img\\mark.bmp",scr);
	cvSetImageROI(scr,roi);
	cvResize(scr,sScr);
	cvResize(IpMark,sTemp);
	roi2 = cvGetImageROI(sScr);	
	//cvSaveImage("D:\\2.bmp",sScr);

	//sprintf(cMaxPath,"d:\\test_Img\\Mark_1%d.bmp",scr->imageData);
	//cvSaveImage(cMaxPath,scr);	

	TEMP_MATCH_DATA data1 = Cv_TempleateMath(roi2,sScr,sTemp);

	cvResetImageROI(scr);
	cvReleaseImage(&sScr);
	cvReleaseImage(&sTemp);

	if(data1.dMaxRate > dRate)
	{		
		roi.x		= roi.x  +  (int)data1.dPt_X * nDiv  - IpMark->width;
		roi.y		= roi.y  +  (int)data1.dPt_Y * nDiv  - IpMark->height;

		roi.width	= IpMark->width * 2;
		roi.height	= IpMark->height *2;
	}

	TEMP_MATCH_DATA data = Cv_TempleateMath(roi,scr,IpMark);
	
	result.fRat  = (float)data.dMaxRate;
	result.pt.x  = (float)data.dPt_X;
	result.pt.y  = (float)data.dPt_Y;

	cvReleaseImageHeader(&scr);
	return TRUE;
}

TEMP_MATCH_DATA CEdgeProc::Cv_TempleateMath(CvRect roi,IplImage* IpSrc,IplImage* IpTem)
{
	TEMP_MATCH_DATA data;

	cvSetImageROI(IpSrc,roi);

	roi = cvRect(IpSrc->roi->xOffset, IpSrc->roi->yOffset, IpSrc->roi->width, IpSrc->roi->height);

	IplImage *ftmp = cvCreateImage(cvSize(roi.width - IpTem->width+1, roi.height - IpTem->height+1 ),32,1);
	int mathod;
	//mathod = CV_TM_SQDIFF;      
	//mathod = CV_TM_SQDIFF_NORMED;      
	//mathod = CV_TM_CCORR      
	//mathod = CV_TM_CCORR_NORMED;      
	//mathod = CV_TM_CCOEFF      
	mathod = CV_TM_CCOEFF_NORMED;

	CvPoint ptMax;

	cvMatchTemplate(IpSrc, IpTem, ftmp, mathod);
	cvMinMaxLoc(ftmp,&data.dMinRate,&data.dMaxRate,NULL,&ptMax);

	data.dPt_X = ptMax.x + roi.x + IpTem->width /2.0;
	data.dPt_Y = ptMax.y + roi.y + IpTem->height/2.0;
		
	cvReleaseImage(&ftmp);
	cvResetImageROI(IpSrc);
	return data;
}

// rotate a point based by (0,0)
BOOL CEdgeProc::IMRotatePoint(IMPOS2D iPos, IMPOS2D *oPos, double sinRad, double cosRad)
{
	// rotate transform(Rad)
	oPos->u = (LONG)ROUND(iPos.u*cosRad + iPos.v*(-sinRad));
	oPos->v = (LONG)ROUND(iPos.u*sinRad + iPos.v*cosRad);

	return TRUE;
}

BOOL CEdgeProc::IMMovePoint(IMPOS2D iPos, IMPOS2D *oPos, IMPOS2D B_P)
{
	oPos->u = iPos.u + B_P.u;
	oPos->v = iPos.v + B_P.v;
	return TRUE;
}

//rotate a point based by Base Point
BOOL CEdgeProc::IMRotatePoint(IMPOS2D iPos, IMPOS2D *oPos, IMPOS2D B_P, double sinRad, double cosRad)
{
	IMPOS2D		 MovePos1, MovePos2;
	IMPOS2D		 RotatePos;
	IMPOS2D		 inverseB_P;

	inverseB_P.u = -B_P.u;
	inverseB_P.v = -B_P.v;

	IMMovePoint(iPos, &MovePos1, inverseB_P);
	if (!IMRotatePoint(MovePos1, &RotatePos, sinRad, cosRad)) return FALSE;
	IMMovePoint(RotatePos, &MovePos2, B_P);

	oPos->u = MovePos2.u;
	oPos->v = MovePos2.v;

	return TRUE;
}

BOOL CEdgeProc::IMGetRotImg(ROI aoi, CSISBuffer Org, CSISBuffer Tgt, IMPOS2D pCen, double sinRad, double cosRad)
{
	unsigned char	brightness;
	IMPOS2D		pIn, pOut;
	int		u, v, ou, ov;
	int		scu = Org.GetWidth();
	int		ocu = Tgt.GetWidth();

	for (ov = 0, v = aoi.v1; v <= aoi.v2; v++, ov++) {
		for (ou = 0, u = aoi.u1; u < aoi.u2; u++, ou++) {
			pIn.u = u;
			pIn.v = v;
			IMRotatePoint(pIn, &pOut, pCen, sinRad, cosRad); // rotate 1 point

			//source assert
			if (!RANGEIN(pOut.u, 0, Org.GetWidth())) return FALSE;
			if (!RANGEIN(pOut.v, 0, Org.GetHeight())) return FALSE;

			brightness = (unsigned char)Org.GetPixel((int)pOut.u, (int)pOut.v);

			//target assert
			if (!RANGEIN(ou, 0, Tgt.GetWidth())) return FALSE;
			if (!RANGEIN(ov, 0, Tgt.GetHeight())) return FALSE;

			Tgt.SetPixel(ou, ov, brightness);			
		}
	}

	return TRUE;
}