
// zlib open source license
//
// Copyright (c) 2010 to 2013 David Forsgren Piuva
// 
// This software is provided 'as-is', without any express or implied
// warranty. In no event will the authors be held liable for any damages
// arising from the use of this software.
// 
// Permission is granted to anyone to use this software for any purpose,
// including commercial applications, and to alter it and redistribute it
// freely, subject to the following restrictions:
// 
//    1. The origin of this software must not be misrepresented; you must not
//    claim that you wrote the original software. If you use this software
//    in a product, an acknowledgment in the product documentation would be
//    appreciated but is not required.
// 
//    2. Altered source versions must be plainly marked as such, and must not be
//    misrepresented as being the original software.
// 
//    3. This notice may not be removed or altered from any source
//    distribution.

void CDFPGECtrl::Physics_Step(float TimeStep) {
	CRITICAL_CODE_SECTION(
		if (!DGE.m_Running) {
			REPORT_NOT_RUNNING(L"Physics_Step")
		} else if (TimeStep <= 0) {
			MQ->InsertMessage(L"Physics_Step: TimeStep is not greater than zero.");
		} else {
			DGE.Physics_Step(TimeStep);
		}
	)
}

int CDFPGECtrl::Physics_GetAllContactsFromLastStep_OutCB(bool IncludeContactsFromSleepingBodies) {
	int Result;
	CRITICAL_CODE_SECTION(
		int I; int J;
		CB.Clear();
		LoopForwardLengthFromZero(I,DGE.m_PhysicsCore.dispatcher->getNumManifolds()) {
			btPersistentManifold* contactManifold = DGE.m_PhysicsCore.dispatcher->getManifoldByIndexInternal(I);
			btCollisionObject* BodyA = static_cast<btCollisionObject*>(contactManifold->getBody0());
			btCollisionObject* BodyB = static_cast<btCollisionObject*>(contactManifold->getBody1());
			if (IncludeContactsFromSleepingBodies || BodyA->isActive() || BodyB->isActive()) {
				LoopForwardLengthFromZero(J,contactManifold->getNumContacts()) {
					btManifoldPoint& cp = contactManifold->getContactPoint(J);
					CB.addSingleResult(cp,BodyA,0,0,BodyB,0,0);
				}
			}
		}
		Result = CB.ContactArray.size();
	)
	return Result;
}

// Sum of impulses in the pair at the average position and normal
int CDFPGECtrl::Physics_GetAllPairSumContactsFromLastStep_OutCB(bool IncludeContactsFromSleepingBodies) {
	int Result;
	START_OF_CRITICAL_SECTION
		int I; int J; int Contacts;
		btVector3 PositionSum;
		btVector3 NormalSum;
		float ImpulseSum;
		CB.Clear();
		LoopForwardLengthFromZero(I,DGE.m_PhysicsCore.dispatcher->getNumManifolds()) {
			btPersistentManifold* contactManifold = DGE.m_PhysicsCore.dispatcher->getManifoldByIndexInternal(I);
			btCollisionObject* BodyA = static_cast<btCollisionObject*>(contactManifold->getBody0());
			btCollisionObject* BodyB = static_cast<btCollisionObject*>(contactManifold->getBody1());
			if (IncludeContactsFromSleepingBodies || BodyA->isActive() || BodyB->isActive()) {
				Contacts = 0;
				ImpulseSum = 0.0f;
				PositionSum = btVector3(0.0f,0.0f,0.0f);
				NormalSum = btVector3(0.0f,0.0f,0.0f);
				btManifoldPoint cp;
				LoopForwardLengthFromZero(J,contactManifold->getNumContacts()) {
					cp = contactManifold->getContactPoint(J);
					ImpulseSum += cp.m_appliedImpulse;
					NormalSum += cp.m_normalWorldOnB;
					PositionSum += cp.m_positionWorldOnB;
					Contacts++;
				}
				if (Contacts > 0) {
					cp.m_appliedImpulse = ImpulseSum;
					cp.m_positionWorldOnB = PositionSum / (float)Contacts;
					cp.m_normalWorldOnB = NormalSum.normalize();
					CB.addSingleResult(cp,BodyA,0,0,BodyB,0,0);
				}
			}
		}
		Result = CB.ContactArray.size();
	END_OF_CRITICAL_SECTION
	return Result;
}

void CDFPGECtrl::Physics_SetGravity(float X,float Y,float Z) {
	CRITICAL_CODE_SECTION(
		if (DGE.m_Running) {
			DGE.m_PhysicsCore.dynamicsWorld->setGravity(btVector3(X,Y,Z));
		} else {
			REPORT_NOT_RUNNING(L"Physics_GetGravity_OutV3")
		}
	)
}

void CDFPGECtrl::Physics_GetGravity_OutV3(void) {
	CRITICAL_CODE_SECTION(
		ClearMatrixBuffer();
		if (DGE.m_Running) {
			btVector3 Result;
			Result = DGE.m_PhysicsCore.dynamicsWorld->getGravity();
			X1 = Result.x();
			Y1 = Result.y();
			Z1 = Result.z();
		} else {
			REPORT_NOT_RUNNING(L"Physics_GetGravity_OutV3")
		}
	)
}

float CDFPGECtrl::Physics_GetDebugDrawRadius(void) {
	float Result;
	CRITICAL_CODE_SECTION(
		Result = DGE.m_DebugDrawRadius;
	)
	return Result;
}

void CDFPGECtrl::Physics_SetDebugDrawRadius(float NewValue) {
	CRITICAL_CODE_SECTION(
		DGE.m_DebugDrawRadius = NewValue;
	)
}

void CDFPGECtrl::PlaceAtRigidBody(Instance_Struct* pInstance, RigidBody_Struct* pRigidBody, bool UseLocalScaling) {
	btTransform Trans = pRigidBody->Body->getWorldTransform();
	if (UseLocalScaling) {
		btVector3 LocalScaling = pRigidBody->CollisionShape->CollisionShape->getLocalScaling();
		Instance_PlaceAtBulletTransform_Scaled(pInstance,&Trans,&LocalScaling);
	} else {
		Instance_PlaceAtBulletTransform(pInstance,&Trans);
	}
	
}
void CDFPGECtrl::PlaceAtRigidBody_Interpolated(Instance_Struct* pInstance, RigidBody_Struct* pRigidBody, float TimeOffset, bool UseLocalScaling) {
	btTransform Trans = pRigidBody->Body->getWorldTransform();
	if (pRigidBody->Body->isStaticOrKinematicObject() || pRigidBody->Body->getActivationState() == ISLAND_SLEEPING) {
		// No interpolation needed
		if (UseLocalScaling) {
			btVector3 LocalScaling = pRigidBody->CollisionShape->CollisionShape->getLocalScaling();
			Instance_PlaceAtBulletTransform_Scaled(pInstance,&Trans,&LocalScaling);
		} else {
			Instance_PlaceAtBulletTransform(pInstance,&Trans);
		}
	} else {
		// Use interpolation
		btTransform InterpolatedTrans;
		btTransformUtil::integrateTransform(Trans,pRigidBody->Body->getInterpolationLinearVelocity(),pRigidBody->Body->getInterpolationAngularVelocity(),TimeOffset*pRigidBody->Body->getHitFraction(),InterpolatedTrans);
		if (UseLocalScaling) {
			btVector3 LocalScaling = pRigidBody->CollisionShape->CollisionShape->getLocalScaling();
			Instance_PlaceAtBulletTransform_Scaled(pInstance,&InterpolatedTrans,&LocalScaling);
		} else {
			Instance_PlaceAtBulletTransform(pInstance,&InterpolatedTrans);
		}
	}
}

void CDFPGECtrl::FollowRigidBody(RigidBody_Struct* pRigidBody) {
	DoubleLinkListBody<Instance_Struct>* FollowerIterator;
	FollowerIterator = pRigidBody->LL_Follow.Head;
	while (FollowerIterator) {
		if (FollowerIterator->Content->FollowedRigidBody) {
			PlaceAtRigidBody(FollowerIterator->Content,pRigidBody,FollowerIterator->Content->UseLocalScaling);
		}
		FollowerIterator = FollowerIterator->Next;
	}
}

void CDFPGECtrl::FollowRigidBody_Interpolated(RigidBody_Struct* pRigidBody,float TimeOffset) {
	DoubleLinkListBody<Instance_Struct>* FollowerIterator;
	FollowerIterator = pRigidBody->LL_Follow.Head;
	while (FollowerIterator) {
		if (FollowerIterator->Content->FollowedRigidBody) {
			PlaceAtRigidBody_Interpolated(FollowerIterator->Content,pRigidBody,TimeOffset,FollowerIterator->Content->UseLocalScaling);
		}
		FollowerIterator = FollowerIterator->Next;
	}
}

void CDFPGECtrl::Physics_LetInstancesFollowRigidBodies(void) {
	START_OF_CRITICAL_SECTION
		int BodyIndex;
		Try_Start_Ctrl {
			btAlignedObjectArray<btRigidBody*>* NonStaticBodies = DGE.m_PhysicsCore.dynamicsWorld->getNonStaticRigidBodyCollection();
			LoopForwardLengthFromZero(BodyIndex,NonStaticBodies->size()) {
				btRigidBody* RB = (*NonStaticBodies)[BodyIndex];
				if (RB->isActive()) {
					RigidBody_Struct* RigidBody = (RigidBody_Struct*)(RB->getUserPointer());
					if (RigidBody) {
						Try_Start_Ctrl {
							FollowRigidBody(RigidBody);
						} Try_Else_Ctrl {
							MQ->InsertMessage(L"Access violation in Physics_LetInstancesFollowRigidBodies while iterating the double link list of following instances. Report how you got this error if you found it in the latest version of the graphics engine.");
						}
					}
				}
			}
		} Try_Else_Ctrl {
			MQ->InsertMessage(L"Access violation in Physics_LetInstancesFollowRigidBodies while iterating the dynamic array of non static rigid bodies. Report how you got this error if you found it in the latest version of the graphics engine.");
		}
	END_OF_CRITICAL_SECTION
}

void CDFPGECtrl::Physics_LetInstancesFollowRigidBodies_Interpolated(float TimeOffset) {
	START_OF_CRITICAL_SECTION
		int BodyIndex;
		Try_Start_Ctrl {
			btAlignedObjectArray<btRigidBody*>* NonStaticBodies = DGE.m_PhysicsCore.dynamicsWorld->getNonStaticRigidBodyCollection();
			LoopForwardLengthFromZero(BodyIndex,NonStaticBodies->size()) {
				btRigidBody* RB = (*NonStaticBodies)[BodyIndex];
				if (RB->isActive()) {
					RigidBody_Struct* RigidBody = (RigidBody_Struct*)(RB->getUserPointer());
					if (RigidBody) {
						Try_Start_Ctrl {
							FollowRigidBody_Interpolated(RigidBody,TimeOffset);
						} Try_Else_Ctrl {
							MQ->InsertMessage(L"Access violation in Physics_LetInstancesFollowRigidBodies_Interpolated while iterating the double link list of following instances. Report how you got this error if you found it in the latest version of the graphics engine.");
						}
					}
				}
			}
		} Try_Else_Ctrl {
			MQ->InsertMessage(L"Access violation in Physics_LetInstancesFollowRigidBodies_Interpolated while iterating the dynamic array of non static rigid bodies. Report how you got this error if you found it in the latest version of the graphics engine.");
		}
	END_OF_CRITICAL_SECTION
}
