#ifndef __CONTACT_CONSTRAINT_H__
#define __CONTACT_CONSTRAINT_H__

#include "CRigidBody.h"
#include "CContactManifold.h"

#define IRPL_BAUMGARTE 0.1f
#define IRPL_ALLOWED_PENETRATION 0.01f

class CContactConstraint {
	friend class CContactManager;
public:
	inline void PreStep(float _fInvDt) {
		for (unsigned int I = 0U; I < m_cmContactManifold.m_ui32TotalContacts; ++I) {
			CContactManifold::CM_MANIFOLD_CONTACT& cContact = m_cmContactManifold.m_cContacts[I];

			CVector3 vR0 = cContact.vWorldPointOnA - m_prbRigidBody0->m_vPos;
			CVector3 vR1 = cContact.vWorldPointOnB - m_prbRigidBody1->m_vPos;

			//Normal:
			{
				float fMassSum = m_prbRigidBody0->InvMass() + m_prbRigidBody1->InvMass();
				if (!m_prbRigidBody0->IsStatic()) {
					fMassSum += (m_prbRigidBody0->m_mWorldInvInertia * vR0.Cross(cContact.vWorldNormalOnB)).Cross(vR0).Dot(cContact.vWorldNormalOnB);
				}
				if (!m_prbRigidBody1->IsStatic()) {
					fMassSum += (m_prbRigidBody1->m_mWorldInvInertia * vR1.Cross(cContact.vWorldNormalOnB)).Cross(vR1).Dot(cContact.vWorldNormalOnB);
				}
				cContact.fNormalMass = ML_ONE / fMassSum;
			}

                        //Tangents (two).
			for (unsigned int J = 0U; J < 2U; ++J) {
				//Tangent:
				const CVector3& vWorldTangent = cContact.vWorldTangents[J];
				float fMassSum = m_prbRigidBody0->InvMass() + m_prbRigidBody1->InvMass();
				if (!m_prbRigidBody0->IsStatic()) {
					fMassSum += (m_prbRigidBody0->m_mWorldInvInertia * vR0.Cross(vWorldTangent)).Cross(vR0).Dot(vWorldTangent);
				}
				if (!m_prbRigidBody1->IsStatic()) {
					fMassSum += (m_prbRigidBody1->m_mWorldInvInertia * vR1.Cross(vWorldTangent)).Cross(vR1).Dot(vWorldTangent);
				}
				cContact.fTangentMasses[J] = ML_ONE / fMassSum;
			}

			cContact.fBias = IRPL_BAUMGARTE * _fInvDt * CMathLib::Max(ML_ZERO, cContact.fDistance - IRPL_ALLOWED_PENETRATION);
		}
	}

	inline void WarmStart() {
		for (unsigned int I = 0U; I < m_cmContactManifold.m_ui32TotalContacts; ++I) {
			CContactManifold::CM_MANIFOLD_CONTACT& cContact = m_cmContactManifold.m_cContacts[I];

			CVector3 vR0 = cContact.vWorldPointOnA - m_prbRigidBody0->m_vPos;
			CVector3 vR1 = cContact.vWorldPointOnB - m_prbRigidBody1->m_vPos;

			CVector3 vP = cContact.vWorldNormalOnB * cContact.fPN;

			for (unsigned int J = 0U; J < 2U; ++J) {
				vP += cContact.vWorldTangents[J] * cContact.fPT[J];
			}

			if (!m_prbRigidBody0->IsStatic()) {
				m_prbRigidBody0->m_vLinVel -= vP * m_prbRigidBody0->InvMass();
				m_prbRigidBody0->m_vAngVel -= m_prbRigidBody0->m_mWorldInvInertia * vR0.Cross(vP);
			}

			if (!m_prbRigidBody1->IsStatic()) {
				m_prbRigidBody1->m_vLinVel += vP * m_prbRigidBody1->InvMass();
				m_prbRigidBody1->m_vAngVel += m_prbRigidBody1->m_mWorldInvInertia * vR1.Cross(vP);
			}
		}
	}


	inline void ApplyImpulse() {
		for (unsigned int I = 0U; I < m_cmContactManifold.m_ui32TotalContacts; ++I) {
			CContactManifold::CM_MANIFOLD_CONTACT& cContact = m_cmContactManifold.m_cContacts[I];

			CVector3 vR0 = cContact.vWorldPointOnA - m_prbRigidBody0->m_vPos;
			CVector3 vR1 = cContact.vWorldPointOnB - m_prbRigidBody1->m_vPos;

			//Tangents (two):
			{
				for (unsigned int J = 0U; J < 2U; ++J) {
					const CVector3& vTangent = cContact.vWorldTangents[J];

					CVector3 vVR0 = m_prbRigidBody0->m_vLinVel + m_prbRigidBody0->m_vAngVel.Cross(vR0);
					CVector3 vVR1 = m_prbRigidBody1->m_vLinVel + m_prbRigidBody1->m_vAngVel.Cross(vR1);
					CVector3 vDV = vVR1 - vVR0;

					float fLambda = cContact.fTangentMasses[J] * -vDV.Dot(vTangent);
					float fMaxLambda = 0.5f * cContact.fPN;

					float fPT0 = cContact.fPT[J];
					cContact.fPT[J] = CMathLib::Clamp(fPT0 + fLambda, -fMaxLambda, fMaxLambda);
					fLambda = cContact.fPT[J] - fPT0;
					CVector3 vPT = vTangent * fLambda;

					if (!m_prbRigidBody0->IsStatic()) {
						m_prbRigidBody0->m_vLinVel -= vPT * m_prbRigidBody0->InvMass();
						m_prbRigidBody0->m_vAngVel -= m_prbRigidBody0->m_mWorldInvInertia * vR0.Cross(vPT);
					}

					if (!m_prbRigidBody1->IsStatic()) {
						m_prbRigidBody1->m_vLinVel += vPT * m_prbRigidBody1->InvMass();
						m_prbRigidBody1->m_vAngVel += m_prbRigidBody1->m_mWorldInvInertia * vR1.Cross(vPT);
					}
				}
			}

			//Normal:
			{
				CVector3 vVR0 = m_prbRigidBody0->m_vLinVel + m_prbRigidBody0->m_vAngVel.Cross(vR0);
				CVector3 vVR1 = m_prbRigidBody1->m_vLinVel + m_prbRigidBody1->m_vAngVel.Cross(vR1);
				CVector3 vDV = vVR1 - vVR0;
				float fLambda = cContact.fNormalMass * -vDV.Dot(cContact.vWorldNormalOnB);
				float fPN0 = cContact.fPN;
				cContact.fPN = CMathLib::Max(fPN0 + fLambda, ML_ZERO);
				fLambda = cContact.fPN - fPN0;
				CVector3 vPN = cContact.vWorldNormalOnB * fLambda;

				if (!m_prbRigidBody0->IsStatic()) {
					m_prbRigidBody0->m_vLinVel -= vPN * m_prbRigidBody0->InvMass();
					m_prbRigidBody0->m_vAngVel -= m_prbRigidBody0->m_mWorldInvInertia * vR0.Cross(vPN);
				}

				if (!m_prbRigidBody1->IsStatic()) {
					m_prbRigidBody1->m_vLinVel += vPN * m_prbRigidBody1->InvMass();
					m_prbRigidBody1->m_vAngVel += m_prbRigidBody1->m_mWorldInvInertia * vR1.Cross(vPN);
				}
			}

			//Biased Normal:
			{
				CVector3 vVR0 = m_prbRigidBody0->m_vBiasLinVel + m_prbRigidBody0->m_vBiasAngVel.Cross(vR0);
				CVector3 vVR1 = m_prbRigidBody1->m_vBiasLinVel + m_prbRigidBody1->m_vBiasAngVel.Cross(vR1);
				CVector3 vDV = vVR1 - vVR0;
				float fLambda = cContact.fNormalMass * (-vDV.Dot(cContact.vWorldNormalOnB) + cContact.fBias);
				float fPNB0 = cContact.fPNB;
				cContact.fPNB = CMathLib::Max(fPNB0 + fLambda, ML_ZERO);
				fLambda = cContact.fPNB - fPNB0;

				CVector3 vPN = cContact.vWorldNormalOnB * fLambda;

				if (!m_prbRigidBody0->IsStatic()) {
					m_prbRigidBody0->m_vBiasLinVel -= vPN * m_prbRigidBody0->InvMass();
					m_prbRigidBody0->m_vBiasAngVel -= m_prbRigidBody0->m_mWorldInvInertia * vR0.Cross(vPN);
				}

				if (!m_prbRigidBody1->IsStatic()) {
					m_prbRigidBody1->m_vBiasLinVel += vPN * m_prbRigidBody1->InvMass();
					m_prbRigidBody1->m_vBiasAngVel += m_prbRigidBody1->m_mWorldInvInertia * vR1.Cross(vPN);
				}
			}
		}
	}
	//protected :
	CRigidBody* m_prbRigidBody0;
	CRigidBody* m_prbRigidBody1;
	CContactManifold m_cmContactManifold;
};


#endif //#ifndef __CONTACT_CONSTRAINT_H__