/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/      *
* Copyright (c) 2010-2013 Daniel Chappuis                                       *
*********************************************************************************
*                                                                               *
* 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.    *
*                                                                               *
********************************************************************************/

// Libraries
#include "ContactManifold.h"

using namespace reactphysics3d;

// Constructor
ContactManifold::ContactManifold(Body* const body1, Body* const body2,
                                 MemoryPool<ContactPoint>& memoryPoolContacts)
                : mBody1(body1), mBody2(body2), mNbContactPoints(0), mFrictionImpulse1(0.0),
                  mFrictionImpulse2(0.0), mFrictionTwistImpulse(0.0),
                  mMemoryPoolContacts(memoryPoolContacts) {
    
}

// Destructor
ContactManifold::~ContactManifold() {
    clear();
}

// Add a contact point in the manifold
void ContactManifold::addContactPoint(ContactPoint* contact) {
	
    // For contact already in the manifold
    for (uint i=0; i<mNbContactPoints; i++) {

		// Check if the new point point does not correspond to a same contact point
        // already in the manifold.
        decimal distance = (mContactPoints[i]->getWorldPointOnBody1() -
                            contact->getWorldPointOnBody1()).lengthSquare();
        if (distance <= PERSISTENT_CONTACT_DIST_THRESHOLD*PERSISTENT_CONTACT_DIST_THRESHOLD) {

            // Delete the new contact
            contact->ContactPoint::~ContactPoint();
            mMemoryPoolContacts.freeObject(contact);
            //removeContact(i);

            return;
            //break;
		}
	}
    
    // If the contact manifold is full
    if (mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD) {
        int indexMaxPenetration = getIndexOfDeepestPenetration(contact);
        int indexToRemove = getIndexToRemove(indexMaxPenetration, contact->getLocalPointOnBody1());
        removeContactPoint(indexToRemove);
    }

    // Add the new contact point in the manifold
    mContactPoints[mNbContactPoints] = contact;
    mNbContactPoints++;
}

// Remove a contact point from the manifold
void ContactManifold::removeContactPoint(int index) {
    assert(index >= 0 && index < mNbContactPoints);
    assert(mNbContactPoints > 0);
	
	// Call the destructor explicitly and tell the memory pool that
	// the corresponding memory block is now free
    mContactPoints[index]->ContactPoint::~ContactPoint();
    mMemoryPoolContacts.freeObject(mContactPoints[index]);
	
    // If we don't remove the last index
    if (index < mNbContactPoints - 1) {
        mContactPoints[index] = mContactPoints[mNbContactPoints - 1];
    }

    mNbContactPoints--;
}

// Update the contact manifold
// First the world space coordinates of the current contacts in the manifold are recomputed from
// the corresponding transforms of the bodies because they have moved. Then we remove the contacts
// with a negative penetration depth (meaning that the bodies are not penetrating anymore) and also
// the contacts with a too large distance between the contact points in the plane orthogonal to the
// contact normal
void ContactManifold::update(const Transform& transform1, const Transform& transform2) {
    if (mNbContactPoints == 0) return;

    // Update the world coordinates and penetration depth of the contact points in the manifold
    for (int i=0; i<mNbContactPoints; i++) {
        mContactPoints[i]->setWorldPointOnBody1(transform1 * mContactPoints[i]->getLocalPointOnBody1());
        mContactPoints[i]->setWorldPointOnBody2(transform2 * mContactPoints[i]->getLocalPointOnBody2());
        mContactPoints[i]->setPenetrationDepth((mContactPoints[i]->getWorldPointOnBody1() - mContactPoints[i]->getWorldPointOnBody2()).dot(mContactPoints[i]->getNormal()));
    }

    const decimal squarePersistentContactThreshold = PERSISTENT_CONTACT_DIST_THRESHOLD *
                                                     PERSISTENT_CONTACT_DIST_THRESHOLD;

    // Remove the contact points that don't represent very well the contact manifold
    for (int i=mNbContactPoints-1; i>=0; i--) {
        assert(i>= 0 && i < mNbContactPoints);

        // Compute the distance between contact points in the normal direction
        decimal distanceNormal = -mContactPoints[i]->getPenetrationDepth();
        
        // If the contacts points are too far from each other in the normal direction
        if (distanceNormal > squarePersistentContactThreshold) {
            removeContactPoint(i);
        }
        else {
            // Compute the distance of the two contact points in the plane
            // orthogonal to the contact normal
            Vector3 projOfPoint1 = mContactPoints[i]->getWorldPointOnBody1() +
                                   mContactPoints[i]->getNormal() * distanceNormal;
            Vector3 projDifference = mContactPoints[i]->getWorldPointOnBody2() - projOfPoint1;

            // If the orthogonal distance is larger than the valid distance
            // threshold, we remove the contact
            if (projDifference.lengthSquare() > squarePersistentContactThreshold) {
                removeContactPoint(i);
            }
        }
    }    
}

// Return the index of the contact point with the larger penetration depth. This
// corresponding contact will be kept in the cache. The method returns -1 is
// the new contact is the deepest.
int ContactManifold::getIndexOfDeepestPenetration(ContactPoint* newContact) const {
    assert(mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD);
    int indexMaxPenetrationDepth = -1;
    decimal maxPenetrationDepth = newContact->getPenetrationDepth();

    // For each contact in the cache
    for (uint i=0; i<mNbContactPoints; i++) {

        // If the current contact has a larger penetration depth
        if (mContactPoints[i]->getPenetrationDepth() > maxPenetrationDepth) {
            maxPenetrationDepth = mContactPoints[i]->getPenetrationDepth();
            indexMaxPenetrationDepth = i;
        }
    }

    // Return the index of largest penetration depth
    return indexMaxPenetrationDepth;
}

// Return the index that will be removed. The index of the contact point with the larger penetration
// depth is given as a parameter. This contact won't be removed. Given this contact, we compute
// the different area and we want to keep the contacts with the largest area. The new point is also
// kept. In order to compute the area of a quadrilateral, we use the formula :
// Area = 0.5 * | AC x BD | where AC and BD form the diagonals of the quadrilateral. Note that
// when we compute this area, we do not calculate it exactly but we
// only estimate it because we do not compute the actual diagonals of the quadrialteral. Therefore,
// this is only a guess that is faster to compute.
int ContactManifold::getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const {

    assert(mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD);

    decimal area0 = 0.0;       // Area with contact 1,2,3 and newPoint
    decimal area1 = 0.0;       // Area with contact 0,2,3 and newPoint
    decimal area2 = 0.0;       // Area with contact 0,1,3 and newPoint
    decimal area3 = 0.0;       // Area with contact 0,1,2 and newPoint

    if (indexMaxPenetration != 0) {
        // Compute the area
        Vector3 vector1 = newPoint - mContactPoints[1]->getLocalPointOnBody1();
        Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() - mContactPoints[2]->getLocalPointOnBody1();
        Vector3 crossProduct = vector1.cross(vector2);
        area0 = crossProduct.lengthSquare();
    }
    if (indexMaxPenetration != 1) {
        // Compute the area
        Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1();
        Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() - mContactPoints[2]->getLocalPointOnBody1();
        Vector3 crossProduct = vector1.cross(vector2);
        area1 = crossProduct.lengthSquare();
    }
    if (indexMaxPenetration != 2) {
        // Compute the area
        Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1();
        Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() - mContactPoints[1]->getLocalPointOnBody1();
        Vector3 crossProduct = vector1.cross(vector2);
        area2 = crossProduct.lengthSquare();
    }
    if (indexMaxPenetration != 3) {
        // Compute the area
        Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1();
        Vector3 vector2 = mContactPoints[2]->getLocalPointOnBody1() - mContactPoints[1]->getLocalPointOnBody1();
        Vector3 crossProduct = vector1.cross(vector2);
        area3 = crossProduct.lengthSquare();
    }
    
    // Return the index of the contact to remove
    return getMaxArea(area0, area1, area2, area3);
}

// Return the index of maximum area
int ContactManifold::getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const {
    if (area0 < area1) {
        if (area1 < area2) {
            if (area2 < area3) return 3;
            else return 2;
        }
        else {
            if (area1 < area3) return 3;
            else return 1;
        }
    }
    else {
        if (area0 < area2) {
            if (area2 < area3) return 3;
            else return 2;
        }
        else {
            if (area0 < area3) return 3;
            else return 0;
        }
    }
}

// Clear the contact manifold
void ContactManifold::clear() {
    for (uint i=0; i<mNbContactPoints; i++) {
		
		// Call the destructor explicitly and tell the memory pool that
		// the corresponding memory block is now free
        mContactPoints[i]->ContactPoint::~ContactPoint();
        mMemoryPoolContacts.freeObject(mContactPoints[i]);
    }
    mNbContactPoints = 0;
}