#include "SCENE.h"

#define MYDEBUG

#define BORDER_LIMIT 6
#define RAIDUS 6
#define RAIDUSD 4

typedef struct GC_C_
{
	float X,Y;
	float XC,YC,Div;
	u32 PITCH;
	u32 Counter;
	u32 Or;
} GC_C;

#define NB_LEVELS 3

/* Valeur OK a remettre si jamais ça cafouille *******
#define FERN_POINTS				(16)

#define FERN_PATCH_SX			8
#define FERN_PATCH_SY			8
#define FERN_MAX_CORRELATION	(16)
#define FERN_MAX_PT_DISTANCE	(5*5) 
#define INTER_LEVEL_SEARCH		2
*****************************************************/

#define FERN_POINTS				(16)

#define FERN_PATCH_SX			10
#define FERN_PATCH_SY			10
#define FERN_MAX_CORRELATION	(16)
#define FERN_MAX_PT_DISTANCE	(5*5) 
#define INTER_LEVEL_SEARCH		2
#define FERN_MAX_LIMIT			48


static BOOL		FERN_INIT = true;
static POINT	FERN_CATCH[FERN_POINTS];

typedef struct
{
	POINT	coords;
	POINT	coordsNext;
	u32		weight;
	u32		tag;					// 1 -> featPoint,  2-> usedPoint
	u8		FernMask[FERN_POINTS];
} ZoneInfo;

typedef struct 
{
	int			size;
	u32			count;
	ZoneInfo	*src;
	POINT		P;
	
	int			nbTrackPoints;
	POINT		*trackPoints;
	int			trackSearch;
} GGPoint;

#define MIN_POINTS_RATIO 4
#define MAX_TRACK_POINTS	256

static BOOL		GGPInit = true;
static GGPoint	GGP[MAX_TRACK_POINTS][NB_LEVELS];

#define MAX_IMG_SIZE	512

static s32 M6InSave[2 * MAX_IMG_SIZE * MAX_IMG_SIZE];
static s32 M6BaseSave[2 * MAX_IMG_SIZE * MAX_IMG_SIZE];
static s32 ImgBaseSave[2 * MAX_IMG_SIZE * MAX_IMG_SIZE];

void GET_GC(GC_C *GG,s32 *Pixel,s32 And,s32 AddX,s32 AddY);

inline u32 FernListEq(u8 *FernList1, u8 *FernList2)
{
	int Counter = FERN_POINTS;
	u32	result	= 0;
	while(Counter--)
	result += GetBitNumber(FernList1[Counter] ^ FernList2[Counter]);
	return result;
}

inline bool PointsInitRandom(BOOL *isInit, POINT *list, int nbPoints, int SX, int SY)
{
	if (!*isInit) return false;
	
	int qtInNb = nbPoints / 4;
	int isAllreadyPresent;

	SX /= 2;
	SY /= 2;
	for (int qt=0; qt<4; qt++)
	{
		for (int qtIn=0; qtIn<qtInNb; qtIn++)
		{
			do 
			{
				list[qtIn + qt*qtInNb].x = (rand()* SX) / RAND_MAX;
				list[qtIn + qt*qtInNb].y = (rand()* SY) / RAND_MAX;
				
				isAllreadyPresent = false;
				for (int qtInIn=0; !isAllreadyPresent && qtInIn<qtIn; qtInIn++)
				{
					if ((list[qtIn + qt*qtInNb].x == list[qtInIn + qt*qtInNb].x) &&
						(list[qtIn + qt*qtInNb].y == list[qtInIn + qt*qtInNb].y))
						isAllreadyPresent = true;
				}
			} while (isAllreadyPresent);
			if (qt / 2) list[qtIn + qt*qtInNb].x = -list[qtIn + qt*qtInNb].x;
			if (qt % 2) list[qtIn + qt*qtInNb].y = -list[qtIn + qt*qtInNb].y;
		}
	}
	*isInit = false;
	return true;
}

inline void PointsInitRec(POINT *list, int nbPoints, RECT bounds, int dir)
{
	int		nbPoints1, nbPoints2;
	RECT	bounds1, bounds2;
	BOOL	way;

	if (nbPoints <= 0) return ;
	if (nbPoints == 1)
	{
		list->x = (bounds.left + bounds.right)/2;
		list->y = (bounds.top + bounds.bottom)/2;
		if (dir)
		{
			list->x += (bounds.left + bounds.right) & 0x1;
			list->y += (bounds.top + bounds.bottom) & 0x1;
		}
		return;
	}
	
	nbPoints1 = 
	nbPoints2 = nbPoints/2;

	if (dir)	nbPoints2 += nbPoints&0x1;
	else		nbPoints1 += nbPoints&0x1;
	
	bounds1 =
	bounds2 = bounds;

	if ((bounds.right - bounds.left) == (bounds.bottom - bounds.top))
		way = dir;
	else if ((bounds.right - bounds.left) > (bounds.bottom - bounds.top))
		way = 1;
	else
		way = 0;

	if (way)
	{
		bounds1.right = (bounds.right + bounds.left)/2;
		bounds2.left  = bounds1.right + 1;
	} else
	{
		bounds1.bottom	= (bounds.top + bounds.bottom)/2;
		bounds2.top		= bounds1.bottom + 1;
	}

	PointsInitRec(list,				nbPoints1, bounds1, !dir);
	PointsInitRec(list+nbPoints1,	nbPoints2, bounds2, !dir);
}

inline bool PointsInit(BOOL *isInit, POINT *list, int nbPoints, int SX, int SY)
{
	if (!*isInit) return false;

	RECT bounds;
	list[0].x = list[0].y = 0;
	
	bounds.right	= SX/2 + (SX&0x1);
	bounds.left		= -SX/2;
	bounds.bottom	= SY/2;
	bounds.top		= -SY/2 - (SX&0x1);
	PointsInitRec(list+1, nbPoints-1, bounds, 0);

	*isInit = false;
	return true;
}


void GetFern(MAP *map, int x, int y, u8 *FernList)
{
	int xIn, yIn;
	PointsInit(&FERN_INIT, FERN_CATCH, FERN_POINTS, FERN_PATCH_SX, FERN_PATCH_SY);

	int Counter = FERN_POINTS;
	memset(FernList, 0, FERN_POINTS*sizeof(u8));
	while (Counter--)
	{
		xIn = FERN_CATCH[Counter].x + x;
		yIn = FERN_CATCH[Counter].y + y;
		if ((xIn			> BORDER_LIMIT) &&
			(yIn			> BORDER_LIMIT) &&
			(map->SX - xIn	> BORDER_LIMIT) &&
			(map->SY - yIn	> BORDER_LIMIT))
			FernList[Counter] = *(map->GetBase() + xIn +  yIn*map->PITCH) & 0xFF;
	}
}


void MGet(MAP *CurrentMap, MAP *M6Ptr)
{
	static MAP		WorkMap;
	static u32		WorkBufSize = 0;
	MAP	M1,M2,M3,M4,M5,M6;

	if (WorkBufSize < 512 + (CurrentMap->SX + 16)* CurrentMap->SY)
	{
		WorkMap.SetSize(CurrentMap->SX*3, CurrentMap->SY*2, 1);
	}
	
	M1 = M2	= M3 = M4 = M5 = M6 = *CurrentMap;
	M6.PITCH = M5.PITCH = M4.PITCH	= M3.PITCH = M2.PITCH = M1.PITCH = CurrentMap->SX;
	M6.SetBase(WorkMap.GetBase());
	M5.SetBase(WorkMap.GetBase() + CurrentMap->SX *	CurrentMap->SY);
	M4.SetBase(WorkMap.GetBase() + CurrentMap->SX *	CurrentMap->SY * 2);
	M3.SetBase(WorkMap.GetBase() + CurrentMap->SX *	CurrentMap->SY * 3);
	M2.SetBase(WorkMap.GetBase() + CurrentMap->SX *	CurrentMap->SY * 4);
	M1.SetBase(WorkMap.GetBase() + CurrentMap->SX *	CurrentMap->SY * 5);


	// maxi compute
	COPY_BW(CurrentMap,&M1);
	SHIFT(&M1,2);
	
	//COPY_RGBA(&M1,CurrentMap);
	//SET(CurrentMap, 0);

	LF_GaussianFilter(&M1,&M1,&M3,0,3);
	LF_GaussianFilter(&M1,&M2,&M3,0,3);

	MAX_CROSS(&M1,&M5);
	MAX(&M5,&M3);

	MIN_CROSS(&M1,&M5);
	MIN(&M5,&M4);

	//DARKER(CurrentMap);
	
	ADD_VALUE(&M3,3);
	ADD_VALUE(&M4,-3);
	SET(&M5,0);



	//////////////////////////////////////////////////
	// First -> Compute neighbourought flags

	{
		s32 *m1,*m2,*m3,*m4,*m5,*m6;
		for (int y = BORDER_LIMIT; y < CurrentMap->SY - BORDER_LIMIT; y++)
		{
			for (int x = BORDER_LIMIT; x < CurrentMap->SX - BORDER_LIMIT; x++)
			{
				s32 *VMa,*VMi,VA;
				u32 RES,RESM,RESE;
				RES =RESM= 0;
				VMa = M4.GetBase() + x + (y) * M3.PITCH;
				VMi = M3.GetBase() + x + (y) * M3.PITCH;
				VA = *(M2.GetBase() + x + (y) * M2.PITCH) ;
				if (*VMi - *VMa > 10)
				{
					if ((*(VMa - RAIDUSD - RAIDUSD * M2.PITCH)) > VA) RES |= 1;
					if ((*(VMa - RAIDUS * M2.PITCH))			> VA) RES |= 2;
					if ((*(VMa + RAIDUSD - RAIDUSD * M2.PITCH)) > VA) RES |= 4;
					if ((*(VMa + RAIDUS ))						> VA) RES |= 8;
					if ((*(VMa + RAIDUSD + RAIDUSD * M2.PITCH)) > VA) RES |= 16;
					if ((*(VMa + RAIDUS * M2.PITCH))			> VA) RES |= 32;
					if ((*(VMa - RAIDUSD + RAIDUSD * M2.PITCH)) > VA) RES |= 64;
					if ((*(VMa - RAIDUS ))						> VA) RES |= 128;

					*(M5.GetBase() + x + y * M5.PITCH) = RES | ((*VMi - *VMa) << 8);
				}
				*(M2.GetBase() + x + y * M5.PITCH) = RES | ((*VMi - *VMa) << 8);

			}
		}
	}
	ORb(&M5,&M1);
	ORb(&M1,&M6);

	SET(&M1,0);
	COPY(&M5,&M1);

	//////////////////////////////////////////////////
	// Second -> Compute non - symetry in flags
	{
		for (int y = BORDER_LIMIT; y < CurrentMap->SY - BORDER_LIMIT; y++)
		{
			for (int x = BORDER_LIMIT; x < CurrentMap->SX - BORDER_LIMIT; x++)
			{
				s32 *VMa,*VMi,VA;
				u32 RES,RESM,RESE;
				RES =RESM= 0;
				RES = *(M5.GetBase() + x + (y) * M5.PITCH) & 0xff;
				*(M5.GetBase() + x + y * M5.PITCH) = *(M2.GetBase() + x + y * M2.PITCH)  & 0xff00;
				*(M2.GetBase() + x + y * M2.PITCH) &= 0xff;
				u32 Color;

				if (GetBitNumber(RES) > 7)
				{
					*(M5.GetBase() + x + y * M5.PITCH) |= 0x4;
				}
				else
				{
					u32 RESO;
					RESO = RES ;
					RESO <<= 4;
					RESO |= RES>>4;
					RESO ^= RES;
					RESO &= 0xff;

					RESE = RES >> 1;
					RESE |= RES << 7;
					RES &= RESE;
					RESE = RES >> 1;
					RESE |= RES << 7;
					RES &= RESE;

					if (GetBitNumber(RES) > 3)
					{
						*(M5.GetBase() + x + y * M5.PITCH) |= 0x2;
					} else
					if (GetBitNumber(RES) > 2)
					{
						*(M5.GetBase() + x + y * M5.PITCH) |= 0x1;
					}
				}
			}
		}
	}
	
	if (M6Ptr)
		*M6Ptr = M6;
}

void MGet(MAP *CurrentMap, MAP *M6Ptr, RECT *bounds)
{
	MAP map = *CurrentMap;
	map.SX	= bounds->right - bounds->left;
	map.SY	= bounds->bottom - bounds->top;
	map.SetBase(CurrentMap->GetBase() + bounds->left + bounds->top*map.PITCH);

	MGet(&map, M6Ptr);

	s32 *newPtrIn	= M6InSave + bounds->left + bounds->top*CurrentMap->PITCH;
	memset(M6InSave, 0, CurrentMap->PITCH*CurrentMap->SY*4);
	for (int i=0; i<M6Ptr->SY; i++, newPtrIn+=CurrentMap->PITCH)
	{
		memcpy(newPtrIn, M6Ptr->GetBase()+i*M6Ptr->PITCH, M6Ptr->PITCH*4);
	}

	M6Ptr->SX		= CurrentMap->SX;
	M6Ptr->SY		= CurrentMap->SY;
	M6Ptr->PITCH	= CurrentMap->PITCH;
	M6Ptr->SetBase(M6InSave);
}


int SearchNewBary(GGPoint *GGPPtr, POINT *vMove)
{
	POINT nextP;
	POINT midBase, midDest, midError;

	int		error;
	int		usedPoints;
	BOOL	isFirst = true;
	BOOL	isLast = false;

	do
	{
		midBase.x = midBase.y = 0;
		midDest.x = midDest.y = 0;
		usedPoints = 0;
		
		for (int ct=0; ct<GGPPtr->count; ct++)
		{
			GGPPtr->src[ct].tag &= 0x1;					
			if (GGPPtr->src[ct].coordsNext.x != 0 ||
				GGPPtr->src[ct].coordsNext.y != 0)
			{
				if (!isFirst)
				{
					nextP.x = GGPPtr->src[ct].coords.x + vMove->x;
					nextP.y = GGPPtr->src[ct].coords.y + vMove->y;

					if (((GGPPtr->src[ct].coordsNext.x-nextP.x)*(GGPPtr->src[ct].coordsNext.x-nextP.x) + 
						 (GGPPtr->src[ct].coordsNext.y-nextP.y)*(GGPPtr->src[ct].coordsNext.y-nextP.y)) > error)
						continue;
				}
				
				midBase.x += GGPPtr->src[ct].coords.x;
				midBase.y += GGPPtr->src[ct].coords.y;
				midDest.x += GGPPtr->src[ct].coordsNext.x;
				midDest.y += GGPPtr->src[ct].coordsNext.y;
				GGPPtr->src[ct].tag |= 0x2;
				usedPoints++;
			}
		}
		if (usedPoints == 0 || usedPoints < GGPPtr->nbTrackPoints/MIN_POINTS_RATIO/*MIN_POINTS_FOUNDS*/)
			return 0;
		
		midBase.x /= usedPoints;
		midBase.y /= usedPoints;
		midDest.x /= usedPoints;
		midDest.y /= usedPoints;
		
		vMove->x = midDest.x - midBase.x;
		vMove->y = midDest.y - midBase.y;

		// calc mid error
		midError.x = midError.y = 0;
		usedPoints = 0;
		for (int ct=0; ct<GGPPtr->count; ct++)
		{
			if (GGPPtr->src[ct].tag & 0x2)
			{
				nextP.x = GGPPtr->src[ct].coords.x + vMove->x;
				nextP.y = GGPPtr->src[ct].coords.y + vMove->y;

				midError.x += (GGPPtr->src[ct].coordsNext.x - nextP.x)*(GGPPtr->src[ct].coordsNext.x - nextP.x);
				midError.y += (GGPPtr->src[ct].coordsNext.y - nextP.y)*(GGPPtr->src[ct].coordsNext.y - nextP.y);
				usedPoints++;
			}
		}
		midError.x /= usedPoints;
		midError.y /= usedPoints;
		error	= midError.x + midError.y;
		isFirst	= false;
} while (error > FERN_MAX_PT_DISTANCE);
	//	if (isLast) error = 0;
	//	if (error <= FERN_MAX_PT_DISTANCE)
	//		isLast = true;
	//} while (error > 0);
//	} while (error > FERN_MAX_PT_DISTANCE);
	return usedPoints;
}

int SearchNewCenter(POINT *vMove, GGPoint *GGPPtr)
{
	 return SearchNewBary(GGPPtr, vMove);
	// maybe add new traitment ...
}

void SearchBestPoint(GGPoint *GGPPtr, MAP *M6, int lvlOO)
{
	u32	MaskEqu;
	u8 FERN_IN[FERN_POINTS];
	RECT searchRect;
	int	lvl, lvlOut;
	int searchZone;


	//SEARCH_PATCH_SX_BASE
	for (int ggpIdx=0; ggpIdx<GGPPtr[0].count; ggpIdx++)
	{
		if (lvlOO < 0)
			lvlOut			= NB_LEVELS-1;
		else
			lvlOut			= lvlOO;
		while (lvlOut >= 0 && !(GGPPtr[lvlOut].src[ggpIdx].tag & 0x1)) lvlOut--;
		if (lvlOut < 0) continue;

		GGPPtr[0].src[ggpIdx].weight = FERN_MAX_CORRELATION;
		lvl = lvlOut;
		searchZone			= (GGPPtr->trackSearch / (1<<lvl))/2;
		if (searchZone < 1) searchZone = 1;
		searchRect.left		= GGPPtr[lvl].src[ggpIdx].coords.x - searchZone;
		searchRect.right	= GGPPtr[lvl].src[ggpIdx].coords.x + searchZone;
		searchRect.top		= GGPPtr[lvl].src[ggpIdx].coords.y - searchZone;
		searchRect.bottom	= GGPPtr[lvl].src[ggpIdx].coords.y + searchZone;

		if (searchRect.left		< BORDER_LIMIT)					searchRect.left		= BORDER_LIMIT;
		if (searchRect.top		< BORDER_LIMIT)					searchRect.top		= BORDER_LIMIT;
		if (searchRect.right	> (M6[lvl].SX-BORDER_LIMIT))	searchRect.right	= M6[lvl].SX-BORDER_LIMIT;
		if (searchRect.bottom	> (M6[lvl].SY-BORDER_LIMIT))	searchRect.bottom	= M6[lvl].SY-BORDER_LIMIT;

		GGPPtr[lvl].src[ggpIdx].weight = FERN_MAX_LIMIT;
		for (int y=searchRect.top; y<=searchRect.bottom; y++)
		{		
			for (int x=searchRect.left; x<=searchRect.right; x++)
			{
				GetFern(&M6[lvl], x, y, FERN_IN);			
				MaskEqu = FernListEq(FERN_IN, GGPPtr[lvl].src[ggpIdx].FernMask);
				if (MaskEqu <  GGPPtr[lvl].src[ggpIdx].weight)
				{
					GGPPtr[lvl].src[ggpIdx].weight		 = MaskEqu;
					GGPPtr[lvl].src[ggpIdx].coordsNext.x = x;
					GGPPtr[lvl].src[ggpIdx].coordsNext.y = y;
					GGPPtr[lvl].src[ggpIdx].tag			|= 0x2;
				}			
			}
		}
		while (lvl--)
		{
			if (lvl > 0)
				GGPPtr[lvl].src[ggpIdx].weight	= FERN_MAX_LIMIT;
			else
				GGPPtr[lvl].src[ggpIdx].weight	= FERN_MAX_CORRELATION;
			for (int y=GGPPtr[lvl+1].src[ggpIdx].coordsNext.y*2-INTER_LEVEL_SEARCH;
					y<=GGPPtr[lvl+1].src[ggpIdx].coordsNext.y*2+INTER_LEVEL_SEARCH; y++)
			{
				if (y<BORDER_LIMIT || y>(M6[lvl].SY-BORDER_LIMIT)) continue;
				for (int x=GGPPtr[lvl+1].src[ggpIdx].coordsNext.x*2-INTER_LEVEL_SEARCH;
						x<=GGPPtr[lvl+1].src[ggpIdx].coordsNext.x*2+INTER_LEVEL_SEARCH; x++)
				{
					if (x<BORDER_LIMIT || x>(M6[lvl].SX-BORDER_LIMIT)) continue;
					GetFern(&M6[lvl], x, y, FERN_IN);			
					MaskEqu = FernListEq(FERN_IN, GGPPtr[lvl].src[ggpIdx].FernMask);
					if (MaskEqu <  GGPPtr[lvl].src[ggpIdx].weight)
					{
						GGPPtr[lvl].src[ggpIdx].weight		 = MaskEqu;
						GGPPtr[lvl].src[ggpIdx].coordsNext.x = x;
						GGPPtr[lvl].src[ggpIdx].coordsNext.y = y;
						GGPPtr[lvl].src[ggpIdx].tag			|= 0x2;
					}			
				}
			}
		}
	}
}


// Step 2 : use map to search images
inline void SetHalfMap(MAP *src, MAP *dst)
{
	dst->PITCH	= 
	dst->SX		= src->SX>>1;
	dst->SY		= src->SY>>1;
	dst->SetBase(src->GetBase() + src->PITCH*src->SY);
}


int ZoneCompute(MAP *imageMap, int index, int *ptX, int *ptY, int forceReset, int newImage, int nbTrkPoint, int trkZone, int trkSearch)
{
	MAP img[NB_LEVELS];
	MAP M6[NB_LEVELS];
	int newX, newY;
	int ptXIn, ptYIn;

	float FACTOR		= 1;
	float FACTOR_INV	= 1;

	if (index >= MAX_TRACK_POINTS)
		return 0;

	START_RASTER(Feature_Test1)

	img[0] = *imageMap;

	ptXIn = *ptX;
	ptYIn = *ptY;
	if (imageMap->SX > MAX_IMG_SIZE)
	{
		FACTOR			= (float)MAX_IMG_SIZE / imageMap->SX;
		FACTOR_INV		= 1*FACTOR_INV;
		ptXIn			= (int)(FACTOR*ptXIn);
		ptYIn			= (int)(FACTOR*ptYIn);
		trkZone			= (int)(FACTOR*trkZone);
		trkSearch		= (int)(FACTOR*trkSearch);
	}
		
	if 	(newImage)
	{
		s32 *M6BasePtr;
		s32 *imgBasePtr;
		RECT zoneBounds[NB_LEVELS];

		if (newImage == 2)
		{
			SetRect(&zoneBounds[0],	ptXIn - (trkSearch + trkZone)/2,
									ptYIn - (trkSearch + trkZone)/2,
									ptXIn + (trkSearch + trkZone)/2,
									ptYIn + (trkSearch + trkZone)/2);
			if (zoneBounds[0].left	< 0)			zoneBounds[0].left	= 0;
			if (zoneBounds[0].top	< 0)			zoneBounds[0].top	= 0;
			if (zoneBounds[0].right	> imageMap->SX)	zoneBounds[0].right	= imageMap->SX;
			if (zoneBounds[0].bottom> imageMap->SY)	zoneBounds[0].bottom= imageMap->SY;
		}
		
		if (FACTOR != 1)
		{
			img[0].SX		= (int)(FACTOR*img[0].SX);
			img[0].PITCH	= img[0].SX;
			img[0].SY		=(int)(FACTOR*img[0].SY);
			img[0].SetBase(ImgBaseSave);
			COPY_OX_RGBA(imageMap, &img[0]);
		}
		imgBasePtr = img[0].GetBase();
		img[0].SetBase(ImgBaseSave);
		for (int i=1; i<NB_LEVELS; i++)
		{
			SetHalfMap(&img[i-1], &img[i]);
			zoneBounds[i].left	=	zoneBounds[i-1].left	/2;
			zoneBounds[i].right	=	zoneBounds[i-1].right	/2;
			zoneBounds[i].top	=	zoneBounds[i-1].top		/2;
			zoneBounds[i].bottom=	zoneBounds[i-1].bottom	/2;
		}
		img[0].SetBase(imgBasePtr);
		for (int i=1; i<NB_LEVELS; i++)
		{
			COPY_OX_RGBA(&img[i-1], &img[i]);
		}
		
		M6BasePtr = M6BaseSave;
		for (int i=0; i<NB_LEVELS; i++)
		{
			if (newImage == 2)
				MGet(&img[i], &M6[i], &zoneBounds[i]);
			else
				MGet(&img[i], &M6[i]);

			memcpy(M6BasePtr, M6[i].GetBase(), M6[i].PITCH*M6[i].SY*4);
			M6BasePtr += M6[i].PITCH*M6[i].SY;
		}
	}
	M6[0] = img[0]; M6[0].SetBase(M6BaseSave);
	for (int i=1; i<NB_LEVELS; i++)
	{
		SetHalfMap(&M6[i-1], &M6[i]);
	}

	GGPoint *GGPPtr;
	GGPoint *GGPPtr0 = GGP[index];

	if (forceReset)
	{
		if (GGPInit)
		{
			for (int idx=0; idx<MAX_TRACK_POINTS; idx++)
				memset(GGP[idx], 0, sizeof(GGP[idx]));
			GGPInit = false;
		}
		
		for (int i=0; i<NB_LEVELS; i++)
		{
			GGPPtr = GGPPtr0 + i;
		
			if (GGPPtr->size < M6[i].SX * M6[i].SY)
			{
				if (GGPPtr->src)
					CC_free(GGPPtr->src);
				
				GGPPtr->size		= M6[i].SX * M6[i].SY;
				GGPPtr->src			= (ZoneInfo *)CC_malloc(GGPPtr->size*sizeof(ZoneInfo));
			}

			GGPPtr->trackSearch = trkSearch;
			if (GGPPtr->trackPoints == NULL || GGPPtr->nbTrackPoints != nbTrkPoint)
			{
				int isInit = 1;

				if (GGPPtr->trackPoints)
					CC_free(GGPPtr->trackPoints);
				GGPPtr->nbTrackPoints	= nbTrkPoint;
				GGPPtr->trackPoints		= (POINT *)CC_malloc(GGPPtr->nbTrackPoints*sizeof(POINT));
				PointsInit(&isInit, GGPPtr->trackPoints, GGPPtr->nbTrackPoints, trkZone, trkZone);
			}

			for (GGPPtr->count = 0; GGPPtr->count < GGPPtr->nbTrackPoints; GGPPtr->count++)
			{
				GGPPtr->src[GGPPtr->count].coords.x = ptXIn + GGPPtr->trackPoints[GGPPtr->count].x/(1<<i);
				GGPPtr->src[GGPPtr->count].coords.y = ptYIn + GGPPtr->trackPoints[GGPPtr->count].y/(1<<i);

				if (GGPPtr->src[GGPPtr->count].coords.x < BORDER_LIMIT ||
					GGPPtr->src[GGPPtr->count].coords.y < BORDER_LIMIT ||
					(M6[i].SX - GGPPtr->src[GGPPtr->count].coords.x < BORDER_LIMIT)	||
					(M6[i].SY - GGPPtr->src[GGPPtr->count].coords.y < BORDER_LIMIT))
				{
					GGPPtr->src[GGPPtr->count].tag	= 0;
				} else
				{
					GGPPtr->src[GGPPtr->count].tag	= 1;
					GetFern(&M6[i], GGPPtr->src[GGPPtr->count].coords.x,
									GGPPtr->src[GGPPtr->count].coords.y,
									GGPPtr->src[GGPPtr->count].FernMask);
				}
			}
			GGPPtr->P.x = ptXIn;
			GGPPtr->P.y = ptYIn;
			
			ptXIn = ptXIn>>1;
			ptYIn = ptYIn>>1;
		}
		STOP_RASTER(Feature_Test1)
		return 0;
	}

	// search image process
	STOP_RASTER(Feature_Test1);

	POINT	FinalV = {0,0};
	int		searchRes;
	int res = false;

/* Calcul complet ----------
	START_RASTER(Feature_Test1);
	for (int i=0; i<NB_LEVELS; i++)
	{
		for (int ct=0; ct<GGPPtr0[i].count; ct++)
		{
			GGPPtr0[i].src[ct].weight		= FERN_MAX_CORRELATION;
			GGPPtr0[i].src[ct].coordsNext.x	= 
			GGPPtr0[i].src[ct].coordsNext.y	= 0;
			GGPPtr0[i].src[ct].tag			&= 0x1;
		}
	}	
	
	SearchBestPoint(GGP[index], M6, 0);

	FinalV.x = FinalV.y = 0;
	searchRes = SearchNewCenter(&FinalV, GGPPtr0, 5);
	
	if (searchRes > 0)
	{


		*ptX = (GGPPtr0->P.x + FinalV.x)*FACTOR_INV;
		*ptY = (GGPPtr0->P.y + FinalV.y)*FACTOR_INV;

		POINT P;
		P.x = *ptX;
		P.y = *ptY;

#ifdef MYDEBUG
		for (int ct=0; ct<GGPPtr0->count; ct++)
		{
			if ((GGPPtr0->src[ct].tag & 0x3) == 0x3)
			{
				DrawLine(imageMap,	GGPPtr0->src[ct].coords.x*FACTOR_INV,
									GGPPtr0->src[ct].coords.y*FACTOR_INV,
									GGPPtr0->src[ct].coordsNext.x*FACTOR_INV,
									GGPPtr0->src[ct].coordsNext.y*FACTOR_INV, 0xFF0000);
			}
		}
		DrawCross(imageMap, P, 0xFF0000,3);		
#endif
		res = true;
	}

	STOP_RASTER(Feature_Test1);
*/


	START_RASTER(Feature_Test2);
	for (int i=0; i<NB_LEVELS; i++)
	{
		for (int ct=0; ct<GGPPtr0[i].count; ct++)
		{
			GGPPtr0[i].src[ct].weight		= FERN_MAX_CORRELATION;
			GGPPtr0[i].src[ct].coordsNext.x	= 
			GGPPtr0[i].src[ct].coordsNext.y	= 0;
			GGPPtr0[i].src[ct].tag			&= 0x1;
		}
	}	
	
	SearchBestPoint(GGP[index], M6, -1);

	FinalV.x = FinalV.y = 0;
	searchRes = SearchNewCenter(&FinalV, GGPPtr0);
	
	if (searchRes > 0)
	{


		*ptX = (GGPPtr0->P.x + FinalV.x)*FACTOR_INV;
		*ptY = (GGPPtr0->P.y + FinalV.y)*FACTOR_INV;

		POINT P;
		P.x = *ptX;
		P.y = *ptY;

#ifdef MYDEBUG
		for (int ct=0; ct<GGPPtr0->count; ct++)
		{
			if ((GGPPtr0->src[ct].tag & 0x3) == 0x3)
			{
				DrawLine(imageMap,	GGPPtr0->src[ct].coords.x*FACTOR_INV,
									GGPPtr0->src[ct].coords.y*FACTOR_INV,
									GGPPtr0->src[ct].coordsNext.x*FACTOR_INV,
									GGPPtr0->src[ct].coordsNext.y*FACTOR_INV, 0xFF00);
			}
		}
		DrawCross(imageMap, P, 0xFF0000,3);		
#endif
		res = true;
	}

	STOP_RASTER(Feature_Test2);


	void DrawLine(MAP *MapDST,float X1,float Y1,float X2,float Y2,u32 Color);
#ifdef MYDEBUG
	DrawLine(imageMap,	*ptX - (trkZone*FACTOR_INV)/2, *ptY - (trkZone*FACTOR_INV)/2,
						*ptX - (trkZone*FACTOR_INV)/2, *ptY + (trkZone*FACTOR_INV)/2, 0xFF00);
	DrawLine(imageMap,	*ptX + (trkZone*FACTOR_INV)/2, *ptY + (trkZone*FACTOR_INV)/2,
						*ptX + (trkZone*FACTOR_INV)/2, *ptY - (trkZone*FACTOR_INV)/2, 0xFF00);
	DrawLine(imageMap,	*ptX + (trkZone*FACTOR_INV)/2, *ptY - (trkZone*FACTOR_INV)/2,
						*ptX - (trkZone*FACTOR_INV)/2, *ptY - (trkZone*FACTOR_INV)/2, 0xFF00);
	DrawLine(imageMap,	*ptX - (trkZone*FACTOR_INV)/2, *ptY + (trkZone*FACTOR_INV)/2,
						*ptX + (trkZone*FACTOR_INV)/2, *ptY + (trkZone*FACTOR_INV)/2, 0xFF00);

#endif
	return res;
}