#include "windows.h"
#include "CCMap.h"
#include "math.h"

void ExtractPoint(MAP8 *MapDST,float X,float Y,float *Dest)
{
	s32 BX,BY;
	u8 *pBase;
	BX = (s32)(X * 256.0f);
	BY = (s32)(Y * 256.0f);
	pBase = MapDST->GetBase() + (BX>>8) + (BY>>8) * MapDST->PITCH;
	BX &= 255;
	BY &= 255;
	float VX1 = (float)*(pBase+0) * (float)(255 - BX) + (float)*(pBase+1) * (float)BX;
	pBase += MapDST->PITCH;
	float VX2 = (float)*(pBase+0) * (float)(255 - BX) + (float)*(pBase+1) * (float)BX;
	*Dest = (VX1 * (float)(255 - BY) + VX2 * (float)BY) / 65536.0f;
}

float GetLineError(MAP8 *MapDST,float X1,float Y1,float X2,float Y2,float *Coefs,u32 Number)
{
	float *pCoefs = (float *)Coefs;
	float Ret = 0.0f;
	X2 = X2 - X1;
	Y2 = Y2 - Y1;
	X2 /= (float)(Number - 1);
	Y2 /= (float)(Number - 1);
	X1 -= X2 * (float)(Number - 1) / 2.0f;
	Y1 -= Y2 * (float)(Number - 1) / 2.0f;
/*
	Eq = (Vi-VM)(Vi-VM) + (Vi-VM)(Vi-VM) + ....
	Eq = Vi² + VM² - 2 * VM * Vi + ....
	Eq = e{Vi²} + N*VM² - N * 2 * VM * Vi + ....
	=> VM = e{Vi} / N
	Eq = e{Vi²} + N*e{Vi}² / N² - (2 * (e{Vi} / N)) e{Vi} 
	Eq = e{Vi²} + e{Vi}² / N - 2 * e{Vi}² / N 
	Eq = e{Vi²} - e{Vi}² / N 


	Eq = e{Vi²} + e{Vi}² * 1/N² - e{Vi} * 2 * VM 
	Eq = e{Vi²} + e{Vi}² * 1/N² - e{Vi} * e{Vi} * 2/N
	Eq = e{Vi²} + e{Vi}² * 1/N² - e{Vi}² * 2/N
	Eq = e{Vi²} - e{Vi}² / N

	Norm = 1.0f / sqrt(Eq);

	VV = ((Vi-VM) * Coef[i] / Norm);

	DP = (Vi0-VM)(Ci0) + (Vi0-VM)(Ci1) + ....
	DP = Ci0 * Vi0 - Ci0 * VM + ...
	DP = Ci0 * Vi0 + Ci1 * Vi1 - VM (e{Ci}) ...
	e{Ci} = 0.0

	DP = Ci0 * Vi0 + Ci1 * Vi1 + ...

*/
	float DotProd	= 0.0f;
	float eVi		= 0.0f;
	float eViDP		= 0.0f;
	float eSqrVi	= 0.0f;
	while (Number --)
	{
		float C;
		ExtractPoint(MapDST,X1,Y1,&C);
		eVi += C;
		eSqrVi += C * C;
		eViDP += C * *pCoefs;
		X1 += X2;
		Y1 += Y2;
		pCoefs++;
	}
	//VM = eVi / 6.0f;
	//return eViDP ;
	float Eq = eSqrVi - eVi * eVi / 8.0f;

	return eViDP / sqrtf(Eq);
}

void SET_BORDER8(MAP8 *MapSRC,s32 Size,u8 Value)
{
	u8 *pSrc,*pLst,*pLst2;
	pSrc = MapSRC->GetBase();
	pLst = MapSRC->GetBase() + Size * MapSRC->PITCH;
	while (pSrc < pLst)
		*(pSrc++) = Value;
	
	pLst = MapSRC->GetBase() + (MapSRC->SY-Size) * MapSRC->PITCH;

	while (pSrc < pLst)
	{
		pLst2 = pSrc + Size;
		while (pSrc < pLst2)
			*(pSrc++) = Value;
		pSrc+= MapSRC->SX - Size*2;
		pLst2 = pSrc + Size;
		while (pSrc < pLst2)
			*(pSrc++) = Value;
	}

	pLst = MapSRC->GetBase() + (MapSRC->SY) * MapSRC->PITCH;

	while (pSrc < pLst)
		*(pSrc++) = Value;

}


void MAP8_SQREType_H_Inplace(MAP8 *pSrc,u32 Rpo2)
{
	// In place compute ET 8 bits horizontaly
	u8 *PPd,*PPl,*PPdOfset;
	u32 R = (1<<Rpo2 );
	u32 INC = 0;
	u32 INCSQR = 0;
	PPd = pSrc->GetBase();
	PPl = pSrc->GetBase() + pSrc->PITCH * pSrc->SY - R;
	for (int i = 0 ; i < R ; i ++) 	{INC += (u32)PPd[i];INCSQR += (u32)PPd[i] * (u32)PPd[i];};
	while (PPd < PPl)
	{
		u8 Lv = *PPd;
		*PPd = (INCSQR - ((INC*INC)>>Rpo2))>>(Rpo2+8);
		*PPd = sqrtf((float)((INCSQR - ((INC*INC)>>Rpo2))>>(Rpo2)));
		INC -= (u32)Lv;
		INCSQR -= (u32)Lv * (u32)Lv;
		INC += *(PPd + R);
		INCSQR += (u32)*(PPd + R) * (u32)*(PPd + R);
		PPd ++;
	}
	PPd = pSrc->GetBase();
	PPdOfset = PPl - (R>>1);
	while (PPdOfset > PPd) 
	{
		*(PPl--) = *(PPdOfset) + *(PPdOfset+1);
		PPdOfset--;
	}
}

void MAP8_SQREType_V_Inplace(MAP8 *pSrc,u32 Rpo2)
{
	// In place blur 8 bits verticaly
	u8 *PPd,*PPl,*PPdOfset;
	u32 R = 1<<Rpo2;
	u32 INC[1024];
	u32 INCSQR[1024];
	memset(INC,0,sizeof(INC));
	memset(INCSQR,0,sizeof(INCSQR));
	PPd =  pSrc->GetBase();
	for (int i = 0 ; i < pSrc->SX ; i ++)
	{
		for (int j = 0 ; j < R ; j ++) 	
		{
			u32 V = (u32)PPd[i+j *pSrc->PITCH];
			INC[i] += V;
			INCSQR[i] += V * V;
		}
	}
	for (int i = 0 ; i < pSrc->SY-R ; i ++)
	{
		u32 *pINC,*pINCSQR;
		u8 *PPd2;
		pINC = INC;
		pINCSQR = INCSQR;
		PPd = pSrc->GetBase() + i * pSrc->PITCH;
		PPl = pSrc->GetBase() + i * pSrc->PITCH + pSrc->SX;
		PPd2 = PPd + R * pSrc->PITCH;
		while (PPd < PPl)
		{
			u32 Lv = *PPd;
			*PPd = (*pINCSQR - ((*pINC**pINC)>>Rpo2))>>(Rpo2+8);
			*PPd = sqrtf((float)((*pINCSQR - ((*pINC**pINC)>>Rpo2))>>(Rpo2)));


			*pINC -= Lv;
			*pINC += *(PPd2);
			*pINCSQR -= Lv * Lv;
			*pINCSQR += (u32)*(PPd2) * (u32)*(PPd2++);
			pINC++;
			pINCSQR++;
			PPd ++;
		}
	}
	for (int i = pSrc->SY-R; i < pSrc->SY ; i ++)
	{
		u32 *pINC;
		pINC = INC;
		PPd = pSrc->GetBase() + i * pSrc->PITCH;
		PPl = pSrc->GetBase() + i * pSrc->PITCH + pSrc->SX;
		while (PPd < PPl)
		{
			u8 Lv = *PPd;
			*PPd = (u8)(*pINC >>Rpo2 );
			*pINC -= Lv;
			pINC++;
			PPd ++;
		}
	}
	PPd = pSrc->GetBase();
	PPdOfset = PPl - (R>>1) * pSrc->PITCH;
	while (PPdOfset > PPd) 
	{
		*(PPl--) = *(PPdOfset) + *(PPdOfset + pSrc->PITCH);
		PPdOfset--;
	}
}

void MAP8_BLUR8_H_Inplace(MAP8 *pSrc,u32 Rpo2)
{
	// In place blur 8 bits horizontaly
	u8 *PPd,*PPl,*PPdOfset;
	u32 R = (1<<Rpo2 );
	u32 INC = 0;
	Rpo2++;
	PPd = pSrc->GetBase();
	PPl = pSrc->GetBase() + pSrc->PITCH * pSrc->SY - R;
	for (int i = 0 ; i < R ; i ++) 	INC += (u32)PPd[i];
	while (PPd < PPl)
	{
		u8 Lv = *PPd;
		*PPd = (u8)(INC>>Rpo2);
		INC -= Lv;
		INC += *(PPd + R);
		PPd ++;
	}
	PPd = pSrc->GetBase();
	PPdOfset = PPl - (R>>1);
	while (PPdOfset > PPd) 
	{
		*(PPl--) = *(PPdOfset) + *(PPdOfset+1);
		PPdOfset--;
	}
}


void MAP8_BLUR8_V_Inplace(MAP8 *pSrc,u32 Rpo2)
{
	// In place blur 8 bits verticaly
	u8 *PPd,*PPl,*PPdOfset;
	u32 R = 1<<Rpo2;
	u32 INC[1024];
	Rpo2 ++;
	memset(INC,0,sizeof(INC));
	PPd = pSrc->GetBase();
	for (int i = 0 ; i < pSrc->SX ; i ++)
	{
		for (int j = 0 ; j < R ; j ++) 	
			INC[i] += (u32)PPd[i+j *pSrc->PITCH];
	}
	for (int i = 0 ; i < pSrc->SY-R ; i ++)
	{
		u32 *pINC;
		u8 *PPd2;
		pINC = INC;
		PPd = pSrc->GetBase() + i * pSrc->PITCH;
		PPl = pSrc->GetBase() + i * pSrc->PITCH + pSrc->SX;
		PPd2 = PPd + R * pSrc->PITCH;
		while (PPd < PPl)
		{
			u8 Lv = *PPd;
			*PPd = (u8)(*pINC>>Rpo2);
			*pINC -= Lv;
			*pINC += *(PPd2++);
			pINC++;
			PPd ++;
		}
	}
	for (int i = pSrc->SY-R; i < pSrc->SY ; i ++)
	{
		u32 *pINC;
		pINC = INC;
		PPd = pSrc->GetBase() + i * pSrc->PITCH;
		PPl = pSrc->GetBase() + i * pSrc->PITCH + pSrc->SX;
		while (PPd < PPl)
		{
			u8 Lv = *PPd;
			*PPd = (u8)(*pINC >>Rpo2 );
			*pINC -= Lv;
			pINC++;
			PPd ++;
		}
	}
	PPd = pSrc->GetBase();
	PPdOfset = PPl - (R>>1) * pSrc->PITCH;
	while (PPdOfset > PPd) 
	{
		*(PPl--) = *(PPdOfset) + *(PPdOfset + pSrc->PITCH);
		PPdOfset--;
	}
}




void MAP8_WAVELET_H_Inplace(MAP8 *pSrc)
{
	u8 *PPSrc,*PPSrcppitch,*PPDst,*PPl,*PPdOfset;
	u32 Counter;
	Counter = pSrc->SY;
	PPDst = pSrc->GetBase();
	while (Counter--)
	{
		PPSrc = PPDst;
		PPl = PPSrc+pSrc->SX;
		while (PPSrc < PPl)
		{
			*PPDst = (u8)(((u32)*PPSrc + (u32)*(PPSrc+1))>>1);
			PPDst++;
			PPSrc+=2;
		}
		PPSrc = PPl-pSrc->SX;
		while (PPSrc < PPl)
		{
			*PPDst = (u8)(((u32)*PPSrc + (u32)*(PPSrc+1))>>1);
			PPDst++;
			PPSrc+=2;
		}
		PPDst+=pSrc->PITCH - pSrc->SX;
	}
}
void MAP8_WAVELET_V_Inplace(MAP8 *pSrc)
{
	u8 *PPSrc,*PPDst,*PPl,*PPdOfset;
	u32 Counter;
	PPDst = pSrc->GetBase();
	PPSrc = PPDst;
	Counter = pSrc->SY>>1;
	while (Counter--)
	{
		PPl = PPDst+pSrc->SX;
		while (PPDst < PPl)
		{
			*PPDst = (u8)(((u32)*PPSrc + (u32)*(PPSrc + pSrc->PITCH))>>1);
			PPDst++;
			PPSrc++;
		}
		PPDst+=pSrc->PITCH - pSrc->SX;
		PPSrc+=pSrc->PITCH*2 - pSrc->SX;
	}
	Counter = pSrc->SY>>1;
	PPSrc = pSrc->GetBase();
	while (Counter--)
	{
		PPl = PPDst+pSrc->SX;
		while (PPDst < PPl)
		{
			*PPDst = (u8)(((u32)*PPSrc + (u32)*(PPSrc + pSrc->PITCH))>>1);
			PPDst++;
			PPSrc++;
		}
		PPDst+=pSrc->PITCH - pSrc->SX;
		PPSrc+=pSrc->PITCH*2 - pSrc->SX;
	}
}
//#pragma optimize("",off)

#define U8_SWAP(a,b) {u8 Swp;Swp = a;a = b ; b = Swp; }
void MAP8_ShiftLine(u8 *pSrc,s32 Shift,s32 Lenght)
{
	u8 CpyL[2048];
	// In place shift 8 bits horizontaly
	if (Shift < 0) Shift = Lenght + Shift;
	if (Shift < 0) return;
	if (Shift >= Lenght) Shift = Lenght -1;
	memcpy(CpyL,pSrc,Lenght);
	memcpy(pSrc,CpyL+Lenght-Shift,Shift);
	memcpy(pSrc+Shift,CpyL,Lenght-Shift);
}
void MAP8_BLUR8_ScissorX_Inplace(MAP8 *pSrc,s32 FactorX)
{
	u8 *pCrnt,*pLast;
	float Inc,Base;
	pCrnt = pSrc->GetBase();
	pLast = pCrnt + pSrc->PITCH * pSrc->SY;
	Base = 0.0f;
	Inc = (float)FactorX / (float)pSrc->SY;
	while (pCrnt < pLast)
	{
		MAP8_ShiftLine(pCrnt,(s32)Base,pSrc->SX);
		Base+=Inc;
		pCrnt+=pSrc->PITCH;
	}
}
	
void MAP8_BLUR8_CopyTranspose(MAP8 *pSrc,MAP8 *pDst)
{
	u8 *pucS,*pucD,*pucL,*pucLL;
	u32 HL;
	pDst->PITCH = pDst->SX = pSrc->SY;
	pDst->SY = pSrc->SX;
	pucS = pSrc->GetBase();
	pucD = pDst->GetBase();
	HL = pSrc->PITCH * pSrc->SY;
	pucLL = pucS + pSrc->SX;
	while (pucS < pucLL)
	{
		pucL = pucS + HL;
		while (pucS < pucL)
		{
			*(pucD++) = *pucS;
			pucS+=pSrc->PITCH;
		}
		pucS -= HL - 1;
	}
}
	
void MAP8_BLUR_XY(MAP8 *pSrc,MAP8 *pTmp,u8 Alpha, u32 Rpo2 , u32 Iteration)
{
	SET_BORDER8(pSrc,1<<(Rpo2-1),0);
	if ((Alpha & 63) == 0) // Is a quadrant
	{
		if (Alpha & 64)
			while (Iteration--) MAP8_BLUR8_H_Inplace(pSrc,Rpo2); // 64 or 192
		else
			while (Iteration--) MAP8_BLUR8_V_Inplace(pSrc,Rpo2); // 0 or 128
		return;
	}

	if ( Alpha > 128) Alpha -= 128;

	u32 Xalpha = ((u32)Alpha) + 32;
	if (Xalpha & 64)
	{
		s32 FactorX = (float)pSrc->SY * tanf((64.0f + (float)Alpha) * 2.0f * 3.1415927f / 256.0f );
		MAP8_BLUR8_CopyTranspose(pSrc,pTmp);
		MAP8_BLUR8_ScissorX_Inplace(pTmp,-FactorX);
		while (Iteration--) MAP8_BLUR8_V_Inplace(pTmp,Rpo2);
		MAP8_BLUR8_ScissorX_Inplace(pTmp,FactorX);
		MAP8_BLUR8_CopyTranspose(pTmp,pSrc);
	} else
	{
		s32 FactorX = (float)pSrc->SY * tanf((float)Alpha * 2.0f * 3.1415927f / 256.0f );
		MAP8_BLUR8_ScissorX_Inplace(pSrc,FactorX);
		while (Iteration--) MAP8_BLUR8_V_Inplace(pSrc,Rpo2);
		MAP8_BLUR8_ScissorX_Inplace(pSrc,-FactorX);
	}
}


void MAP8_BLUR8_Inplace(MAP8 *pSrc, u32 Rpo2 , u32 Iteration)
{
	SET_BORDER8(pSrc,1<<(Rpo2-1),0);
	while (Iteration--)
	{
		MAP8_BLUR8_V_Inplace(pSrc,Rpo2);
		MAP8_BLUR8_H_Inplace(pSrc,Rpo2);
	}
}


void MAP8_COMPUTEO2(MAP8 *pSrc,MAP8 *pDst)
{
	*pDst = *pSrc;
	pDst->MoveBase(pSrc->SY*pSrc->PITCH) ;
	pDst->SX>>=1;
	pDst->SY>>=1;
	pDst->PITCH>>=1;
	u32 LineCounter = pDst->SY & (~1);
	while (LineCounter--)
	{
		u8 *PixSrc,*PixLast,*PixDest;
		PixDest = pDst->GetBase()+LineCounter*pDst->PITCH;
		PixSrc = pSrc->GetBase()+(LineCounter*2)*pSrc->PITCH;
		PixLast = PixSrc+pSrc->SX;
		while (PixSrc<PixLast)
		{
			*PixDest = (u8)((((u32)*PixSrc) + ((u32)*(PixSrc+1)) + ((u32)*(PixSrc+pSrc->PITCH)) + ((u32)*(PixSrc+pSrc->PITCH+1)))>>2);
			PixSrc+=2;
			PixDest++;
		}

	}
}

void MAP_2_MAP8(/*IN*/MAP *pSrc,/*OUT*/MAP8 *pDst,/*IN*/u8 *pTempBuffer)
{
	if (pTempBuffer) pDst->SetBase(pTempBuffer);
	pDst->PITCH = pDst->SX = pSrc->SX;
	pDst->SY = pSrc->SY;
	s32 *PPs;
	u8 *PPd,*PPl,*PPll,*PPDeriv;
	/* 1 Compute 8b */
	PPs = pSrc->GetBase();
	PPd = pDst->GetBase();
	PPl = pDst->GetBase() + pDst->SX * pDst->SY;
	while (PPd < PPl)
	{
		PPll = PPd + pDst->SX;
		while (PPd < PPll)
		{
			*PPd = (u8)(*PPs>>8);
			PPd ++;
			PPs ++;
		}
		PPs += pSrc->PITCH - pSrc->SX;
	}
}

void MAP8_2_MAP(/*IN*/MAP8 *pSrc,MAP *pDst)
{
	u8	*PPs;
	s32 *PPd,*PPl,*PPll,*PPDeriv;
	/* 1 Compute 8b */
	PPs = pSrc->GetBase();
	PPd = pDst->GetBase();
	PPl = pDst->GetBase() + pDst->PITCH * pDst->SY;
	while (PPd < PPl)
	{
		PPll = PPd + pDst->SX;
		while (PPd < PPll)
		{
			*PPd = (s32)(*PPs);
			*PPd |= *PPd<<8;
			*PPd |= *PPd<<8;
			PPd ++;
			PPs ++;
		}
		PPs += pSrc->PITCH - pSrc->SX;
		PPd += pDst->PITCH - pDst->SX;
	}
}

void MAP8_2_MAP_DIFF(/*IN*/MAP8 *pSrc,MAP *pDst)
{
	u8	*PPs;
	s32 *PPd,*PPl,*PPll,*PPDeriv;
	/* 1 Compute 8b */
	PPs = pSrc->GetBase();
	PPd = pDst->GetBase();
	PPl = pDst->GetBase() + pDst->PITCH * pDst->SY;
	while (PPd < PPl)
	{
		PPll = PPd + pDst->SX;
		while (PPd < PPll)
		{
			s32 V;
			V = ((((*PPd>>8)&0xff) - (s32)(*PPs) )>>1) + 128;
			//if (V > 128) V ^= 255;
			*PPd = V;
			*PPd |= *PPd<<8;
			*PPd |= *PPd<<8;
			PPd ++;
			PPs ++;
		}
		PPs += pSrc->PITCH - pSrc->SX;
		PPd += pDst->PITCH - pDst->SX;
	}
}