#include "SCENE.h"
//#pragma optimize("",off)

MAP		MyMap;
MAP16	MyMap16;
#define XOZ 655360.0f

				static double OoZFactor = 1000000.0f ;

void MAP_ValidMyMap(MAP *pColorMap)
{
	MyMap.SetSize(pColorMap->SX, pColorMap->SY);
	MyMap16.SetSize(pColorMap->SX, pColorMap->SY);
}
void MAP_PPRIMESENS_ComputeMatting(MAP *pColorMap )
{
	MAP_ValidMyMap(pColorMap);
	s32 *pColor,*pColor2;
	s16 *MMPtr;
	MMPtr = MyMap16.GetBase();
	pColor = pColorMap->GetBase();
	pColor2 = MyMap.GetBase();
	for (int Y = 0 ; Y < pColorMap->SY ; Y++)
		for (int X = 0 ; X < pColorMap->SX ; X++)
		{
			if (*pColor & 1<<24)
				*MMPtr = 0;
			else
				*MMPtr = 0xff;
			MMPtr++;
			pColor++;
		}
	MAP16_BLUR_Inplace(&MyMap16,3,1);

	MMPtr = MyMap16.GetBase();
	pColor = pColorMap->GetBase();
	pColor2 = MyMap.GetBase();

	for (int Y = 0 ; Y < pColorMap->SY ; Y++)
		for (int X = 0 ; X < pColorMap->SX ; X++)
		{
			//if (*pColor & 2<<24) *MMPtr = 255;
			//if (*pColor & 1<<24) *MMPtr = 0;
			
			if (*MMPtr < 16)
				*MMPtr = 0xff;
			else
			if (*MMPtr < 256-4) 
			{
				float R,G,B,D;
				R = (*pColor & 0xff) - (*pColor2 & 0xff);
				G = ((*pColor>>8) & 0xff) - ((*pColor2>>8) & 0xff);
				B = ((*pColor>>16) & 0xff) - ((*pColor2>>16) & 0xff);
				D = R*R + G*G + B*B;
				if (D > 1024)
					*MMPtr = 0xff;
				else
					*MMPtr = 0;
			}
			else
			{
				//if (*MMPtr == 255)
				//if (!(*pColor & 2<<24))
					*pColor2 = *pColor;
				*MMPtr = 0;
			}
			MMPtr++;
			pColor++;
			pColor2++;
		}

	//MAP16_BLUR_Inplace(&MyMap16,1,2);
	MAP16_2_MAP(&MyMap16,pColorMap,24);

}


void MAP_PPRIMESENS_ComputeNormales(u16 *ZOoMap , MAP *pColorMap )
{
	s32 *pColor = pColorMap->GetBase();
	ZOoMap += pColorMap->SX * pColorMap->SY;
	for (int Y = 0 ; Y < pColorMap->SY ; Y++)
		for (int X = 0 ; X < pColorMap->SX ; X++)
		{
			
			ZOoMap--;

			//*pColor = 0;
			float R,G,B;
			static float OoZfactorDim = 1.001;
			float DX1,DX2;
			float Z1,Z2,Z3,Z4;
			static u32 Deltr = 4;
			static float Errormetrometer = 0.00001f;
			Z1 = (*(ZOoMap )+Errormetrometer);
			Z2 = (*(ZOoMap +Deltr)+Errormetrometer);
			Z3 = (*(ZOoMap -Deltr)+Errormetrometer);

			DX1 = (Z1 - Z3);
			DX2 = (Z2 - Z1);
			if (fabs(DX1) < fabs(DX2)) DX1 = DX2;
			R = OoZfactorDim * DX1;

			Z2 = (*(ZOoMap +Deltr*pColorMap->PITCH)+Errormetrometer);
			Z3 = (*(ZOoMap -Deltr*pColorMap->PITCH)+Errormetrometer);

			DX1 = (Z1 - Z3);
			DX2 = (Z2 - Z1);
			if (fabs(DX1) > fabs(DX2)) DX1 = DX2;

			G = OoZfactorDim * DX1;
			B = 0.0f;

			B += 128.0f;
			R += 128.0f;
			G += 128.0f;
			if (R > 255.0f) R = 255.0f;
			if (G > 255.0f) G = 255.0f;
			if (B > 255.0f) B = 255.0f;
			if (R < 0.0f) R = 0.0f;
			if (G < 0.0f) G = 0.0f;
			if (B < 0.0f) B = 0.0f;


			((u8*)pColor)[0] = R;
			((u8*)pColor)[1] = G;
			((u8*)pColor)[2] = B;
			((u8*)pColor)[3] = 255;
			
			pColor++;
		}
}



void MAP_PPRIMESENS_ReLightWithZ(u16 *ZOoMap , MAP *pColorMap , MAP *pShadowMAP , Vector *GCenter , float DX, float DY)
{
//	MAP_PPRIMESENS_ComputeNormales(ZOoMap , pColorMap );
	//return;
	MAP_ValidMyMap(pColorMap);

	s32 *pColor = pColorMap->GetBase();
	ZOoMap += pColorMap->SX * pColorMap->SY;
	s32 *LocalOoZMap = MyMap.GetBase();
	for (int Y = 0 ; Y < pColorMap->SY ; Y++)
	for (int X = 0 ; X < pColorMap->SX ; X++)
	{
		float Z;
		ZOoMap--;

		*pColor = 0;
/*		if (!*ZOoMap)
		{
			Z = ((((float)X - GCenter->x) * DX + ((float)Y - GCenter->y) * DY + GCenter->z));
			Z = XOZ / Z;
			if (Z > 0) *ZOoMap = Z;
		}*/
		if (*ZOoMap)
		{
			Z = 1024.0f * 4.0f * 65536.0f / (float)*ZOoMap ;
			*pColor = *LocalOoZMap = (s32)Z;
		}
		
		LocalOoZMap++;
		pColor++;
	}
	pColor -= pColorMap->SX * pColorMap->SY;
	ZOoMap += pColorMap->SX * pColorMap->SY;

	/* */

#define BOXBLUR 8
	for (int Y = BOXBLUR ; Y < pColorMap->SY - BOXBLUR; Y++)
		for (int X = BOXBLUR ; X < pColorMap->SX - BOXBLUR; X++)
		{
			s32 *pBase;
			pBase = MyMap.GetBase() + X + Y * MyMap.PITCH;
			if (*pBase)
			{
				double lDX,lDY;
				lDX = (*(pBase + 1) - *(pBase - 1))>>1;
				lDY = (*(pBase + MyMap.PITCH) - *(pBase - MyMap.PITCH))>>1;
				double Value = 0,Pond = 0;
				Value += (double)*pBase * (float)(BOXBLUR * BOXBLUR) *1.0f;
				Pond +=(BOXBLUR * BOXBLUR) *1.f;

				for (int YP = Y-BOXBLUR ; YP < Y + BOXBLUR ; YP+=2)
					for (int XP = X-BOXBLUR ; XP < X + BOXBLUR ; XP+=2)
					{
						s32 *pBase2;
						float Radius = (X-XP) * (X-XP) + (Y-YP) * (Y-YP);
						if (Radius < BOXBLUR * BOXBLUR)
						{
						pBase2 = MyMap.GetBase() + XP + YP * MyMap.PITCH;
static double TTTTTSSSSFCQSDSD = 0.200;
						double THRESH = TTTTTSSSSFCQSDSD * (double)*pBase;
						if (*pBase2 )
						{
							Radius = 1.0f - sqrt(Radius+0.001) / (float)(BOXBLUR);
							double EstimatedZ =  ((double)(XP - X) * -lDX + (double)(YP - Y) * -lDY + *pBase2);
							if (EstimatedZ > *pBase + 1000)
							{
/*							if ((abs(*pBase2 - *pBase)<TTTTTSSSSFCQSDSD))
							{
								Value += *pBase2;
								Pond ++;
							} else*/
								
								if ((abs(EstimatedZ - *pBase)<THRESH))
								{
									double Factor = 1.0f - (abs(EstimatedZ - *pBase))/THRESH;
									Factor *= Radius ;
									Value += (double)EstimatedZ * Factor;
									Pond += Factor;
								} 
							}
						}
						}
						
						
					}
				
				if (Pond) 
				{
					Value/=Pond;
					*pBase = (s32)(Value);
				}
			}
		}
	
	//MAP16_BLUR_Inplace(&MyMap , 5  , 1);


	LocalOoZMap = MyMap.GetBase();
	pColor = pColorMap->GetBase();
	for (int Y = 0 ; Y < pColorMap->SY ; Y++)
		for (int X = 0 ; X < pColorMap->SX ; X++)
		{
			if (*pColor) 
			{
				double ZFac = OoZFactor / (float)*pColor;
				*pColor -= (s32)*LocalOoZMap;
				*pColor *= ZFac;
				*pColor |= 1;
			}
			LocalOoZMap++;
			pColor++;
		}//*/
	LocalOoZMap = MyMap.GetBase();
	pColor = pColorMap->GetBase();
	for (int Y = 0 ; Y < pColorMap->SY -1; Y++)
	for (int X = 0 ; X < pColorMap->SX ; X++)
	{
		float Z;

		Z = 0;
		ZOoMap--;

		if (*pColor)
		{
			Z = *pColor;
#define ZThresh  16.0*32.0f
/*			if (Z > ZThresh) 
			{
				Z = ZThresh - Z;
				if (Z < 0) Z = 0;
			}
			if (Z < -ZThresh) 
			{
				Z = -Z;
				Z = ZThresh - Z;
				if (Z < 0) Z = 0;
				Z = -Z;
			}*/
			//if (Z>0) Z = 0.0f;
			Z /= 128.0f;
		}
			Z += 128.0f;
			if (Z > 255.0f) Z = 255.0f;
			if (Z < 0.0f) Z = 0.0f;

			*pColor = (s32)Z;
			*pColor |= *pColor<<8;
			*pColor |= *pColor<<8;
			*pColor |= 0xff000000;

		LocalOoZMap++;
		pColor++;
	}

}