#include "SCENE.h"
static volatile u32 IKFREEZER = 1;
//#pragma	optimize("",off)
void WORLD::DrawSmallSceneIK(SmallScene *SmSc)
{
	MATRIX MatrixToSet,CP;
	_3D_Object *pObj;
	MatrixToSet.Identity();
	CP = SmSc->CameraPosition;
	CP.K *= -1.0f;
	CP.Inverse(MatrixToSet);//*/
	if (SmSc->IK == NULL) return;
	ComputeSmallSceneHierarchie(SmSc);
	for (int G = 0 ; G < SmSc->IK->ulNumberOfPatchs ; G++)
	{
		u32 Father;
		RS LineRS;
		MATRIX WM;
		Vector A,B;
		pObj = SmSc->ObjectsPtrs[SmSc->IK->pObjPatchs[G].ObjectRefIndex];
		Father = pObj->FatherIndex;
		if(Father != -1)
		{
			GLOB_SetDefaultRS(&LineRS);
			DI->Matrix(&MatrixToSet);
			LineRS.NOZTEST = 1;
			DI->SetRS(&LineRS,-1);
			A = pObj->GlobalMatrix.T;
			B = SmSc->ObjectsPtrs[Father]->GlobalMatrix.T;
			DI->DrawLine(0xFFFFFF,&A,&B);
		}
	}

	for ( int i = 0 ; i < SmSc->IK->ulNumberOfCollider; i++ )
	{
		u32 Father,Current;
		RS LineRS;
		MATRIX WM,WMi;
		Vector A,B,CPiC;
		pObj = SmSc->ObjectsPtrs[SmSc->IK->pColliders[i].ObjectRefIndex];
		Father = pObj->FatherIndex;
		WM.Identity();
		WM.I &= SmSc->IK->pColliders[i].Radius;
		WM.J &= SmSc->IK->pColliders[i].Radius;
		WM.K &= SmSc->IK->pColliders[i].Radius;
		WM.T = SmSc->IK->pColliders[i].Center;
		WM *= pObj->GlobalMatrix;
		WM.Inverse(WMi);

		CPiC = WMi * CP.T;

		WM *= MatrixToSet;
		GLOB_SetDefaultRS(&LineRS);
		DI->Matrix(&WM);
		//LineRS.NOZTEST = 1;
		DI->SetRS(&LineRS,-1);
#define DISC_SEG 16
#define DISC_SEG2 16


		float X,Y,XN,YN,Alpha,Alpha2;
		Alpha2 = 0;
		u32 SegCounter2 = DISC_SEG2;
		while (SegCounter2--)
		{
			u32 SegCounter = DISC_SEG;
			Alpha2 += 3.1415927f / (float)(DISC_SEG2 + 1);
			X = sinf(Alpha2);
			Y = 0.0f;
			Alpha = 0;
			while (SegCounter--)
			{
				Alpha += 3.1415927f * 2.0f / (float)(DISC_SEG);
				XN = cosf(Alpha) * sinf(Alpha2);
				YN = sinf(Alpha) * sinf(Alpha2);
				A = Vector(cosf(Alpha2),X,Y);
				B = Vector(cosf(Alpha2),XN,YN);
				if (CPiC * A > 0.0f)
					DI->DrawLine(0x8080,&A,&B);
				A = Vector(X,cosf(Alpha2),Y);
				B = Vector(XN,cosf(Alpha2),YN);
				if (CPiC * A > 0.0f)
					DI->DrawLine(0x8080,&A,&B);
				A = Vector(X,Y,cosf(Alpha2));
				B = Vector(XN,YN,cosf(Alpha2));
				if (CPiC * A > 0.0f)
					DI->DrawLine(0x8080,&A,&B);
				X = XN;
				Y = YN;
			} 
		}
	}		
}

void WORLD::ComputeLocalMatrixes_IK(SmallScene *SmSc)
{
	for (int Opi = 0 ; Opi < SmSc->IK->ulNumberOfPatchs ; Opi++)
	{
		/* recompute GlobalMatrix IJK */
		u32 Fi;
		MATRIX mFi;
		MATRIX mGi;
		u32 G = SmSc->IK->pObjPatchs[Opi].ObjectRefIndex;
		Fi = SmSc->ObjectsPtrs[G]->FatherIndex;
		if (Fi != -1)
		{
			Vector To,TIK;
			MATRIX TMat,mFi;
			MATRIX LocaLIK;
			SmSc->ObjectsPtrs[Fi]->GlobalMatrix.Inverse(mFi);
			LocaLIK = SmSc->ObjectsPtrs[G]->GlobalMatrix;
			LocaLIK *= mFi;
			To = SmSc->ObjectsPtrs[G]->OriginalLocalMatrix.T ;
			TIK = LocaLIK.T;
			/* Rotate LM */
			Vector V1,V2,VCP;
			Quat  CP;
			CP.ExtractFrom2Edges(&To,&TIK);
			CP.TransforToMatrix(&TMat);
			VCP = SmSc->ObjectsPtrs[G]->GlobalMatrix.T;
			TMat *= SmSc->ObjectsPtrs[G]->OriginalLocalMatrix;
			TMat *= SmSc->ObjectsPtrs[Fi]->GlobalMatrix;
			SmSc->ObjectsPtrs[G]->GlobalMatrix = TMat;
			SmSc->ObjectsPtrs[G]->GlobalMatrix.T = VCP;
		}
	}
}

u32 IsNan(float F)
{
	if ((*(u32 *)&F & 0x7f800000) == 0x7f800000) 
		return 1;
	else 
		return 0;
}




void WORLD::ComputeSmallSceneIK(SmallScene *SmSc,u32 Instance)
{
	IK_ObjectPatchInstance *pCurrentHinstance;
	_3D_Object **pObj;
	
	if (Instance > 64) return;
	if (SmSc->IK == NULL) return;
	
	ComputeSmallSceneHierarchie(SmSc);

	/* Verify and Set the number of Instance */
	if (SmSc->IK->ulNumberOfInstance <= Instance)
	{
		IK_ObjectPatchInstance **ppPreviousPI;
		ppPreviousPI = SmSc->IK->pPI;
		SmSc->IK->pPI = (IK_ObjectPatchInstance **)CC_malloc(sizeof(IK_ObjectPatchInstance *) * (Instance + 1));
		memset(SmSc->IK->pPI,0,sizeof(IK_ObjectPatchInstance *) * (Instance + 1));
		if (ppPreviousPI)
		{
			for (int K = 0;K < SmSc->IK->ulNumberOfInstance ; K++)
				SmSc->IK->pPI[K] = ppPreviousPI[K];
			CC_free(ppPreviousPI);
		}
		SmSc->IK->ulNumberOfInstance = Instance + 1;
	}
	if (!SmSc->IK->pPI[Instance])
	{
		SmSc->IK->pPI[Instance] = (IK_ObjectPatchInstance *)CC_malloc(sizeof(IK_ObjectPatchInstance) * (SmSc->IK->ulNumberOfPatchs));
		memset(SmSc->IK->pPI[Instance],0,sizeof(IK_ObjectPatchInstance) * (SmSc->IK->ulNumberOfPatchs));
		pCurrentHinstance = SmSc->IK->pPI[Instance];
		for (int G = 0 ; G < SmSc->IK->ulNumberOfPatchs ; G++)
		{
			u32 Oi;
			Oi = SmSc->IK->pObjPatchs[G].ObjectRefIndex;
			pCurrentHinstance[G].ComputedMatrix = SmSc->ObjectsPtrs[Oi]->OriginalGlobalMatrix;
			pCurrentHinstance[G].LastGlobalMatrix = SmSc->ObjectsPtrs[Oi]->OriginalGlobalMatrix;
			pCurrentHinstance[G].vSpeed = Vector(0,0,0);
		}
		SmSc->IK->LastTimeCalled[Instance] = timeGetTime() - 20;
	}

	pCurrentHinstance = SmSc->IK->pPI[Instance];

	//if (UpdateWithoutCompute) return;
	/* Compute DT */
	float GDT;
	u32 LoopNum;
	LoopNum = timeGetTime();
	if (LoopNum - SmSc->IK->LastTimeCalled[Instance] > 200) 
		SmSc->IK->LastTimeCalled[Instance] = timeGetTime()-200;
#define IK_BIG_DT 16


	if (SmSc->IK->LastTimeCalled[Instance] + IK_BIG_DT >= LoopNum)
	{
		for (int G = 0 ; G < SmSc->IK->ulNumberOfPatchs ; G++)
		{
			u32 Oi = SmSc->IK->pObjPatchs[G].ObjectRefIndex;
			if (pCurrentHinstance[G].ComputedMatrix.CheckNAN())
				pCurrentHinstance[G].ComputedMatrix.Identity();
			SmSc->ObjectsPtrs[Oi]->GlobalMatrix = pCurrentHinstance[G].ComputedMatrix;
		}
		for (int G = 0 ; G < SmSc->ulNumberOfObjects ; G++)
		{
			/* recompute localmatrixes */
			u32 Fi;
			MATRIX mFi;
			MATRIX mGi;
			Fi = SmSc->ObjectsPtrs[G]->FatherIndex;
			if (Fi != -1)
			{
				SmSc->ObjectsPtrs[Fi]->GlobalMatrix.Inverse(mFi);
				SmSc->ObjectsPtrs[G]->LocalMatrix = SmSc->ObjectsPtrs[G]->GlobalMatrix;
				SmSc->ObjectsPtrs[G]->LocalMatrix *= mFi;
			}
		}
		return;
	}

	/* RESET TEST in case of big JUMPS (movie cut or...), detect it and solve it */
	/* BEGIN *********************************************************************/
	if ((SmSc->IK->ulNumberOfInstance <= 1))
	{
		u32 MustFreeze = 0;
		for (int G = 0 ; G < SmSc->IK->ulNumberOfPatchs ; G++)
		{
			u32 Oi = SmSc->IK->pObjPatchs[G].ObjectRefIndex;
			Quat A,B;
			Vector D;
			D = SmSc->ObjectsPtrs[Oi]->GlobalMatrix.T - pCurrentHinstance[G].LastGlobalMatrix.T;
			if (D.Norm() > 100.0f)
				MustFreeze= 1;
			else
			{
				A.ComputeFromMatrix(&SmSc->ObjectsPtrs[Oi]->GlobalMatrix);
				B.ComputeFromMatrix(&pCurrentHinstance[G].LastGlobalMatrix);
				if (((*(Vector *)&A) ^ (*(Vector *)&B)).Norm() > 0.03f)
				{
					MustFreeze= 1;
				}
			}
		}

		if (MustFreeze)
			// Then "FREEZE" IK.
		{
			for (int G = 0 ; G < SmSc->IK->ulNumberOfPatchs ; G++)
			{
				u32 Oi = SmSc->IK->pObjPatchs[G].ObjectRefIndex;

				MATRIX TMAT;
				pCurrentHinstance[G].LastGlobalMatrix.Inverse(TMAT);
 				TMAT *= SmSc->ObjectsPtrs[Oi]->GlobalMatrix;
				pCurrentHinstance[G].ComputedMatrix *= TMAT;
				if (MustFreeze)
					pCurrentHinstance[G].vSpeed = Vector(0,0,0);
				SmSc->ObjectsPtrs[Oi]->NV = Vector(0,0,0);
				pCurrentHinstance[G].LastGlobalMatrix = SmSc->ObjectsPtrs[Oi]->GlobalMatrix;
				SmSc->ObjectsPtrs[Oi]->GlobalMatrix = pCurrentHinstance[G].ComputedMatrix;
			}
			for (int G = 0 ; G < SmSc->ulNumberOfObjects ; G++)
			{
				/* recompute localmatrixes */
				u32 Fi;
				MATRIX mFi;
				MATRIX mGi;
				Fi = SmSc->ObjectsPtrs[G]->FatherIndex;
				if (Fi != -1)
				{
					SmSc->ObjectsPtrs[Fi]->GlobalMatrix.Inverse(mFi);
					SmSc->ObjectsPtrs[G]->LocalMatrix = SmSc->ObjectsPtrs[G]->GlobalMatrix;
					SmSc->ObjectsPtrs[G]->LocalMatrix *= mFi;
				}
			}
			MustFreeze = 0;
			return;
		}

		for (int G = 0 ; G < SmSc->IK->ulNumberOfPatchs ; G++)
		{
			u32 Oi = SmSc->IK->pObjPatchs[G].ObjectRefIndex;
			pCurrentHinstance[G].LastGlobalMatrix = SmSc->ObjectsPtrs[Oi]->GlobalMatrix;
		}
	}
	/* END *********************************************************************/
	



	/* Compute mechanics */
	
	while (SmSc->IK->LastTimeCalled[Instance] + IK_BIG_DT < LoopNum)
	{
		SmSc->IK->LastTimeCalled[Instance] += IK_BIG_DT;
		GDT = 1.0f / 30.0f;
		
		/* Reset forces */
		pObj = SmSc->ObjectsPtrs;
		for ( int i = 0 ; i < SmSc->ulNumberOfObjects ; i++ , pObj++)
			(*pObj)->NV = Vector(0,0,0);

		/* Compute forces */
		for (int G = 0 ; G < SmSc->IK->ulNumberOfPatchs ; G++)
		{
			u32 Oi = SmSc->IK->pObjPatchs[G].ObjectRefIndex;
			SmSc->ObjectsPtrs[Oi]->GlobalMatrix = pCurrentHinstance[G].ComputedMatrix;

			//Gravity forces 
			SmSc->ObjectsPtrs[Oi]->NV += SmSc->IK->pObjPatchs[G].vWeight * GDT;
			// Torque forces 
	/*		u32 Fi;
			Fi = SmSc->ObjectsPtrs[Oi]->FatherIndex;
			if (Fi != -1)
			{
				Vector To,Tk;
				float PS,nTK;
				To = SmSc->ObjectsPtrs[Fi]->GlobalMatrix * (SmSc->ObjectsPtrs[Oi]->OriginalLocalMatrix.T);
				To = To - SmSc->ObjectsPtrs[Oi]->GlobalMatrix.T;
				SmSc->ObjectsPtrs[Oi]->NV += To * (GDT * SmSc->IK->pObjPatchs[G].fSpringStrength);
				SmSc->ObjectsPtrs[Fi]->NV -= To * (GDT * SmSc->IK->pObjPatchs[G].fSpringStrength);
			}*/
		}
		// Apply forces 
		for (int G = 0 ; G < SmSc->IK->ulNumberOfPatchs ; G++)
		{
			u32 Oi = SmSc->IK->pObjPatchs[G].ObjectRefIndex;
			pCurrentHinstance[G].vSpeed += SmSc->ObjectsPtrs[Oi]->NV * GDT;
			SmSc->ObjectsPtrs[Oi]->GlobalMatrix.T += pCurrentHinstance[G].vSpeed * GDT;
		}//*/

		// Compute matrix colliders 
		for ( int i = 0 ; i < SmSc->IK->ulNumberOfCollider; i++ )
		{
			pObj = &SmSc->ObjectsPtrs[SmSc->IK->pColliders[i].ObjectRefIndex];
			SmSc->IK->pColliders[i].TransMatrix.Identity();
			SmSc->IK->pColliders[i].TransMatrix.I &= SmSc->IK->pColliders[i].Radius;
			SmSc->IK->pColliders[i].TransMatrix.J &= SmSc->IK->pColliders[i].Radius;
			SmSc->IK->pColliders[i].TransMatrix.K &= SmSc->IK->pColliders[i].Radius;
			SmSc->IK->pColliders[i].TransMatrix.T = SmSc->IK->pColliders[i].Center;
			SmSc->IK->pColliders[i].TransMatrix *= (*pObj)->GlobalMatrix;
			SmSc->IK->pColliders[i].TransMatrix.Inverse(SmSc->IK->pColliders[i].InvMatrix);
		}		//*/

		/* Compute IK */
		for (u32 IKLoop = 0; IKLoop < 12 ; IKLoop++)
		{
			ComputeLocalMatrixes_IK(SmSc);
			pObj = SmSc->ObjectsPtrs;
			for ( int i = 0 ; i < SmSc->ulNumberOfObjects ; i++ , pObj++)
				(*pObj)->NV = Vector(0,0,0);
			pObj = SmSc->ObjectsPtrs;
			for ( int i = 0 ; i < SmSc->ulNumberOfObjects ; i++ , pObj++)
			{
				float N;
				N = (*pObj)->GlobalMatrix.I.Norm();
				N = (*pObj)->GlobalMatrix.J.Norm();
				N = (*pObj)->GlobalMatrix.K.Norm();
			}
			for (int G0 = 0 ; G0 < SmSc->IK->ulNumberOfPatchs ; G0++)
			{
				u32 G = SmSc->IK->pObjPatchs[G0].ObjectRefIndex;
				float DistanceToF,DistanceToFIK;
				Vector NormalDTFK;
				u32 Fi;
				Fi = SmSc->ObjectsPtrs[G]->FatherIndex;
				if (Fi != -1)
				{
					/* Max angle constraint */
					if ((IKLoop > 3)&&(SmSc->IK->pObjPatchs[G0].MaxAngle < 1.6f))
					{
						Vector To,Tk;
						Vector ToN,TkN,okCP;
						float PS,nTK;
						To = SmSc->ObjectsPtrs[Fi]->GlobalMatrix * SmSc->ObjectsPtrs[G]->OriginalLocalMatrix.T;
						Tk = SmSc->ObjectsPtrs[G]->GlobalMatrix.T;
						To -= SmSc->ObjectsPtrs[Fi]->GlobalMatrix.T;
						Tk -= SmSc->ObjectsPtrs[Fi]->GlobalMatrix.T;
						ToN = To;
						TkN = Tk;
						ToN.Normalize();
						TkN.Normalize();
						TkN += ToN;
						TkN.Normalize();
						okCP = ToN ^ TkN;
						if (okCP.Norm() > SmSc->IK->pObjPatchs[G0].MaxAngle)
						{
							Quat  CP,QVQ;
							Vector VQ;
							MATRIX Torque;
							MATRIX TorqueMax;

							CP.ExtractFrom2Edges(&Tk,&To);
							VQ = *(Vector *)&CP;
							VQ.Normalize();
							VQ *= -SmSc->IK->pObjPatchs[G0].MaxAngle ;
							QVQ = Quat(VQ);
							CP= CP * QVQ;
							CP.NormalizeW();
							CP.TransforToMatrix(&Torque);
							VQ = Torque * Tk;
							Tk = VQ - Tk;
							if (!Tk.CheckNAN())
							{
								SmSc->ObjectsPtrs[G]->GlobalMatrix.T += Tk*0.5f;
								SmSc->ObjectsPtrs[Fi]->GlobalMatrix.T -= Tk*0.5f;
							}
						}
					}
					//Lenght Constraint
					{
						/* Bone lenght constraint */
						DistanceToF		= (SmSc->ObjectsPtrs[G]->OriginalGlobalMatrix.T - SmSc->ObjectsPtrs[Fi]->OriginalGlobalMatrix.T).Norm();
						NormalDTFK		= SmSc->ObjectsPtrs[G]->GlobalMatrix.T;
						NormalDTFK		-= SmSc->ObjectsPtrs[Fi]->GlobalMatrix.T;
						DistanceToFIK	=  NormalDTFK.Norm();
						NormalDTFK.Normalize();
						DistanceToFIK -= DistanceToF;
						DistanceToFIK /= 2.0f;
						NormalDTFK *= DistanceToFIK;
						if (!NormalDTFK.CheckNAN())
						{
							SmSc->ObjectsPtrs[G]->NV -= NormalDTFK;
							SmSc->ObjectsPtrs[Fi]->NV += NormalDTFK;
						}
					}
					// Colliders constraint
					if (SmSc->IK->ulNumberOfCollider)
					{

						IK_Collider	*pCol,*pColLast;
						pCol = SmSc->IK->pColliders;
						pColLast = pCol + SmSc->IK->ulNumberOfCollider;
						while (pCol < pColLast)
						{
							Vector Into;
							Into = pCol->InvMatrix * SmSc->ObjectsPtrs[G]->GlobalMatrix.T;
							if (Into.SqrNorm() < 1.0f)
							{
								Into.Normalize();
								Into = pCol->TransMatrix * Into;
								Into -= SmSc->ObjectsPtrs[G]->GlobalMatrix.T;
								if (!Into.CheckNAN())
									SmSc->ObjectsPtrs[G]->NV += Into;
							};
							pCol++;
						}
						
					}//*/
				}
			}
			for (int G = 0 ; G < SmSc->IK->ulNumberOfPatchs ; G++)
			{
				u32 Oi = SmSc->IK->pObjPatchs[G].ObjectRefIndex;
				SmSc->ObjectsPtrs[Oi]->GlobalMatrix.T += SmSc->ObjectsPtrs[Oi]->NV;
			}

		}


		for (int G = 0 ; G < SmSc->ulNumberOfObjects ; G++)
		{
			/* recompute localmatrixes */
			u32 Fi;
			MATRIX mFi;
			MATRIX mGi;
			Fi = SmSc->ObjectsPtrs[G]->FatherIndex;
			if (Fi != -1)
			{
				SmSc->ObjectsPtrs[Fi]->GlobalMatrix.Inverse(mFi);
				SmSc->ObjectsPtrs[G]->LocalMatrix = SmSc->ObjectsPtrs[G]->GlobalMatrix;
				SmSc->ObjectsPtrs[G]->LocalMatrix *= mFi;
			}
		}
		for (int G = 0 ; G < SmSc->IK->ulNumberOfPatchs ; G++)
		{
			u32 Oi = SmSc->IK->pObjPatchs[G].ObjectRefIndex;
			pCurrentHinstance[G].vSpeed = SmSc->ObjectsPtrs[Oi]->GlobalMatrix.T - pCurrentHinstance[G].ComputedMatrix.T;
			pCurrentHinstance[G].vSpeed *= 1.0f / GDT;
			pCurrentHinstance[G].vSpeed *= 1.0f - SmSc->IK->pObjPatchs[G].VelocityAbsorbtion;
			if (pCurrentHinstance[G].vSpeed.CheckNAN() || pCurrentHinstance[G].vSpeed.SqrNorm()> 10000.0) 
				pCurrentHinstance[G].vSpeed = Vector(0,0,0);
				
			pCurrentHinstance[G].ComputedMatrix = SmSc->ObjectsPtrs[Oi]->GlobalMatrix;
//			if (pCurrentHinstance[G].ComputedMatrix.CheckNAN()) pCurrentHinstance[G].ComputedMatrix.Identity();
		}
	}

}