#include "TRACK.h"
//#include "SHOTS.h"
//#include "SHOTS.h"
//#pragma optimize("",off)
void MAP16_Neg(MAP16 *MapSRC);

u32	F2DT_FRAME(MAP *CurrentMap,u32 options);

#define F2DTLevels 1
static MAP MWork[F2DTLevels];
static MAP MWork_Help[F2DTLevels];
static MAP MWork_Help2[F2DTLevels];

#define HistoryPo2 6
#define HistorySize (1<<HistoryPo2)

static MAP Dancer_Past[HistorySize + 1];

#define SynchroSize 11
static MAP Dancer_Synchro[SynchroSize];
static u32 SynchroPos = 0;

static MAP *LastCurrentMap = NULL;
static u32 CounterInc = 0;
static u32 CounterHistory = 0;

void MAP32_NormalizeBW(MAP *MapBW,u32 One,u32 Border);
float MAP32_DotProductNormalized1(MAP *Map1,MAP *Map2)
{
	s32 *LAST,*M1,*M2;
	float Average1 = 0,Average2 = 0;
	float Sqr1 = 0,DotProd = 0;
	float Sqr2 = 0;
	SET_BORDER(Map1,Map1->SX>>3,0);
	SET_BORDER(Map2,Map2->SX>>3,0);
	M1 = Map1->GetBase();
	M2 = Map2->GetBase();
	for (u32 SizeY = 0; SizeY < Map1->SY;SizeY ++)
	{
		LAST = M1 + (Map1->SX);
		while (M1 < LAST) 
		{
			Average1 += (float)*M1;
			Average2 += (float)*M2;
			M1++;
			M2++;
		}
		M1+=Map1->PITCH-Map1->SX;
		M2+=Map2->PITCH-Map2->SX;
	}	
	Average1 /= (float)Map1->SX * (float)(Map1->SY);
	Average2 /= (float)Map1->SX * (float)(Map1->SY);
	M1 = Map1->GetBase();
	M2 = Map2->GetBase();
	for (u32 SizeY = 0; SizeY < Map1->SY;SizeY ++)
	{
		LAST = M1 + (Map1->SX);
		while (M1 < LAST) 
		{
			float fM1 = *M1 - Average1;
			float fM2 = *M2 - Average2;
			Sqr1 += fM1 * fM1;
			Sqr2 += fM2 * fM2;
			DotProd += fM1 * fM2;
			M1++;
			M2++;
		}
		M1+=Map1->PITCH-Map1->SX;
		M2+=Map2->PITCH-Map2->SX;
	}
	return DotProd * FastInvSqrt(Sqr1 * Sqr2);
}
u32 GetHistoryIndex(s32 K) 
{
	u32 R = (HistorySize + CounterHistory - K) & (HistorySize-1);
	return R;
}
float GetDaNumber()
{
	float DP1[128],AvgS1 = 0,SqrS1 = 0;
	float DP2[128],AvgS2 = 0,SqrS2 = 0;
	float FinalDotProd = 0;
#define Depth 12
	MAP32_Set(&Dancer_Past[HistorySize],0);
	for (int i  = 1 ; i < Depth+1 ; i ++)
		MAP32_Add(&Dancer_Past[GetHistoryIndex(i)],&Dancer_Past[HistorySize]);
	for (int i  = 1 ; i < Depth+1 ; i ++)
	{
		u32 I1 = HistorySize ,I2 = GetHistoryIndex(i);
		Dancer_Past[I1].SetBase(Dancer_Past[I1].GetBase());
		Dancer_Past[I2].SetBase(Dancer_Past[I2].GetBase());
		Dancer_Past[I1].SX >>= 1;
		Dancer_Past[I2].SX >>= 1;
		Dancer_Past[I1].SetBase(Dancer_Past[I1].GetBase()+Dancer_Past[I1].SX);
		Dancer_Past[I2].SetBase(Dancer_Past[I2].GetBase()+Dancer_Past[I2].SX);
		DP1[i] = MAP32_DotProductNormalized1(&Dancer_Past[I1],&Dancer_Past[I2]);
		AvgS1 += DP1[i];
		Dancer_Past[I1].SetBase(Dancer_Past[I1].GetBase()-Dancer_Past[I1].SX);
		Dancer_Past[I2].SetBase(Dancer_Past[I2].GetBase()-Dancer_Past[I2].SX);
		Dancer_Past[I1].SX <<= 1;
		Dancer_Past[I2].SX <<= 1;
		
	}
	for (int i  = 1 ; i < Depth+1 ; i ++)
	{
		u32 I1 = HistorySize ,I2 = GetHistoryIndex(i);
		Dancer_Past[I1].SetBase(Dancer_Past[I1].GetBase());
		Dancer_Past[I2].SetBase(Dancer_Past[I2].GetBase());
		Dancer_Past[I1].SX >>= 1;
		Dancer_Past[I2].SX >>= 1;
		DP2[i] = MAP32_DotProductNormalized1(&Dancer_Past[I1],&Dancer_Past[I2]);
		AvgS2 += DP2[i];
		Dancer_Past[I1].SX <<= 1;
		Dancer_Past[I2].SX <<= 1;
	}
	AvgS1 /= Depth;
	AvgS2 /= Depth;
	for (int i  = 1 ; i < Depth+1 ; i ++)
	{
		DP1[i] -= AvgS1;
		DP2[i] -= AvgS2;
	}//*/
	AvgS1 = SqrS1 = AvgS2 = SqrS2 = 0.0;
	for (int i  = 1 ; i < Depth+1 ; i ++)
	{
		SqrS1 += DP1[i] * DP1[i];
		SqrS2 += DP2[i] * DP2[i];
		FinalDotProd += DP1[i] * DP2[i];
	}
	FinalDotProd = FinalDotProd * FastInvSqrt(SqrS1 * SqrS2);
	return FinalDotProd;
}
float Fast2DCompareLeftAndRight(MAP *pCurrent)
{
	u32 F2DT_CreateFeatureBox(u32 Index, s32 X,s32 Y,s32 W, s32 H, float Rotation);
	float FAST2D_FeatureBoxGetPertinence(u32 Index);
	void F2DT_FeatureBoxSetParam(u32 Index,char *String , float Value);
	pCurrent->SX >>= 1;
	F2DT_FRAME(pCurrent,0);
#define Kin (pCurrent->SX>>4)
	F2DT_CreateFeatureBox(0, Kin,Kin,pCurrent->SX-Kin*2,pCurrent->SY-Kin*2, 0.0f);
	pCurrent->SetBase(pCurrent->GetBase()+pCurrent->SX);
	F2DT_FeatureBoxSetParam(0,"TURN",0);
	F2DT_FeatureBoxSetParam(0,"SCAL",1);
	F2DT_FeatureBoxSetParam(0,"JUMP",0);
	F2DT_FeatureBoxSetParam(0,"INIT",1);
	F2DT_FeatureBoxSetParam(0,"EROR",0.6);

	F2DT_FRAME(pCurrent,3);
	pCurrent->SetBase(pCurrent->GetBase()-pCurrent->SX);
	pCurrent->SX <<= 1;
	return FAST2D_FeatureBoxGetPertinence(0)/100.0f;
}

void DanceDance_NoiseMap(MAP *CurrentMap)
{
	char TTTT[256];

	float SqrSumm = 0,Average = 0;
	float SqrSumm2 = 0,Average2 = 0;
	static float LastAverage = 0;
	static float LastAverage2 = 0;
	static float Blended = 0.0;
	static float BlendedBlended = 0.0;
#define MedianSize 9
	static float Median[MedianSize];
	static float MedianSorted[MedianSize];

	if (CurrentMap->SX*CurrentMap->SY != MWork[0].SX*MWork[0].SY)
	{
		MWork[0].SetSize(CurrentMap->SX*6, CurrentMap->SY);
		MWork[0].PITCH = MWork[0].SX = CurrentMap->SX;
		MWork[0].SY = CurrentMap->SY;
		for (int i = 1 ; i < F2DTLevels; i++ ) {MWork[i].PITCH = MWork[i].SX = MWork[i-1].SX >> 1;MWork[i].SY = MWork[i-1].SY >> 1;MWork[i].SetBase(MWork[i-1].GetBase() + MWork[i-1].SX * MWork[i-1].SY * 4);};
		for (int i = 0 ; i < F2DTLevels; i++ ) {MWork_Help[i] = MWork[i];MWork_Help[i].staticBase = 1;MWork_Help[i].MoveBase(MWork_Help[i].SX * MWork_Help[i].SY) ;};
		for (int i = 0 ; i < F2DTLevels; i++ ) {MWork_Help2[i] = MWork_Help[i];MWork_Help2[i].staticBase = 1;MWork_Help2[i].MoveBase(MWork_Help2[i].SX * MWork_Help2[i].SY) ;};
		for (int i = 0 ; i < HistorySize+1; i++ ) 
		{
			Dancer_Past[i].SetSize(CurrentMap->SX,CurrentMap->SY,1);
		};
		for (int i = 0 ; i < SynchroSize; i++ ) 
		{
			Dancer_Synchro[i].SetSize(CurrentMap->SX>>1,CurrentMap->SY,1);
		};
	}

	/* Synchroise */
	CurrentMap->SX >>= 1;
	MAP32_Copy(CurrentMap,&Dancer_Synchro[SynchroPos]);
	SynchroPos++;
	if (SynchroPos == SynchroSize) SynchroPos = 0;
	MAP32_Copy(&Dancer_Synchro[SynchroPos],CurrentMap);
	CurrentMap->SX <<= 1;

	CounterInc ++;
	if (LastCurrentMap != CurrentMap)
	{

#if 0
		MAP32_BLUR_Inplace(&MWork_Help2[0],1,3);
		MAP32_Copy(&MWork_Help2[0],&Dancer_Past[GetHistoryIndex(0)]);
//		MAP32_Shift(&Dancer_Past[GetHistoryIndex(0)],2);
		COPY_RGBA(&Dancer_Past[GetHistoryIndex(0)],&Dancer_Past[GetHistoryIndex(0)]);

		Blended = Fast2DCompareLeftAndRight(&Dancer_Past[GetHistoryIndex(0)]);
		BlendedBlended = BlendedBlended * 0.95f + Blended * 0.05f;

		CounterHistory ++;
		CounterInc = 1;
		CounterHistory &= HistorySize-1;
#else
		MAP32_Copy(&MWork_Help2[0],&Dancer_Past[GetHistoryIndex(0)]);
//		MAP32_Sub(&MWork_Help2[0],&Dancer_Past[GetHistoryIndex(0)]);
		//MAP32_Add(&Dancer_Past[GetHistoryIndex(0)],128);
		//MAP32_BLUR_Inplace(&Dancer_Past[GetHistoryIndex(0)],2,3);
		CounterHistory ++;
		CounterHistory &= HistorySize-1;
		CounterInc = 1;
		MAP32_Set(&MWork[0],0);
		MAP32_Set(&MWork_Help[0],0);
		MAP32_Set(&MWork_Help2[0],0);

		Blended = Blended * 0.95f + GetDaNumber() * 0.05f;
		Blended = GetDaNumber();
		if (Blended < 0) Blended = 0;
		Blended *= Blended;
		for (int i = 1 ; i < MedianSize ; i++)
			Median[i-1] = Median[i];
		Median[MedianSize-1] = Blended;
		for (int i = 0 ; i < MedianSize ; i++)
			MedianSorted[i] = Median[i];
		for (int i = 0 ; i < MedianSize - 1 ; i++)
			for (int j = i+1 ; i < MedianSize ; i++)
			 if (MedianSorted[i] < MedianSorted[j])
			 {
				 float Swap = MedianSorted[i];MedianSorted[i] = Median[i]; Median[i] = Swap;
			 }
		//BlendedBlended = MedianSorted[(MedianSize>>1) + 1];
		BlendedBlended = BlendedBlended * 0.95f + Blended * 0.05f;
#endif
		//Blended = GetDaNumber();
	}

	LastCurrentMap = CurrentMap;

	//CounterInc = 1;
#if 0
	MAP32_COPY_BW(CurrentMap,CurrentMap);
	MAP32_BLUR_Inplace(CurrentMap,1,3);
	MAP32_Shift(CurrentMap,-2);
	MAP32_COPY_RGBA(CurrentMap,CurrentMap);
#endif

/*

	CurrentMap->SetValue(2, rand()%321654);
	return;//*/
	
	//MAP32_And(CurrentMap,0xff);
	//MAP32_BLUR_Inplace(CurrentMap,1,3);
	{
		s32 *LAST,*DST;
		s32 *WH0,*WH1,*WH2;
		DST = CurrentMap->GetBase();
		WH0 = MWork[0].GetBase();
		WH1 = MWork_Help[0].GetBase();
		WH2 = MWork_Help2[0].GetBase();
		for (u32 SizeY = 0; SizeY < CurrentMap->SY;SizeY ++)
		{
			LAST = DST + (CurrentMap->SX>>1);
			while (DST < LAST) 
			{
				s32 MAX;
				MAX = ((u8*)DST)[0];
				if (((u8*)DST)[1] > MAX) MAX = ((u8*)DST)[1];
				if (((u8*)DST)[2] > MAX) MAX = ((u8*)DST)[2];
				*WH0 += (MAX);
				*WH1 += (MAX * MAX);
				MAX = (*WH1 / CounterInc) - (*WH0 / CounterInc) * (*WH0 / CounterInc);
				if (MAX) MAX = FastSqrt(MAX);
				Average += (float)MAX;
				SqrSumm += (float)MAX * (float)MAX;
				*WH2 =MAX;

				//if (!MAX) *((s32*)DST) = 0;


				WH0 ++;
				WH1 ++;
				WH2 ++;
				DST++;
			}
			LAST = DST + (CurrentMap->SX>>1);
			while (DST < LAST) 
			{
				s32 MAX;
				MAX = ((u8*)DST)[0];
				MAX += ((u8*)DST)[1];
				MAX += ((u8*)DST)[2];
				MAX /= 3;
				//if (((u8*)DST)[3] > MAX) MAX = ((u8*)DST)[3];
				//((u8*)DST)[0] = ((u8*)DST)[1] = ((u8*)DST)[2] = ((u8*)DST)[3] = MAX>>1;
				*WH0 += (MAX);
				*WH1 += (MAX * MAX);
				MAX = (*WH1 / CounterInc) - (*WH0 / CounterInc) * (*WH0 / CounterInc);
				if (MAX) MAX = FastSqrt(MAX);
				Average2 += (float)MAX;
				SqrSumm2 += (float)MAX * (float)MAX;
				*WH2 =MAX;
//				*WH2 =MAX;
				//if (!MAX) *((s32*)DST) = 0;


				WH0 ++;
				WH1 ++;
				WH2 ++;
				DST ++;
			}
			DST+=CurrentMap->PITCH-CurrentMap->SX;
			WH0+=MWork[0].PITCH-MWork[0].SX;
			WH1+=MWork_Help[0].PITCH-MWork_Help[0].SX;
			WH2+=MWork_Help2[0].PITCH-MWork_Help2[0].SX;
		}
	}
/*	MAP32_MIN_CROSS(CurrentMap,&MWork_Help2[0]);
	MAP32_MIN_CROSS(&MWork_Help2[0],CurrentMap);
*/
	Average = 2.0 * Average / ((float)CurrentMap->SX * (float)CurrentMap->SY);
	SqrSumm = 2.0 * SqrSumm / ((float)CurrentMap->SX * (float)CurrentMap->SY);
	SqrSumm -= Average * Average;
	SqrSumm = FastSqrt(SqrSumm);
	SqrSumm = 32 / (SqrSumm + 1);

	Average2 = 2.0 * Average2 / ((float)CurrentMap->SX * (float)CurrentMap->SY);
	SqrSumm2 = 2.0 * SqrSumm2 / ((float)CurrentMap->SX * (float)CurrentMap->SY);
	SqrSumm2 -= Average2 * Average2;
	SqrSumm2 = FastSqrt(SqrSumm2);
	SqrSumm2 = 32 / (SqrSumm2 + 1);

	LastAverage = Average;
	LastAverage2 = Average2;

	/*	{
		s32 *LAST,*DST;
		DST = CurrentMap->GetBase();
		for (u32 SizeY = 0; SizeY < CurrentMap->SY;SizeY ++)
		{
			LAST = DST + (CurrentMap->SX>>1);
			while (DST < LAST) 
			{
				*DST = (*DST - Average)*SqrSumm + 128;
				DST++;
			}
			LAST = DST + (CurrentMap->SX>>1);
			while (DST < LAST) 
			{
				*DST = (*DST - Average2)*SqrSumm2 + 128;
				DST++;
			}
			DST+=CurrentMap->PITCH-CurrentMap->SX;
		}
	}
	*/
/*	
	MAP32_Copy(&Dancer_Past[GetHistoryIndex(1)],CurrentMap);
	MAP32_Copy(&MWork_Help2[0],CurrentMap);
	
	MAP32_Add(CurrentMap,128);

	MAP32_COPY_RGBA(CurrentMap,CurrentMap);*/
	MAP32_COPY_RGBA(&Dancer_Past[GetHistoryIndex(1)],CurrentMap);
	MAP32_COPY_RGBA(&MWork_Help2[0],CurrentMap);
	
	/*MAP32_Copy(&MWork_Help2[0],CurrentMap);
	MAP32_COPY_RGBA(CurrentMap,CurrentMap);*/
//*/
/*	{
		s32 *LAST,*DST;
		DST = CurrentMap->GetBase();
		for (u32 SizeY = 0; SizeY < CurrentMap->SY;SizeY ++)
		{
			LAST = DST + (CurrentMap->SX>>1);
			while (DST < LAST) 
			{
				if (*DST< 0) *DST = 0;
				if ((*DST & 0xffff00))
					*DST = 0xffffff;
				DST++;
			}
			LAST = DST + (CurrentMap->SX>>1);
			while (DST < LAST) 
			{
				if (*DST< 0) *DST = 0;
				if (*DST & 0xffff00)
					*DST = 0xffffff;
				else
					*DST = (*DST<<16);
				DST++;
			}
			DST+=CurrentMap->PITCH-CurrentMap->SX;
		}
	}*/
	sprintf(TTTT , "%.2f",Blended);
	DrawQuadFull(CurrentMap,0,0,CurrentMap->SX*BlendedBlended,CurrentMap->SY>>3,0xff00);
	DRawTEXT_Soft_B((u8*)TTTT ,(u32*)CurrentMap->GetBase(),10,10,CurrentMap->PITCH,CurrentMap->SY,0xffffff);

	CurrentMap->SetValue(2, rand()%321654);
}