#include "TRACK.h"
#include "SHOTS.h"

#define MORE_PREC 1
#define STEP_ST_FACTOR 1.5f 
//#define SHOT_DRAW_DEBUG

//#pragma optimize( "", off )


float FACTOR_ForceVDir = 0.0f;
float GetLineError(MAP8 *MapDST,float X1,float Y1,float X2,float Y2,float *Coefs,u32 Number);

#ifdef SHOT_DRAW_DEBUG
#define DBGTEST
MAP *MapDSTDEBUG = NULL;
#endif

float MATCHEllipse2(MAP8 *MapDST,float X,float Y,Vector *pBig,float SmallFactor,float K = 2)
{
	InitcosTable();
	float RetValu;
#define DISC_NUMBER 16
#define DISC_DISC (FAST_COS_DISCREET / DISC_NUMBER)

	int Alpha;
	Vector Small;
	Vector Big2,Small2;
	float BX,BY;
	float BX2,BY2;
	Alpha = 0.0f;
	Small.x = -pBig->y * fabs(SmallFactor);
	Small.y = pBig->x * fabs(SmallFactor);
	float Local;
	Local = pBig->NormXY();
	K = Local * 0.125f;
	if (K < 4.0f) K = 4.0f;
	Big2 = *pBig * ((Local + K) / Local);
	Local *= fabs(SmallFactor);
	Small2 = Small * ((Local + K) / Local);
	u32 Ep = FAST_COS_DISCREET / DISC_DISC;
	RetValu = 0.0f;
	u32 NTT = 0;
	while (Ep--)
	{
		Alpha += DISC_DISC;
		BX = (FASTCOS[Alpha] * pBig->x) + X; 
		BY = (FASTCOS[Alpha] * pBig->y) + Y; 
		BX += (FASTSIN[Alpha] * Small.x); 
		BY += (FASTSIN[Alpha] * Small.y); 
		BX2 = (FASTCOS[Alpha] * Big2.x) + X;
		BY2 = (FASTCOS[Alpha] * Big2.y) + Y;
		BX2 += (FASTSIN[Alpha] * Small2.x); 
		BY2 += (FASTSIN[Alpha] * Small2.y); 
		if ((BX2 > 0) && (BX2 < MapDST->SX) && (BY2 > 0) && (BY2 < MapDST->SY))
		{
#define Norm8 0.35355339059327376220042218105242f
			float Coefs[] = {-Norm8,-Norm8,-Norm8,-Norm8,Norm8,Norm8,Norm8,Norm8};
			float L = GetLineError(MapDST,BX,BY,BX2,BY2,Coefs,8);
			NTT++;
			if (SmallFactor > 0)
			{
				if (L>0.0) RetValu += L;
			}
			else
			{
				if (L<0.0) RetValu -= L;
			}
			
#ifdef DBGTEST
			if (MapDSTDEBUG && (MapDSTDEBUG->SX == MapDST->SX))
			{
				*(MapDSTDEBUG->GetBase() + (s32)BX + (s32)BY * MapDSTDEBUG->PITCH) |= 0xff;
				*(MapDSTDEBUG->GetBase() + (s32)BX2 + (s32)BY2 * MapDSTDEBUG->PITCH) |= 0xff0000;
			}//*/
#endif
		}
	}
	if (NTT < DISC_NUMBER>>1) return 0;
	return RetValu / (float)NTT;
}

float MATCHEllipse(MAP8 *MapDST,float X,float Y,Vector *pBig,float SmallFactor,float K = 2)
{
	return MATCHEllipse2(MapDST,X,Y,pBig,SmallFactor);
	Vector BA2;
	BA2 = *pBig * SmallRaidusFactor;
	return MATCHEllipse2(MapDST,X,Y,pBig,SmallFactor) + MATCHEllipse2(MapDST,X,Y,&BA2,-SmallFactor);
}

float SHOT_TrackSHOT_T(MAP8 *pSrc,Vector *pCenter,Vector *pBigAxis)
{
	/* Make it Converge */
	float CurrentET[4],CurrentE;

	u32 Conv = 2 + MORE_PREC;
	float K = 2.f ;//(float)(1<<Conv)/0.75f;

	while (Conv--)
	{
		u32 i;
		CurrentE	 = MATCHEllipse(pSrc,pCenter->x,pCenter->y,pBigAxis,pBigAxis->z);
		CurrentET[0] = MATCHEllipse(pSrc,pCenter->x+K,pCenter->y,pBigAxis,pBigAxis->z);
		CurrentET[1] = MATCHEllipse(pSrc,pCenter->x-K,pCenter->y,pBigAxis,pBigAxis->z);
		CurrentET[2] = MATCHEllipse(pSrc,pCenter->x,pCenter->y+K,pBigAxis,pBigAxis->z);
		CurrentET[3] = MATCHEllipse(pSrc,pCenter->x,pCenter->y-K,pBigAxis,pBigAxis->z);
		i = -1;
		if (CurrentET[0] > CurrentE) {CurrentE = CurrentET[0];i=0;};
		if (CurrentET[1] > CurrentE) {CurrentE = CurrentET[1];i=1;};
		if (CurrentET[2] > CurrentE) {CurrentE = CurrentET[2];i=2;};
		if (CurrentET[3] > CurrentE) {CurrentE = CurrentET[3];i=3;};
		if (i != -1)
		{
			switch (i)
			{
			case 0:pCenter->x+=K;break;
			case 1:pCenter->x-=K;break;
			case 2:pCenter->y+=K;break;
			case 3:pCenter->y-=K;break;
			}
		}

		K *= 0.4f;
		if (!K) K = 1;
	}
	return CurrentE;
}

float SHOT_TrackSHOT_T_UnAffect(MAP8 *pSrc,Vector *pCenter,Vector *pBigAxis)
{
	/* Make it Converge */
	float Ret;
	Vector SaveT;
	SaveT = *pCenter;
	Ret = SHOT_TrackSHOT_T(pSrc,pCenter,pBigAxis);
	*pCenter = SaveT;
	return Ret;
}
float SHOT_TrackSHOT_ST(MAP8 *pSrc,Vector *pCenter,Vector *pBigAxis)
{
	float CurrentET[6],CurrentE;
	Vector BA[6];
	Vector CenterSave;
	/* Make it Converge */
	u32 Conv = 4 + MORE_PREC;
	float K = 1.5f * STEP_ST_FACTOR;
	while (Conv--)
	{
		u32 i;
		CurrentE = SHOT_TrackSHOT_T(pSrc,pCenter,pBigAxis);

		BA[0] = BA[1] = BA[2] = BA[3] = BA[4] = BA[5] = *pBigAxis ; 
		BA[0].x += K*2.0f; 
		BA[1].x -= K*2.0f; 
		BA[2].y += K*2.0f; 
		BA[3].y -= K*2.0f; 

		BA[4] = BA[5] = *pBigAxis ; 

		float N = pBigAxis->NormXY() * pBigAxis->z;
		BA[4].z *= (N + K) / N;
		BA[5].z *= (N - K) / N;
		if (BA[4].z > 1.0f) BA[4].z = 1.0f;
		if (BA[5].z > 1.0f) BA[5].z = 1.0f;

		CurrentET[0] = SHOT_TrackSHOT_T_UnAffect(pSrc,pCenter,&BA[0]);
		CurrentET[1] = SHOT_TrackSHOT_T_UnAffect(pSrc,pCenter,&BA[1]);
		CurrentET[2] = SHOT_TrackSHOT_T_UnAffect(pSrc,pCenter,&BA[2]);
		CurrentET[3] = SHOT_TrackSHOT_T_UnAffect(pSrc,pCenter,&BA[3]);
		CurrentET[4] = SHOT_TrackSHOT_T_UnAffect(pSrc,pCenter,&BA[4]);
		CurrentET[5] = SHOT_TrackSHOT_T_UnAffect(pSrc,pCenter,&BA[5]);//*/

		i = -1;
		if (CurrentET[0] > CurrentE) {CurrentE = CurrentET[0];i=0;};
		if (CurrentET[1] > CurrentE) {CurrentE = CurrentET[1];i=1;};
		if (CurrentET[2] > CurrentE) {CurrentE = CurrentET[2];i=2;};
		if (CurrentET[3] > CurrentE) {CurrentE = CurrentET[3];i=3;};
		if (CurrentET[4] > CurrentE) {CurrentE = CurrentET[4];i=4;};
		if (CurrentET[5] > CurrentE) {CurrentE = CurrentET[5];i=5;};//*/

		if (i != -1)
		{
			*pBigAxis = BA[i];
			if (BA[i].z > 1.0f) BA[i].z = 1.0f;
			SHOT_TrackSHOT_T(pSrc,pCenter,pBigAxis);
		} 

		K *= 0.3333f;
		if (!K) K = 1;
	}
	return CurrentE;
}

float SHOT_TrackSHOT_ST_UnAffect(MAP8 *pSrc,Vector *pCenter,Vector *pBigAxis)
{
	/* Make it Converge */
	float ret;
	Vector SaveT,SaveBA;
	SaveT = *pCenter;
	SaveBA = *pBigAxis;
	ret = SHOT_TrackSHOT_ST(pSrc,pCenter,pBigAxis);
	*pCenter = SaveT;
	*pBigAxis= SaveBA;
	return ret;
}

float SHOT_TrackSHOT(MAP8 *pSrc,MAP *pSrcR,Vector *pCenter,Vector *pCenter3D,Vector *pBigAxis,u32 TOnly)
{
	float CurrentET[4],CurrentE;
	Vector BA[4];
	Vector BA3D;
	Vector CenterSave;
	/* Make it Converge */
		//pBigAxis->z = 1.0f;
	if (TOnly)
		CurrentE = SHOT_TrackSHOT_T(pSrc,pCenter,pBigAxis);
	else
		CurrentE = SHOT_TrackSHOT_ST(pSrc,pCenter,pBigAxis);


	if (pCenter3D)
	{
		BA3D = *pBigAxis * SmallRaidusFactor;
		BA3D.z = -pBigAxis->z;
		SHOT_TrackSHOT_T(pSrc,pCenter3D,&BA3D);
	}


	if (pSrcR)
	{
#ifdef DBGTEST
		MapDSTDEBUG = pSrcR;//*/
		//DARKER(pSrcR);//*/
		Vector BA2;
		DrawEllipse(pSrcR,pCenter->x,pCenter->y,pBigAxis,pBigAxis->z,0xff80);
		DrawEllipse(pSrcR,pCenter3D->x,pCenter3D->y,&BA3D,pBigAxis->z,0xff80);
		char TEXT[2048];
		sprintf(TEXT,"%2.2f",pBigAxis->NormXY());
		DRawTEXT_Soft_B((u8*)TEXT,(u32*)pSrcR->GetBase(),16,16,pSrcR->PITCH,pSrcR->SY,0xff00);
#endif
	}
	return CurrentE;
}


u32 SHOTS_CLASS::SHOTS_TRACK(MAP8 *pSrc,MAP8 *pSrcO2,MAP8 *pSrcO4,/*IN - OUT*/GravC *OutpL,MAP *pSrcR)
{
	OutpL->Coord *= 0.125f;
	OutpL->Coord2D *= 0.125f;
	OutpL->BigAxis.x *= 0.125f;
	OutpL->BigAxis.y *= 0.125f;
	//Tracking Mode
//		SHOT_TrackSHOT(&MHSO8,NULL,&OutpL->Coord2D, &OutpL->BigAxis,1);
	OutpL->Coord *= 2.0f;
	OutpL->Coord2D *= 2.0f;
	OutpL->BigAxis.x *= 2.0f;
	OutpL->BigAxis.y *= 2.0f;
	SHOT_TrackSHOT(pSrcO4,NULL,&OutpL->Coord2D, &OutpL->Coord, &OutpL->BigAxis,1);
	OutpL->Coord = OutpL->Coord2D;
	OutpL->Coord *= 2.0f;
	OutpL->Coord2D *= 2.0f;
	OutpL->BigAxis.x *= 2.0f;
	OutpL->BigAxis.y *= 2.0f;
	SHOT_TrackSHOT(pSrcO2,NULL,&OutpL->Coord2D, &OutpL->Coord, &OutpL->BigAxis,0);
//		OutpL->Coord = OutpL->Coord2D;
	OutpL->Coord *= 2.0f;
	OutpL->Coord2D *= 2.0f;
	OutpL->BigAxis.x *= 2.0f;
	OutpL->BigAxis.y *= 2.0f;
	OutpL->ContourError = SHOT_TrackSHOT(pSrc,pSrcR,&OutpL->Coord2D, &OutpL->Coord, &OutpL->BigAxis,0);
	if ((OutpL->BigAxis.NormXY() < 4.0f) ||
		(OutpL->BigAxis.z < 0.1f))
		return 0;

	OutpL->PerspectiveDir *= 0.90f;
	OutpL->PerspectiveDir = (OutpL->Coord2D - OutpL->Coord) * 0.1;
	OutpL->PerspectiveDir.y -= FACTOR_ForceVDir;
	{
		MATRIX OriginalMatrix;
		MATRIX TranMat;
		Quat TransQuat;
		OriginalMatrix = OutpL->Matrix;

		// 1:Recompute MATRIX
		if (!SHOT_ComputeMatrix(OutpL,pSrc,0))
		{
			OutpL->Matrix.I = OriginalMatrix.I;
			OutpL->Matrix.J = OriginalMatrix.J;
			OutpL->Matrix.K = OriginalMatrix.K;
		} else
		{
			Vector DP,T;
			T = OutpL->Matrix.T;
			DP = OutpL->Matrix.I + OriginalMatrix.I;
			DP.Normalize();
			*(Vector *)&TransQuat = DP ^OutpL->Matrix.I;
			TransQuat.NormalizeW();
			TransQuat.TransforToMatrix(&TranMat);
			OutpL->Matrix = OriginalMatrix;
			OutpL->Matrix *= TranMat;
			OutpL->Matrix.T = T;
		}
		
		// 2:Compute Real 3D Center
		{
			float Factor;
			Vector P1,P2,P1p,P2p,PC,PCS;
			P2.x = -OutpL->BigAxis.y * OutpL->BigAxis.z;
			P2.y = OutpL->BigAxis.x * OutpL->BigAxis.z;
			P1.x = OutpL->Coord2D.x + P2.x;
			P1.y = OutpL->Coord2D.y + P2.y;
			P2.x = OutpL->Coord2D.x - P2.x;
			P2.y = OutpL->Coord2D.y - P2.y;
#ifdef SHOT_DRAW_DEBUG
			if (pSrcR)
			{
				DrawQuadFull(pSrcR,P1.x-4,P1.y-4,P1.x+4,P1.y+4,0xff00);
				DrawQuadFull(pSrcR,P2.x-4,P2.y-4,P2.x+4,P2.y+4,0xff00);
			}
#endif
			Factor = sqrtf(1.0f - OutpL->Matrix.I.z * OutpL->Matrix.I.z);
			P1.z = OutpL->Matrix.T.z- Factor * 1.0f;
			P2.z = OutpL->Matrix.T.z+ Factor * 1.0f;
			Factor = 0.5f / P1.z + 0.5f / P2.z;
			P1p = P1 * (1.0f / P1.z);
			P2p = P2 * (1.0f / P2.z);
			PC = (P1p + P2p) * 0.5f;
			PC *= 1.0f / Factor;
			P1p = P1 * (1.0f / P2.z);
			P2p = P2 * (1.0f / P1.z);
			PCS = (P1p + P2p) * 0.5f;
			PCS *= 1.0f / Factor;
			Vector BA[2];
			float BRA[2];
			BA[0] = PC;
			BA[1] = PCS;
			Vector BA3D;
			BA3D = OutpL->BigAxis * SmallRaidusFactor;
			BA3D.z = -OutpL->BigAxis.z;

			BRA[0] = SHOT_TrackSHOT(pSrc,NULL,&BA[0], NULL, &BA3D,1);
			BRA[1] = SHOT_TrackSHOT(pSrc,NULL,&BA[1], NULL, &BA3D,1);
			if (BRA[0] > BRA[1])
				OutpL->Coord = BA[0];
			else
				OutpL->Coord = BA[1];
#ifdef SHOT_DRAW_DEBUG
			if (pSrcR)
			{
				Vector BA3D = OutpL->BigAxis * SmallRaidusFactor;
				DrawQuadFull(pSrcR, PC.x-4, PC.y-4, PC.x+4, PC.y+4,0xff00);
				DrawQuadFull(pSrcR,PCS.x-4,PCS.y-4,PCS.x+4,PCS.y+4,0xff00);
				DrawQuadFull(pSrcR,OutpL->Coord.x-4,OutpL->Coord.y-4,OutpL->Coord.x+4,OutpL->Coord.y+4,0xff8080);
				DrawEllipse(pSrcR,OutpL->Coord.x,OutpL->Coord.y,&BA3D,OutpL->BigAxis.z,0xff80);
			}
#endif
			if (!SHOT_ComputeMatrix(OutpL,pSrc,0))
			{
				OutpL->Matrix.I = OriginalMatrix.I;
				OutpL->Matrix.J = OriginalMatrix.J;
				OutpL->Matrix.K = OriginalMatrix.K;
			} else
			{
				Vector DP,T;
				T = OutpL->Matrix.T;
				DP = OutpL->Matrix.I + OriginalMatrix.I;
				DP.Normalize();
				*(Vector *)&TransQuat = DP ^OutpL->Matrix.I;
				TransQuat.NormalizeW();
				TransQuat.TransforToMatrix(&TranMat);
				OutpL->Matrix = OriginalMatrix;
				OutpL->Matrix *= TranMat;
				OutpL->Matrix.T = T;
			}

//			SHOT_ComputeMatrix(OutpL,pSrc,0);
		}//*/

		// 2:Recompute MATRIX I Angle
		{
			MATRIX Smat;
			Smat = OutpL->Matrix;
			Get3DCode(pSrc,&OutpL->Matrix,CODERADIUSCOEF,OutpL->CODE);
			float CODE[RAW_CODESAMPLEL];
			u32 ShiftV;
			ShiftV = GetCodeOrigin(OutpL->CODE);
			memcpy(CODE,OutpL->CODE,sizeof(OutpL->CODE));
			for (u32 i = 0 ; i < RAW_CODESAMPLEL - ShiftV ; i++)
			{
				OutpL->CODE[i] = CODE[i+ShiftV];
			}
			for (u32 i = RAW_CODESAMPLEL - ShiftV ; i < RAW_CODESAMPLEL; i++)
			{
				OutpL->CODE[i] = CODE[i-(RAW_CODESAMPLEL - ShiftV)];
			}
			u32 NewCN;
			NewCN = GetCode(OutpL->CODE);
			// Extract Code 
			if ((OutpL->ContourError > 0.9f) || (OutpL->CodeNumber == NewCN))
			{
				if (NewCN != -1)
				if (ModeReverse & 1)
					OutpL->Matrix.RotateAround_I(-((float)ShiftV) * 3.1415927f * 2.0f / (float)RAW_CODESAMPLEL);
				else
					OutpL->Matrix.RotateAround_I(((float)ShiftV) * 3.1415927f * 2.0f / (float)RAW_CODESAMPLEL);
			} else
				OutpL->CodeNumber = NewCN;
			//if (pShot->CodeNumber == -1) return 0;
			//if (OutpL->CodeNumber == -1) OutpL->Matrix = Smat;
		}//*/
/*		s32 Prec = 4;
		while (Prec)
		{
			float Error = -1000000000000000.0f;
			MATRIX FM;
			FM = OutpL->Matrix;
			for (s32 C = -2 * Prec; C < 2 * Prec; C += Prec)
			{
				float LocdalCode[RAW_CODESAMPLEL],LocalEr;
				MATRIX M2;
				M2 = OutpL->Matrix;
				M2.RotateAround_I((float)C * 3.1415927f / 1024.0f);
				Get3DCode(pSrc,&M2,CODERADIUSCOEF,(float*)LocdalCode);
				LocalEr = 0.0f;
				for (int j = 0 ; j < RAW_CODESAMPLEL ; j ++)
					LocalEr += LocdalCode[j] * OutpL->CODE[j];
				if (LocalEr > Error)
				{
					FM = M2;
					Error = LocalEr;
				}
			}
			OutpL->Matrix = FM;
			Prec >>= 1;
		}//*/

		// 3:SmoothMatrix
		SHOT_ComputeSmooth(OutpL);
	}
	return 1;
}