#if 1
#include <stdio.h>

#include "math.h"
#include "memory.h"
#include "CCMath.h"
#include "CCmap.h"


//#pragma optimize("",off)
#define PLANE_TileSize 18
#define PRIMESENSFOV 58.512135f
#define PRIMESENSDISTORTION_Y 1.0f
#define EcartType_Tolerance 2.5f 
#define DY_Z_Tolerance -1.0f
#define XOZ 655360.0f

#define Focale (1.f / tanf(PRIMESENSFOV / 2.0f * 3.1415927f / 180.0f))

//#define DRAWGREENTILES
#define DRAWGROUND

Vector	PRIMESENS_GroundZ;
float	PRIMESENS_GroundDistance = 0.0f;

double MEGADX = 0;
double MEGADY = 0;
Vector MEGADZ = Vector(0,0,0);


u32 IsNan(float F);
void MAP_PPRIMESENS_ReLightWithZ(u16 *ZOoMap , MAP *pColorMap , MAP *pShadowMAP , Vector *GCenter , float DX, float DY);

void PRIMESENS_3D_to_2D_Coordinate(MAP *ZMap, Vector *p3D , Vector *p2D)
{
	p2D->z = p3D->z;
	p2D->x = ((float)ZMap->SX/2.0f) + p3D->x * (((float)ZMap->SX/2.0f) * Focale) / p3D->z ;
	p2D->y = ((float)ZMap->SY/2.0f) + p3D->y * (((float)ZMap->SX/2.0f) * Focale) / p3D->z ;
}
void PRIMESENS_2D_to_3D_Coordinate(MAP *ZMap, Vector *p2D , Vector *p3D)
{
	p3D->z = p2D->z;
	p3D->x = (p2D->x - ((float)ZMap->SX/2.0f)) * p3D->z / (((float)ZMap->SX/2.0f) * Focale);
	p3D->y = (p2D->y - ((float)ZMap->SY/2.0f)) * p3D->z / (((float)ZMap->SX/2.0f) * Focale);
}


float ComputeDXDYAndEcarType(MAP *ZMap,int X,int Y, float &DX, float &DY , Vector &GCenter , float *Ponderation /* PLANE_TileSize * PLANE_TileSize */, float Threshold,u32 &Counter)
{
	float Pond= 0;
	s32 * pBASE = ZMap->GetBase();
	/* 1 Comppute DX - DY */
	Counter = 0;
	GCenter = Vector(0,0,0);

	for (int XT = 0 ; XT < PLANE_TileSize ; XT++ )
	{
		for (int YT = 0 ; YT < PLANE_TileSize ; YT++ )
		{
			s32 *pBase = pBASE + X + XT + (Y + YT) * ZMap ->PITCH;
			if (*(pBase))
			{
				GCenter += Vector(XT , YT , *(pBase)) * Ponderation[XT + YT * PLANE_TileSize] ;
				Pond += Ponderation[XT + YT * PLANE_TileSize];
				if (Ponderation[XT + YT * PLANE_TileSize])
					Counter++;
			}
		}
	}
	if (Pond )
	{

		GCenter *= 1.0f / (float)Pond;
		DX = DY = 0.0f;
		Pond = 0.0f;
		for (int XT = 0 ; XT < PLANE_TileSize ; XT++ )
		{
			for (int YT = 0 ; YT < PLANE_TileSize ; YT++ )
			{
				s32 *pBase = pBASE + X + XT + (Y + YT) * ZMap ->PITCH;
				if (*(pBase))
				{
					float Factor,Distance;
					Distance = ((float)XT - GCenter.x) * ((float)XT - GCenter.x) + ((float)YT - GCenter.y) * ((float)YT - GCenter.y);
					if (Distance)
					{
						//Distance = sqrt(Distance) ;
						DX += ((float)XT - GCenter.x) * ((float)*(pBase) - GCenter.z) * Ponderation[XT + YT * PLANE_TileSize];
						DY += ((float)YT - GCenter.y) * ((float)*(pBase) - GCenter.z) * Ponderation[XT + YT * PLANE_TileSize];
						Pond += Distance * Ponderation[XT + YT * PLANE_TileSize];//= abs(((float)*(pBase) - GCenter.z));
					}
				}
			}
		}
		DX /= 0.5f * (float)Pond;
		DY /= 0.5f * (float)Pond;//*/
		Pond = 0;
		float SqrValue = 0.0f , Value = 0.0f;
		for (int XT = 0 ; XT < PLANE_TileSize ; XT++ )
			for (int YT = 0 ; YT < PLANE_TileSize ; YT++ )
			{
				s32 *pBase = pBASE + X + XT + (Y + YT) * ZMap ->PITCH;
				if (*(pBase))
				{
					float Distance = *(pBase) - ((((float)XT - GCenter.x) * DX + ((float)YT - GCenter.y) * DY + GCenter.z));
					SqrValue += Distance * Distance  * Ponderation[XT + YT * PLANE_TileSize];
					Value += Distance  * Ponderation[XT + YT * PLANE_TileSize];
					Pond+=Ponderation[XT + YT * PLANE_TileSize];
					if (Distance > Threshold)
						Ponderation[XT + YT * PLANE_TileSize] = 0;
					else
						Ponderation[XT + YT * PLANE_TileSize] = 1;
				}
			}
		SqrValue /= Pond;
		Value /= Pond;
		return sqrtf(SqrValue - Value * Value);;
	} else return 10000000.0f;
}
u16 ZMAXMAP[640*480];
void MAP_ComputePlanes(MAP *ZMap,u16 *PrimsensZMapSave)
{
	static u32 BasicTimeStamp = 0;
	u32 TIMESTAMPAND = (BasicTimeStamp < 16) ? 15 : (BasicTimeStamp < 64) ? 63 : (BasicTimeStamp < 1000) ? 127 : 511;
	/*	for (int X = 0 ; X < ZMap->SX - PLANE_TileSize; X+=PLANE_TileSize )
	for (int Y = 0 ; Y < ZMap->SY - PLANE_TileSize ; Y+=PLANE_TileSize )*/
	if ((BasicTimeStamp & TIMESTAMPAND) == 0) memset(ZMAXMAP,0,sizeof(ZMAXMAP));
	u32 CCounter = ZMap->SX * ZMap->SY;
	u16 *PrimsensZMap = PrimsensZMapSave;
	u16 *PrimsensZMapDst = ZMAXMAP;
	while (CCounter--)
	{
		if (*PrimsensZMap && (*PrimsensZMap > *PrimsensZMapDst))
			*PrimsensZMapDst = *PrimsensZMap;
		PrimsensZMap++;
		PrimsensZMapDst++;
	}
	BasicTimeStamp++;
	if ((BasicTimeStamp & TIMESTAMPAND) == TIMESTAMPAND) return;
	PrimsensZMap = ZMAXMAP;
	float Ponderation[PLANE_TileSize * PLANE_TileSize];
	Vector GroundVectorStack[8192];
	Vector GroundVectorStackDXDY[8192];
	u32 ulNumberOfGVS = 0;
	s32 * pBASE = ZMap->GetBase();
	{
		s32 *pBase;
		pBase = pBASE + ZMap->SX * ZMap->SY;
		for (s32 Y = ZMap->SY-1 ;Y>=0 ; Y--)
			for (s32 X = ZMap->SX-1 ;X>=0 ; X--)
			{
				pBase--;
				if (*PrimsensZMap)
					*(pBase) = XOZ / (s32)*PrimsensZMap;
				else
					*(pBase) = 0;
				PrimsensZMap++;
			}
		PrimsensZMap -= ZMap->SX * ZMap->SY;
	}

	for (int X = 0 ; X < ZMap->SX - PLANE_TileSize; X+=PLANE_TileSize )
		for (int Y = 0 ; Y < ZMap->SY - PLANE_TileSize ; Y+=PLANE_TileSize)
		{
			float EcartType = 10000000000.0;
	
			float DX = 0 , DY = 0 ;
			u32 Counter = PLANE_TileSize * PLANE_TileSize;
			while (Counter--) Ponderation[Counter] = 1.f;

			Vector GCenter = Vector(0,0,0);
			for (int ITER = 0 ; ITER < 1 ; ITER++)
			{
				EcartType = ComputeDXDYAndEcarType(ZMap,X,Y, DX, DY , GCenter ,Ponderation,EcartType * 2 + EcartType_Tolerance,Counter);
			}
			if ((EcartType < EcartType_Tolerance) && (DY < DY_Z_Tolerance) && (Counter > (PLANE_TileSize * PLANE_TileSize)>>3) )
			{
				GroundVectorStackDXDY[ulNumberOfGVS].x = DX;
				GroundVectorStackDXDY[ulNumberOfGVS].y = DY;
				GroundVectorStack[ulNumberOfGVS++] = Vector (GCenter.x + X ,GCenter.y + Y,GCenter.z );
#ifdef DRAWGREENTILES
				for (int XT = 0 ; XT < PLANE_TileSize ; XT++ )
					for (int YT = 0 ; YT < PLANE_TileSize ; YT++ )
					{
						s32 *pBase = pBASE + X + XT + (Y + YT) * ZMap ->PITCH;
						//if (*(pBase))
						{
							*(pBase) = 0xff00ff00;
						}
					}
#endif
			}
		}

	if (ulNumberOfGVS < 3) 
	{
		//MAP_PPRIMESENS_ReLightWithZ(PrimsensZMap, ZMap, NULL, &MEGADZ, MEGADX , MEGADY);
		return;
	}
	/* Then treat GVS to Extract Ground Matrix */
	Vector GravityCenter = Vector (0,0,0);
	static float LastAverage = 10000.0f;
	float NewAverage = 0.0f;
	float DX = 0 , DY = 0 ;
	{
		float Pond;
		Pond = 0.0f;
		for (int K = 0 ; K < ulNumberOfGVS ; K ++)
		{
			float Factor = (sqrt(((MEGADX - GroundVectorStackDXDY[K].x)*(MEGADX - GroundVectorStackDXDY[K].x)
				+ (MEGADY - GroundVectorStackDXDY[K].y)*(MEGADY - GroundVectorStackDXDY[K].y)) + 0.1f));
			NewAverage += Factor;
/*			if (Factor < LastAverage * 2) 
				Factor = LastAverage * 2;*/
			Factor = 1.0f / Factor; 
/*			float Distance = *(pBase) - ((((float)X - GravityCenter.x) * DX + ((float)Y - GravityCenter.y) * DY + GravityCenter.z));
					if (fabs(Distance) < EcartType_Tolerance  * 4.0f) NewNumber++;*/

			GravityCenter += GroundVectorStack[K] * Factor;
			DX += Factor * GroundVectorStackDXDY[K].x;
			DY += Factor * GroundVectorStackDXDY[K].y;
			Pond+=Factor;//*/
/*
			float Factor,Distance;
			Distance =  (GroundVectorStack[K].x - GravityCenter.x) * (GroundVectorStack[K].x - GravityCenter.x) + 
						(GroundVectorStack[K].y - GravityCenter.y) * (GroundVectorStack[K].y - GravityCenter.y);
			if (Distance)
			{
				Distance = 1.0f/ (Distance) ;
				DX += Distance*GroundVectorStackDXDY[ulNumberOfGVS].x;
				DY += Distance*GroundVectorStackDXDY[ulNumberOfGVS].y;
				Pond += Distance;
			}*/
		}
		
		LastAverage = NewAverage / (float)ulNumberOfGVS;
		GravityCenter *= 1.0f / Pond;
			DX /= Pond;
			DY /= Pond;//*/

		u32 LastNumber = 0, NewNumber = 0;

		float NewDX = DX,NewDY = DY;
		Vector NewGravity;
		NewGravity = GravityCenter;
		for (int X = 0 ; X < ZMap->SX; X++)
			for (int Y = 0 ; Y < ZMap->SY ; Y++)
			{
				s32 *pBase = pBASE + X + (Y ) * ZMap ->PITCH;
				if (*pBase)
				{
					float Distance = *(pBase) - ((((float)X - GravityCenter.x) * DX + ((float)Y - GravityCenter.y) * DY + GravityCenter.z));
					if (fabs(Distance) < EcartType_Tolerance * 2.0f) LastNumber++;
				}
			}

		if (Pond && !IsNan(DX) && !IsNan(DY))
		{
			if (abs(MEGADX) > 1000000.0f) MEGADX = DX;
			if (abs(MEGADY) > 1000000.0f) MEGADY = DY;
			if ((BasicTimeStamp & TIMESTAMPAND) > TIMESTAMPAND - (TIMESTAMPAND>>2))
			{
				MEGADX = DX;
				MEGADY = DY;
				MEGADZ = GravityCenter;
			}
			DX = MEGADX;
			DY = MEGADY;
			GravityCenter = MEGADZ;
		}//*/

		for (int X = 0 ; X < ZMap->SX; X++)
			for (int Y = 0 ; Y < ZMap->SY ; Y++)
			{
				s32 *pBase = pBASE + X + (Y ) * ZMap ->PITCH;
				if (*pBase)
				{
					float Distance = *(pBase) - ((((float)X - GravityCenter.x) * DX + ((float)Y - GravityCenter.y) * DY + GravityCenter.z));
					if (fabs(Distance) < EcartType_Tolerance  * 4.0f) NewNumber++;
				}
			}

		if (NewNumber << 3 < LastNumber )
		{
			MEGADX = DX = NewDX;
			MEGADY = DY = NewDY;
			MEGADZ = GravityCenter = NewGravity;
		}
#ifdef DRAWGROUND
		float DX2 = DX,DY2 = DY;
		DX2 *= 10.0f;
		DY2 *= 10.0f;
		DX2 += 128.0f;
		DY2 += 128.0f;
		if(DX2 > 255) DX2 = 255;
		if(DY2 > 255) DY2 = 255;
		if(DX2 < 0) DX2 = 0;
		if(DY2 < 0) DY2 = 0;
		u32 Color = DX2 + ((u32)DY2<<8);

		PrimsensZMapSave = (u16*)ZMAXMAP;
		s32 * pBASE = ZMap->GetBase();
		{
			s32 *pBase;
			pBase = pBASE + ZMap->SX * ZMap->SY;
			for (s32 Y = ZMap->SY-1 ;Y>=0 ; Y--)
				for (s32 X = ZMap->SX-1 ;X>=0 ; X--)
				{
					pBase--;
					if ((*pBase & 0xff000000) == 0)
					{
					if (*PrimsensZMapSave)
						*(pBase) = XOZ / (s32)*PrimsensZMapSave;
					else
						*(pBase) = 0;
					}
					PrimsensZMapSave++;
				}
			PrimsensZMapSave -= ZMap->SX * ZMap->SY;
		}
		//PRIMESENS_GroundDistance = XOZ / pBASE[(ZMap->SX>>1) + ZMap->PITCH * (ZMap->SY>>1)];
		for (int X = 0 ; X < ZMap->SX; X++)
			for (int Y = 0 ; Y < ZMap->SY-11 ; Y++)
			{
				float TheoricalD;
				s32 *pBase = pBASE + X + (Y+10)* ZMap ->PITCH;
				float TheoricalD2 = ((((float)X - GravityCenter.x) * DX + ((float)(Y+10) - GravityCenter.y) * DY + GravityCenter.z));

				pBase = pBASE + X + Y* ZMap ->PITCH;
				TheoricalD = ((((float)X - GravityCenter.x) * DX + ((float)Y - GravityCenter.y) * DY + GravityCenter.z));
				//void PRIMESENS_2D_to_3D_Coordinate(MAP *ZMap, Vector *p2D , Vector *p3D)
/*					if ((TheoricalD > 0) && (TheoricalD2<0))//fabs(Distance) < EcartType_Tolerance * 2)
					{
						*pBase = 0xffffffff;
					} else
						*pBase = 0;*/

				if (*pBase)
				{
					float Distance = *(pBase) - TheoricalD;
					if (TheoricalD > 0)//fabs(Distance) < EcartType_Tolerance * 2)
					{
						*pBase = Color;
						*pBase = 0xffffffff;
					} else
						if ((*pBase & 0xff000000) == 0)
					{
						u8 *pImage = (u8 *)pBase;
						s32 Value = 2024*fabs(Distance) / *pBase;
						pImage[0] = (Value>>0) & 0xff;
						if ((Value >>0) & 256) pImage[0] ^= 0xff;
						pImage[1] = (Value>>2) & 0xff;
						if ((Value >>2) & 256) pImage[1] ^= 0xff;
						pImage[2] = (Value>>4) & 0xff;
						if ((Value >>3) & 256) pImage[2] ^= 0xff;
						if (Value & 0x100)
							pImage[2] ^= 0xff;
					}
				}
			}
		u8 TEXT[512];
		sprintf((char*)TEXT,"Camera Z = %.2f m ",PRIMESENS_GroundDistance/1000);
		DRawTEXT(TEXT,(u32*)ZMap->GetBase(),10,10,ZMap->PITCH,ZMap->SY,0xff00ffff);
		//pBASE[(ZMap->SX>>1) + ZMap->PITCH * (ZMap->SY>>1)]= 0xff;
		
#endif
	}

	/* Compute RealZ of Plane */
	{
		Vector Point4[4];
		/*
			2 3
			0 1

			XE = M->SX/2 + M->SX/2 * Focale * XW / ZW
			XW = (XE - M->SX/2) * ZW / (M->SX/2 * Focale)
		*/
		DY *= PRIMESENSDISTORTION_Y;
		GravityCenter.y *= PRIMESENSDISTORTION_Y;

		float FX,FY,CosZ,Z;
		Vector GravityCenterW;
		
		Point4[0].z = (float)XOZ / GravityCenter.z;
		Point4[0].x = (GravityCenter.x - ((float)ZMap->SX/2.0f)) * Point4[0].z / (((float)ZMap->SX/2.0f) * Focale);
		Point4[0].y = (GravityCenter.y - ((float)ZMap->SY/2.0f)) * Point4[0].z / (((float)ZMap->SX/2.0f) * Focale);

		GravityCenterW = GravityCenter;
		GravityCenterW.x += DX / 10.0f;
		GravityCenterW.y += DY / 10.0f;
		GravityCenterW.z = (((GravityCenterW.x - GravityCenter.x) * DX + (GravityCenterW.y - GravityCenter.y) * DY + GravityCenter.z));
		Point4[1].z = (float)XOZ / GravityCenterW.z;
		Point4[1].x = (GravityCenterW.x - ((float)ZMap->SX/2.0f)) * Point4[1].z / (((float)ZMap->SX/2.0f) * Focale);
		Point4[1].y = (GravityCenterW.y - ((float)ZMap->SY/2.0f)) * Point4[1].z / (((float)ZMap->SX/2.0f) * Focale);
		
		GravityCenterW = GravityCenter;
		GravityCenterW.x += DY / 10.0f;
		GravityCenterW.y -= DX / 10.0f;
		GravityCenterW.z = (((GravityCenterW.x - GravityCenter.x) * DX + (GravityCenterW.y - GravityCenter.y) * DY + GravityCenter.z));
		Point4[2].z = (float)XOZ / GravityCenterW.z;
		Point4[2].x = (GravityCenterW.x - ((float)ZMap->SX/2.0f)) * Point4[2].z / (((float)ZMap->SX/2.0f) * Focale);
		Point4[2].y = (GravityCenterW.y - ((float)ZMap->SY/2.0f)) * Point4[2].z / (((float)ZMap->SX/2.0f) * Focale);

		//CosZ = 

		Vector AXIS1 = Point4[2] - Point4[0];
		Vector AXIS2 = Point4[1] - Point4[0];

		PRIMESENS_GroundZ = AXIS1 ^ AXIS2;
		PRIMESENS_GroundZ = PRIMESENS_GroundZ * -1.0;
		//PRIMESENS_GroundZ.z /= 2.0f;
		PRIMESENS_GroundZ.Normalize();

		static float Eval1 = 100.0f;
		PRIMESENS_GroundDistance = Point4[0] * PRIMESENS_GroundZ;
		if (PRIMESENS_GroundDistance < 0) PRIMESENS_GroundDistance = -PRIMESENS_GroundDistance;
	}

	//MAP_PPRIMESENS_ReLightWithZ(PrimsensZMap, ZMap, NULL, &GravityCenter, DX , DY);
}

 
#endif