//Contact Manager:

#ifndef __CONTACT_MANAGER_H__
#define __CONTACT_MANAGER_H__

#define SEQUENTIAL_ITERATIONS 10U

#include <vector>
#include "CContactConstraint.h"

class CContactManager {
public :
        inline void AddContact(CRigidBody* _prbBody0, CRigidBody* _prbBody1, const CContactManifold::CM_MANIFOLD_CONTACT& _cContact) {
                for ( unsigned int I = 0U; I < m_vContactConstraints.size(); ++I ) {
                        CContactConstraint& ccContactConstraint = m_vContactConstraints[I];

                        CContactManifold& cmContactManifold = ccContactConstraint.m_cmContactManifold;
                        if (_prbBody0 == ccContactConstraint.m_prbRigidBody0 &&
                                _prbBody1 == ccContactConstraint.m_prbRigidBody1) {

                                if ( cmContactManifold.LookUpAndReplace(_cContact) ) {
                                        return;
                                }
                                else {
                                        cmContactManifold.AddContact(_cContact);
                                        return;
                                }
                        }
                }

                m_vContactConstraints.push_back( CContactConstraint() );

                CContactConstraint& ccContactConstraint = m_vContactConstraints.back();
                CContactManifold& cmContactManifold = ccContactConstraint.m_cmContactManifold;
                ccContactConstraint.m_prbRigidBody0 = _prbBody0;
                ccContactConstraint.m_prbRigidBody1 = _prbBody1;
                cmContactManifold.AddContact(_cContact);
        }

        inline void PreStep(float _fInvDt) {
                for (unsigned int I = 0U; I < m_vContactConstraints.size(); ++I) {
                        m_vContactConstraints[I].PreStep(_fInvDt);
                }
        }

        inline void WarmStart() {
                for (unsigned int I = 0U; I < m_vContactConstraints.size(); ++I) {
                        m_vContactConstraints[I].WarmStart();
                }
        }

        inline void ApplyImpulses() {
                for (unsigned int I = 0UL; I < SEQUENTIAL_ITERATIONS; ++I) {
                        for (unsigned int J = 0UL; J < m_vContactConstraints.size(); ++J) {
                                m_vContactConstraints[J].ApplyImpulse();
                        }
                }
        }

        inline void UpdateContacts() {
                for ( unsigned int I = 0U; I < m_vContactConstraints.size(); ++I ) {
                        CContactConstraint& ccContactConstraint = m_vContactConstraints[I];
                        ccContactConstraint.m_cmContactManifold.Update(ccContactConstraint.m_prbRigidBody0->GetTransform(), ccContactConstraint.m_prbRigidBody1->GetTransform());
                }
        }
//protected :
        std::vector<CContactConstraint> m_vContactConstraints;
};


#endif //#ifndef __CONTACT_MANAGER_H__

//Contact Manifold (each constraint has one):

#ifndef __CONTACT_MANIFOLD_H__
#define __CONTACT_MANIFOLD_H__

#define MANIFOLD_CACHE_SIZE 4U
#define CONTACT_POINT_BREAKING_LENGTH 0.2f
#define CONTACT_POINT_BREAKING_LENGTH_SQ CONTACT_POINT_BREAKING_LENGTH * CONTACT_POINT_BREAKING_LENGTH

#include "CIntersect.h"
#include "CDistance.h"

class CContactManifold {
        friend class CContactConstraint;
public:
        struct CM_MANIFOLD_CONTACT {
                unsigned long long ui64Time;
                CVector3 vLocalPointOnA;
                CVector3 vLocalPointOnB;

                CVector3 vWorldPointOnA;
                CVector3 vWorldPointOnB;

                CVector3 vWorldNormalOnB;
                CVector3 vWorldTangents[2];

                float fDistance;

                float fNormalMass;
                float fTangentMasses[2];

                float fPN;
                float fPT[2];

                float fPNB;
                float fBias;
                bool bWarmStart;

                inline CM_MANIFOLD_CONTACT() :
                        ui64Time(0ULL),
                        fPN(ML_ZERO),
                        fBias(ML_ZERO),
                        fPNB(ML_ZERO),
                        fDistance(ML_ZERO),
                        bWarmStart(false) {
                        fNormalMass = fTangentMasses[0] = fTangentMasses[1] = fPT[0] = fPT[1] = ML_ZERO;
                }
        };

        inline CContactManifold() :     m_ui32TotalContacts(0U) {
        }

        inline void ReplaceWhenFull(const CM_MANIFOLD_CONTACT& _cNewContact) {
                //First (deepest).
                const CM_MANIFOLD_CONTACT* pmcDeepest = &m_cContacts[0];
                float fMaxPenetration = pmcDeepest->fDistance;
                for ( unsigned int I = 1U; I < MANIFOLD_CACHE_SIZE; ++I ) {
                        const CM_MANIFOLD_CONTACT& cContact = m_cContacts[I];
                        if (cContact.fDistance > fMaxPenetration) {
                                fMaxPenetration = cContact.fDistance;
                                pmcDeepest = &cContact;
                        }
                }

                //Second (furthest).
                const CM_MANIFOLD_CONTACT* pmcFurthest1 = &m_cContacts[0];
                float fDistanceSq1 = pmcFurthest1->fDistance;
                for ( unsigned int I = 1U; I < MANIFOLD_CACHE_SIZE; ++I ) {
                        const CM_MANIFOLD_CONTACT& cContact = m_cContacts[I];
                        float fDistance = (cContact.vWorldPointOnA - pmcDeepest->vWorldPointOnA).LenSq();
                        if (fDistance > fDistanceSq1) {
                                fDistanceSq1 = fDistance;
                                pmcFurthest1 = &cContact;
                        }
                }

                //Third (furthest).
                const CM_MANIFOLD_CONTACT* pmcFurthest2 = &m_cContacts[0];
                float fDistanceSq2 = pmcFurthest2->fDistance;
                for (unsigned int I = 1U; I < MANIFOLD_CACHE_SIZE; ++I) {
                        const CM_MANIFOLD_CONTACT& cContact = m_cContacts[I];
                        float fDistanceToSegment = CDistance::SqDistPointSegment(cContact.vWorldPointOnA, pmcDeepest->vWorldPointOnA, pmcFurthest1->vWorldPointOnA);
                        if (fDistanceToSegment > fDistanceSq2) {
                                fDistanceSq2 = fDistanceToSegment;
                                pmcFurthest2 = &cContact;
                        }
                }

                //Fourth
                const CM_MANIFOLD_CONTACT* pmcFurthest3 = &m_cContacts[0];
                float fDistanceSq3 = pmcFurthest3->fDistance;
                for (unsigned int I = 1U; I < MANIFOLD_CACHE_SIZE; ++I) {
                        const CM_MANIFOLD_CONTACT& cContact = m_cContacts[I];
                        float fDistanceFromTriangle = CDistance::SqDistPointTriangle(cContact.vWorldPointOnA, pmcDeepest->vWorldPointOnA, pmcFurthest1->vWorldPointOnA, pmcFurthest2->vWorldPointOnA);
                        if (fDistanceFromTriangle > fDistanceSq3) {
                                fDistanceSq3 = fDistanceFromTriangle;
                                pmcFurthest3 = &cContact;
                        }
                }

                //Clear the manifold.
                m_ui32TotalContacts = 0U;

                m_cContacts[m_ui32TotalContacts++] = *pmcDeepest;
                m_cContacts[m_ui32TotalContacts++] = *pmcFurthest1;
                m_cContacts[m_ui32TotalContacts++] = *pmcFurthest2;

                if ( !CIntersect::PointInTriangle(pmcFurthest3->vWorldPointOnA, pmcDeepest->vWorldPointOnA, pmcFurthest1->vWorldPointOnA, pmcFurthest2->vWorldPointOnA) ) {
                        m_cContacts[m_ui32TotalContacts++] = *pmcFurthest3;
                }
        }

        inline void AddContact(const CM_MANIFOLD_CONTACT& _cNewContact) {
                if (m_ui32TotalContacts == MANIFOLD_CACHE_SIZE) {
                        ReplaceWhenFull( _cNewContact );
                }
                else {
                        m_cContacts[m_ui32TotalContacts++] = _cNewContact;
                }
        }

        inline void RemoveContact(unsigned int _ui32Index) {
                unsigned int ui32LastIndex = --m_ui32TotalContacts;
                if (ui32LastIndex != _ui32Index) {
                        m_cContacts[_ui32Index] = m_cContacts[ui32LastIndex];
                        EmptyContact(ui32LastIndex);
                }
        }

        inline void ReplaceContact(const CM_MANIFOLD_CONTACT& _cNewContact, unsigned int _ui32Index) {
                CM_MANIFOLD_CONTACT& cContact = m_cContacts[_ui32Index];

                unsigned long long ui64Time = cContact.ui64Time;
                float fPN = cContact.fPN;
                CVector3 vLambdaT = cContact.vWorldTangents[0] * cContact.fPT[0] + cContact.vWorldTangents[1] * cContact.fPT[1];

                cContact = _cNewContact;

                cContact.ui64Time = ui64Time;
                cContact.fPN = fPN;
                cContact.fPT[0] = vLambdaT.Dot(_cNewContact.vWorldTangents[0]);
                cContact.fPT[1] = vLambdaT.Dot(_cNewContact.vWorldTangents[1]);
        }

        inline void EmptyContact(unsigned int _ui32Index) {
                m_cContacts[_ui32Index] = CM_MANIFOLD_CONTACT();
        }

        inline void Update(const CMatrix4x4& _mWorldA, const CMatrix4x4& _mWorldB) {
                for (unsigned int I = 0U; I < m_ui32TotalContacts; ++I) {
                        CM_MANIFOLD_CONTACT& cContact = m_cContacts[I];
                        ++cContact.ui64Time;

                        CVector3 vWorldA = _mWorldA * cContact.vLocalPointOnA;
                        CVector3 vWorldB = _mWorldB * cContact.vLocalPointOnB;

                        CVector3 vDiffA = vWorldA - cContact.vWorldPointOnA;
                        CVector3 vDiffB = vWorldB - cContact.vWorldPointOnB;
                        CVector3 vDiff = vWorldB - vWorldA;

                        cContact.vWorldPointOnA = vWorldA;
                        cContact.vWorldPointOnB = vWorldB;
                        cContact.fDistance = vDiff.Dot(cContact.vWorldNormalOnB);
                        cContact.fPNB = ML_ZERO;

                        bool bStillPenetrating = cContact.fDistance <= ML_ZERO;
                        bool bRACloseEnough = vDiffA.LenSq() < CONTACT_POINT_BREAKING_LENGTH_SQ;
                        bool bRBCloseEnough = vDiffB.LenSq() < CONTACT_POINT_BREAKING_LENGTH_SQ;

                        if (!(bRACloseEnough && bRBCloseEnough)) {
                                RemoveContact(I);
                        }
                }
        }

        inline bool LookUpAndReplace(const CM_MANIFOLD_CONTACT& _cNewContact) {
                for (unsigned int I = 0U; I < m_ui32TotalContacts; ++I) {
                        CM_MANIFOLD_CONTACT& cContact = m_cContacts[I];

                        CVector3 vDiffA = _cNewContact.vWorldPointOnA - cContact.vWorldPointOnA;
                        CVector3 vDiffB = _cNewContact.vWorldPointOnB - cContact.vWorldPointOnB;

                        bool bRAFarEnough = vDiffA.LenSq() > CONTACT_POINT_BREAKING_LENGTH_SQ;
                        bool bRBFarEnough = vDiffB.LenSq() > CONTACT_POINT_BREAKING_LENGTH_SQ;

                        if ( bRAFarEnough && bRBFarEnough ) {
                                ReplaceContact(_cNewContact, I);
                                return true;
                        }
                }
                return false;
        }

//protected :
        CM_MANIFOLD_CONTACT m_cContacts[MANIFOLD_CACHE_SIZE];
        unsigned int m_ui32TotalContacts;
};

#endif //#ifndef __CONTACT_MANIFOLD_H__