// HeadDetector.cpp : Defines the entry point for the DLL application.
//

#include <stdlib.h>
#include <math.h>
#include <memory.h>
#include <string.h>
#include "HeadDetector.h"
#include "HAAR.h"
#include "MAP_SpFunctions.h"
#include "Viola2.h"
#include "BottomFaceShapeExtraction.h"

/****************************************************/
u32 HAAR_Detect(MAP *pSrcMAP,HAAR_Classifier *pHFF,MAP *pDrawMAP,float Factor,float gAddX,float gAddY,u32 AGE,float Angle, MAP8_Quad *MAP8_QuadStack, u32 *MAP8_QuadStackNumber, MAP8_Quad *pBlendQuad = NULL);
u32 MAP8Q_IsIn(MAP8_Quad *p1,MAP8_Quad *p2);
int __cdecl TRACKEDPOINT_FACE_Compare(const void *elem1, const void *elem2 );

void DrawCross(MAP *image, Vector *v, u32 color, int len, char orient = 3);
u32  ASM_GetCenterG(MAP *pSrc,s32 X1,s32 Y1,s32 X2,s32 Y2,POINT *pCenter);
void ASM_DrawFace(MAP *pSrc,DETECTEDFace *pDF);
void ASM_DetectFeatures(MAP *pFeatureMap, MAP *pSourceMap, MAP *pSourceMapDRW, DETECTEDFace *pLocalDF);

//#pragma optimize("",off)

/****************************************************/
/**** DEFINES **************************/
#define ANGLE_TRACK

#define FMDMS_FM (85)
#define FMDMS FMDMS_FM
#define GROW_FACTOR (ulNumberOfHeads ? 1.10 : 1.04)
#define BASE_FACTOR 2.0
#define START_WindowX 30
#define ASM_COMMPUTE_SHAPE_SIZEMAP 84
#define WINDOW_SUM_FACTOR_II	0.5f

#define ANGLE_FACTOR_TRAK (3.1415927f / 12.0f)
#define GROW_FACTOR_TRAK 0.10f

#ifdef ANGLE_TRACK
#define GROW_TRAK_WINDOW 0.1f
#else
#define GROW_TRAK_WINDOW 0.2f
#endif


#ifdef _MANAGED
#pragma managed(push, off)
#endif

#ifdef HEADDETECTOR_DLL_MODE
BOOL APIENTRY DllMain( HMODULE hModule,
                       DWORD  ul_reason_for_call,
                       LPVOID lpReserved
					 )
{
	switch (ul_reason_for_call)
	{
	case DLL_PROCESS_ATTACH:
	case DLL_THREAD_ATTACH:
	case DLL_THREAD_DETACH:
	case DLL_PROCESS_DETACH:
		break;
	}
    return TRUE;
}
#endif

#ifdef _MANAGED
#pragma managed(pop)
#endif


HeadDetector::HeadDetector(void)
{
	memset(this, 0, sizeof(*this));
}

void  HeadDetector::CopyAsync()
{
	memcpy(ASYNC_ASM_AllDF, ASM_AllDF ,sizeof(ASYNC_ASM_AllDF));
	ASYNC_ulNumberOfHeads = ulNumberOfHeads;
}

u32  HeadDetector::GetNumberOfFace()
{
	HeadTrackerRequested = 4;
	return ASYNC_ulNumberOfHeads;
}
void  HeadDetector::DrawDetectedFaces(u32 Enable)
{
	HeadTrackerRequested_DRAW = Enable;
}
u32  HeadDetector::GetFace(u32 INdex, Vector *Center,Vector *Size)
{
	if (INdex >= ASYNC_ulNumberOfHeads) return 0;
	*Center = ASYNC_ASM_AllDF[INdex].HeadPos;
	*Size = ASYNC_ASM_AllDF[INdex].HeadSize;
	return 1;
}
u32  HeadDetector::GetFaceFeature(u32 i, u32 FeatureIndexn, Vector *Center)
{
	if (i >= ASYNC_ulNumberOfHeads) return 0;
	switch(FeatureIndexn)
	{
		case 0: *Center = ASYNC_ASM_AllDF[i].RightEye		; return (u32)ASYNC_ASM_AllDF[i].RightEye		.z;break;
		case 1: *Center = ASYNC_ASM_AllDF[i].LeftEye		; return (u32)ASYNC_ASM_AllDF[i].LeftEye		.z;break;
		case 2: *Center = ASYNC_ASM_AllDF[i].Nose			; return (u32)ASYNC_ASM_AllDF[i].Nose			.z;break;
		case 3: *Center = ASYNC_ASM_AllDF[i].RightMouse	; return (u32)ASYNC_ASM_AllDF[i].RightMouse	.z;break;
		case 4: *Center = ASYNC_ASM_AllDF[i].LeftMouse	; return (u32)ASYNC_ASM_AllDF[i].LeftMouse	.z;break;
		case 5: *Center = ASYNC_ASM_AllDF[i].UpMouse		; return (u32)ASYNC_ASM_AllDF[i].UpMouse		.z;break;
		case 6: *Center = ASYNC_ASM_AllDF[i].DownMouse	; return (u32)ASYNC_ASM_AllDF[i].DownMouse	.z;break;
	}
	return 0;
}

void HeadDetector::SetDrawImage(MAP *pImgDraw)
{
	pSrcDRAW = pImgDraw;
	//HeadTrackerRequested_DRAW = pImgDraw != NULL;
}


#include <crtdbg.h>

int HeadDetector::ProcessImage(MAP *pImg)
{
	pSrc = pImg;

	MAP SRC2,Local,LocalO2;
	static s32 *TEMPA = NULL;
	static s32 *TEMPB = NULL;
	static s32 TEMPASize = 0;

	if (!HeadTrackerRequested) 
	{
		ulNumberOfHeads = ulOldNumberOfHeads = 0;
		return 0;
	}
	HeadTrackerRequested--;

	float CurrentFactor = 1.0f;
	u32 PreviousMAP8_QuadStackNumber = MAP8_QuadStackNumber;
	MAP8_QuadStackNumber = 0;
	
	static MAP BMP;
	static u32 Bfirst = 1;

	if (pHFF && *((u32*)&pHFF) != pSrc->PITCH)
	{
		HAAR_Classifier_free(&pHFFOptimized);
		pHFF = NULL;
	}
	if (pHFF == NULL)
	{
		pHFF = HAAR_Create_FaceFront();
		pHFFOptimized = HAAR_Optimize(pHFF,CurrentFactor,pSrc->PITCH);
		HAAR_Classifier_free(&pHFF);
		*((u32*)&pHFF) = pSrc->PITCH;
	}
	
	if (512 + (pSrc->SY + 32) * (pSrc->PITCH + 32) * 4 > TEMPASize)
	{
		if (TEMPA) free(TEMPA);
		if (TEMPB) free(TEMPB);
		TEMPASize = 512 + pSrc->SY * (pSrc->PITCH + 16) * 4;
		TEMPASize = 512 + (pSrc->SY + 32) * (pSrc->PITCH + 32) * 4;
		TEMPA = (s32 *)malloc(TEMPASize);
		TEMPB = (s32 *)malloc(TEMPASize);
	}

	
	Local = *pSrc; Local.staticBase = 1;
	Local.SetBase(TEMPA);
	SRC2 = *pSrc; SRC2.staticBase = 1;
	SRC2.SetBase(TEMPB);

	COPY(pSrc,&SRC2);
	/* Track Existing quads ************************************************************************************/
	memmove(
		MAP8_QuadStack+MAX_MAP8_QUAD-PreviousMAP8_QuadStackNumber,
		MAP8_QuadStack,PreviousMAP8_QuadStackNumber*sizeof(MAP8_Quad));
	for (int tq = MAX_MAP8_QUAD-PreviousMAP8_QuadStackNumber ; tq < MAX_MAP8_QUAD ; tq++)
	{
		MAP ClmapDst,ClmapDst2,ClmapDst3,ClmapSrc;
		float Factor,WX,WY,WW,WH,OX,OY;
		if (MAP8_QuadStack[tq].W)
		{
			MAP8_Quad SaveQ = MAP8_QuadStack[tq];
#ifdef ANGLE_TRACK
			for (int An = -1 ; An < 2 ; An++)
#endif
			{
				for (int Fr = -1 ; Fr < 2 ; Fr++)
				{
					WX = MAP8_QuadStack[tq].X1;
					WY = MAP8_QuadStack[tq].Y1;
					WW = MAP8_QuadStack[tq].X2;
					WH = MAP8_QuadStack[tq].Y2;
					WW -= WX;
					WH -= WY;

					WX += WW*0.5f;
					WY += WH*0.5f;
					OX = WX;
					OY = WY;
					Factor = 1.0f + (float)Fr * GROW_FACTOR_TRAK;
					WW *= Factor;
					WH *= Factor;
					WX -= WW*0.5f;
					WY -= WH*0.5f;

					Factor = pHFFOptimized->WindowSize.X / (WW);
					WX -= GROW_TRAK_WINDOW * WW;
					WY -= GROW_TRAK_WINDOW * WH;
					WW += GROW_TRAK_WINDOW * 2.0f * WW;
					WH += GROW_TRAK_WINDOW * 2.0f * WH;
					if (WX<0.0f)	WX=0.0f;
					if (WY<0.0f)	WY=0.0f;
					ClmapDst = Local; ClmapDst.staticBase = 1;
					ClmapSrc = *pSrc; ClmapSrc.staticBase = 1;
					if (WX + WW > pSrc->SX) WX = pSrc->SX - WW ;
					if (WY + WH > pSrc->SY) WY = pSrc->SY - WH ;
					ClmapSrc.MoveBase((u32)WX + (u32)WY * ClmapSrc.PITCH);
					ClmapSrc.SX = WW;
					ClmapSrc.SY = WH;
					ClmapDst.SX = WW * Factor;
					ClmapDst.SY = WH * Factor;
#ifndef ANGLE_TRACK
					COPY_OX_RGBA(&ClmapSrc,&ClmapDst);
					RGBA_2_II(&ClmapDst,&ClmapDst);
					HAAR_Detect(&ClmapDst,pHFFOptimized,pSrc,1.0f / Factor,WX,WY,MAP8_QuadStack[tq].Age+1,0, MAP8_QuadStack, &MAP8_QuadStackNumber);
#else
					ClmapDst3 = ClmapDst2 = ClmapDst;
					ClmapDst2.SetBase(SRC2.GetBase());
					ClmapDst2.SX <<= 1;
					ClmapDst2.SY <<= 1;
					ClmapDst.SX <<= 1;
					ClmapDst.SY <<= 1;
					COPY_OX_RGBA(&ClmapSrc,&ClmapDst2);
					COPY_PATCH_RGBA(
						&ClmapDst2,
						&ClmapDst,
						ClmapDst2.SX<<7,ClmapDst2.SY<<7,
						0,0,
						ClmapDst2.SX<<8,ClmapDst2.SY<<8,
						ClmapDst2.SX<<8,ClmapDst2.SY<<8,
						MAP8_QuadStack[tq].Angle + (float)An * ANGLE_FACTOR_TRAK,1);//*/
					ClmapDst2 = ClmapDst;
					ClmapDst.SX >>= 1;
					ClmapDst.SY >>= 1;
					COPY_O2_RGBA(&ClmapDst2,&ClmapDst);

					COPY_BW(&ClmapDst,&ClmapDst);
					SHIFT(&ClmapDst,2);
					COPY_RGBA(&ClmapDst,&ClmapDst);

					RGBA_2_II(&ClmapDst,&ClmapDst);
					HAAR_Detect(&ClmapDst,pHFFOptimized,pSrc,1.0f / Factor,WX,WY,MAP8_QuadStack[tq].Age+1,MAP8_QuadStack[tq].Angle + (float)An * ANGLE_FACTOR_TRAK, MAP8_QuadStack, &MAP8_QuadStackNumber, &SaveQ);
#endif
				}
			}
		}
	}
	/* find RAW quads ******************************************************************************************/
	u32 Counter = 250;//;
	float CurrentFactorPo2 = BASE_FACTOR;
	while (Counter--)
	{
		LocalO2 = Local;
		MAP *pSrcLocal;
		pSrcLocal = &SRC2;
		CurrentFactorPo2 *= GROW_FACTOR;
		LocalO2.SX = pSrc->SX / CurrentFactorPo2;
		LocalO2.SY = pSrc->SY / CurrentFactorPo2;
		if ((LocalO2.SX < SRC2.SX>>1) && (LocalO2.SY < SRC2.SY>>1))
		{
			MAP SRC3;
			SRC3 = SRC2; SRC3.staticBase = 1;
			SRC2.SX >>= 1;
			SRC2.SY >>= 1;
			COPY_O2_RGBA(&SRC3,&SRC2);
		}

		if ((pHFFOptimized->WindowSize.X < LocalO2.SX) && 
			(pHFFOptimized->WindowSize.Y < LocalO2.SY))
		{
			if (START_WindowX < pHFFOptimized->WindowSize.X * CurrentFactorPo2)
			{
				COPY_OX_RGBA(pSrcLocal,&LocalO2);
				RGBA_2_II(&LocalO2,&LocalO2);
				HAAR_Detect(&LocalO2,pHFFOptimized,pSrc,CurrentFactorPo2,0.0f,0.0f,0,0,MAP8_QuadStack, &MAP8_QuadStackNumber);
			}
		} else
			Counter = 0;
		Local = LocalO2;
	}

	_ASSERTE( _CrtIsValidHeapPointer(pSrc->GetBase()));
	if (!Bfirst) COPY(&BMP,pSrc);
	_ASSERTE( _CrtIsValidHeapPointer(pSrc->GetBase()));

	ulOldNumberOfHeads = ulNumberOfHeads;
	ulNumberOfHeads = 0;
	/* Merge computed quads */
	if (MAP8_QuadStackNumber)
	{
		qsort(MAP8_QuadStack,MAP8_QuadStackNumber,sizeof(MAP8_Quad),TRACKEDPOINT_FACE_Compare);

		MAP8_Quad *pBaseQ,*pLastQ,*pParseQ;
		pLastQ = MAP8_QuadStack + MAP8_QuadStackNumber;
		pBaseQ = MAP8_QuadStack;
		while (pBaseQ < pLastQ)
		{
			if (pBaseQ->W != 0.0f)
			{
				pParseQ = pBaseQ+1;
				while (pParseQ < pLastQ)
				{
					// recover ?
					if (MAP8Q_IsIn(pBaseQ,pParseQ))
					{
						// -> Merge 
						if (pBaseQ->Age > pParseQ->Age)
							*pBaseQ = *pBaseQ;
						else
						if (pBaseQ->Age < pParseQ->Age)
							*pBaseQ = *pParseQ;
						else
						{
							pBaseQ->X1 += pParseQ->X1;
							pBaseQ->Y1 += pParseQ->Y1;
							pBaseQ->X2 += pParseQ->X2;
							pBaseQ->Y2 += pParseQ->Y2;
							pBaseQ->Angle += pParseQ->Angle;
							pBaseQ->W  += pParseQ->W ;
							pBaseQ->Age = pBaseQ->Age > pParseQ->Age ? pBaseQ->Age : pParseQ->Age;
						}
						pParseQ->W = 0.0f;
					}
					pParseQ++;
				}
			} 
			pBaseQ++;
		}//*/

		pBaseQ = MAP8_QuadStack;
		_ASSERTE( _CrtIsValidHeapPointer(pSrc->GetBase()));
		while (pBaseQ < pLastQ)
		{

			if (pBaseQ -> W > 1.0f)
			{

				pBaseQ ->X1 /= pBaseQ->W;
				pBaseQ ->Y1 /= pBaseQ->W;
				pBaseQ ->X2 /= pBaseQ->W;
				pBaseQ ->Y2 /= pBaseQ->W;
				pBaseQ ->Angle /= pBaseQ->W;
				pBaseQ->W = 1;
				if (pBaseQ->Age >= 0)
				{
					DETECTEDFace LocalDF;
					LocalDF.BaseHead = *pBaseQ;
					Local = *pSrc; Local.staticBase = 1;
					Local.SetBase(TEMPA);
					_ASSERTE( _CrtIsValidHeapPointer(pSrc->GetBase()));
					ASM_DetectFeatures(&Local, pSrc, pSrcDRAW, &LocalDF);
					_ASSERTE( _CrtIsValidHeapPointer(pSrc->GetBase()));
					if (ulNumberOfHeads < MAX_HEAD_DETECTED)
					{
						/* Retreive old one (if exist)*/
						ASM_AllDF[ulNumberOfHeads++] = LocalDF;
					}
				}
			}
			pBaseQ++;
		}
	};

	for (int i=0 ; i < ulOldNumberOfHeads ; i++)
	{
		ASM_AllDF[i].HeadPos.x *= pSrcDRAW->SX / pSrc->SX;
		ASM_AllDF[i].HeadPos.y *= pSrcDRAW->SX / pSrc->SX;
		ASM_AllDF[i].HeadSize.x *= pSrcDRAW->SX / pSrc->SX;
		ASM_AllDF[i].HeadSize.y *= pSrcDRAW->SX / pSrc->SX;
		ASM_AllDF[i].LeftEye *= pSrcDRAW->SX / pSrc->SX;
		ASM_AllDF[i].RightEye *= pSrcDRAW->SX / pSrc->SX;
		ASM_AllDF[i].LeftMouse *= pSrcDRAW->SX / pSrc->SX;
		ASM_AllDF[i].RightMouse *= pSrcDRAW->SX / pSrc->SX;
		ASM_AllDF[i].Nose *= pSrcDRAW->SX / pSrc->SX;
	}
	if (HeadTrackerRequested_DRAW)
		for (int i=0 ; i < ulNumberOfHeads ; i++)
			ASM_DrawFace(pSrcDRAW,&ASM_AllDF[i]);

	return ulNumberOfHeads;
}



/*******************************************************************************************/
/***** FUNCTIONS ***************************************************************************/
/*******************************************************************************************/

#define TURN_POINT() \
		{\
			float SaveX = (float)Center.x - (float)(pFeatureMap->SX>>1);\
			float SaveY = (float)Center.y - (float)(pFeatureMap->SY>>1);\
			Center.x = (float)(pFeatureMap->SX>>1) + SaveX * cosf(-pLocalDF->BaseHead.Angle) + SaveY * sinf(-pLocalDF->BaseHead.Angle);\
			Center.y = (float)(pFeatureMap->SY>>1) - SaveX * sinf(-pLocalDF->BaseHead.Angle) + SaveY * cosf(-pLocalDF->BaseHead.Angle);\
		};\

void RecenterFace(DETECTEDFace *pLocalDF)
{
	if ((pLocalDF->LeftEye.z!=0) && (pLocalDF->RightEye.z!=0))
	{
		Vector Distance;
		float Dis;
		Distance = pLocalDF->LeftEye-pLocalDF->RightEye;
		Dis = Distance.NormXY();
		pLocalDF->HeadPos = (pLocalDF->LeftEye+pLocalDF->RightEye) * 0.5f;
		pLocalDF->HeadPos.x -= Distance.y * 0.5f;
		pLocalDF->HeadPos.y -= Distance.x * 0.5f;
		pLocalDF->HeadPos.z = pLocalDF->BaseHead.Angle = asinf(Distance.y / Dis);
		pLocalDF->HeadSize.x = pLocalDF->HeadSize.y = Dis*2.4;
	}
}


u32 ASM_IsFeatureColor(u32 Color)
{
		u8 R,G,B;
		u8 MAX,MIN;
		MIN = MAX = R = (u8)((Color >> 16) & 0xff);
		G = (u8)((Color >> 8) & 0xff);
		B = (u8)((Color >> 0) & 0xff);
		if (G < MIN) MIN = G;
		if (B < MIN) MIN = B;
		if (G > MAX) MAX = G;
		if (B > MAX) MAX = B;
		if (MAX-MIN > 100)
		{
			return 1;
		}
		return 0;
}


#define ASM8_SCLALAR float
ASM8_SCLALAR ASM8_Feature(s32 *Ptr,HAAR_Feature *pFeatures,u32 Num,ASM8_SCLALAR WindowSum)
{
	ASM8_SCLALAR All;
	HAAR_Feature *pFeaturesLast;
	s32 *Ptr2;
	All = 0;
	pFeaturesLast = pFeatures + Num;
	while (pFeatures < pFeaturesLast)
	{
		ASM8_SCLALAR BigSumm;
		HAAR_RECT *pFeat;
		pFeat = pFeatures->Feat;		
		Ptr2 = Ptr+pFeat->Offset0;
		BigSumm = (ASM8_SCLALAR)pFeat->W * (ASM8_SCLALAR)(*(Ptr2) - *(Ptr2+pFeat->Offset1) - *(Ptr2+pFeat->Offset2) + *(Ptr2+pFeat->Offset1+pFeat->Offset2));
		pFeat++;
		Ptr2 = Ptr+pFeat->Offset0;
		BigSumm += (ASM8_SCLALAR)pFeat->W * (ASM8_SCLALAR)(*(Ptr2) - *(Ptr2+pFeat->Offset1) - *(Ptr2+pFeat->Offset2) + *(Ptr2+pFeat->Offset1+pFeat->Offset2));
		pFeat++;
		if (pFeat->W)
		{
			Ptr2 = Ptr+pFeat->Offset0;
			BigSumm += (ASM8_SCLALAR)pFeat->W * (ASM8_SCLALAR)(*(Ptr2) - *(Ptr2+pFeat->Offset1) - *(Ptr2+pFeat->Offset2) + *(Ptr2+pFeat->Offset1+pFeat->Offset2));
		}
		BigSumm  *= HAAR_SCALAR_MULT ;
		BigSumm = (ASM8_SCLALAR)pFeatures->Thresh * ((ASM8_SCLALAR)WindowSum) - BigSumm;
		All += pFeatures->LeftRight[*(u32 *)&BigSumm>>31];
		pFeatures++;
	}
	return All;
}


u32 ASM8_DETECT(s32 *Ptr,HAAR_Classifier *p_HAAR,ASM8_SCLALAR WindowSumm )
{
	u32 REturnV;
	HAAR_Stage *p_Current;
	p_Current = p_HAAR->p_Stages;
	REturnV = 0;
	u32 StCounter = 0;
	while (p_Current)
	{
		float Res;
		Res = (float)ASM8_Feature(Ptr,p_Current->pStageFeatures,p_Current->ulNumberOfFeatures,WindowSumm);
		REturnV = (*(u32*)&Res)>>31;
		p_Current = p_Current->pNext[REturnV];
		if ((StCounter++) > 20) 
			p_Current = NULL;
	}
	return REturnV^1;
};


u32 MAP8Q_IsIn(MAP8_Quad *p1,MAP8_Quad *p2)
{
	float X,Y,W;
	W = p1->W / p2->W;
	X = (p2->X1 + p2->X2) * 0.5f * W;
	Y = (p2->Y1 + p2->Y2) * 0.5f * W;
	if ((X > p1->X1) &&(Y > p1->Y1) &&(X < p1->X2) &&(Y < p1->Y2) )return 1;
	return 0;
}

int __cdecl TRACKEDPOINT_FACE_Compare(const void *elem1, const void *elem2 )
{
	if (((MAP8_Quad *)elem2)->X1 < ((MAP8_Quad *)elem1)->X1)
		return -1;
	else
		return 1;
}


void DrawCross(MAP *image, Vector *v, u32 color, int len, char orient)
{
	if (!v) return;
	POINT p = {	v->x, v->y };
	DrawCross(image, p, color, len, orient);
}

void ASM_DrawFace(MAP *pSrc,DETECTEDFace *pDF)
{
	DrawQuad_R(pSrc,pDF->HeadPos.x - pDF->HeadSize.x * 0.375f,pDF->HeadPos.y - pDF->HeadSize.y * 0.375f,
					pDF->HeadPos.x + pDF->HeadSize.x * 0.375f,pDF->HeadPos.y + pDF->HeadSize.y * 0.375f,
					0xFF0000 ,3,pDF->HeadPos.z);//*/
	if (pDF->RightEye.z)	DrawCross(pSrc,&pDF->RightEye,0xff00ff00,4);
	if (pDF->Nose.z)		DrawCross(pSrc,&pDF->Nose,0xff00ff00,4);
	if (pDF->RightMouse.z)	DrawCross(pSrc,&pDF->RightMouse,0xff00ff00,4);
	if (pDF->LeftEye.z)		DrawCross(pSrc,&pDF->LeftEye,0xff00ff00,4);
	if (pDF->LeftMouse.z)	DrawCross(pSrc,&pDF->LeftMouse,0xff00ff00,4);//*/
}


u32 ASM_GetCenterG(MAP *pSrc,s32 X1,s32 Y1,s32 X2,s32 Y2,POINT *pCenter)
{
	u32 Weight = 0;
	pCenter->x = pCenter->y = 0;
	for (int X = X1 ; X < X2 ;X++)
		for (int Y = Y1 ; Y < Y2 ;Y++)
		{
			u32 D;
			D = *((u32*)pSrc->GetBase() + X + Y * pSrc->PITCH)>>24;
			if (D)
			{
				pCenter->x += X * D;
				pCenter->y += Y * D;
				Weight += D;
			}
		}
	if (Weight)
	{
		pCenter->x /= Weight;
		pCenter->y /= Weight;
		return 1;
	}
	return 0;
}

void ASM_DetectFeatures(MAP *pFeatureMap, MAP *pSourceMap, MAP *pSourceMapDRW, DETECTEDFace *pLocalDF)
{
	POINT A,B;
	DETECTEDFace SaveDf;
	u32 SX,SY;
	MAP8_Quad SBH;
	MAP *pFeatureMapSave;
	pFeatureMapSave = pFeatureMap;
	SaveDf = *pLocalDF;
	memset((void*)pLocalDF,0,sizeof(*pLocalDF));
	SBH = pLocalDF->BaseHead = SaveDf.BaseHead;
	pLocalDF->DeadTime = SaveDf.DeadTime;

	A.x = (SBH.X1 / SBH.W);
	A.y = SBH.Y1 / SBH.W;
	B.x = SBH.X2 / SBH.W;
	B.y = SBH.Y2 / SBH.W;
	SX = B.x - A.x;
	SY = B.y - A.y;
	A.x += SX / 10;
	B.x += SX / 10;
	pLocalDF->HeadPos.x = (A.x + B.x) * 0.5f;
	pLocalDF->HeadPos.y = (A.y + B.y) * 0.5f;
	pLocalDF->HeadPos.z = pLocalDF->BaseHead.Angle;
	pLocalDF->HeadSize.x = (B.x - A.x);
	pLocalDF->HeadSize.y = (B.y - A.y);
	pLocalDF->HeadSize.z = 0.0f;

	COPY_PATCH_RGBA(pSourceMap,pFeatureMap,(SX<<7) + (A.x<<8),(SY<<7) + (A.y<<8),0,0,SX<<8,SY<<8,FMDMS<<8,FMDMS<<8,pLocalDF->BaseHead.Angle,1);
	
	MAP FeatureMap;
	
	FeatureMap = *pFeatureMap;
	FeatureMap.SetBase(pFeatureMap->GetBase()) ;
	FeatureMap.SX = FMDMS;
	FeatureMap.SY = FMDMS;
	pFeatureMap = &FeatureMap;
	AND(pFeatureMap,0xffffff);
	POINT Center;
	s32 DeltaH;
	s32 DeltaV;
/* Eyes Detection *****************************************************************************************/
	DeltaH = ((FMDMS>>1) - (FMDMS>>3))>>2;
	DeltaV = ((FMDMS>>1) - (FMDMS>>3))>>2;
	s32 EyeFinder[4],MCFinder[4];
	EyeFinder[0] = (FMDMS>>3) + DeltaH; // X1
	EyeFinder[1] = (FMDMS>>1) + DeltaV; // Y1
	EyeFinder[2] = (FMDMS>>1) - DeltaH; // X2 
	EyeFinder[3] = (FMDMS>>1) + (FMDMS>>2)  + (FMDMS>>3) - DeltaV; // Y2

	Feature_DETECT(pFeatureMap,NULL,0,0);
	if (ASM_GetCenterG(pFeatureMap,EyeFinder[0],EyeFinder[1],EyeFinder[2],EyeFinder[3],&Center))
	{
		TURN_POINT();
		pLocalDF->RightEye.x = Center.x;
		pLocalDF->RightEye.y = Center.y;
		pLocalDF->RightEye.z = 1.0f;
	}

	FLIP_H(pFeatureMap);
	Feature_DETECT(pFeatureMap,NULL,0,0);
	if (ASM_GetCenterG(pFeatureMap,EyeFinder[0],EyeFinder[1],EyeFinder[2],EyeFinder[3],&Center))
	{
		Center.x = pFeatureMap->SX - Center.x;
		TURN_POINT();
		pLocalDF->LeftEye.x = Center.x;
		pLocalDF->LeftEye.y = Center.y;
		pLocalDF->LeftEye.z = 1.0f;

	}
	FLIP_H(pFeatureMap);
/* NOSE *****************************************************************************************************************/
	MCFinder[0] = (FMDMS>>1)-(FMDMS>>3); // X1
	MCFinder[2] = (FMDMS>>1)+(FMDMS>>3); // X2 
	MCFinder[1] = (FMDMS>>1)-(FMDMS>>3); // Y1
	MCFinder[3] = (FMDMS>>1)+(FMDMS>>3); // Y2
	Feature_DETECT(pFeatureMap,NULL,0,2);
	if (ASM_GetCenterG(pFeatureMap,MCFinder[0],MCFinder[1],MCFinder[2],MCFinder[3],&Center))
	{
		TURN_POINT();
		pLocalDF->Nose.x = Center.x;
		pLocalDF->Nose.y = Center.y;
		pLocalDF->Nose.z = 1.0f;

	}
/* Mouth  *****************************************************************************************************************/
/* Left corner (on the screen)*****************************************************************************************************************/

	Feature_DETECT(pFeatureMap,NULL,0,1);
	MCFinder[0] = (FMDMS>>2); // X1
	MCFinder[1] = (FMDMS>>2)-(FMDMS>>3); // Y1
	MCFinder[2] = (FMDMS>>1)-(FMDMS>>4); // X2 
	MCFinder[3] = (FMDMS>>1)-(FMDMS>>3); // Y2

	if (ASM_GetCenterG(pFeatureMap,MCFinder[0],MCFinder[1],MCFinder[2],MCFinder[3],&Center))
	{
		TURN_POINT();
		pLocalDF->RightMouse.x = Center.x;
		pLocalDF->RightMouse.y = Center.y;
		pLocalDF->RightMouse.z = 1.0f;
	}

/* Right corner (on the screen)*****************************************************************************************************************/
	FLIP_H(pFeatureMap);
	Feature_DETECT(pFeatureMap,NULL,0,1);
	if (ASM_GetCenterG(pFeatureMap,MCFinder[0],MCFinder[1],MCFinder[2],MCFinder[3],&Center))
	{
		Center.x = pFeatureMap->SX - Center.x;
		TURN_POINT();
		pLocalDF->LeftMouse.x = Center.x;
		pLocalDF->LeftMouse.y = Center.y;
		pLocalDF->LeftMouse.z = 1.0f;

	}
	FLIP_H(pFeatureMap);

	/* Transform Feature Coordinate into real space */
	{
		Vector Add,Mul;
		Add.x = SBH.X1 + (SBH.X2-SBH.X1)/10.0f;
		Add.y = SBH.Y1;
		Add.z = 0.0f;
		Mul.x = (float)(SBH.X2-SBH.X1) / (float)FMDMS;
		Mul.y = (float)(SBH.Y2-SBH.Y1) / (float)FMDMS;
		Mul.z = 1.0f;
		pLocalDF->RightEye = Add + (pLocalDF->RightEye & Mul);
		pLocalDF->LeftEye = (pLocalDF->LeftEye & Mul) + Add;
		pLocalDF->Nose = (pLocalDF->Nose & Mul) + Add;
		pLocalDF->RightMouse = (pLocalDF->RightMouse & Mul) + Add;
		pLocalDF->LeftMouse = (pLocalDF->LeftMouse & Mul) + Add;
		pLocalDF->UpMouse = (pLocalDF->UpMouse & Mul) + Add;
		pLocalDF->DownMouse = (pLocalDF->DownMouse & Mul) + Add;
	}

	DETECTEDFace LocalDF;
	if (0)
	{
		LocalDF = *pLocalDF;
		RecenterFace(pLocalDF);
		{
			/* compute the shape of the face */
			LocalDF.HeadPos.x -= LocalDF.HeadSize.x/20.0;
			pFeatureMap = pFeatureMapSave;
			FeatureMap.SetBase(pFeatureMap->GetBase()) ;
			FeatureMap.SX = ASM_COMMPUTE_SHAPE_SIZEMAP;
			FeatureMap.SY = (s32)((float)ASM_COMMPUTE_SHAPE_SIZEMAP * ((float)SY / (float)SX));
			pFeatureMap = &FeatureMap;
			pFeatureMapSave = pFeatureMap;

			s32 SourceSizeX = (s32)((float)LocalDF.HeadSize.x * 1.10f);
			s32 SourceSizeY = (s32)((float)LocalDF.HeadSize.y * 1.10f);

			MAP32_SET(pFeatureMap,0);
			/* compute a "working map" close to the template of the face shape */
			COPY_PATCH_RGBA(
				pSourceMap,
				pFeatureMap,
				((s32)(LocalDF.HeadPos.x)<<8),
				((s32)LocalDF.HeadPos.y<<8),
				0,
				0,
				SourceSizeX<<8,SourceSizeY<<8,
				FeatureMap.SX<<8,FeatureMap.SY<<8,LocalDF.HeadPos.z,1);
			MATRIX TMatrix;
			MATRIX TMatrix2;
			TMatrix.Identity();
			TMatrix2.Identity();
			float ScaleX,ScaleY;
			ScaleX = (float)FeatureMap.SX / (float)SourceSizeX ;
			ScaleY = (float)FeatureMap.SY / (float)SourceSizeY;
			TMatrix2.T.x = - LocalDF.HeadPos.x;
			TMatrix2.T.y = - LocalDF.HeadPos.y;
			TMatrix.I.x *= ScaleX;
			TMatrix.J.y *= ScaleY;
			TMatrix.RotateAround_K(-LocalDF.HeadPos.z);
			TMatrix2 *= TMatrix;
			TMatrix2.T.x += (float)FeatureMap.SX * 0.5f;
			TMatrix2.T.y += (float)FeatureMap.SY * 0.5f;

			if ((LocalDF.RightMouse.z!=0) && (LocalDF.LeftMouse.z!=0))
				/* Compute shape with the mouth */
				ComputeBottomShape(pFeatureMap,pSourceMapDRW,&LocalDF,&TMatrix2);
			else
				/* Compute shape without mouth */
				ComputeBottomShape(pFeatureMap,pSourceMapDRW,NULL,&TMatrix2);
		}
	}
}

u32 HAAR_Detect(MAP *pSrcMAP,HAAR_Classifier *pHFF,MAP *pDrawMAP,
				float Factor,float gAddX,float gAddY,u32 AGE,float Angle,
				MAP8_Quad *MAP8_QuadStack, u32 *MAP8_QuadStackNumber, MAP8_Quad *pBlendQuad)
{
	s32 *pSrc,*pSrcL,*pSrcLL,dX,dY = 0,AddX,AddY;
	u32 p1,p2;
	if (pHFF->WindowSize.X > pSrcMAP->SX) return 0 ;
	if (pHFF->WindowSize.Y > pSrcMAP->SY) return 0;

	AddX = 1;
	AddY = 1;

	p1 = pHFF->WindowSize.X ;
	p2 = pSrcMAP->PITCH*(pHFF->WindowSize.Y );

	pSrc = pSrcMAP->GetBase();
	pSrcLL = pSrc + pSrcMAP->SX + pSrcMAP->PITCH * (pSrcMAP->SY - pHFF->WindowSize.Y);
	pSrcL = pSrc + pSrcMAP->SX - pHFF->WindowSize.X;
	while (pSrc<pSrcLL)
	{
		pSrcL = pSrc + pSrcMAP->SX - pHFF->WindowSize.X;
		dX = 0;
		while (pSrc<pSrcL)
		{
			ASM8_SCLALAR WindowSumm ;
			WindowSumm = (ASM8_SCLALAR)(*(pSrc) - *(pSrc + p1) - *(pSrc + p2) + *(pSrc + p1 + p2));
			if (ASM8_DETECT(pSrc,pHFF,WindowSumm * WINDOW_SUM_FACTOR_II))
			{
				if (*MAP8_QuadStackNumber < MAX_MAP8_QUAD)
				{
					memset(&MAP8_QuadStack[*MAP8_QuadStackNumber],0,sizeof(MAP8_QuadStack[*MAP8_QuadStackNumber]));
					MAP8_QuadStack[*MAP8_QuadStackNumber].X1 = Factor*(float)(dX ) + gAddX;
					MAP8_QuadStack[*MAP8_QuadStackNumber].Y1 = Factor*(float)(dY ) + gAddY;
					MAP8_QuadStack[*MAP8_QuadStackNumber].X2 = Factor*(float)(dX+pHFF->WindowSize.X)  + gAddX;
					MAP8_QuadStack[*MAP8_QuadStackNumber].Y2 = Factor*(float)(dY+pHFF->WindowSize.Y)  + gAddY;
					MAP8_QuadStack[*MAP8_QuadStackNumber].W = 1.0f;
					MAP8_QuadStack[*MAP8_QuadStackNumber].Age = AGE;
					MAP8_QuadStack[*MAP8_QuadStackNumber].Angle = Angle;

					if (pBlendQuad)
					{
#define BlendFactorQuad 1.0f
						MAP8_QuadStack[*MAP8_QuadStackNumber].Angle += pBlendQuad->Angle * BlendFactorQuad;
						MAP8_QuadStack[*MAP8_QuadStackNumber].X1 += pBlendQuad->X1 * BlendFactorQuad;
						MAP8_QuadStack[*MAP8_QuadStackNumber].X2 += pBlendQuad->X2 * BlendFactorQuad;
						MAP8_QuadStack[*MAP8_QuadStackNumber].Y1 += pBlendQuad->Y1 * BlendFactorQuad;
						MAP8_QuadStack[*MAP8_QuadStackNumber].Y2 += pBlendQuad->Y2 * BlendFactorQuad;
						MAP8_QuadStack[*MAP8_QuadStackNumber].W += pBlendQuad->W * BlendFactorQuad;
					}

					if (*MAP8_QuadStackNumber < MAX_MAP8_QUAD)
						(*MAP8_QuadStackNumber)++;
				}
			}
			dX+=AddX;
			pSrc+=AddX;
		}
		dY+=AddY;
		pSrc = pSrcMAP->GetBase() + dY*pSrcMAP->PITCH ;
	}
	return 1;
}