#include "Fast2D.h"
#include <math.h>
#include <stdio.h>
#include "memory.h"
//#pragma optimize("",off)

s32 Copied = 0;
/*************************************************************************************************************/
/*************************************************************************************************************/
/********* BEGIN RANSAC ****************************************************************************************/
/*************************************************************************************************************/
/*************************************************************************************************************/
u32					CFast2D::FB_init = 1;
Fast2DFeatureBox	CFast2D::FB_Table[MAX_FEATBOX];
void DRawTEXT_Soft_B(u8 *pTEXT,u32 *pBase,u32 X, u32 Y, s32 Pitch , u32 Height , u32 Color);

u32 CFast2D::EvaluateNumber(u8 *Valid , u32 ulNumber)
{
	u32 Result = 0,Counter = ulNumber;
	while (Counter--){ if (*(Valid++) == 1) Result++;}
	return Result;
}
/*	Compute Error from estimation matrix by projecting Lastpos and compare it with Newpos */
float CFast2D::EvaluateError(u8 *Valid , u32 ulNumber, const Vector *Lastpos ,  const Vector *Newpos , MATRIX *Matrix23 , float Threshold)
{
	float Error = 0.0f , Pond = 0.0f;
	u32 Counter = ulNumber;
	while (Counter--)
	{
		if (*(Valid))
		{
			Vector Result;
			Result = *Matrix23 * *Lastpos;
			float ErrorLocal = (*Newpos - Result).NormXY();
			//Newpos->z = 1.0f / (ErrorLocal+1.0f);
			if (ErrorLocal < Threshold)
			{
				if (*(Valid) == 1)
				{
					Error += ErrorLocal;
					Pond++;
				}//*/
				*(Valid) = 1;
			} else 
				*(Valid) = 2;
		}
		Valid++;
		Newpos++;
		Lastpos++;
	}
	if (Pond)
		return Error/(float)Pond;
	else
		return 0.0f;

}

// Compute gravity center of the set by pondere it with Z ( = Matching quality ) 
u32 CFast2D::ComputeGCXY(u8 *Valid ,  u32 ulNumber , const Vector *Positions , Vector *GCenter )
{
	*GCenter = Vector(0,0,0);
	u32 Counter = ulNumber;
	while (Counter--) 
	{
		if (*(Valid++) == 1)
		{
			GCenter->x += Positions->x;
			GCenter->y += Positions->y;
			GCenter->z ++;
		}
		Positions++;
	}
	if (GCenter->z)
	{
		GCenter->x /= GCenter->z;
		GCenter->y /= GCenter->z;
		return 1;
	} else
		return 0;
}

/*	
		Compute scale factor from GravityCenter 1 & 2
*/
float CFast2D::ComputeGCScale(u8 *Valid ,  u32 ulNumber , const Vector *Lastpos ,  const Vector *Newpos , Vector *LastGC, Vector *NewGC , float *Scale , float *Angle)
{
	float R1 = 0.0f, R2 = 0.0f,SinusAlpha = 0.0f , Pond = 0.0f , CosAlpha = 0.0f;
	u32 Counter1 = ulNumber,RES = 0;
	while (Counter1--) 
	{
		if (*(Valid++) == 1)
		{
			float LastNorm = (*Lastpos - *LastGC).NormXY();
			float NewNorm = (*Newpos - *NewGC).NormXY();
			if (LastNorm * NewNorm  > 2.0f*2.0f)
			{
				R1 += LastNorm ;
				R2 += NewNorm ;
				Vector V1,V2;
				V1 = (*Lastpos - *LastGC);
				V2 = (*Newpos - *NewGC);
				V1.z = V2.z = 0.0f;
				V1 *= NewNorm / V1.NormXY();
				V2 *= 1.0f / V2.NormXY();
				SinusAlpha +=  Newpos->z * (V1 ^ V2).z;
				CosAlpha += Newpos->z * (V1 * V2);
				Pond += Newpos->z * NewNorm/** NewNorm * LastNorm*/;
				RES++;
			}
		}
		Newpos++;
		Lastpos++;
	}
	Valid-=ulNumber;
	*Scale = 1.0f;
	*Angle = 0.0f;
	if (R1 * R2 != 0.0f)
	{
		//*Scale = R2 / R1 ;
		if (Pond) 
		{
			
			*Angle = asinf(SinusAlpha / Pond);
			if (CosAlpha < 0)
				*Angle = 3.1415927f - *Angle;
			if (*Angle > 3.1415927f / 4) *Angle = 3.1415927f / 4;
			if (*Angle < -3.1415927f / 4) *Angle = -3.1415927f / 4;
		}
		/* Compute Ratio Y */
	}
	return Pond;
}

void CFast2D::ComputeGCXYScale(u8 *Valid ,  u32 ulNumber , const Vector *Lastpos ,  const Vector *Newpos , Vector *YAxis,float *XScale,float *YScale)
{
	float YPreviousSQR = 0,YPrevious = 0;
	float YCurrentSQR = 0,YCurrent = 0;
	float XPreviousSQR = 0,XPrevious = 0;
	float XCurrentSQR = 0,XCurrent = 0;
	float Pond = 0.0f ;
	u32 Counter1 = ulNumber,RES = 0;
	while (Counter1--) 
	{
		if (*(Valid++) == 1)
		{
			float P = 1.0f;
			float DPNew = (Newpos->x * YAxis->x + Newpos->y * YAxis->y) ;
			YCurrentSQR += DPNew * DPNew * P ;
			YCurrent += DPNew * P;
			float DPOld = (Lastpos->x * YAxis->x + Lastpos->y * YAxis->y) ;
			YPreviousSQR += DPOld * DPOld* P ;
			YPrevious += DPOld* P ;
			DPNew = (Newpos->x * YAxis->y - Newpos->y * YAxis->x) ;
			XCurrentSQR += DPNew * DPNew* P ;
			XCurrent += DPNew* P ;
			DPOld = (Lastpos->x * YAxis->y - Lastpos->y * YAxis->x) ;
			XPreviousSQR += DPOld * DPOld* P ;
			XPrevious += DPOld* P ;
			Pond += P ;
			RES++;
		}
		Newpos++;
		Lastpos++;
	}
	float OoP = 1.0f/Pond;
	YPrevious = sqrtf(OoP * YPreviousSQR - OoP * OoP * YPrevious * YPrevious );
	YCurrent = sqrtf(OoP * YCurrentSQR - OoP * OoP * YCurrent * YCurrent );
	XPrevious = sqrtf(OoP * XPreviousSQR - OoP * OoP * XPrevious * XPrevious );
	XCurrent = sqrtf(OoP * XCurrentSQR - OoP * OoP * XCurrent * XCurrent );
	*YScale = YCurrent / YPrevious ;
	*XScale = XCurrent / XPrevious ;
#define LIMITSCALE 0.25
	if (*YScale  < 0.25f) *YScale = 0.25f;
	if (*YScale  > 4.f) *YScale = 4.f;
	if (*XScale  < 0.25f) *XScale = 0.25f;
	if (*XScale  > 4.f) *XScale = 4.f;
}

/*
		Try to find the best projection matrix as possible.
*/
float CFast2D::FeatureBoxRansac(u8 *Valid ,  u32 ulNumber , const Vector *LastPosition ,  const Vector *NewPosition , Vector *SpeedScaleDest , float *Rotation,Fast2DFeatureBox *pFB ,float LowLimit)
{
	MATRIX Estim,Estim2,Covariance,InvCovariance;
	Vector OldGC,NewGC,Speed,NewT;
	float ErrorEst,Sclae,Rot,ErrorFactor;
	u32 Converge = 10;
	Sclae = 1.0f;
	Rot = 0.0f;
	ErrorFactor = 1.0f;
	u32 FullMatrixMode = pFB->FixeMode * 3;

#define DOOMA LastPosition
#define DOOMB NewPosition  
	while (Converge--)
	{
		if (EvaluateNumber(Valid,ulNumber) < ulNumber * 0.75f) FullMatrixMode = 0;

		/* 1 Evaluate a Speed */
		ComputeGCXY(Valid ,ulNumber, DOOMA , &OldGC );
		ComputeGCXY(Valid ,ulNumber, DOOMB , &NewGC );	
		if (FullMatrixMode) 
		{
			/* Full matrix evaluation */
			memset(&Covariance,0,sizeof(Covariance));
			float RealNum = 0.0f;

			Vector I = Vector(0,0,0);
			Vector J = Vector(0,0,0);
			Vector Res = Vector(0,0,0);
			for (u32 i = 0; i < ulNumber ; i++)
			{
				if (Valid[i] == 1)
				{
					Vector Last,New;
					Last = DOOMA[i] - OldGC;
					New = DOOMB[i] - NewGC;
					Covariance.I.x += Last.x * Last.x;
					Covariance.I.y += Last.x * Last.y;
					Covariance.J.y += Last.y * Last.y;
					I.x += Last.x * New.x;
					I.y += Last.y * New.x;
					J.x += Last.x * New.y;
					J.y += Last.y * New.y;
					RealNum+=1.0f;
				}
			}
			RealNum = 1.0f / RealNum;
			Covariance.I *= RealNum;
			Covariance.J *= RealNum;
			Covariance.J.x = Covariance.I.y;
			I *= RealNum;
			J *= RealNum;
			Covariance.K.z = 1;
			Covariance.Inverse(InvCovariance);
			I = InvCovariance * I;
			J = InvCovariance * J;
			Estim.Identity();
			Estim2.Identity();
			Estim2.T = OldGC * -1;
			I -= Vector(1,0,0);
			J -= Vector(0,1,0);
			float LocalN = I.NormXY() + J.NormXY();
#define Smooth 0.25f
			if (LocalN < Smooth) 
			{
				LocalN = 15*LocalN / Smooth;
				LocalN += 1;
				LocalN /= 16;
				I = I * LocalN ;
				J = J * LocalN ;
			}
			I += Vector(1,0,0);
			J += Vector(0,1,0);
			Estim.I.x = I.x;
			Estim.J.x = I.y;
			Estim.I.y = J.x;
			Estim.J.y = J.y;
			Estim.T = NewGC;
			Estim.T.z = 0;
			Estim2 *= Estim;
			Estim =Estim2 ; 
		} else {
			ComputeGCScale(Valid , ulNumber,DOOMA, DOOMB, &OldGC , &NewGC , &Sclae, &Rot);
			ComputeGCXYScale(Valid ,  ulNumber , DOOMA, DOOMB, &Vector(sinf(Rot),cosf(Rot),0),&pFB->XScale,&pFB->YScale);
			if (pFB->ComputeRotation == 0) Rot = 0.0f;
			if (pFB->ComputeScale == 0) pFB->XScale = pFB->YScale = 1.0f;
			if (!pFB->FixeMode) 
			{
				pFB->XScale = (pFB->YScale + pFB->XScale) / 2.0f;
				pFB->YScale = pFB->XScale ;
				pFB->XScale = pFB->YScale = 1.0f;
			}
			Estim2.Identity();
			Estim2.T = OldGC * -1;
			Estim.Identity();
			Estim.I.x = pFB->XScale;
			Estim.J.y = pFB->YScale;
			Estim.RotateAround_K(Rot);
			Estim.T = NewGC ;
			Estim.T.z = 0.0f;//*/
			Estim2 *= Estim;
			Estim =Estim2 ; 
		}
		/* 4 Decimate & Reinsert */
		ErrorEst = EvaluateError(Valid , ulNumber , DOOMA, DOOMB, &Estim);
		EvaluateError(Valid , ulNumber , DOOMA, DOOMB, &Estim , ErrorEst * ErrorFactor + LowLimit );//*/
		ErrorFactor *= 1.0f/2.0f;
	}
	Speed = NewGC - OldGC;
	SpeedScaleDest->x = Speed.x;
	SpeedScaleDest->y = Speed.y;
	SpeedScaleDest->z = (pFB->XScale + pFB->YScale) / 2;
	*Rotation = Rot;//*/
	pFB->LastTransforMatrix = Estim;
	return (float) EvaluateNumber(Valid,ulNumber);
}
/*************************************************************************************************************/
/*************************************************************************************************************/
/********* END RANSAC ****************************************************************************************/
/*************************************************************************************************************/
/*************************************************************************************************************/


void CFast2D::FeatureBoxUpdateOne(u32 Index)
{
	if (FB_init) return ;
	{
		FB_Table[Index].ulNumberOfParticules = 0;
		s32 Counter = FB_Table[Index].DXSize * FB_Table[Index].DYSize;
		
		while (Counter--)
		{
			s32 cX,cY;
			cX = (float)(Counter % FB_Table[Index].DXSize) * (float)FB_Table[Index].W / (float)FB_Table[Index].DXSize + FB_Table[Index].X - (float)FB_Table[Index].W/2 + (float)FB_Table[Index].W / ((float)FB_Table[Index].DXSize * 2.0f);
			cY = (float)(Counter / FB_Table[Index].DXSize) * (float)FB_Table[Index].H / (float)FB_Table[Index].DYSize + FB_Table[Index].Y - (float)FB_Table[Index].H/2 + (float)FB_Table[Index].H / ((float)FB_Table[Index].DYSize * 2.0f);
			FB_Table[Index].FirstPos[FB_Table[Index].ulNumberOfParticules].x = cX;
			FB_Table[Index].FirstPos[FB_Table[Index].ulNumberOfParticules].y = cY;
			FB_Table[Index].FirstPos[FB_Table[Index].ulNumberOfParticules].z = 1.0f;
			if (FB_Table[Index].TrackerID[FB_Table[Index].ulNumberOfParticules] == -1) 
				FB_Table[Index].TrackerID[FB_Table[Index].ulNumberOfParticules] = New(cX , cY);
			else
			{
				Set(FB_Table[Index].TrackerID[FB_Table[Index].ulNumberOfParticules],&FB_Table[Index].FirstPos[FB_Table[Index].ulNumberOfParticules]);
			}
			UpdateData(FB_Table[Index].TrackerID[FB_Table[Index].ulNumberOfParticules]);

			/* Max Optimize */
			u32 FalsePoint = 0;
/*			float Max = GetNorm(FB_Table[Index].TrackerID[FB_Table[Index].ulNumberOfParticules], 0);
//			int CounterIter = ((int)(0.5f * (float)FB_Table[Index].W / (float)FB_Table[Index].DXSize))<<2;
			Vector FinalPos = FB_Table[Index].FirstPos[FB_Table[Index].ulNumberOfParticules];
#define RadiusSearch 6*1
			for(int X = -RadiusSearch + 1 ; X < RadiusSearch ; X++)
			for(int Y = -RadiusSearch + 1 ; Y < RadiusSearch ; Y++)
			{
				Vector Srch = FB_Table[Index].FirstPos[FB_Table[Index].ulNumberOfParticules];
				Srch.x += X;
					Srch.y += Y;
				Set(FB_Table[Index].TrackerID[FB_Table[Index].ulNumberOfParticules],&Srch);
				UpdateData(FB_Table[Index].TrackerID[FB_Table[Index].ulNumberOfParticules]);
				if (Max >= GetNorm(FB_Table[Index].TrackerID[FB_Table[Index].ulNumberOfParticules], 0))
				{
					FinalPos = Srch;
					Max = GetNorm(FB_Table[Index].TrackerID[FB_Table[Index].ulNumberOfParticules], 0);
					FalsePoint = 0;
				}
			}
			FB_Table[Index].FirstPos[FB_Table[Index].ulNumberOfParticules] = FinalPos;
			Set(FB_Table[Index].TrackerID[FB_Table[Index].ulNumberOfParticules],&FB_Table[Index].FirstPos[FB_Table[Index].ulNumberOfParticules]);
			UpdateData(FB_Table[Index].TrackerID[FB_Table[Index].ulNumberOfParticules]);//*/
			ComputeTrack(FB_Table[Index].TrackerID[FB_Table[Index].ulNumberOfParticules],2);
			if ((FalsePoint) || (GetError(FB_Table[Index].TrackerID[FB_Table[Index].ulNumberOfParticules]) <= 0.6))
			{
				Destroy(FB_Table[Index].TrackerID[FB_Table[Index].ulNumberOfParticules]);
				FB_Table[Index].TrackerID[FB_Table[Index].ulNumberOfParticules] = -1;
			} else
			{
				FB_Table[Index].ParticlesPonderation[FB_Table[Index].ulNumberOfParticules] = 1.0f / GetNorm(FB_Table[Index].TrackerID[FB_Table[Index].ulNumberOfParticules], 0);
				Set(FB_Table[Index].TrackerID[FB_Table[Index].ulNumberOfParticules],&FB_Table[Index].FirstPos[FB_Table[Index].ulNumberOfParticules]);
				FB_Table[Index].ulNumberOfParticules ++;
			}
			
		}
		float ParticlesPonderationSumm = 0;
		for (int i = 0 ; i < FB_Table[Index].ulNumberOfParticules ; i ++)
			ParticlesPonderationSumm += FB_Table[Index].ParticlesPonderation[i];
		ParticlesPonderationSumm = 1.0f / ParticlesPonderationSumm;
		for (int i = 0 ; i < FB_Table[Index].ulNumberOfParticules ; i ++)
			FB_Table[Index].ParticlesPonderation[i] *= ParticlesPonderationSumm;
	}
}
u32 CFast2D::DestroyFeatureBox(u32 Index)
{
	if (Index > MAX_FEATBOX) return 0;
	s32 Counter = MAX_QFB;
	while (Counter--) Destroy(FB_Table[Index].TrackerID[Counter]);
	return 1;
}


u32 CFast2D::CreateFeatureBox(u32 Index, s32 X,s32 Y,s32 W, s32 H, float Rotation)
{
	if (FB_init)
	{
		memset(FB_Table,0,sizeof(FB_Table));
		FB_init = 0;
	}
	if (Index > MAX_FEATBOX) return 0;
	if (FB_Table[Index].IsInit) DestroyFeatureBox(Index);
	memset(FB_Table[Index].TrackerID,-1,sizeof(FB_Table[Index].TrackerID));
	FB_Table[Index].IsInit = 0;//*/
	FB_Table[Index].X = X + abs(W) / 2;
	FB_Table[Index].Y = Y + abs(H) / 2;
	FB_Table[Index].SearchX = FB_Table[Index].SearchY = 0.0f;
	FB_Table[Index].W = abs(W) ;
	FB_Table[Index].H = abs(H) ;
	FB_Table[Index].DXSize = W / DISC_PIXEL;
	FB_Table[Index].DYSize = H / DISC_PIXEL;
	FB_Table[Index].BigSearchBase = 1;
	if (FB_Table[Index].DXSize < 3) FB_Table[Index].DXSize = 3;
	if (FB_Table[Index].DYSize < 3) FB_Table[Index].DYSize = 3;
	if (FB_Table[Index].DXSize > MAX_QFB_DX) FB_Table[Index].DXSize = MAX_QFB_DX;
	if (FB_Table[Index].DYSize > MAX_QFB_DY) FB_Table[Index].DYSize = MAX_QFB_DY;
	if (!FB_Table[Index].IsInit)
	{
		s32 Counter = FB_Table[Index].DXSize * FB_Table[Index].DYSize;
		while (Counter--) FB_Table[Index].TrackerID[Counter] = -1;
	}
	FeatureBoxUpdateOne(Index);
	FB_Table[Index].IsInit = 1;
	FB_Table[Index].ComputeScale = 1;
	FB_Table[Index].ComputeRotation = 1;
	FB_Table[Index].LearningMode = 0;
	FB_Table[Index].LastTransforMatrix.Identity();
	FB_Table[Index].PertinenceLimiteDown = PertinenceLimiteDownDefault;
	FB_Table[Index].DontMoveMode = 0;
	FB_Table[Index].PausedMode = 0;
	FB_Table[Index].MINIMALERROR = MINIMALERRORDEFAULT;
	FB_Table[Index].TOLERANCE = 0;
	return 1;
}

void CFast2D::FeatureBoxSetParam(u32 Index,char *String , float Value)
{
	u32 P = ((u32)String[0]<<0) | ((u32)String[1]<<8) | ((u32)String[2]<<16) | ((u32)String[3]<<24);
	if (Index > MAX_FEATBOX) return;
	if (!FB_Table[Index].IsInit) return;
	switch(P)
	{
	case 'TURN':case 'NRUT':
		FB_Table[Index].ComputeRotation = 0;
		if (Value != 0.0f)
			FB_Table[Index].ComputeRotation = 1;
		break;
	case 'SCAL':case 'LACS':
		FB_Table[Index].ComputeScale = 0;
		if (Value != 0.0f)
			FB_Table[Index].ComputeScale = 1;
		break;
	case 'INIT':case 'TINI':
		FB_Table[Index].FixeMode = 0;
		if (Value != 0.0f)
		{
			FB_Table[Index].FixeMode = 1;
		}
		break;
	case 'JUMP':case 'PMUJ':
		FB_Table[Index].BigSearchBase
			= 1;
		if (Value != 0.0f)
		{
			FB_Table[Index].BigSearchBase = (int)Value;
		}
		break;
	case 'BLND':case 'DNLB':
		FB_Table[Index].BigSearchBase = 1;
		if (Value != 0.0f)
		{
			FB_Table[Index].BigSearchBase = (int)Value;
		}
		break;
	case 'REST':case 'TSER':
		FB_Table[Index].DontMoveMode = 0;
		if (Value != 0.0f)
		{
			FB_Table[Index].DontMoveMode = 1;
		}
		break;
	case 'PELD':case 'DLEP':
		FB_Table[Index].PertinenceLimiteDown = Value;
		break;
	case 'EROR':case 'RORE':
		FB_Table[Index].MINIMALERROR = Value;
		break;
	case 'LEAR':case 'RAEL':
		FB_Table[Index].LearningMode = Value;
		break;
	case 'TOLE':case 'ELOT':
		FB_Table[Index].TOLERANCE = Value;
		break;
	case 'STOP':case 'POTS':
		FB_Table[Index].PausedMode = 0;
		if (Value != 0.0f) FB_Table[Index].PausedMode = 1;
		break;
		
	}
}

u32 CFast2D::FeatureBoxGetXYS(u32 Index,Vector *Result)
{
	if (Index > MAX_FEATBOX) return 0;
	*Result = FB_Table[Index].LastMovesXYS;
	return 1;
}
u32 CFast2D::FeatureBoxGetSXSY(u32 Index,Vector *Result)
{
	if (Index > MAX_FEATBOX) return 0;
	Result->x = FB_Table[Index].XScale;
	Result->y = FB_Table[Index].YScale;
	Result->z = FB_Table[Index].LastMovesXYS.z;
	return 1;
}
u32 CFast2D::FeatureBoxGetRot(u32 Index,float *Result)
{
	if (Index > MAX_FEATBOX) return 0;
	*Result = FB_Table[Index].Rotation;
	return 1;
}
float CFast2D::FeatureBoxGetPertinence(u32 Index)
{
	if (Index > MAX_FEATBOX) return 0.0f;
	return FB_Table[Index].Pertinence;
}
MATRIX *CFast2D::FeatureBoxGetMatrix(u32 Index)
{
	if (Index > MAX_FEATBOX) return NULL;
	return &FB_Table[Index].LastTransforMatrix;
}

u32 CFast2D::FeatureBoxFrame(MAP *DbugMap,u32 Only4Clear)
{
	Vector C,NewPos[MAX_QFB],SpeedScale;
	float Rotation;
	u8 Valid[MAX_QFB];
	if (FB_init) return 0;
	for (u32 FeatBoxIndex = 0; FeatBoxIndex < MAX_FEATBOX; FeatBoxIndex++)
	{
		u32 DrawIt = 0;
		MATRIX NewM ;
		if (!FB_Table[FeatBoxIndex].IsInit) continue;
		if (FB_Table[FeatBoxIndex].PausedMode) 
		{
			FB_Table[FeatBoxIndex].Pertinence = 0;
			FB_Table[FeatBoxIndex].LastMovesXYS = Vector(0,0,1);
			FB_Table[FeatBoxIndex].Rotation = 0.0f;
			FB_Table[FeatBoxIndex].XScale = FB_Table[FeatBoxIndex].YScale = 1.0f;
			FB_Table[FeatBoxIndex].LastTransforMatrix.Identity();
			continue;
		}
		if (!Only4Clear)
		{
			u32 UlNumber = FB_Table[FeatBoxIndex].ulNumberOfParticules;
			u32 CounterDest = 0;
			if (FB_Table[FeatBoxIndex].LearningMode) FB_Table[FeatBoxIndex].Pertinence = 0;
			float FinalFactor = 0;
			FB_Table[FeatBoxIndex].LastMovesXYS = Vector(0,0,1);
			FB_Table[FeatBoxIndex].Rotation = 0.0f;
			FB_Table[FeatBoxIndex].XScale = FB_Table[FeatBoxIndex].YScale = 1.0f;
			u32 BigSearchMode = 0;
			if (FB_Table[FeatBoxIndex].Pertinence < 10.0) BigSearchMode = 2;
			float OldPertinence = FB_Table[FeatBoxIndex].Pertinence;
			FB_Table[FeatBoxIndex].Pertinence = 0.0f; 

			memset(Valid , 0 , sizeof(Valid ));
			if ((FeatBoxIndex & 1) && (FB_Table[FeatBoxIndex-1].Pertinence > FB_Table[FeatBoxIndex-1].PertinenceLimiteDown * 0.6666f))
				goto ByPass;

			for (u32 Counter = 0 ; Counter < UlNumber;Counter++)
			{
				//if ((OldPertinence == 0.0) && ((Counter & 1) == 0)) continue;
				u32 TrackedID = FB_Table[FeatBoxIndex].TrackerID[Counter];
				if (Get(TrackedID, &C))
				{
					s32 BigSearch = 0;
					C.z = 0;
					do
					{
						Set(TrackedID, &C);
						ComputeTrack(TrackedID,BigSearch++);
					} while ((GetError(TrackedID) < FB_Table[FeatBoxIndex].MINIMALERROR) && (BigSearch < FB_Table[FeatBoxIndex].BigSearchBase + BigSearchMode));
					if (GetError(TrackedID) < FB_Table[FeatBoxIndex].MINIMALERROR)
					{
						Set(TrackedID, &C);
					}


				}
			}


			for (u32 Counter = 0 ; Counter < UlNumber;Counter++)
			{
				NewPos[Counter].z = 0;
				if (Get(FB_Table[FeatBoxIndex].TrackerID[Counter], &C) &&	(C.z > FB_Table[FeatBoxIndex].MINIMALERROR) )
				{
					NewPos[Counter] = C;
					Valid[Counter] = 1;	
				} 
			}
			// LAUNCH SIMILI RANSAC ******************************************************************
			s32 MustRedo = 1;
			s32 MustRedoCounter = 3;
			do 
			{
				float Tolerance = FB_Table[FeatBoxIndex].FixeMode ? TOLERANSAC_FIXE: TOLERANSAC_NOTFIXE;
				if (FB_Table[FeatBoxIndex].TOLERANCE != 0) Tolerance = FB_Table[FeatBoxIndex].TOLERANCE;
				FeatureBoxRansac(Valid,UlNumber, FB_Table[FeatBoxIndex].FirstPos, NewPos , &SpeedScale , &Rotation , &FB_Table[FeatBoxIndex], Tolerance) ;
				MustRedo = 0;
				for (u32 Counter = 0 ; Counter < UlNumber;Counter++)
				{
					if (Valid[Counter] == 2) 
					{
						Vector Result , FirstPos = FB_Table[FeatBoxIndex].FirstPos[Counter];
						Result = FB_Table[FeatBoxIndex].LastTransforMatrix * FirstPos;
						u32 TrackedID = FB_Table[FeatBoxIndex].TrackerID[Counter];
						Set(TrackedID, &Result);
						ComputeTrack(TrackedID,0);
						if (GetError(TrackedID) > FB_Table[FeatBoxIndex].MINIMALERROR )
						{
							Get(FB_Table[FeatBoxIndex].TrackerID[Counter], &Result);
							NewPos[Counter] = Result;
							Valid[Counter] = 1;
							MustRedo = 1;
						}
					}
				}
				MustRedoCounter--;
			} while (MustRedo && MustRedoCounter);
			FB_Table[FeatBoxIndex].Pertinence = floor(100.0f * EvaluateNumber(Valid , UlNumber ) / (float)UlNumber );


			for (u32 Counter = 0 ; Counter < UlNumber;Counter++)
			{ 
				if (Valid[Counter] == 1) 
				{
					float Factor = NewPos[Counter].z * 1.25f;//FB_Table[FeatBoxIndex].ParticlesPonderation[Counter];
					if (Factor > 1.0f) Factor = 1.0f;
					
					Factor = FB_Table[FeatBoxIndex].ParticlesPonderation[Counter] * NewPos[Counter].z * 1.25f;

					FinalFactor += Factor;
					CounterDest++;
				} 
			}
			if (CounterDest>3)
			{
				FB_Table[FeatBoxIndex].LastMovesXYS = SpeedScale;
				FB_Table[FeatBoxIndex].Rotation = Rotation;
			} 


			if (CounterDest > 4)
			{
				//FinalFactor /= (float)CounterDest;
				FB_Table[FeatBoxIndex].Pertinence *= FinalFactor;
				FB_Table[FeatBoxIndex].Pertinence = (float)(int)FB_Table[FeatBoxIndex].Pertinence;
				/*if (!FB_Table[FeatBoxIndex].LearningMode) 
					FB_Table[FeatBoxIndex].Pertinence = OldPertinence * PertinenceBlend + FB_Table[FeatBoxIndex].Pertinence * (1.0f - PertinenceBlend);*/
//				FB_Table[FeatBoxIndex].Pertinence = FB_Table[FeatBoxIndex].Pertinence ;
			}
			else
				FB_Table[FeatBoxIndex].Pertinence = 0;



			if (FB_Table[FeatBoxIndex].Pertinence < FB_Table[FeatBoxIndex].PertinenceLimiteDown / 2) 
			{
				FB_Table[FeatBoxIndex].LastMovesXYS.x = FB_Table[FeatBoxIndex].LastMovesXYS.y = 0.0f;
				FB_Table[FeatBoxIndex].LastMovesXYS.z = 1.0f;
				FB_Table[FeatBoxIndex].Rotation = 0.0f;
				FB_Table[FeatBoxIndex].XScale = FB_Table[FeatBoxIndex].YScale = 1.0f;
				FB_Table[FeatBoxIndex].LastTransforMatrix.Identity();
			}
ByPass:
			NewM = FB_Table[FeatBoxIndex].LastTransforMatrix;
			if (FB_Table[FeatBoxIndex].LearningMode || FB_Table[FeatBoxIndex].DontMoveMode) NewM.Identity();
			if ((FeatBoxIndex & 1) && (FB_Table[FeatBoxIndex -1].Pertinence > FB_Table[FeatBoxIndex -1].PertinenceLimiteDown * 0.6666f))
			{
				// Help From Static
				FB_Table[FeatBoxIndex].LastTransforMatrix = FB_Table[FeatBoxIndex-1].LastTransforMatrix;
				NewM = FB_Table[FeatBoxIndex].LastTransforMatrix;
				FB_Table[FeatBoxIndex].LastMovesXYS = FB_Table[FeatBoxIndex-1].LastMovesXYS;
				FB_Table[FeatBoxIndex].Rotation = FB_Table[FeatBoxIndex-1].Rotation;
				FB_Table[FeatBoxIndex].XScale = FB_Table[FeatBoxIndex-1].YScale;
				FB_Table[FeatBoxIndex].YScale = FB_Table[FeatBoxIndex-1].YScale;
				FB_Table[FeatBoxIndex].Pertinence = 100;
				DrawIt = 1;
			}

			u32 Done = 0;
			for (u32 Counter = 0 ; Counter < UlNumber;Counter++)
			{
				Vector Result , FirstPos ;

				FirstPos = FB_Table[FeatBoxIndex].FirstPos[Counter];
				Result = NewM * FirstPos;
				Result.z = NewPos[Counter].z;
				if (Valid[Counter] != 1) Result.z = 0;

				Set(FB_Table[FeatBoxIndex].TrackerID[Counter], &Result);
				//if (Valid[Counter] != 1) UpdateData(FB_Table[FeatBoxIndex].TrackerID[Counter]);
				if (!FB_Table[FeatBoxIndex].FixeMode)
				{
					float L = (FirstPos -NewPos[Counter]).SqrNormXY();
					if  (L > 1)
						UpdateData(FB_Table[FeatBoxIndex].TrackerID[Counter]);
				}
				if (DbugMap)
				{
					if (FB_Table[FeatBoxIndex].ParticlesPonderation[Counter] > 0.01)
					CopyDataRGBA(DbugMap, 
							TRACKSTACK[FB_Table[FeatBoxIndex].TrackerID[Counter]].InitMap , 
							TRACKSTACK[FB_Table[FeatBoxIndex].TrackerID[Counter]].X , 
							TRACKSTACK[FB_Table[FeatBoxIndex].TrackerID[Counter]].Y , F2DT8Size ,
							TRACKSTACK[FB_Table[FeatBoxIndex].TrackerID[Counter]].Norm[0]);

				}

			}

		}
		if (DbugMap)
		{
			RECT r;
			r.right = FB_Table[FeatBoxIndex].LastMovesXYS.x + FB_Table[FeatBoxIndex].X - FB_Table[FeatBoxIndex].XScale *  FB_Table[FeatBoxIndex].W  / 2;
			r.top = FB_Table[FeatBoxIndex].LastMovesXYS.y + FB_Table[FeatBoxIndex].Y - FB_Table[FeatBoxIndex].YScale *  FB_Table[FeatBoxIndex].H  / 2;
			r.left = r.right + FB_Table[FeatBoxIndex].XScale * FB_Table[FeatBoxIndex].W ;
			r.bottom = r.top + FB_Table[FeatBoxIndex].YScale * FB_Table[FeatBoxIndex].H ;
			if (FB_Table[FeatBoxIndex].Pertinence>FB_Table[FeatBoxIndex].PertinenceLimiteDown)
			{
				Vector Center ;
				NewM = FB_Table[FeatBoxIndex].LastTransforMatrix;
				Center.x = FB_Table[FeatBoxIndex].X ;
				Center.y = FB_Table[FeatBoxIndex].Y ;
				Center = NewM * Center;
				Vector XAXIs = Vector(FB_Table[FeatBoxIndex].W*0.5,0,0);
				Vector YAXIs = Vector(0,FB_Table[FeatBoxIndex].H*0.5,0);
				NewM.T = Vector(0,0,0);
				XAXIs = NewM * XAXIs;
				YAXIs = NewM * YAXIs;
				void DrawLine_Clip(MAP *MapDST,float X1,float Y1,float X2,float Y2,u32 Color);
				for (int epx = -1 ;epx < 2 ; epx  ++)
				for (int epy = -1 ;epy < 2 ; epy  ++)
				{
				DrawLine_Clip(DbugMap,
						epx+(Center + XAXIs + YAXIs).x,epy+(Center + XAXIs + YAXIs).y,
						epx+(Center - XAXIs + YAXIs).x,epy+(Center - XAXIs + YAXIs).y,
						FB_Table[FeatBoxIndex].FixeMode?0xff0000:(Copied ? 0xffff00:0xff00ff));
				DrawLine(DbugMap,
						epx+(Center + XAXIs - YAXIs).x,epy+(Center + XAXIs - YAXIs).y,
						epx+(Center - XAXIs - YAXIs).x,epy+(Center - XAXIs - YAXIs).y,
						FB_Table[FeatBoxIndex].FixeMode?0xff0000:(Copied ? 0xffff00:0xff00ff));
				DrawLine(DbugMap,
						epx+(Center + XAXIs - YAXIs).x,epy+(Center + XAXIs - YAXIs).y,
						epx+(Center + XAXIs + YAXIs).x,epy+(Center + XAXIs + YAXIs).y,
						FB_Table[FeatBoxIndex].FixeMode?0xff0000:(Copied ? 0xffff00:0xff00ff));
				DrawLine(DbugMap,
						epx+(Center - XAXIs + YAXIs).x,epy+(Center - XAXIs + YAXIs).y,
						epx+(Center - XAXIs - YAXIs).x,epy+(Center - XAXIs - YAXIs).y,
						FB_Table[FeatBoxIndex].FixeMode?0xff0000:(Copied ? 0xffff00:0xff00ff));
				}
				//DrawQuad_R(DbugMap,r.right,r.top,r.left,r.bottom,FB_Table[FeatBoxIndex].FixeMode?0xff0000:(Copied ? 0xffff00:0xff00ff),FB_Table[FeatBoxIndex].FixeMode?4:3,FB_Table[FeatBoxIndex].Rotation);
			}
			

			char DEBUGTEXT[2048];
			void DRawTEXT_Soft_B(u8 *pTEXT,u32 *pBase,u32 X, u32 Y, s32 Pitch , u32 Height , u32 Color);
			sprintf(DEBUGTEXT,"%d",(u32)(FB_Table[FeatBoxIndex].YScale * 100));//,SPm.SX * SPm.SY ,CHKDBG*100 / (SPm.SX * SPm.SY));
//			DRawTEXT_Soft_B((u8*)DEBUGTEXT , (u32*)DbugMap->GetBase() , 16 , 32 + 16 * FeatBoxIndex, DbugMap->PITCH , DbugMap->SY , 0xff00ff );
			r.right = FB_Table[FeatBoxIndex].X - 5;
			r.top = FB_Table[FeatBoxIndex].Y - 5;
			r.left = FB_Table[FeatBoxIndex].X + 5;
			r.bottom = FB_Table[FeatBoxIndex].Y + 5;
			DrawQuad_R(DbugMap,r.right,r.top,r.left,r.bottom,0xffff00,3,0);
		}

	}
	return 0;
}