///#include "stdafx.h"
#include "HeadTracker.h"
#include "POsit_3D.h"

u32 From_Scratch = 1;
float AverageE_2D = 0.0f;

#define ROTATION_FREEDOM 1
//#pragma optimize ("",off)


Vector TransformedPoints[MAX_POINTS];
Vector ProjectedPoints[MAX_POINTS];
Vector ScreenDirs[MAX_POINTS];

Vector Project(Vector &V, float focale)
{
	Vector Ret;
	float Ooz;
	Ooz = 1.0f / V.z;
	Ret.x = focale * V.x * Ooz;
	Ret.y = focale * V.y * Ooz;
	Ret.z = Ooz;
	//Ret = V;
	return Ret;
}


static void UpdateMatrix(MATRIX *MatrixDST, float CurrentFocale, Vector *DataPoints , Vector *ImagePoints , u32 ulNumberOfPoints )
{
	Vector *TranDest,*TranDestLast,*ProjDest;
	TranDest = TransformedPoints;
	ProjDest = ProjectedPoints;
	TranDestLast = TranDest+ulNumberOfPoints;
	while (TranDest < TranDestLast)
	{
		TranDest->z = -1.0f;
		if (*(u32 *)&ImagePoints->z != 0xC0DE2006)
		{
			*TranDest = *MatrixDST * *DataPoints;
			if (TranDest->z > 0.0f)
			{
				Vector Ret;
				float Ooz;
				Ooz = 1.0f / TranDest->z;
				ProjDest->x = CurrentFocale * TranDest->x * Ooz;
				ProjDest->y = CurrentFocale * TranDest->y * Ooz;
				ProjDest->z = Ooz;
			}
		}
		ImagePoints++;
		TranDest++;
		DataPoints++;
		ProjDest++;
	}
}


float Compute_Matrix_E(MATRIX *MatrixDST, float CurrentFocale, Vector *ImagePoints , Vector *DataPoints , u32 ulNumberOfPoints ,u32 LasKeyIndex)
{
	float fRealNumber = 0.0f;
	float Norm1 = 0.0f;
	for (u32 Counter2 = 0; Counter2 < ulNumberOfPoints ; Counter2 ++)
	{
		if (TransformedPoints[Counter2].z > 0.0f)
		{
			Vector Local;
			Local = ImagePoints[Counter2] - ProjectedPoints[Counter2];
			Norm1 += Local.NormXY();
			fRealNumber++;
		}
	};
	if (fRealNumber) Norm1 /= fRealNumber;
	return Norm1;
}

static float Compute_Matrix_T(MATRIX *MatrixDST, float CurrentFocale, Vector *ImagePoints , Vector *DataPoints , u32 ulNumberOfPoints ,u32 Mode3D)
{ 
	Vector Projected;
	Vector Centroide_DP,Centroide_IP;
	Vector Centroide_DPX,TRANSLATOR,T;
	float fRealNumber,Norm_DP,Norm_IP;
	float Oofoc = 1.0f/CurrentFocale;
	Centroide_DPX = Centroide_DP = Centroide_IP = Vector(0,0,0);
	fRealNumber = 0.0f;
	TRANSLATOR = Vector(0,0,0);
	for (u32 Counter2 = 0; Counter2 < ulNumberOfPoints ; Counter2 ++)
	{
		if (*(u32 *)&ImagePoints[Counter2].z != 0xC0DE2006)
		{
			T = *MatrixDST * DataPoints[Counter2];
			if (T.z > 0.0f)
			{
				TRANSLATOR += (ImagePoints[Counter2] * (T.z * Oofoc));
				TRANSLATOR -= T;
				fRealNumber ++;
			}
		}
	};
	if (fRealNumber)
	{
		Centroide_IP.z = Centroide_DP.z = 0.0f;
		fRealNumber = 1.0f/fRealNumber;
		TRANSLATOR *= fRealNumber ;
		TRANSLATOR.z = 0.0f;
		MatrixDST->T += TRANSLATOR;
		if (Mode3D == 1)
		{
			MatrixDST->K = MatrixDST->T ;
			MatrixDST->K.Normalize();
			MatrixDST->I = MatrixDST->J^MatrixDST->K;
			MatrixDST->I.Normalize();
			MatrixDST->J = MatrixDST->K^MatrixDST->I;
			MatrixDST->J.Normalize();
		}

		UpdateMatrix(MatrixDST, CurrentFocale, DataPoints , ImagePoints , ulNumberOfPoints );
		fRealNumber = 0.0f;
		for (u32 Counter2 = 0; Counter2 < ulNumberOfPoints ; Counter2 ++)
		{
			if (TransformedPoints[Counter2].z > 0.0f)
			{
				Centroide_DPX += TransformedPoints[Counter2];
				Centroide_DP += ProjectedPoints[Counter2];
				Centroide_IP += ImagePoints[Counter2];
				fRealNumber++;
			}
		};
		Centroide_IP.z = Centroide_DP.z = 0.0f;
		fRealNumber = 1.0f / fRealNumber;
		Centroide_IP *= fRealNumber;
		Centroide_DP *= fRealNumber;
		Centroide_DPX*= fRealNumber;
		Norm_DP = Norm_IP = 0.0f;
		for (u32 Counter2 = 0; Counter2 < ulNumberOfPoints ; Counter2 ++)
		{
			if (TransformedPoints[Counter2].z > 0.0f)
			{
				Norm_DP += (ProjectedPoints[Counter2] - Centroide_DP).NormXY();
				Norm_IP += (ImagePoints[Counter2] - Centroide_IP).NormXY();
			}
		};
		if (Norm_IP && Norm_DP)
		{
			MatrixDST->T -= Centroide_DPX;
			Centroide_DPX *= (Norm_DP / Norm_IP);
			MatrixDST->T += Centroide_DPX;//*/
			UpdateMatrix(MatrixDST, CurrentFocale, DataPoints , ImagePoints , ulNumberOfPoints );
		}
	}

	return Compute_Matrix_E(MatrixDST, CurrentFocale, ImagePoints , DataPoints , ulNumberOfPoints ,0);
}

static void Compute_Matrix(u32 Dim,MATRIX *MatrixDST, float CurrentFocale, Vector *ImagePoints , Vector *DataPoints , u32 ulNumberOfPoints , u32 LasKeyIndex,u32 Mode3D)
{
	Vector Local;
	Vector Projected;
	float Norm1,Alpha,Angle;
	MATRIX MatrixRes;

	MatrixRes = *MatrixDST;

	Norm1 = Compute_Matrix_T(&MatrixRes, CurrentFocale, ImagePoints , DataPoints , ulNumberOfPoints ,Dim);
	for (u32 Counter = 0; Counter < 1 ; Counter ++)
	{
		/* 2 : Rotation */
		u32 Zou;
#define MAX_CONVERG 7

		Alpha = 3.1415927f*2.0f ;Angle = 0;Zou=0;
		if (!From_Scratch)
		{
			while (Zou < MAX_CONVERG - 5) 
			{
				Alpha*=0.25f;
				Zou++;
			}
		}
		for  (; Zou < MAX_CONVERG ; Zou++,Alpha *= 0.25f)
		{
			float Norm[6],NormMin;
			MATRIX MatrixRot[6];
			u32 GoodI;
			NormMin = Norm1;
			for (int g = 0 ; g < 6; g ++)
			{
				MatrixRot[g] = MatrixRes;
				if (Dim == 1)
					MatrixRot[g].RotateAround(0,g & 1 ? + Alpha : - Alpha);
				else
					MatrixRot[g].RotateAround(g>>1,g & 1 ? + Alpha : - Alpha);
				Norm[g] = Compute_Matrix_T(&MatrixRot[g], CurrentFocale, ImagePoints , DataPoints , ulNumberOfPoints ,Dim);
				if (NormMin > Norm[g])
				{
					GoodI = g;
					NormMin = Norm[g];
				}
			}
			if (NormMin < Norm1)
			{
				Norm1 = NormMin;
				MatrixRes = MatrixRot[GoodI];
			}
		};
	};
	*MatrixDST = MatrixRes;
}
static void Estimate_Z(MATRIX *MatrixDST, float CurrentFocale, Vector *ImagePoints , Vector *DataPoints , Vector *CameraPos , u32 ulNumberOfPoints , u32 Reset)
{
	Vector TEST,Gerr,Projected,CameraInObjectSpace;
	MATRIX MatrixDST_I;
	if (Reset) return;
	MatrixDST->Inverse(MatrixDST_I);
	CameraInObjectSpace = Vector(0,0,0);
	CameraInObjectSpace = MatrixDST_I * CameraInObjectSpace;
	MatrixDST_I.T = Vector(0,0,0);
	{
		for (u32 Counter2 = 0; Counter2 < ulNumberOfPoints ; Counter2 ++)
		{
			if (*(u32 *)&ImagePoints[Counter2].z  != 0xC0DE2006)
			{
				Vector Transformed,Transformed2,Local2,Local;
				Vector V1,V2,V3,V4,V5;
				float NV1;
				V1 = DataPoints[Counter2] - CameraPos[Counter2];
				NV1 = V1.Norm();
				if (NV1 > 0.00001f)
				{
					float Angle;
					Transformed = ImagePoints[Counter2];
					Transformed.z = CurrentFocale;
					Transformed.Normalize();
					V2 = MatrixDST_I * Transformed;
					V1.Normalize();
					V2.Normalize();
					Angle = V1 * V2;
					if (Angle < 0.95f)
					{
						float Error;
						V3 = V1 ^ V2;
						V3.Normalize();
						V5 = CameraInObjectSpace - DataPoints[Counter2];
						Error = (V5 * V3) / NV1;
						Error *= Error;
						if (Error < 1.0f/ 100000.0f)
						{
							V4 = V2 ^ V3;
							V4.Normalize();
							V1 *= 1.0f / (V1 * V4);
							V1 *= (V5 * V4);
							DataPoints[Counter2] += V1;
							//CameraPos[Counter2] = DataPoints[Counter2];
						}
					}
				}
			}
		};
	};
}


static u32 Decimate(MATRIX *MatrixDST, float CurrentFocale, Vector *ImagePoints , Vector *DataPoints , u32 ulNumberOfPoints , u32 SizeX )
{
	Vector Error,Gerr,Projected,L;
	float AverageE;
	u32 ReturnValue;
	AverageE = 0;
	ReturnValue = 0;

	for (u32 Counter2 = 0; Counter2 < ulNumberOfPoints ; Counter2 ++)
	{
		if (*(u32 *)&ImagePoints[Counter2].z != 0xC0DE2006)
		{
			L = *MatrixDST * DataPoints[Counter2];
			if (L .z > 0.0f)
			{
				ReturnValue++;
				Projected = Project(L, CurrentFocale);
				Gerr = ImagePoints[Counter2] - Projected;
				AverageE += (Gerr.x * Gerr.x  + Gerr.y * Gerr.y);
			}
		}
	};
	if (ReturnValue)
	{
		AverageE *= ((float)1.0f / (float)ReturnValue);
#define E_MIN  (12.0f / 320.0f)
#define E_MAX  (25.0f / 320.0f)
		if (AverageE < E_MIN* E_MIN)  AverageE = E_MIN* E_MIN;//*/
		//if (AverageE > E_MAX* E_MAX)  AverageE = E_MAX* E_MAX;//*/
		ReturnValue = 0;
		for (u32 Counter2 = 0; Counter2 < ulNumberOfPoints ; Counter2 ++)
		{
			float LocalEr;
			L = *MatrixDST * DataPoints[Counter2];
			if (L.z > 0.0f)
			{
				Projected = Project(L, CurrentFocale);
				Error = ImagePoints[Counter2] - Projected;
				LocalEr = (Error.x * Error.x  + Error.y * Error.y);
				if (LocalEr > AverageE)
				{
					*(u32 *)&ImagePoints[Counter2].z = 0xC0DE2006;
				}  else 
				{
					ImagePoints[Counter2].z = 1.0f;
					ReturnValue++;
				}
			}
			else 
				*(u32 *)&ImagePoints[Counter2].z = 0xC0DE2006;
		};
	};
	return ReturnValue;
}


u32 POsit(u32 _2D_3D, MATRIX *MatrixSrc, float CurrentFocale, Vector *ImagePoints , Vector *DataPoints , Vector *CameraPos , u32 ulNumberOfPoints , u32 LasKeyIndex , u32 SizeX, u32 SizeY)
{
	u32 Ret;
	MATRIX MatrixSrc2;
	static u32 Countttt;
	MatrixSrc2 = *MatrixSrc;

	Countttt = ulNumberOfPoints;
	while (Countttt--) ImagePoints[Countttt].z = 1.0f;
	SizeX>>=1;
	u32 Conv = 3;
	//From_Scratch = 1;
	if (From_Scratch)
	{
		MatrixSrc->Identity();
		if (_2D_3D == 1)
			MatrixSrc->T.z = 50.0f;
		else
			MatrixSrc->T.z = 50.0f;
		MatrixSrc2 = *MatrixSrc;
		Conv = 10;
		Compute_Matrix(_2D_3D,MatrixSrc, CurrentFocale, (Vector *)ImagePoints , (Vector *)DataPoints , ulNumberOfPoints ,LasKeyIndex, 1);
	}

	Compute_Matrix_T(MatrixSrc, CurrentFocale, (Vector *)ImagePoints , (Vector *)DataPoints , ulNumberOfPoints ,_2D_3D);

	while (Conv--)
	{
		Ret = Decimate(MatrixSrc, CurrentFocale, (Vector *)ImagePoints , (Vector *)DataPoints , ulNumberOfPoints,SizeX);
		if (!From_Scratch) *MatrixSrc = MatrixSrc2;
		Compute_Matrix(_2D_3D,MatrixSrc, CurrentFocale, (Vector *)ImagePoints , (Vector *)DataPoints , ulNumberOfPoints ,LasKeyIndex, 1);
	};

	Ret = Decimate(MatrixSrc, CurrentFocale, (Vector *)ImagePoints , (Vector *)DataPoints , ulNumberOfPoints,SizeX);
	MatrixSrc->I.Normalize();
	MatrixSrc->J.Normalize();
	MatrixSrc->K.Normalize();


	if (Ret < 8) 
		From_Scratch = 1;
	else 
	{
		From_Scratch = 0;
		if (Ret > ulNumberOfPoints - (ulNumberOfPoints >> 3))
		{
			if (CameraPos)
				Estimate_Z(MatrixSrc, CurrentFocale, (Vector *)ImagePoints , (Vector *)DataPoints , (Vector *)CameraPos , ulNumberOfPoints ,0);
		}
	}
	return Ret;
}
 

u32 POsit_3D(MATRIX *MatrixSrc, float CurrentFocale, Vector *ImagePoints , Vector *DataPoints , Vector *CameraPos , u32 ulNumberOfPoints , u32 LasKeyIndex , u32 SizeX, u32 SizeY)
{
	return POsit(3,MatrixSrc,CurrentFocale,ImagePoints , DataPoints , CameraPos , ulNumberOfPoints , LasKeyIndex , SizeX, SizeY);

}
 
u32 POsit_2D(MATRIX *MatrixSrc, float CurrentFocale, Vector *ImagePoints , Vector *DataPoints , Vector *CameraPos , u32 ulNumberOfPoints , u32 LasKeyIndex , u32 SizeX, u32 SizeY)
{
	return POsit(1,MatrixSrc,CurrentFocale,ImagePoints , DataPoints , CameraPos , ulNumberOfPoints , LasKeyIndex , SizeX, SizeY);
}