/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
* Copyright (c) 2010-2020 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 <reactphysics3d/collision/narrowphase/GJK/GJKAlgorithm.h>
#include <reactphysics3d/constraint/ContactPoint.h>
#include <reactphysics3d/engine/OverlappingPairs.h>
#include <reactphysics3d/collision/shapes/TriangleShape.h>
#include <reactphysics3d/configuration.h>
#include <reactphysics3d/utils/Profiler.h>
#include <reactphysics3d/containers/Array.h>
#include <reactphysics3d/collision/narrowphase/NarrowPhaseInfoBatch.h>
#include <reactphysics3d/collision/narrowphase/GJK/VoronoiSimplex.h>
#include <cassert>

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

// Compute a contact info if the two collision shapes collide.
/// This method implements the Hybrid Technique for computing the penetration depth by
/// running the GJK algorithm on original objects (without margin). If the shapes intersect
/// only in the margins, the method compute the penetration depth and contact points
/// (of enlarged objects). If the original objects (without margin) intersect, we
/// call the computePenetrationDepthForEnlargedObjects() method that run the GJK
/// algorithm on the enlarged object to obtain a simplex polytope that contains the
/// origin, they we give that simplex polytope to the EPA algorithm which will compute
/// the correct penetration depth and contact points between the enlarged objects.
void GJKAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex,
                                 uint batchNbItems, Array<GJKResult>& gjkResults) {

    RP3D_PROFILE("GJKAlgorithm::testCollision()", mProfiler);
    
    // For each item in the batch
    for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) {

        Vector3 suppA;             // Support point of object A
        Vector3 suppB;             // Support point of object B
        Vector3 w;                 // Support point of Minkowski difference A-B
        Vector3 pA;                // Closest point of object A
        Vector3 pB;                // Closest point of object B
        decimal vDotw;
        decimal prevDistSquare;
        bool contactFound = false;

        assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1->isConvex());
        assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2->isConvex());

        const ConvexShape* shape1 = static_cast<const ConvexShape*>(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1);
        const ConvexShape* shape2 = static_cast<const ConvexShape*>(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2);

        // Get the local-space to world-space transforms
        const Transform& transform1 = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform;
        const Transform& transform2 = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform;

        // Transform a point from local space of body 2 to local
        // space of body 1 (the GJK algorithm is done in local space of body 1)
        Transform transform1Inverse = transform1.getInverse();
        Transform body2Tobody1 = transform1Inverse * transform2;

        // Quaternion that transform a direction from local
        // space of body 1 into local space of body 2
        Quaternion rotateToBody2 = transform2.getOrientation().getInverse() * transform1.getOrientation();

        // Initialize the margin (sum of margins of both objects)
        decimal margin = shape1->getMargin() + shape2->getMargin();
        decimal marginSquare = margin * margin;
        assert(margin > decimal(0.0));

        // Create a simplex set
        VoronoiSimplex simplex;

        // Get the last collision frame info
        LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].lastFrameCollisionInfo;

        // Get the previous point V (last cached separating axis)
        Vector3 v;
        if (lastFrameCollisionInfo->isValid && lastFrameCollisionInfo->wasUsingGJK) {
            v = lastFrameCollisionInfo->gjkSeparatingAxis;
            assert(v.lengthSquare() > decimal(0.000001));
        }
        else {
            v.setAllValues(0, 1, 0);
        }

        // Initialize the upper bound for the square distance
        decimal distSquare = DECIMAL_LARGEST;

        bool noIntersection = false;

        do {

            // Compute the support points for original objects (without margins) A and B
            suppA = shape1->getLocalSupportPointWithoutMargin(-v);
            suppB = body2Tobody1 * shape2->getLocalSupportPointWithoutMargin(rotateToBody2 * v);

            // Compute the support point for the Minkowski difference A-B
            w = suppA - suppB;

            vDotw = v.dot(w);

            // If the enlarge objects (with margins) do not intersect
            if (vDotw > decimal(0.0) && vDotw * vDotw > distSquare * marginSquare) {

                // Cache the current separating axis for frame coherence
                lastFrameCollisionInfo->gjkSeparatingAxis = v;

                // No intersection, we return
                assert(gjkResults.size() == batchIndex);
                gjkResults.add(GJKResult::SEPARATED);
                noIntersection = true;
                break;
            }

            // If the objects intersect only in the margins
            if (simplex.isPointInSimplex(w) || distSquare - vDotw <= distSquare * REL_ERROR_SQUARE) {

                // Contact point has been found
                contactFound = true;
                break;
            }

            // Add the new support point to the simplex
            simplex.addPoint(w, suppA, suppB);

            // If the simplex is affinely dependent
            if (simplex.isAffinelyDependent()) {

                // Contact point has been found
                contactFound = true;
                break;
            }

            // Compute the point of the simplex closest to the origin
            // If the computation of the closest point fails
            if (!simplex.computeClosestPoint(v)) {

                // Contact point has been found
                contactFound = true;
                break;
            }

            // Store and update the squared distance of the closest point
            prevDistSquare = distSquare;
            distSquare = v.lengthSquare();

            // If the distance to the closest point doesn't improve a lot
            if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) {

                simplex.backupClosestPointInSimplex(v);

                // Get the new squared distance
                distSquare = v.lengthSquare();

                // Contact point has been found
                contactFound = true;
                break;
            }

        } while(!simplex.isFull() && distSquare > MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint());

        if (noIntersection) {
            continue;
        }

        if (contactFound && distSquare > MACHINE_EPSILON) {

            // Compute the closet points of both objects (without the margins)
            simplex.computeClosestPointsOfAandB(pA, pB);

            // Project those two points on the margins to have the closest points of both
            // object with the margins
            decimal dist = std::sqrt(distSquare);
            assert(dist > decimal(0.0));
            pA = (pA - (shape1->getMargin() / dist) * v);
            pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);

            // Compute the contact info
            Vector3 normal = transform1.getOrientation() * (-v.getUnit());
            decimal penetrationDepth = margin - dist;

            // If the penetration depth is negative (due too numerical errors), there is no contact
            if (penetrationDepth <= decimal(0.0)) {
                assert(gjkResults.size() == batchIndex);
                gjkResults.add(GJKResult::SEPARATED);
                continue;
            }

            // Do not generate a contact point with zero normal length
            if (normal.lengthSquare() < MACHINE_EPSILON) {
                assert(gjkResults.size() == batchIndex);
                gjkResults.add(GJKResult::SEPARATED);
                continue;
            }

            // If we need to report contacts
            if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) {

                // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
                TriangleShape::computeSmoothTriangleMeshContact(shape1, shape2, pA, pB, transform1, transform2,
                                                                penetrationDepth, normal);

                // Add a new contact point
                narrowPhaseInfoBatch.addContactPoint(batchIndex, normal, penetrationDepth, pA, pB);
            }

            assert(gjkResults.size() == batchIndex);
            gjkResults.add(GJKResult::COLLIDE_IN_MARGIN);

            continue;
        }

        assert(gjkResults.size() == batchIndex);
        gjkResults.add(GJKResult::INTERPENETRATE);
    }
}