/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
* Copyright (c) 2010-2016 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"
#include "collision/CollisionDetection.h"
#include "engine/Profiler.h"

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

// Constructor
BroadPhaseAlgorithm::BroadPhaseAlgorithm(CollisionDetection& collisionDetection)
                    :mDynamicAABBTree(collisionDetection.getMemoryManager().getPoolAllocator(), DYNAMIC_TREE_AABB_GAP),
                     mNbMovedShapes(0), mNbAllocatedMovedShapes(8),
                     mNbNonUsedMovedShapes(0), mNbPotentialPairs(0), mNbAllocatedPotentialPairs(8),
                     mCollisionDetection(collisionDetection) {

    PoolAllocator& poolAllocator = collisionDetection.getMemoryManager().getPoolAllocator();

    // Allocate memory for the array of non-static proxy shapes IDs
    mMovedShapes = static_cast<int*>(poolAllocator.allocate(mNbAllocatedMovedShapes * sizeof(int)));
    assert(mMovedShapes != nullptr);

    // Allocate memory for the array of potential overlapping pairs
    mPotentialPairs = static_cast<BroadPhasePair*>(poolAllocator.allocate(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair)));
    assert(mPotentialPairs != nullptr);

#ifdef IS_PROFILING_ACTIVE

	mProfiler = nullptr;

#endif

}

// Destructor
BroadPhaseAlgorithm::~BroadPhaseAlgorithm() {

    // Get the memory pool allocatory
    PoolAllocator& poolAllocator = mCollisionDetection.getMemoryManager().getPoolAllocator();

    // Release the memory for the array of non-static proxy shapes IDs
    poolAllocator.release(mMovedShapes, mNbAllocatedMovedShapes * sizeof (int));

    // Release the memory for the array of potential overlapping pairs
    poolAllocator.release(mPotentialPairs, mNbAllocatedPotentialPairs * sizeof(BroadPhasePair));
}

// 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 shapes that have moved if necessary
    if (mNbAllocatedMovedShapes == mNbMovedShapes) {

        // Get the memory pool allocatory
        PoolAllocator& poolAllocator = mCollisionDetection.getMemoryManager().getPoolAllocator();

        uint oldNbAllocatedMovedShapes = mNbAllocatedMovedShapes;
        mNbAllocatedMovedShapes *= 2;
        int* oldArray = mMovedShapes;
        mMovedShapes = static_cast<int*>(poolAllocator.allocate(mNbAllocatedMovedShapes * sizeof(int)));
        assert(mMovedShapes != nullptr);
        std::memcpy(mMovedShapes, oldArray, mNbMovedShapes * sizeof(int));
        poolAllocator.release(oldArray, oldNbAllocatedMovedShapes * sizeof(int));
    }

    // Store the broad-phase ID into the array of shapes that have moved
    assert(mNbMovedShapes < mNbAllocatedMovedShapes);
    assert(mMovedShapes != nullptr);
    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 shapes IDs array
    // are used, we release some allocated memory
    if ((mNbMovedShapes - mNbNonUsedMovedShapes) < mNbAllocatedMovedShapes / 4 &&
            mNbAllocatedMovedShapes > 8) {

        // Get the memory pool allocatory
        PoolAllocator& poolAllocator = mCollisionDetection.getMemoryManager().getPoolAllocator();

        uint oldNbAllocatedMovedShapes = mNbAllocatedMovedShapes;
        mNbAllocatedMovedShapes /= 2;
        int* oldArray = mMovedShapes;
        mMovedShapes = static_cast<int*>(poolAllocator.allocate(mNbAllocatedMovedShapes * sizeof(int)));
        assert(mMovedShapes != nullptr);
        uint nbElements = 0;
        for (uint i=0; i<mNbMovedShapes; i++) {
            if (oldArray[i] != -1) {
                mMovedShapes[nbElements] = oldArray[i];
                nbElements++;
            }
        }
        mNbMovedShapes = nbElements;
        mNbNonUsedMovedShapes = 0;
        poolAllocator.release(oldArray, oldNbAllocatedMovedShapes * sizeof(int));
    }

    // 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, const AABB& aabb) {

    assert(proxyShape->mBroadPhaseID == -1);

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

    // Set the broad-phase ID of the proxy shape
    proxyShape->mBroadPhaseID = nodeId;

    // 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) {

    assert(proxyShape->mBroadPhaseID != -1);

    int broadPhaseID = proxyShape->mBroadPhaseID;

    proxyShape->mBroadPhaseID = -1;

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

    // Remove the collision shape into the array of shapes 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,
                                                    const Vector3& displacement, bool forceReinsert) {

    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, displacement, forceReinsert);

    // 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 shapes that have moved (or have been created)
        // during the last simulation step
        addMovedCollisionShape(broadPhaseID);
    }
}

void BroadPhaseAlgorithm::reportAllShapesOverlappingWithAABB(const AABB& aabb,
                                                             LinkedList<int>& overlappingNodes) const {

    AABBOverlapCallback callback(overlappingNodes);

    // Ask the dynamic AABB tree to report all collision shapes that overlap with this AABB
    mDynamicAABBTree.reportAllShapesOverlappingWithAABB(aabb, callback);
}

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

    // TODO : Try to see if we can allocate potential pairs in single frame allocator

    // Reset the potential overlapping pairs
    mNbPotentialPairs = 0;

    LinkedList<int> overlappingNodes(memoryManager.getPoolAllocator());

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

        if (shapeID == -1) continue;

        AABBOverlapCallback callback(overlappingNodes);

        // Get the AABB of the shape
        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.reportAllShapesOverlappingWithAABB(shapeAABB, callback);

        // Add the potential overlapping pairs
        addOverlappingNodes(shapeID, overlappingNodes);

        // Remove all the elements of the linked list of overlapping nodes
        overlappingNodes.reset();
    }

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

    // Sort the array of potential overlapping pairs in order to remove duplicate pairs
    std::sort(mPotentialPairs, mPotentialPairs + mNbPotentialPairs, BroadPhasePair::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
        BroadPhasePair* pair = mPotentialPairs + i;
        i++;

        assert(pair->collisionShape1ID != pair->collisionShape2ID);

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

        // If the two proxy collision shapes are from the same body, skip it
        if (shape1->getBody()->getID() != shape2->getBody()->getID()) {

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

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

            // Get the next pair
            BroadPhasePair* 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) {

        PoolAllocator& poolAllocator = mCollisionDetection.getMemoryManager().getPoolAllocator();

        // Reduce the number of allocated potential overlapping pairs
        BroadPhasePair* oldPairs = mPotentialPairs;
        uint oldNbAllocatedPotentialPairs = mNbAllocatedPotentialPairs;
        mNbAllocatedPotentialPairs /= 2;
        mPotentialPairs = static_cast<BroadPhasePair*>(poolAllocator.allocate(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair)));
        assert(mPotentialPairs);
        memcpy(mPotentialPairs, oldPairs, mNbPotentialPairs * sizeof(BroadPhasePair));
        poolAllocator.release(oldPairs, oldNbAllocatedPotentialPairs * sizeof(BroadPhasePair));
    }
}

// Notify the broad-phase about a potential overlapping pair in the dynamic AABB tree
void BroadPhaseAlgorithm::addOverlappingNodes(int referenceNodeId, const LinkedList<int>& overlappingNodes) {

    // For each overlapping node in the linked list
    LinkedList<int>::ListElement* elem = overlappingNodes.getListHead();
    while (elem != nullptr) {

        // If both the nodes are the same, we do not create store the overlapping pair
        if (referenceNodeId != elem->data) {

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

                PoolAllocator& poolAllocator = mCollisionDetection.getMemoryManager().getPoolAllocator();

                // Allocate more memory for the array of potential pairs
                BroadPhasePair* oldPairs = mPotentialPairs;
                uint oldNbAllocatedPotentialPairs = mNbAllocatedPotentialPairs;
                mNbAllocatedPotentialPairs *= 2;
                mPotentialPairs = static_cast<BroadPhasePair*>(poolAllocator.allocate(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair)));
                assert(mPotentialPairs);
                memcpy(mPotentialPairs, oldPairs, mNbPotentialPairs * sizeof(BroadPhasePair));
                poolAllocator.release(oldPairs, oldNbAllocatedPotentialPairs * sizeof(BroadPhasePair));
            }

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

        elem = elem->next;
    }
}

// Called when a overlapping node has been found during the call to
// DynamicAABBTree:reportAllShapesOverlappingWithAABB()
void AABBOverlapCallback::notifyOverlappingNode(int nodeId) {
    mOverlappingNodes.insert(nodeId);
}

// Called for a broad-phase shape that has to be tested for raycast
decimal BroadPhaseRaycastCallback::raycastBroadPhaseShape(int32 nodeId, const Ray& ray) {

    decimal hitFraction = decimal(-1.0);

    // Get the proxy shape from the node
    ProxyShape* proxyShape = static_cast<ProxyShape*>(mDynamicAABBTree.getNodeDataPointer(nodeId));

    // Check if the raycast filtering mask allows raycast against this shape
    if ((mRaycastWithCategoryMaskBits & proxyShape->getCollisionCategoryBits()) != 0) {

        // Ask the collision detection to perform a ray cast test against
        // the proxy shape of this node because the ray is overlapping
        // with the shape in the broad-phase
        hitFraction = mRaycastTest.raycastAgainstShape(proxyShape, ray);
    }

    return hitFraction;
}