/********************************************************************************
* 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 "BroadPhaseAlgorithm.h"

// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;

// Constructor
BroadPhaseAlgorithm::BroadPhaseAlgorithm(CollisionDetection& collisionDetection)
                    :mDynamicAABBTree(*this), mNbMovedShapes(0), mNbAllocatedMovedShapes(8),
                     mNbNonUsedMovedShapes(0), mNbPotentialPairs(0), mNbAllocatedPotentialPairs(8),
                     mPairManager(collisionDetection), mCollisionDetection(collisionDetection) {

    // Allocate memory for the array of non-static bodies IDs
    mMovedShapes = (int*) malloc(mNbAllocatedMovedShapes * sizeof(int));
    assert(mMovedShapes);

    // Allocate memory for the array of potential overlapping pairs
    mPotentialPairs = (BroadPair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPair));
    assert(mPotentialPairs);
}

// Destructor
BroadPhaseAlgorithm::~BroadPhaseAlgorithm() {

    // Release the memory for the array of non-static bodies IDs
    free(mMovedShapes);

    // Release the memory for the array of potential overlapping pairs
    free(mPotentialPairs);
}

// Add a collision shape in the array of shapes that have moved in the last simulation step
// and that need to be tested again for broad-phase overlapping.
void BroadPhaseAlgorithm::addMovedCollisionShape(int broadPhaseID) {

    // Allocate more elements in the array of bodies that have moved if necessary
    if (mNbAllocatedMovedShapes == mNbMovedShapes) {
        mNbAllocatedMovedShapes *= 2;
        int* oldArray = mMovedShapes;
        mMovedShapes = (int*) malloc(mNbAllocatedMovedShapes * sizeof(int));
        assert(mMovedShapes);
        memcpy(mMovedShapes, oldArray, mNbMovedShapes * sizeof(int));
        free(oldArray);
    }

    // Store the broad-phase ID into the array of bodies that have moved
    mMovedShapes[mNbMovedShapes] = broadPhaseID;
    mNbMovedShapes++;
}

// Remove a collision shape from the array of shapes that have moved in the last simulation step
// and that need to be tested again for broad-phase overlapping.
void BroadPhaseAlgorithm::removeMovedCollisionShape(int broadPhaseID) {

    assert(mNbNonUsedMovedShapes <= mNbMovedShapes);

    // If less than the quarter of allocated elements of the non-static bodies IDs array
    // are used, we release some allocated memory
    if ((mNbMovedShapes - mNbNonUsedMovedShapes) < mNbAllocatedMovedShapes / 4 &&
            mNbAllocatedMovedShapes > 8) {

        mNbAllocatedMovedShapes /= 2;
        int* oldArray = mMovedShapes;
        mMovedShapes = (int*) malloc(mNbAllocatedMovedShapes * sizeof(int));
        assert(mMovedShapes);
        uint nbElements = 0;
        for (uint i=0; i<mNbMovedShapes; i++) {
            if (oldArray[i] != -1) {
                mMovedShapes[nbElements] = oldArray[i];
                nbElements++;
            }
        }
        mNbMovedShapes = nbElements;
        mNbNonUsedMovedShapes = 0;
        free(oldArray);
    }

    // Remove the broad-phase ID from the array
    for (uint i=0; i<mNbMovedShapes; i++) {
        if (mMovedShapes[i] == broadPhaseID) {
            mMovedShapes[i] = -1;
            mNbNonUsedMovedShapes++;
            break;
        }
    }
}

// Add a proxy collision shape into the broad-phase collision detection
void BroadPhaseAlgorithm::addProxyCollisionShape(ProxyShape* proxyShape) {

    // Add the collision shape into the dynamic AABB tree and get its broad-phase ID
    mDynamicAABBTree.addObject(proxyShape);

    // Add the collision shape into the array of bodies that have moved (or have been created)
    // during the last simulation step
    addMovedCollisionShape(proxyShape->mBroadPhaseID);
}

// Remove a proxy collision shape from the broad-phase collision detection
void BroadPhaseAlgorithm::removeProxyCollisionShape(ProxyShape* proxyShape) {

    int broadPhaseID = proxyShape->mBroadPhaseID;

    // Remove the collision shape from the dynamic AABB tree
    mDynamicAABBTree.removeObject(broadPhaseID);

    // Remove the collision shape into the array of bodies that have moved (or have been created)
    // during the last simulation step
    removeMovedCollisionShape(broadPhaseID);
}

// Notify the broad-phase that a collision shape has moved and need to be updated
void BroadPhaseAlgorithm::updateProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb) {

    int broadPhaseID = proxyShape->mBroadPhaseID;

    assert(broadPhaseID >= 0);

    // Update the dynamic AABB tree according to the movement of the collision shape
    bool hasBeenReInserted = mDynamicAABBTree.updateObject(broadPhaseID, aabb);

    // If the collision shape has moved out of its fat AABB (and therefore has been reinserted
    // into the tree).
    if (hasBeenReInserted) {

        // Add the collision shape into the array of bodies that have moved (or have been created)
        // during the last simulation step
        addMovedCollisionShape(broadPhaseID);
    }
}

// Compute all the overlapping pairs of collision shapes
void BroadPhaseAlgorithm::computeOverlappingPairs() {

    // Reset the potential overlapping pairs
    mNbPotentialPairs = 0;

    // For all collision shapes that have moved (or have been created) during the
    // last simulation step
    for (uint i=0; i<mNbMovedShapes; i++) {
        uint shapeID = mMovedShapes[i];

        if (shapeID == -1) continue;

        // Get the fat AABB of the collision shape from the dynamic AABB tree
        const AABB& shapeAABB = mDynamicAABBTree.getFatAABB(shapeID);

        // Ask the dynamic AABB tree to report all collision shapes that overlap with
        // this AABB. The method BroadPhase::notifiyOverlappingPair() will be called
        // by the dynamic AABB tree for each potential overlapping pair.
        mDynamicAABBTree.reportAllShapesOverlappingWith(shapeID, shapeAABB);
    }

    // Reset the array of collision shapes that have move (or have been created) during the
    // last simulation step
    mMovedShapes = 0;

    // Sort the array of potential overlapping pairs in order to remove duplicate pairs
    std::sort(mPotentialPairs, mPotentialPairs + mNbPotentialPairs, BroadPair::smallerThan);

    // Check all the potential overlapping pairs avoiding duplicates to report unique
    // overlapping pairs
    uint i=0;
    while (i < mNbPotentialPairs) {

        // Get a potential overlapping pair
        BroadPair* pair = mPotentialPairs + i;
        i++;

        // Get the two collision shapes of the pair
        ProxyShape* shape1 = mDynamicAABBTree.getCollisionShape(pair->collisionShape1ID);
        ProxyShape* shape2 = mDynamicAABBTree.getCollisionShape(pair->collisionShape2ID);

        // Notify the collision detection about the overlapping pair
        mCollisionDetection.broadPhaseNotifyOverlappingPair(shape1, shape2);

        // Skip the duplicate overlapping pairs
        while (i < mNbPotentialPairs) {

            // Get the next pair
            BroadPair* nextPair = mPotentialPairs + i;

            // If the next pair is different from the previous one, we stop skipping pairs
            if (nextPair->collisionShape1ID != pair->collisionShape1ID ||
                nextPair->collisionShape2ID != pair->collisionShape2ID) {
                break;
            }
            i++;
        }
    }

    // If the number of potential overlapping pairs is less than the quarter of allocated
    // number of overlapping pairs
    if (mNbPotentialPairs < mNbAllocatedPotentialPairs / 4 && mNbPotentialPairs > 8) {

        // Reduce the number of allocated potential overlapping pairs
        BroadPair* oldPairs = mPotentialPairs;
        mNbAllocatedPotentialPairs /= 2;
        mPotentialPairs = (BroadPair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPair));
        assert(mPotentialPairs);
        memcpy(mPotentialPairs, oldPairs, mNbPotentialPairs * sizeof(BroadPair));
        free(oldPairs);
    }
}

// Notify the broad-phase about a potential overlapping pair in the dynamic AABB tree
void BroadPhaseAlgorithm::notifyOverlappingPair(int node1ID, int node2ID) {

    // If both the nodes are the same, we do not create store the overlapping pair
    if (node1ID == node2ID) return;

    // If we need to allocate more memory for the array of potential overlapping pairs
    if (mNbPotentialPairs == mNbAllocatedPotentialPairs) {

        // Allocate more memory for the array of potential pairs
        BroadPair* oldPairs = mPotentialPairs;
        mNbAllocatedPotentialPairs *= 2;
        mPotentialPairs = (BroadPair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPair));
        assert(mPotentialPairs);
        memcpy(mPotentialPairs, oldPairs, mNbPotentialPairs * sizeof(BroadPair));
        free(oldPairs);
    }

    // Add the new potential pair into the array of potential overlapping pairs
    mPotentialPairs[mNbPotentialPairs].collisionShape1ID = std::min(node1ID, node2ID);
    mPotentialPairs[mNbPotentialPairs].collisionShape2ID = std::max(node1ID, node2ID);
    mNbPotentialPairs++;
}