#ifndef __CONTACT_MANIFOLD_H__
#define __CONTACT_MANIFOLD_H__

#define MANIFOLD_CACHE_SIZE 4U
#define CONTACT_POINT_BREAKING_LENGTH 0.08f
#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 vWorldNormalOnA;
        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).

        static const CM_MANIFOLD_CONTACT* pcmContacts[MANIFOLD_CACHE_SIZE + 1] {
            &m_cContacts[0],
            &m_cContacts[1],
            &m_cContacts[2],
            &m_cContacts[3],
            &_cNewContact,
        };

        const CM_MANIFOLD_CONTACT* pmcDeepest = pcmContacts[0];
        float fMaxPenetration = pmcDeepest->fDistance;
        for ( unsigned int I = 1U; I < MANIFOLD_CACHE_SIZE + 1; ++I ) {
            const CM_MANIFOLD_CONTACT& cContact = *pcmContacts[I];
            if (cContact.fDistance > fMaxPenetration) {
                fMaxPenetration = cContact.fDistance;
                pmcDeepest = &cContact;
            }
        }

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

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

        //Fourth
        const CM_MANIFOLD_CONTACT* pmcFurthest3 = pcmContacts[0];
        float fDistanceSq3 = pmcFurthest3->fDistance;
        for (unsigned int I = 1U; I < MANIFOLD_CACHE_SIZE + 1; ++I) {
            const CM_MANIFOLD_CONTACT& cContact = *pcmContacts[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];
        }
    }

    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;
        float fPNB = cContact.fPNB;
        CVector3 vLambdaT = cContact.vWorldTangents[0] * cContact.fPT[0] + cContact.vWorldTangents[1] * cContact.fPT[1];

        cContact = _cNewContact;

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

    inline void ReplaceAdd(const CM_MANIFOLD_CONTACT& _cNewContact) {
        /*
        * We start with the maximum threshold as initial value.
        * If a less distance is found we know that it is
        * less from the threshold and we have an index to replace.
        */
        float fMinDist = CONTACT_POINT_BREAKING_LENGTH_SQ;
        int iReplaceIndex = -1;
        for (unsigned int I = 0U; I < m_ui32TotalContacts; ++I) {
            const CM_MANIFOLD_CONTACT& cContact = m_cContacts[I];
            CVector3 vDiff = cContact.vLocalPointOnA - _cNewContact.vLocalPointOnA;
            float fDist = vDiff.LenSq();
            if (fDist < fMinDist) {
                fMinDist = fDist;
                iReplaceIndex = I;
            }
        }

        if (iReplaceIndex > -1) {
            ReplaceContact(_cNewContact, iReplaceIndex);
        }
        else {
            AddContact(_cNewContact);
        }
    }

    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];

            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.fDistance = vDiff.Dot(cContact.vWorldNormalOnA);

            if (cContact.fDistance <= ML_ZERO &&
                vDiffA.LenSq() < CONTACT_POINT_BREAKING_LENGTH_SQ &&
                vDiffB.LenSq() < CONTACT_POINT_BREAKING_LENGTH_SQ) {
                cContact.vWorldPointOnA = vWorldA;
                cContact.vWorldPointOnB = vWorldB;
                cContact.fPNB = ML_ZERO;
            }
            else {
                RemoveContact(I);
            }
        }
    }

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

#endif //#ifndef __CONTACT_MANIFOLD_H__