#include "SCENE.h"
#include "HOUGH.h"

void *Image2FFT(int *Picture,int SizePo2,int Pitch);
void FFT2Image(int *Picture,void *FFT,int Pitch);
//#pragma optimize("",off)
#define FFTPPP2 4

#define OVERSHIFT 4
//#define SIMPLELINEMODE
//#define COMPUTE_INVERSE
#define LASTSHIFT 1
#define AverageCAdd 0//CommonValue
//#define VERY_PRE_LAPLACIAN 4
//#define DEENTERLACE
//#define DRAWCURVES
//#define MODE_ABS
#define CHECKDEBUG
u32 MAP_CheckDebug(MAP16 *MapSRC)
{
	u32 Counterrr = 0;
	s16 *LAST,*SRC;
	/* MM H */
	
	for (u32 SizeY = 0; SizeY < MapSRC->SY ;SizeY ++)
	{
		SRC = MapSRC->GetBase()+SizeY*MapSRC->PITCH;
		LAST = SRC + MapSRC->SX;
		while (SRC < LAST) 
		{
			if (*SRC) Counterrr++;
			SRC++;
		}
	}
	return Counterrr;
}


void HOUGHT_PARRAM_INIT(HOUGHParram *pFLGS)
{
	memset(pFLGS,0,sizeof(HOUGHParram));
	pFLGS->SubWindowShift = 8;
	pFLGS->COMPUTE_O2 = 0;
	pFLGS->COMPUTE_E2 = 1;
	pFLGS->INVERSE = 0;
	pFLGS->COMPUTE = 1;
	pFLGS->INPLACE = 1;
	pFLGS->MERGE_QUADRANTS = 1;
	pFLGS->SEPARATE_QUADRANTS = 1;
	pFLGS->MODE_CYCLIQUE = 0;
	pFLGS->POSTFILTERING = 0;
	pFLGS->COMPUTE_LAPLACIAN = 1;
	pFLGS->CLEAN = 1;
	pFLGS->PRE_LAPLACIAN_SHIFT = 2;
	pFLGS->Treshold = 0;
	pFLGS->LimitUp = 15;
	pFLGS->ZeroCrossing1 = 0;
	pFLGS->ZeroCrossing2 = 0;
}


static MAP16 WorkingMap;
static u32 WorkingBufSize =	0;


void MAP16_DXDY2Alpha(MAP16 *MapDX,MAP16 *MapDY)
{
	s16 *DY,*LAST;
	s16 *DX;
	DX = MapDX->GetBase();
	DY = MapDY->GetBase();
	for (u32 SizeY = 0; SizeY < MapDY->SY;SizeY ++)
	{
		LAST = DY + MapDY->SX;
		while (DY < LAST) 
		{
			float fdx,fdy,N;
			fdx = *DX;
			fdy = *DY;
			N = fdx * fdx + fdy * fdy;
			if (N > 0)
			{
				N = sqrtf(N);
				fdx /= N;
				fdy /= N;
				*DX = (2.0f * fdy * fdx) * N;
				*DY = (fdy * fdy - fdx * fdx) * N;
			} else *DX = *DY = 0;
			DX++;
			DY++;
		}
		DX+=MapDX->PITCH-MapDX->SX;
		DY+=MapDY->PITCH-MapDY->SX;
	}
}

void MAP16_DXDYO2Alpha(MAP16 *MapDX,MAP16 *MapDY)
{
	s16 *DY,*LAST;
	s16 *DX;
	DX = MapDX->GetBase();
	DY = MapDY->GetBase();
	for (u32 SizeY = 0; SizeY < MapDY->SY;SizeY ++)
	{
		LAST = DY + MapDY->SX;
		while (DY < LAST) 
		{
			float fdx,fdy,N;
			fdx = *DX;
			fdy = *DY;
			N = fdx * fdx + fdy * fdy;
			if (N > 0)
			{
				N = sqrtf(N);
				fdy += N;
				float N2;
				N2 = fdx*fdx + fdy*fdy;
				if (N > 3) N = 3.0f;
				if (N2 < 0.01f)
				{
					*DX = N;
					*DY = 0;
				} else
				{
					N2 = 1.0f/sqrtf(N2);
					*DX = fdx*N*N2;
					*DY = fdy*N*N2;
				}
				if (fdx < 0.0f)
				{
					*DX = -*DX;
					*DY = -*DY;
				}
			} else *DX = *DY = 0;
			DX++;
			DY++;
		}
		DX+=MapDX->PITCH-MapDX->SX;
		DY+=MapDY->PITCH-MapDY->SX;
	}
}

void MAP16_AA(MAP16 *MapDX,MAP16 *MapDY,MAP16 *MapDST,MAP16 *MapDST2)
{
	s16 *DY,*LAST;
	s16 *DX,*DST,*DST2;
	DX = MapDX->GetBase() + MapDX->PITCH * 10;
	DY = MapDY->GetBase() + MapDY->PITCH * 10;
	DST = MapDST->GetBase() + MapDST->PITCH * 10;
	DST2= MapDST2->GetBase() + MapDST2->PITCH * 10;
	for (u32 SizeY = 10; SizeY < MapDY->SY-10;SizeY ++)
	{
		LAST = DY + MapDY->SX;
		while (DY < LAST) 
		{
			s32 Pitch;
			Pitch = *DY - *DX * MapDST->PITCH;
			*DST2 = (*(DST + Pitch) + *(DST - Pitch))>>1;
			*DX >>= 1;
			*DY >>= 1;
			DX++;
			DY++;
			DST++;
			DST2++;
		}
		DX+=MapDY->PITCH-MapDY->SX;
		DY+=MapDX->PITCH-MapDX->SX;
		DST+=MapDST->PITCH-MapDST->SX;
		DST2+=MapDST2->PITCH-MapDST2->SX;
	}
}

#define PREVIOUSORSHFT 3
void MAP16_MIxIn(MAP16 *MapSRC,MAP16 *MapPrev,MAP16 *MapDST)
{
	s16 *SRC,*LAST,*DST,*PREV,*LastGood;
	DST = MapDST->GetBase();
	SET_BORDER16(MapSRC,8,0);
	SET_BORDER16(MapPrev,8,0);
	SET_BORDER16(MapDST,8,0);
	float AverageError = 0,AverageErrorSqr = 0,AverageNumber = 0;
	SRC = MapSRC->GetBase();
	PREV= MapPrev->GetBase();
	for (u32 SizeY = 0; SizeY < MapDST->SY;SizeY ++)
	{
		LAST = SRC + MapSRC->SX;
		LastGood = DST;
		AverageError = 0;AverageErrorSqr = 0;AverageNumber = 0;
		while (SRC < LAST) 
		{

			s16 Kluk;
			Kluk = *DST - *PREV;
			if (Kluk < 0) Kluk = -Kluk;
			if (Kluk < 50)
			{
				*DST = 0;
			} 

			DST++;
			SRC++;
			PREV++;
		}
		SRC+=MapSRC->PITCH-MapSRC->SX;
		DST+=MapDST->PITCH-MapDST->SX;
		PREV+=MapPrev->PITCH-MapPrev->SX;
	}
}

void MAP16_HarrisSolve(MAP16 *MapDX2,MAP16 *MapDY2,MAP16 *MapDXY,MAP16 *MapGrad)
{
	s16 *DX2,*DY2,*DXY,*GRAD,*LAST;
	DX2 = MapDX2->GetBase();
	DY2 = MapDY2->GetBase();
	DXY = MapDXY->GetBase();
	GRAD= MapGrad->GetBase();
	for (u32 SizeY = 0; SizeY < MapDX2->SY;SizeY ++)
	{
		LAST = DX2 + MapDX2->SX;
		while (DX2 < LAST) 
		{

/*
			µ² - µ(a11 + a22) + (a11 a22  - a12 a21)
			µ² - µ(DX2 + DY2) + (DX2 DY2  - DXY²)

			b² - 4ac

			(DX2 + DY2)² - 4 (DX2 DY2 - DXY²)

			Del = B² - 4AC
			R1 = -(B +- sqrt(Del))  / 2a

*/

			// µ1 = (DX2 + DY2 - sqrt(4 * DXY * DXY + (DX2 - DY2)²)) / 2
			// µ2 = (DX2 + DY2 + sqrt(4 * DXY * DXY + (DX2 - DY2)²)) / 2

			float B,C,HR;
			B = -((float)*DX2 + (float)*DY2);
			C = ((float)*DX2 * (float)*DY2  - (float)*DXY * (float)*DXY);
			HR = B * B - 4.0f * C;
			*DX2 = 0;
			if (/*((*GRAD) > 25) && */(HR > 0.0f))
			{
				HR = sqrt(HR);
				float HR1 = -(B - HR) / 2.0f;
				float HR2 = -(B + HR) / 2.0f;

				if ((HR2 * 30.1f + 200.0f) < (HR1))
					*DX2 = 255;
				else
					*DX2 = 0;//*/

				//*DX2 = HR2 / 100;
/*
				if (((*GRAD) > 75) && ((HR2 * 15.5f) > (HR1 + 30.0f)))
					*DX2 = 255;//*/
					/*
				if ((HR2 * 380.5f) > (HR1 + 0.0f))
					*DX2 = 255;//*/
			} 

			DX2++;
			DY2++;
			DXY++;
			GRAD++;
		}
		DX2+=MapDX2->PITCH-MapDX2->SX;
		DY2+=MapDY2->PITCH-MapDY2->SX;
		DXY+=MapDXY->PITCH-MapDXY->SX;
		GRAD+=MapGrad->PITCH-MapGrad->SX;

	}
}

void SET_BORDER(MAP *MapSRC,s32 Size,s32 Value);
float MAP16_DotProduct(MAP16 *pSrc,MAP16 *pDst);
float MAP16_DotProductNormalized(MAP16 *pSrc,MAP16 *pDst);
void ____DrawingEffect(MAP *CurrentMap,MAP16 *MapDP,s32 Denoise1);

/*
void _DrawingEffect(MAP *CurrentMap,s32 Den)
{
	for (int i=0; i<10; i++)
		//MAP32_SWAP_Inplace(CurrentMap);
		//MAP32_BLUR_H_Inplace(CurrentMap,3);
		MAP32_BLUR_V_Inplace(CurrentMap,3);
}
*/

void DrawingEffect(MAP *CurrentMap,s32 Denoise1)
{
	static float InvNorm1,InvNorm2;
	if (Denoise1)
	{
	} else
		____DrawingEffect(CurrentMap,NULL,Denoise1);
}
void ____DrawingEffect(MAP *CurrentMap,MAP16 *MapDP,s32 Denoise1)
{
	MAP16 M1,M2,M3,M4,M5,M6,M7,M8;
	s32 CurrentMapSY;
	CurrentMapSY = CurrentMap->SY;
#define HOUGHT_SIZE  (1<<FFTPPP2)
	if (CurrentMapSY < HOUGHT_SIZE) CurrentMapSY = HOUGHT_SIZE;
//return;
	
	if (CurrentMap->SX * CurrentMapSY	== 0) return;
	/**************** Init working maps	**************/
	if (WorkingBufSize < CurrentMap->SX	* CurrentMapSY)
	{
		WorkingMap.SetSize(CurrentMap->SX*8, CurrentMap->SY,1);
		WorkingBufSize = CurrentMap->SX	* CurrentMapSY;
	} 
	M1.SX = CurrentMap->SX;
	M1.SY = CurrentMapSY;
	M2	= M3 = M4 = M5 = M6 = M7 = M8 = M1;
	M6.PITCH = M5.PITCH = M4.PITCH	= M3.PITCH = M2.PITCH = M1.PITCH = M7.PITCH = M8.PITCH = CurrentMap->SX;
	M1.SetBase(WorkingMap.GetBase());
	M2.SetBase(WorkingMap.GetBase() + CurrentMap->SX *	CurrentMapSY);
	M3.SetBase(WorkingMap.GetBase() + CurrentMap->SX *	CurrentMapSY * 2);
	M4.SetBase(WorkingMap.GetBase() + CurrentMap->SX *	CurrentMapSY * 3);
	M5.SetBase(WorkingMap.GetBase() + CurrentMap->SX *	CurrentMapSY * 4);
	M6.SetBase(WorkingMap.GetBase() + CurrentMap->SX *	CurrentMapSY * 5);
	M7.SetBase(WorkingMap.GetBase() + CurrentMap->SX *	CurrentMapSY * 6);
	M8.SetBase(WorkingMap.GetBase() + CurrentMap->SX *	CurrentMapSY * 7);


	/* FAST HOUGH TEST */
	if (Denoise1 != 0)
	{
		MAP SPm;
		s32 XS,YS;
		SPm = *CurrentMap; SPm.staticBase = 1;
		SPm.MoveBase(CurrentMap->SX-HOUGHT_SIZE/2);
		if (CurrentMap->SY > HOUGHT_SIZE)
		{
			SPm.MoveBase(((CurrentMap->SY-HOUGHT_SIZE)/2) * SPm.PITCH);
			SPm.SY = HOUGHT_SIZE;
		}
		SPm.SX = HOUGHT_SIZE;
		static float AlphaCool = 0;
#define LineLen (float)(HOUGHT_SIZE>>2) 
#define LineCenter (float)(HOUGHT_SIZE>>1)
		
#ifdef SIMPLELINEMODE
		
		SET(&SPm,0);
	*(SPm.GetBase() + (s32)(LineCenter * 0.33f) *  SPm.PITCH + (s32)(LineCenter*0.5f)) = 0x80808080;
		//*(SPm.GetBase() + (s32)(LineCenter*0.5) *  SPm.PITCH + (s32)(LineCenter*0.5)) = 0xffffffff;
		// HORIZONTAL
		/*DrawLine(&SPm,	0, LineCenter,
						LineCenter*2,LineCenter,
						0xffffff);//*/
		// DIAG
		/*DrawLine(&SPm,	0, 0,
						LineCenter*2 - 2,LineCenter*2,
						0xffffff);//*/
		// SMALL DIAG
/*		DrawLine(&SPm,	LineCenter, 0,
						LineCenter/2-1,LineCenter*2,
						0xffffff);//*/
/*		DrawLine(&SPm,	 cosf(AlphaCool) * LineLen + LineCenter, sinf(AlphaCool) * LineLen + LineCenter,
						-cosf(AlphaCool) * LineLen + LineCenter,-sinf(AlphaCool) * LineLen + LineCenter,
						0xffffff);//*/
		DrawLine(&SPm,	 cosf(AlphaCool + 0.15) * LineLen + LineCenter , sinf(AlphaCool + 0.15) * LineLen + LineCenter,
						-cosf(AlphaCool + 0.15) * LineLen + LineCenter ,-sinf(AlphaCool + 0.15) * LineLen + LineCenter,
						0xffffff);//*/
		AlphaCool += 3.1415927f / 256.0f;//*/
#endif
		
		M8.SY = M7.SY = M6.SY = M2.SY = M3.SY = M1.SY = M5.SY = M4.SY = HOUGHT_SIZE;
		M8.SX = M7.SX = M6.SX = M2.SX = M3.SX = M1.SX = M5.SX = M4.SX = HOUGHT_SIZE;

		u32 CommonValue = MAP_2_MAP16(&SPm,&M5,18 - OVERSHIFT);
		MAP16_Add(&M5,-CommonValue<<OVERSHIFT);


		/*
		MAP16_Shift(&M5,-OVERSHIFT-2);
		MAP16_Copy(&M5,&M1);
		MAP16_DX(&M5);
		MAP16_DY(&M1);
		MAP16_Sqr(&M5);
		MAP16_Sqr(&M1);
		MAP16_Add(&M1,&M5);
		MAP16_Shift(&M5,-2);
		AverageCAdd = 0;	//*/
/*
		MAP16_Copy(&M5,&M6);
		MAP16_YO2(&M6);
		MAP16_XO2(&M6);
		M6.SX>>=1;
		M6.SY>>=1;
*/


		MAP16_YO2(&M5);
		MAP16_XO2(&M5);



#ifdef VERY_PRE_LAPLACIAN
			M5.SX>>=1;
			M5.SY>>=1;
			M2.SX>>=1;
			M2.SY>>=1;
			
			MAP16_Copy(&M5,&M2);
			MAP16_BLUR_Inplace(&M2,1,1);
			MAP16_Sub(&M2,&M5);
			MAP16_Shift(&M5,VERY_PRE_LAPLACIAN);
			SET_BORDER16(&M5,4,0);

			M5.SX<<=1;
			M5.SY<<=1;
			M2.SX<<=1;
			M2.SY<<=1;
#endif//*/

			HOUGHParram HoughtFLags;
			HOUGHT_PARRAM_INIT(&HoughtFLags);
			FAST_HOUGH(&M5,&M1,&HoughtFLags);
#ifdef CHECKDEBUG
			u32 CHKDBG = MAP_CheckDebug(&M5);
#endif
#ifdef COMPUTE_INVERSE
			HoughtFLags.INVERSE = 1;
			FAST_HOUGH(&M5,&M1,&HoughtFLags);
#else
			
#ifdef 	DEENTERLACE
		M5.SY >>= 1;
		M1.SY >>= 1;
		FAST_HOUGH_DEENTERLACE(&M5,&M1,HoughtFLags.SubWindowShift);
		M5.MoveBase(M5.SY * M5.PITCH);
		M1.MoveBase(M1.SY * M1.PITCH);
		FAST_HOUGH_DEENTERLACE(&M5,&M1,HoughtFLags.SubWindowShift);
		M5.MoveBase(-M5.SY * M5.PITCH);
		M1.MoveBase(-M1.SY * M1.PITCH);
		M5.SY <<= 1;
		M1.SY <<= 1;
#endif

#endif

#ifdef MODE_ABS
		MAP16_Abs(&M5);
#endif
			if (LASTSHIFT-OVERSHIFT) MAP16_Shift(&M5,LASTSHIFT-OVERSHIFT);
 
		if (AverageCAdd) MAP16_Add(&M5,(s16)AverageCAdd);//*/

		if (MapDP != NULL)
			MAP16_Copy(&M5,MapDP);

		MAP16_2_MAP(&M5,&SPm,0);
		MAP16_2_MAP(&M5,&SPm,8);
		MAP16_2_MAP(&M5,&SPm,16);
#ifdef DEBUGTRESHOLD
		MAP16_Threshold(&M5,DEBUGTRESHOLD);
		MAP16_Shift(&M5,4);
		MAP16_2_MAP(&M5,&SPm,16);
#endif
#ifdef DRAWCURVES
		for( int y = 1 ; y < (HOUGHT_SIZE>>1)-1 ; y++)
		{
				DrawLine(&SPm,	
				y+1,(HOUGHT_SIZE>>10)+(*(M5.GetBase() +M5.PITCH * (HOUGHT_SIZE>>2) + y + 1)>>(1)),
				y,(HOUGHT_SIZE>>10)+(*(M5.GetBase() +M5.PITCH * (HOUGHT_SIZE>>2) + y)>>(1)) ,
						0xff0000);//*/
				DrawLine(&SPm,	
					(HOUGHT_SIZE>>1)+y+1,(HOUGHT_SIZE>>10)+(*(M5.GetBase() +M5.PITCH * (HOUGHT_SIZE>>2) + (HOUGHT_SIZE>>1) + y + 1)>>(1)),
					(HOUGHT_SIZE>>1)+y,(HOUGHT_SIZE>>10)+(*(M5.GetBase() +M5.PITCH * (HOUGHT_SIZE>>2) + (HOUGHT_SIZE>>1) + y)>>(1)) ,
						0xff0000);//*/
			/*	if (*(M5.GetBase() +M5.PITCH * (HOUGHT_SIZE>>2) + y))
				DrawLine(&SPm,	(HOUGHT_SIZE>>1)+y,(HOUGHT_SIZE>>1),
				(HOUGHT_SIZE>>1)+y,
				(HOUGHT_SIZE>>1)+
				((256**(M5.GetBase() +M5.PITCH * (HOUGHT_SIZE>>2) + y -1 ) / *(M5.GetBase() +M5.PITCH * (HOUGHT_SIZE>>2) + y))>>5) ,
						0xff00);//*/
		}
#endif

#ifdef CHECKDEBUG
		char DEBUGTEXT[2048];
		sprintf(DEBUGTEXT,"%d%%",CHKDBG);//,SPm.SX * SPm.SY ,CHKDBG*100 / (SPm.SX * SPm.SY));
		DRawTEXT_Soft_B((u8*)DEBUGTEXT , (u32*)SPm.GetBase() , 16 , 16 , SPm.PITCH , SPm.SY , 0xff00ff );
#endif

		return;
	}

	if (CurrentMap->GetValue(0) == 0xC0DECAFE) return;


	MAP_2_MAP16(CurrentMap,&M5,16);
	//MAP16_BLUR_Inplace(&M5, 1, 1);
	MAP16_SmallBlur_FAST(&M5,&M1);
	//MAP16_Copy(&M5,&M1);
	MAP16_Copy(&M1,&M5);
	MAP16_BLUR_Inplace(&M1, 1, 2);
	MAP16_Sub(&M5,&M1);//*/
	MAP16_Copy(&M1,&M6);
	MAP16_Shift(&M6,-4);//*/
	MAP16_Copy(&M6,&M5);
	MAP16_Sqr(&M6);
	MAP16_BLUR_Inplace(&M6, 1, 1);
	MAP16_BLUR_Inplace(&M5, 1, 1);
	MAP16_Sqr(&M5);
	MAP16_Sub(&M5,&M6);
#define PPO2 128
	MAP16_Limit_UP(&M6,PPO2-1);
	MAP16_Limit_Down(&M6,0);
	MAP16_Xor(&M6,-1);
	MAP16_And(&M6,(PPO2-1)<<4);
	MAP16_Shift(&M6,-8);
	MAP16_Sub(&M6,&M1);
	MAP16_Shift(&M6,-1);
	MAP16_Sub(&M6,&M1);
	MAP16_Shift(&M1,-15);//*/
	MAP16_And(&M1,255);//*/
	if (Denoise1)
	{
		MAP16_SmallBlur_FAST(&M1,&M6);
		MAP16_Copy(&M6,&M1);
		MAP16_Add(&M1,-(s16)Denoise1);
		MAP16_Shift(&M1,-15);//*/
		MAP16_And(&M1,255);//*/
		MAP16_Xor(&M1,255);//*/
	}

	MAP16_2_MAP(&M1,CurrentMap,0);
	MAP16_2_MAP(&M1,CurrentMap,8);
	MAP16_2_MAP(&M1,CurrentMap,16);

	CurrentMap->SetValue(0,0xC0DECAFE);
	return;

}

void Harris(MAP *CurrentMap)
{
	MAP16 M1,M2,M3,M4,M5,M6;
	if (CurrentMap->SX * CurrentMap->SY	== 0) return;
	/**************** Init working maps	**************/
	if (WorkingBufSize < CurrentMap->SX	* CurrentMap->SY)
	{
		WorkingMap.SetSize(CurrentMap->SX*8, CurrentMap->SY,1);
		WorkingBufSize = CurrentMap->SX	* CurrentMap->SY;
	} 
	M1.SX = CurrentMap->SX;
	M1.SY = CurrentMap->SY;
	M2	= M3 = M4 = M5 = M6 = M1;
	M6.PITCH = M5.PITCH = M4.PITCH	= M3.PITCH = M2.PITCH = M1.PITCH = CurrentMap->SX;
	M1.SetBase(WorkingMap.GetBase());
	M2.SetBase(WorkingMap.GetBase() + CurrentMap->SX *	CurrentMap->SY);
	M3.SetBase(WorkingMap.GetBase() + CurrentMap->SX *	CurrentMap->SY * 2);
	M4.SetBase(WorkingMap.GetBase() + CurrentMap->SX *	CurrentMap->SY * 3);
	M5.SetBase(WorkingMap.GetBase() + CurrentMap->SX *	CurrentMap->SY * 4);
	M6.SetBase(WorkingMap.GetBase() + CurrentMap->SX *	CurrentMap->SY * 5);

	/* HARRIS COHERENCY EDGE MODEL BEGIN *************************************************************************************/
	if (0)
	{
		MAP_2_MAP16(CurrentMap,&M1);
		MAP16_Shift(&M1,5);//*/
		MAP16_Copy(&M1,&M2);
		MAP16_BLUR_Inplace(&M1, 2, 1);
		MAP16_Sub(&M2,&M1);//*/
		MAP16_Shift(&M1,-15);//*/
		MAP16_And(&M1,64);//*/
		MAP16_BLUR_Inplace(&M1, 1, 2);
		//MAP16_Add(&M3,-64);
		MAP16_Copy(&M1,&M2);
		MAP16_DX(&M1);
		MAP16_DY(&M2);
		MAP16_Copy(&M1,&M3);
		MAP16_Mul(&M2,&M3);
		MAP16_Sqr(&M1);
		MAP16_Sqr(&M2);

		MAP16_BLUR_Inplace(&M1, 1, 2);
		MAP16_BLUR_Inplace(&M2, 1, 2);
		MAP16_BLUR_Inplace(&M3, 1, 2);

		MAP16_HarrisSolve(&M1,&M2,&M3,&M3);//*/
		
	
		MAP16_2_MAP(&M1,CurrentMap,0);
		MAP16_2_MAP(&M1,CurrentMap,8);
		MAP16_2_MAP(&M1,CurrentMap,16);
		return;


	}

	/* HARRIS COHERENCY EDGE MODEL END *************************************************************************************/
	/* COHERENCY EQ MODEL BEGIN *************************************************************************************/
	if (1)
	{
		MAP_2_MAP16(CurrentMap,&M1,16);
		MAP16_BLUR_Inplace(&M1, 1, 1);
		
		MAP16_Copy(&M1,&M5);
		MAP16_BLUR_Inplace(&M1, 1, 2);
		MAP16_Sub(&M5,&M1);//*/

		MAP16_Copy(&M1,&M6);

		MAP16_Shift(&M6,-4);//*/
		MAP16_Copy(&M6,&M5);
		MAP16_Sqr(&M6);
		MAP16_BLUR_Inplace(&M6, 1, 1);
		MAP16_BLUR_Inplace(&M5, 1, 1);
		MAP16_Sqr(&M5);
		MAP16_Sub(&M5,&M6);

#define PPO2 128
		MAP16_Limit_UP(&M6,PPO2-1);
		MAP16_Limit_Down(&M6,0);
		MAP16_Xor(&M6,(PPO2-1)<<4);

		MAP16_And(&M6,(PPO2-1)<<4);
		MAP16_Shift(&M6,-8);
		MAP16_Sub(&M6,&M1);
		//MAP16_Add(&M1,-1<<3);
		MAP16_Shift(&M1,-15);//*/
		MAP16_And(&M1,255);//*/

		MAP16_2_MAP(&M1,CurrentMap,0);
		MAP16_2_MAP(&M1,CurrentMap,8);
		MAP16_2_MAP(&M1,CurrentMap,16);
		return;//*/
	}

	/* COHERENCY EQ MODEL END *************************************************************************************/

	/* ANISOTROPIC AA TEST *************************************************************************************/
	for (int T = 0 ; T < 1 ; T++)
	{
		MAP_2_MAP16(CurrentMap,&M1,16);
		
		MAP16_Copy(&M1,&M5);
		MAP16_BLUR_Inplace(&M5, 2, 4);
		MAP16_Sub(&M5,&M1);//*/


		MAP16_Copy(&M1,&M3);
		MAP16_Copy(&M1,&M2);
		MAP16_DX(&M1);
		MAP16_DY(&M2);
		
		MAP16_DXDY2Alpha(&M1,&M2);

		MAP16_BLUR_Inplace(&M1, 1, 3);
		MAP16_BLUR_Inplace(&M2, 1, 3);

		MAP16_DXDYO2Alpha(&M1,&M2);
/*
		MAP16_Sqr(&M1);
		MAP16_Sqr(&M2);
		MAP16_Add(&M1,&M2);
		MAP16_Sqrt(&M2);
		MAP16_Shift(&M2,2);
		SET(CurrentMap,0);
		MAP16_2_MAP(&M2,CurrentMap,0);
		//MAP16_2_MAP(&M2,CurrentMap,8);
		return;//*/

		MAP16_AA(&M1,&M2,&M3,&M4);
		MAP16_AA(&M1,&M2,&M4,&M3);
/*		MAP16_AA(&M1,&M2,&M3,&M4);
		MAP16_AA(&M1,&M2,&M4,&M3);//*/


		MAP16_Add(&M5,&M3);
		MAP16_Shift(&M3,-2);

		MAP16_2_MAP(&M3,CurrentMap,0);
		MAP16_2_MAP(&M3,CurrentMap,8);
		MAP16_2_MAP(&M3,CurrentMap,16);
		return;
	}

	/* ANISOTROPIC AA TEST *************************************************************************************/



	/* HARRIS EDGE TEST *************************************************************************************/


	/* HARRIS EDGE TEST *************************************************************************************/
	
/*
	MAP16_BLUR_Inplace(&M1, 1, 8);
	MAP16_2_MAP(&M1,CurrentMap,0);
	return;
//*/
	MAP16_Copy(&M1,&M4);
	//MAP16_DX(&M1);
	//MAP16_DY(&M2);

	/* TEST B */
/*	MAP16_Sqr(&M1);
	MAP16_BLUR_Inplace(&M1, 1, 3);
	MAP16_BLUR_Inplace(&M2, 1, 3);
	MAP16_Sqr(&M2);
	MAP16_Sub(&M2,&M1);
	MAP16_Copy(&M1,&M2);
	MAP16_BLUR_Inplace(&M2, 4, 3);
	//MAP16_Sub(&M2,&M1);//*/
	MAP16_Set(&M3,0);
	MAP16_Sub(&M1,&M3);
	MAP16_Sub(&M1,&M5);
	//MAP16_Sqr(&M5);
/*	for (int i = 0 ; i < 128 ; i += 16)
	{
		MAP16_BLUR_XY(&M1,&M2,i, 6, 1);
		MAP16_Add(&M1,&M3);
		MAP16_Copy(&M4,&M1);
	}*/
	//MAP16_Shift(&M3,-2);

	//MAP16_Shift(&M5,-2);
//	MAP16_Add(&M3,&M5);//*/
	//MAP16_Add(&M3,128);
	MAP16_Copy(&M5,&M1);
	MAP16_BLUR_Inplace(&M5, 2, 3);
	MAP16_Sub(&M5,&M1);//*/
	MAP16_Add(&M3,128);

	MAP16_2_MAP(&M1,CurrentMap,0);
	MAP16_2_MAP(&M1,CurrentMap,8);
	MAP16_2_MAP(&M1,CurrentMap,16);
	return;
}