#include "ShotTracker.h"
#include "math.h"
#include "string.h"
#include "stdio.h"
#include "stdlib.h"
#include "memory.h"


void SHOTS_CLASS::InitcosTable()
{
	if (IsCosInit) return;
	IsCosInit = 1;
	for (int i = 0 ; i < FAST_COS_DISCREET+1 ; i ++)
	{
		FASTCOS[i] = cosf((float)i * 3.1415927f * 2.0f / (float)FAST_COS_DISCREET);
		FASTSIN[i] = sinf((float)i * 3.1415927f * 2.0f / (float)FAST_COS_DISCREET);
	}

}


Vector SHOTS_Project_2_Screen(Vector &V,s32 MAPSX,s32 MAPSY, float focale)
{
	Vector Ret;
	float Ooz;
	if (V.z > 0.001f)
	{
		Ooz = 1.0f / V.z;
		Ret.x = 0.5f*(focale * V.x * Ooz * MAPSX + MAPSX);
		Ret.y = 0.5f*(focale * V.y * Ooz * MAPSX + MAPSY);
		Ret.z = Ooz;
	} else Ret.z = -1.0f;
	//Ret = V;
	return Ret;
}



void DrawEllipse(MAP *MapDST,s32 X,s32 Y,Vector *pBig,float SmallFactor,u32 Color)
{
	SHOTS_CLASS::InitcosTable();
#define DISC_DISC 16
	int Alpha = 0;
	Vector Small;
	s32 BX,BY;
	s32 BX2,BY2;
	Small.x = -pBig->y * SmallFactor;
	Small.y = pBig->x * SmallFactor;
	BX2 = (int)(SHOTS_CLASS::FASTCOS[Alpha] * pBig->x); 
	BY2 = (int)(SHOTS_CLASS::FASTCOS[Alpha] * pBig->y); 
	BX2 += (int)(SHOTS_CLASS::FASTSIN[Alpha] * Small.x); 
	BY2 += (int)(SHOTS_CLASS::FASTSIN[Alpha] * Small.y); 
	u32 Ep = FAST_COS_DISCREET / DISC_DISC;
	while (Ep--)
	{
		Alpha += DISC_DISC;
		BX = (int)(SHOTS_CLASS::FASTCOS[Alpha] * pBig->x); 
		BY = (int)(SHOTS_CLASS::FASTCOS[Alpha] * pBig->y); 
		BX += (int)(SHOTS_CLASS::FASTSIN[Alpha] * Small.x); 
		BY += (int)(SHOTS_CLASS::FASTSIN[Alpha] * Small.y); 
		DrawLine(MapDST,(float)(X + BX),(float)(Y + BY),(float)(X + BX2),(float)(Y + BY2),Color);
		BX2 = BX;
		BY2 = BY;
	}
}


void DrawMatrix(MAP *MapSRC,MATRIX *M, float focale)
{
	Vector P0,PI,PJ,PK;
	P0 = SHOTS_Project_2_Screen(M->T,MapSRC->SX,MapSRC->SY,focale);
	PI = SHOTS_Project_2_Screen(M->T + M->I,MapSRC->SX,MapSRC->SY,focale);
	PJ = SHOTS_Project_2_Screen(M->T + M->J,MapSRC->SX,MapSRC->SY,focale);
	PK = SHOTS_Project_2_Screen(M->T + M->K,MapSRC->SX,MapSRC->SY,focale);
	DrawLine_Clip(MapSRC,P0.x, P0.y + 10, P0.x, P0.y - 10, 0x808080);//*/
	DrawLine_Clip(MapSRC,P0.x + 10, P0.y, P0.x - 10, P0.y, 0x808080);//*/
	DrawLine_Clip(MapSRC,P0.x, P0.y, PI.x, PI.y, 0xff0000);//*/
	DrawLine_Clip(MapSRC,P0.x, P0.y, PJ.x, PJ.y, 0xff00);//*/
	DrawLine_Clip(MapSRC,P0.x, P0.y, PK.x, PK.y, 0xff);//*/
}
void Draw3DCircle(MAP *MapSRC,MATRIX *M,float Radius,u32 Color, float focale)
{
	Vector P0p,P1p,P0,P1;
	SHOTS_CLASS::InitcosTable();
#define CIRCLE_DISC 32
	int Alpha = 0;
	u32 Ep = FAST_COS_DISCREET / CIRCLE_DISC;
	P0 =  M->J * SHOTS_CLASS::FASTSIN[Alpha] * Radius; 
	P0 += M->K * SHOTS_CLASS::FASTCOS[Alpha] * Radius; 
	P0 += M->T;
	P0p = SHOTS_Project_2_Screen(P0,MapSRC->SX,MapSRC->SY,focale);
	while (Ep--)
	{
		P1 = P0;
		P1p= P0p;
		Alpha += CIRCLE_DISC;
		P0 =  M->J * SHOTS_CLASS::FASTSIN[Alpha] * Radius; 
		P0 += M->K * SHOTS_CLASS::FASTCOS[Alpha] * Radius; 
		P0 += M->T;
		P0p = SHOTS_Project_2_Screen(P0,MapSRC->SX,MapSRC->SY,focale);
		DrawLine(MapSRC,P0p.x,P0p.y,P1p.x,P1p.y,Color);
	}
}



u32 SHOTS_CLASS::SHOT_ComputeMatrix(GravC *pShot , MAP8 *pSrc , u32 ComputeCode)
{
	MATRIX K;
	K.Identity();
	//pShot->BigAxis.z = pShot->BigAxis.z * 0.5f + 0.5f ;
	/* Compute T */
	K.T.x = (pShot->Coord.x - (pSrc->SX/2.0f)) / pSrc->SX;
	K.T.y = (pShot->Coord.y - (pSrc->SY/2.0f)) / pSrc->SX;
	K.T.z = SHOTS_FOCALE/2.0f;

	K.T.Normalize();
	K.T *= 0.5f * SHOTS_FOCALE * ((float)pSrc->SX) / pShot->BigAxis.NormXY();

	/* Compute I */
	Vector Normale,TN;
	Normale.x = -pShot->BigAxis.y;
	Normale.y = pShot->BigAxis.x;
	Normale.z = 0.0f;
	pShot->PerspectiveDir.z = 0.0f;
	if (Normale * pShot->PerspectiveDirSmoothed < 0.0f) Normale *= -1.0f;

	// Anil
	float F,Z;
	Z = pShot->BigAxis.z;
	if (Z > 0.9999999f) Z = 0.9999999f;
	if (Z < 0.0000001f) Z = 0.0000001f;

	TN = K.T;
	TN.Normalize();
	Normale = Normale - TN * (Normale * TN);
	Normale.Normalize();
	TN *= -Z;
	F = 1.0f - Z * Z;// * pShot->BigAxis.z);
	F = sqrtf(F);
	Normale *= F;
	K.I = TN - Normale;


	/* Compute J & K */
	K.I.Normalize();
	K.J = K.I ^ Vector(0,0,1);
	if (K.J.SqrNorm() > 0.25f*0.25f)
	{
		K.K = K.I ^ K.J;
 	} else
	{
		K.K = Vector(0,1,0) ^ K.I;
		K.J = K.K ^ K.I;
	}
	K.J.Normalize();
	K.K.Normalize();//*/

	if (K.CheckNAN()) 
 		return 0;

	// Research and validation of the code contained (or not) in the recognized circle.
	if (ComputeCode)
	{
		// Line code extraction (white circle)
		Get3DCode(pSrc,&K,CODERADIUSCOEF,pShot->CODE);

		float CODE[RAW_CODESAMPLEL];

		// Pixels shift depending on the white anchor
		u32 ShiftV;
		ShiftV = GetCodeOrigin(pShot->CODE);

		memcpy(CODE,pShot->CODE,sizeof(pShot->CODE));

		// Code pixels shift before reading
		for (u32 i = 0 ; i < RAW_CODESAMPLEL - ShiftV ; i++)		
			pShot->CODE[i] = CODE[i+ShiftV];
		
		for (u32 i = RAW_CODESAMPLEL - ShiftV ; i < RAW_CODESAMPLEL; i++)		
			pShot->CODE[i] = CODE[i-(RAW_CODESAMPLEL - ShiftV)];
				
		// Code reading
		pShot->CodeNumber = GetCode(pShot->CODE); 

		//if (pShot->CodeNumber == -1) 
		//	return 0;
	

		// Reverse code reading in case of invalidated code 
		if (pShot->CodeNumber == ~0)
		{
			float tmp;
			// pShot->CODE inversion
			for (int i = 0; i < RAW_CODESAMPLEL; i++)
				pShot->CODE[i] = CODE[RAW_CODESAMPLEL-1-i];
			// CODE inversion
			for (int i = 0; i < RAW_CODESAMPLEL; i++)
				CODE[i] = pShot->CODE[i];			
			// Shift computation
			ShiftV = GetCodeOrigin(pShot->CODE);
			// Code line pixels rotation
			for (u32 i = 0 ; i < RAW_CODESAMPLEL - ShiftV ; i++)			
				pShot->CODE[i] = CODE[i+ShiftV];			
			for (u32 i = RAW_CODESAMPLEL - ShiftV ; i < RAW_CODESAMPLEL; i++)			
				pShot->CODE[i] = CODE[i-(RAW_CODESAMPLEL - ShiftV)];		
			// Code reading
			pShot->CodeNumber = GetCode(pShot->CODE);
			// Axis rotation to be near of the white anchor
			K.RotateAround_I(-(float)ShiftV * 3.1415927f * 2.0f / (float)RAW_CODESAMPLEL);
		}		
		// Validated code
		else
		
			// Axis rotation to be near of the white anchor
			K.RotateAround_I((float)ShiftV * 3.1415927f * 2.0f / (float)RAW_CODESAMPLEL);

		pShot->MatrixSmoothed = K;
	}
	pShot->Matrix = K;
	return 1; 
}

void SHOTS_CLASS::SHOT_ComputeSmooth(GravC *OutpL)
{
	Quat Angle;
	float SinAlpha;
	MATRIX InVertPrev;
	Vector OdlT,NewT;
	OdlT = OutpL->MatrixSmoothed.T;
	NewT = OutpL->Matrix.T;
	OutpL->Matrix.Inverse(InVertPrev);
	InVertPrev *= OutpL->MatrixSmoothed;
	Angle.ComputeFromMatrix(&InVertPrev);
	SinAlpha = (*((Vector *)&Angle)).SqrNorm();
#define COEF 0.10f
	// Blend Angles
	if (SinAlpha < COEF*COEF)
	{
		// blend
		SinAlpha = sqrtf(SinAlpha)/ COEF;
		SinAlpha = sqrtf(SinAlpha);
		OutpL->MatrixSmoothed.Blend(OutpL->Matrix,SinAlpha);
		OutpL->MatrixSmoothed.T = OutpL->Matrix.T;

	} else
		// Copy
		OutpL->MatrixSmoothed = OutpL->Matrix;//*/

	// Blend Translation
	//OutpL->MatrixSmoothed.T = OutpL->Matrix.T * 0.1 + OdlT * 0.9;


	Vector ToleranceVector,ErrorVector;
	ToleranceVector.x = 0.001f * /*Pixel*/ NewT.z * SHOTS_FOCALE;
	ToleranceVector.y = ToleranceVector.x;
	ErrorVector = NewT - OdlT;
	if ((fabs(ErrorVector.x) > ToleranceVector.x) || 
		(fabs(ErrorVector.y) > ToleranceVector.y))
		/* Out Of Tolerance -> Copy */
		OutpL->MatrixSmoothed.T = NewT;
	else
		/* else -> Blend */
		OutpL->MatrixSmoothed.T = NewT * 0.25f + OdlT * 0.75f;



	if (OutpL->MatrixSmoothed.I.z < -0.9f)
		OutpL->PerspectiveDirSmoothed = OutpL->PerspectiveDir;
	else
		OutpL->PerspectiveDirSmoothed = (OutpL->PerspectiveDirSmoothed * 0.925f) + (OutpL->PerspectiveDir * 0.075f);
}

void SHOTS_CLASS::SHOT_DrawDebug(GravC *pShot,MAP *pSrc,u32 DrawCodeBMP , u32 DrawCodeTEXT , u32 Color)
{
	if (DrawCodeBMP)
	{
		float LocdalCode2[RAW_CODESAMPLEL];
		memcpy(LocdalCode2,pShot->CODE,sizeof(pShot->CODE));
		for (int i = 0 ; i < RAW_CODESAMPLEL ; i ++)
		{
			s32 Color = (u32)((LocdalCode2[i]*2.0f+0.5f)*256.0f);
			if (Color > 255) Color = 255;
			if (Color < 0) Color = 0;
			Color |= Color << 8;
			Color |= Color << 8;
			if ((i&((1<<RAW_CODESAMPLEL_Po2)-1)) == 0) Color = Color;
			for (int j = 0 ; j < 20 ; j ++)
			{
				s32 X = (s32)(pShot->Coord.x + i);
				s32 Y = (s32)(pShot->Coord.y + j);
				if (((X>0)&&(Y>0)) && 
				    ((X<(int)pSrc->SX)&&(Y<<pSrc->SY)))
				{
					pSrc->SetValue(X + Y * pSrc->PITCH, Color);
				}
			}
		}
	}

	Draw3DCircle(pSrc,&pShot->Matrix,1,Color, SHOTS_FOCALE);
	Draw3DCircle(pSrc,&pShot->Matrix,CODERADIUSCOEF,0xffffff, SHOTS_FOCALE);
	//DrawEllipse(pSrc,pShot->Coord2D.x,pShot->Coord2D.y,&pShot->BigAxis,pShot->BigAxis.z,0x808080);
	DrawMatrix(pSrc,&pShot->Matrix, SHOTS_FOCALE);
	DrawMatrix(pSrc,&pShot->MatrixSmoothed, SHOTS_FOCALE);
/*
	if (DrawCodeTEXT)
	{
void DRawTEXT_Soft_B(unsigned char *pTEXT,u32 *pBase,u32 X, u32 Y, s32 Pitch , u32 Height , u32 Color);

		char T[1024];
		sprintf(T,"%02d",pShot->CodeNumber);
		DRawTEXT_Soft_B((u8*)T,(u32*)pSrc->GetBase(),pShot->Coord.x - strlen(T) * 4 , pShot->Coord.y - 8, pSrc->PITCH, pSrc->SY, Color);
		sprintf(T,"E=%3.2f",pShot->ContourError);
		DRawTEXT_Soft_B((u8*)T,(u32*)pSrc->GetBase(),pShot->Coord.x - strlen(T) * 4 , pShot->Coord.y - 8 + 16, pSrc->PITCH, pSrc->SY, Color);
	}
*/
}