#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).
        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];
        }
    }

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

    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.vWorldNormalOnA);
            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) {
                cContact.bWarmStart = true;
            }
            else {
                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__