#include "TRACK.h"
#include "Rrasters.h"

float IG_GetError(WORKINGSPACE *WSPC,MATRIX *M,u32 Group2Test,float MAXErr = 100000000.0f)
{
	float Error,NE;
	TRACKEDPOINT *pTPCurrent, *pTPLast, *pTPrevious;
	pTPCurrent = WSPC->Current_WSP->TPS;
	pTPLast = pTPCurrent + WSPC->Current_WSP->nTps;
	pTPrevious = WSPC->Previous_WSP->TPS;
	NE = Error = 0.0f;
	if (MAXErr < 5.0f) MAXErr = 5.0f;
	while (pTPCurrent < pTPLast)
	{
		if (pTPCurrent->GroupIndex & Group2Test)
		{
			Vector V,LastV;
			V.x = pTPCurrent->X;
			V.y = pTPCurrent->Y;
			V.z = 0;
			LastV.x = (pTPrevious+pTPCurrent->PreviousIndex)->X;
			LastV.y = (pTPrevious+pTPCurrent->PreviousIndex)->Y;
			LastV.z = 0;

			V = *M * V;
			V -= LastV;
			float LE = V.SqrNormXY();
			if (LE > MAXErr)
				pTPCurrent->GroupIndex &= ~Group2Test;
			else
			{
				Error += V.SqrNormXY();
				NE++;
			}
		}
		pTPCurrent++;
	}
	if (NE)
		return Error / NE;
	else 
		return 10000000.0f;
}

void IG_ComputeTranslationScale(WORKINGSPACE *WSPC,MATRIX *M,u32 Group2Test)
{
	Vector TGlob;
	TRACKEDPOINT *pTPCurrent, *pTPLast, *pTPrevious;
	pTPCurrent = WSPC->Current_WSP->TPS;
	pTPLast = pTPCurrent + WSPC->Current_WSP->nTps;
	pTPrevious = WSPC->Previous_WSP->TPS;
	/* Compute General translation */
	TGlob = Vector (0,0,0);
	while (pTPCurrent < pTPLast)
	{
		if (pTPCurrent->GroupIndex & Group2Test)
		{
			TGlob.x += pTPCurrent->SpeedX;
			TGlob.y += pTPCurrent->SpeedY;
			TGlob.z ++;
		}
		pTPCurrent++;
	}
	if (TGlob.z)
	{
		TGlob.z = 1.0f / TGlob.z;
		TGlob.x *= TGlob.z;
		TGlob.y *= TGlob.z;

		/* Compute General Scale */

	}
	M->T.x = -TGlob.x;
	M->T.y = -TGlob.y;
	M->T.z = 0.0f;
}





void IG_DisplayGroups(WORKINGSPACE *WSPC)
{
	for (int i  = 0 ; i < 32 ; i ++)
	{
		/* Compute BV */
		Vector Max,Min;
		u32 N;
		Max = Vector(-1000000.0f,-1000000.0f,-1000000.0f);
		Min = Vector(1000000.0f,1000000.0f,1000000.0f);
		TRACKEDPOINT *pTPCurrent, *pTPLast, *pTPrevious;
		pTPCurrent = WSPC->Current_WSP->TPS;
		pTPLast = pTPCurrent + WSPC->Current_WSP->nTps;
		pTPrevious = WSPC->Previous_WSP->TPS;
		/* Compute General translation */
		N= 0;
		while (pTPCurrent < pTPLast)
		{
			if ((pTPCurrent->GroupIndex) == (1<<i))
			{
				Vector V,LastV;
				V.x = pTPCurrent->X * (float)WSPC->RENDERMAP->SX / (float)WSPC->DRAWMAP->SX;
				V.y = pTPCurrent->Y * (float)WSPC->RENDERMAP->SY / (float)WSPC->DRAWMAP->SY;
				V.z = 0;
				Max.Max(V);
				Min.Min(V);
				N++;
			}
			pTPCurrent++;
		}
		if (N > 10)
		{
		u32 Color = 0;
		if (i&1) Color |= 0xff;
		if (i&2) Color |= 0xff00;
		if (i&4) Color |= 0xff0000;
		DrawQuad_R(WSPC->RENDERMAP,Min.x,Min.y,Max.x,Max.y,Color,2,0.0f);
		}
	}
}
 
void IG_Posit(WORKINGSPACE *WSPC)
{
	MATRIX M;
	float Err;
	for (int i=0;i<32;i++)
	{
		M.Identity();
		IG_ComputeTranslationScale(WSPC,&M,1<<i);
		Err = IG_GetError(WSPC,&M,1<<i);
		IG_GetError(WSPC,&M,1<<i,Err/2);
		IG_ComputeTranslationScale(WSPC,&M,1<<i);
		Err = IG_GetError(WSPC,&M,1<<i);
		IG_GetError(WSPC,&M,1<<i,Err/2);
		TRACKEDPOINT *pTPCurrent, *pTPLast, *pTPrevious;
		pTPCurrent = WSPC->Current_WSP->TPS;
		pTPLast = pTPCurrent + WSPC->Current_WSP->nTps;
		while (pTPCurrent < pTPLast)
		{
			if (pTPCurrent->GroupIndex & (1<<i)) 
				pTPCurrent->GroupIndex = (1<<i);
			pTPCurrent++;
		}
	}

}


/* Try to match and find group of uniform affine transform */
void INTERP_GROUP(WORKINGSPACE *WSPC)
{
	TRACKEDPOINT *pTPCurrent, *pTPLast, *pTPrevious;
	u32 SPEED_MAP[64*64];
	memset(SPEED_MAP,0,sizeof(SPEED_MAP));
	float AX,AY,AD;
	AX = AY = AD = 0.0f;

	pTPCurrent = WSPC->Current_WSP->TPS;
	pTPLast = pTPCurrent + WSPC->Current_WSP->nTps;
	pTPrevious = WSPC->Previous_WSP->TPS;
	while (pTPCurrent < pTPLast)
	{
		if (pTPCurrent->PreviousIndex != 0xffffffff)
		{
			pTPCurrent->GroupIndex = 0;
			pTPCurrent->GroupIndex = 0;
			s32 DX = (pTPCurrent->X - pTPrevious[pTPCurrent->PreviousIndex].X) * 3;
			s32 DY = (pTPCurrent->Y - pTPrevious[pTPCurrent->PreviousIndex].Y) * 3;
#define S_BLENDER 0.95f
			pTPCurrent->SpeedX = pTPrevious[pTPCurrent->PreviousIndex].SpeedX * S_BLENDER + (float)DX * (1.0f - S_BLENDER);
			pTPCurrent->SpeedY = pTPrevious[pTPCurrent->PreviousIndex].SpeedY * S_BLENDER + (float)DY * (1.0f - S_BLENDER);
			if (pTPCurrent->TrackingTime > 5)
			{
#define ADRADIUS 14
				if (((pTPCurrent->X > WSPC->DRAWMAP->SX/2 - ADRADIUS) &&(pTPCurrent->X < WSPC->DRAWMAP->SX/2 + ADRADIUS))&&
				    ((pTPCurrent->Y > WSPC->DRAWMAP->SY/2 - ADRADIUS) &&(pTPCurrent->Y < WSPC->DRAWMAP->SY/2 + ADRADIUS)))
					{
						float Pond = (pTPCurrent->SpeedX * pTPCurrent->SpeedX + pTPCurrent->SpeedY * pTPCurrent->SpeedY);
						 Pond = 1.0f;
						
						AX+=(s32)pTPCurrent->SpeedX * Pond;
						AY+=(s32)pTPCurrent->SpeedY * Pond;
						AD += Pond;
					}
				float ZSqrL = pTPCurrent->SpeedX * pTPCurrent->SpeedX + pTPCurrent->SpeedY * pTPCurrent->SpeedY;
				if (ZSqrL > 14.0F * 14.0F)  
				{
					float Renorm;
					Renorm = 14.0f / sqrtf(ZSqrL);
					pTPCurrent->SpeedX *= Renorm;
					pTPCurrent->SpeedY *= Renorm;
				}
				//if (ZSqrL > 3.0f)  
				{
					SPEED_MAP[(s32)pTPCurrent->SpeedX + (s32)pTPCurrent->SpeedY * 64 + 64*32 + 32]++;
				}
			}
		}
		else
		{
			/* Inherit Previous Group */
			pTPCurrent->GroupIndex = -1;//(pTPLast + pTPCurrent->PreviousIndex)->GroupIndex;
			pTPCurrent->SpeedX = pTPCurrent->SpeedY = 0.0f;
		}
		pTPCurrent++;
	}

	/* Find 4 Most significant points */
	u32 MaxV[4],MaxIX[4],MaxIY[4];
	for (u32 M = 0 ; M < 4 ; M ++)
	{
		MaxV[M] = 0;
		MaxIX[M] = MaxIY[M] = 8;
		for (u32 X = 2 ; X < 62 ; X ++)
		{
			for (u32 Y = 2 ; Y < 62 ; Y ++)
			{
				u32 Color = SPEED_MAP[X + (Y<<6)];
				if ((!(Color >> 24)) && Color)
				{
					u32 LocalT = 0;
					LocalT += SPEED_MAP[(X-1) + ((Y-1)<<6)] & 0xffffff;
					LocalT += SPEED_MAP[(X-0) + ((Y-1)<<6)] & 0xffffff;
					LocalT += SPEED_MAP[(X+1) + ((Y-1)<<6)] & 0xffffff;

					LocalT += SPEED_MAP[(X-1) + ((Y-0)<<6)] & 0xffffff;
					LocalT += SPEED_MAP[(X-0) + ((Y-0)<<6)] & 0xffffff;
					LocalT += SPEED_MAP[(X+1) + ((Y-0)<<6)] & 0xffffff;

					LocalT += SPEED_MAP[(X-1) + ((Y+1)<<6)] & 0xffffff;
					LocalT += SPEED_MAP[(X-0) + ((Y+1)<<6)] & 0xffffff;
					LocalT += SPEED_MAP[(X+1) + ((Y+1)<<6)] & 0xffffff;

					if (LocalT > 2)
					{
						if (LocalT > MaxV[M])
						{
							MaxV[M] = LocalT;
							MaxIX[M] = X;
							MaxIY[M] = Y;
						}
					}
				}
			}
		}
#define R 3

		if (MaxIX[M] != 8)
		for (u32 X = MaxIX[M]-R ; X < MaxIX[M]+R ; X ++)
		{
			for (u32 Y = MaxIY[M]-R ; Y < MaxIY[M]+R ; Y ++)
			{
				SPEED_MAP[X + (Y<<6)] = 0x1000000 | M;
				SPEED_MAP[MaxIX[M] + (MaxIY[M]<<6)] = 0x1000000 | M;
			}
		}//*/
	}



	for (u32 X = 0 ; X < 128 ; X ++)
	{
		for (u32 Y = 0 ; Y < 128 ; Y ++)
		{
			u32 Color = SPEED_MAP[(X>>1) + 64*(Y>>1)];
			if (Color & 0xff000000)
			{
				Color = 0xff0000 ;
			} else
			{
				Color <<= 4;
				if (Color > 255) Color = 255;
				Color |= Color<<8;
				Color |= Color<<8;
			}
			*(WSPC->RENDERMAP->BASE + X + Y * WSPC->RENDERMAP->PITCH) = Color;
		}
	}

	pTPCurrent = WSPC->Current_WSP->TPS;
	pTPLast = pTPCurrent + WSPC->Current_WSP->nTps;
	pTPrevious = WSPC->Previous_WSP->TPS;

	while (pTPCurrent < pTPLast)
	{
		if (pTPCurrent->PreviousIndex != 0xffffffff)
		{
			if (SPEED_MAP[(s32)pTPCurrent->SpeedX + (s32)pTPCurrent->SpeedY * 64 + 64*32 + 32] & 0xff000000)
			{
				pTPCurrent->GroupIndex = 1<<(SPEED_MAP[(s32)pTPCurrent->SpeedX + (s32)pTPCurrent->SpeedY * 64 + 64*32 + 32]&0xffff);
			} 
			//else				pTPCurrent->GroupIndex = (pTPrevious + pTPCurrent->PreviousIndex)->GroupIndex;
		}
		else
		{
			/* Inherit Previous Group */
			pTPCurrent->GroupIndex = 0;//(pTPLast + pTPCurrent->PreviousIndex)->GroupIndex;
		}
		pTPCurrent++;
	}
	if (!AD) AD = 100000.0f; 
	if (AD)
	{

		AX/=AD * ADRADIUS * ADRADIUS / 8.0f;
		AY/=AD * ADRADIUS * ADRADIUS / 8.0f;
		AX/=(float)WSPC->DRAWMAP->SX / (float)WSPC->RENDERMAP->SX;
		AY/=(float)WSPC->DRAWMAP->SX / (float)WSPC->RENDERMAP->SX;
		
		//if (AX * AX + AY * AY > 0.0625f / 2.0f)
		DrawPointFull(WSPC->RENDERMAP,AX * 64.0f + WSPC->RENDERMAP->SX/2,AY * 64.0f + WSPC->RENDERMAP->SY/2,6,0xffff);
		AD = ADRADIUS * (float)WSPC->RENDERMAP->SX / (float)WSPC->DRAWMAP->SX;
		DrawQuad_R(WSPC->RENDERMAP,WSPC->RENDERMAP->SX/2 - AD,WSPC->RENDERMAP->SY/2 - AD,WSPC->RENDERMAP->SX/2 + AD,WSPC->RENDERMAP->SY/2 + AD,0xff00,2,0.0f);

	}
//*/
	


	IG_Posit(WSPC);
	IG_DisplayGroups(WSPC);
}