/********************************************************************************
* 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 "VoronoiSimplex.h"
#include <cfloat>

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

decimal VoronoiSimplex::epsilon = decimal(0.0001);

// Constructor
VoronoiSimplex::VoronoiSimplex() : mNbPoints(0), mRecomputeClosestPoint(false), mIsClosestPointValid(false) {

}

// Destructor
VoronoiSimplex::~VoronoiSimplex() {

}

// Add a new support point of (A-B) into the simplex
/// suppPointA : support point of object A in a direction -v
/// suppPointB : support point of object B in a direction v
/// point      : support point of object (A-B) => point = suppPointA - suppPointB
void VoronoiSimplex::addPoint(const Vector3& point, const Vector3& suppPointA, const Vector3& suppPointB) {
    assert(!isFull());

    mPoints[mNbPoints] = point;
    mSuppPointsA[mNbPoints] = suppPointA;
    mSuppPointsB[mNbPoints] = suppPointB;

    mNbPoints++;
    mRecomputeClosestPoint = true;
}

// Remove a point from the simplex
void VoronoiSimplex::removePoint(int index) {
    assert(mNbPoints > 0);
    mNbPoints--;
    mPoints[index] = mPoints[mNbPoints];
    mSuppPointsA[index] = mSuppPointsA[mNbPoints];
    mSuppPointsB[index] = mSuppPointsB[mNbPoints];
}

// Reduce the simplex (only keep vertices that participate to the point closest to the origin)
/// bitsUsedPoints is seen as a sequence of bits representing whether the four points of
/// the simplex are used or not to represent the current closest point to the origin.
/// - The most right bit is set to one if the first point is used
/// - The second most right bit is set to one if the second point is used
/// - The third most right bit is set to one if the third point is used
/// - The fourth most right bit is set to one if the fourth point is used
void VoronoiSimplex::reduceSimplex(int bitsUsedPoints) {

    if ((mNbPoints >= 4) && (bitsUsedPoints & 8) == 0)
        removePoint(3);

    if ((mNbPoints >= 3) && (bitsUsedPoints & 4) == 0)
        removePoint(2);

    if ((mNbPoints >= 2) && (bitsUsedPoints & 2) == 0)
        removePoint(1);

    if ((mNbPoints >= 1) && (bitsUsedPoints & 1) == 0)
        removePoint(0);
}

// Return true if the point is in the simplex
bool VoronoiSimplex::isPointInSimplex(const Vector3& point) const {

    // For each four possible points in the simplex
    for (int i=0; i<mNbPoints; i++) {

        // Compute the distance between the points
        decimal distanceSquare = (mPoints[i] - point).lengthSquare();

        // If the point is very close
        if (distanceSquare <= epsilon) {
            return true;
        }
    }

    return false;
}

// Return the points of the simplex
int VoronoiSimplex::getSimplex(Vector3* suppPointsA, Vector3* suppPointsB,
                                 Vector3* points) const {

    for (int i=0; i<mNbPoints; i++) {
        points[i] = mPoints[i];
        suppPointsA[i] = mSuppPointsA[i];
        suppPointsB[i] = mSuppPointsB[i];
    }

    // Return the number of points in the simplex
    return mNbPoints;
}

// Return true if the set is affinely dependent.
/// A set if affinely dependent if a point of the set
/// is an affine combination of other points in the set
bool VoronoiSimplex::isAffinelyDependent() const {

    assert(mNbPoints >= 0 && mNbPoints <= 4);

    switch(mNbPoints) {
        case 0: return false;
        case 1: return false;

        // Two points are independent if there distance is larger than zero
        case 2: return (mPoints[1] - mPoints[0]).lengthSquare() <= epsilon;

        // Three points are independent if the triangle area is larger than zero
        case 3: return (mPoints[1] - mPoints[0]).cross(mPoints[2] - mPoints[0]).lengthSquare() <= epsilon;

        // Four points are independent if the tetrahedron volume is larger than zero
        // Test in three different ways (for more robustness)
        case 4: return std::abs((mPoints[1] - mPoints[0]).dot((mPoints[2] - mPoints[0]).cross(mPoints[3] - mPoints[0]))) <= epsilon;
    }

    return false;
}

// Compute the closest points "pA" and "pB" of object A and B.
/// The points are computed as follows :
///      pA = sum(lambda_i * a_i)    where "a_i" are the support points of object A
///      pB = sum(lambda_i * b_i)    where "b_i" are the support points of object B
///      with lambda_i = deltaX_i / deltaX
void VoronoiSimplex::computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) const {

    pA = mClosestSuppPointA;
    pB = mClosestSuppPointB;
}

// Recompute the closest point if the simplex has been modified
/// This method computes the point "v" of simplex that is closest to the origin.
/// The method returns true if a closest point has been found.
bool VoronoiSimplex::recomputeClosestPoint() {

    assert(mNbPoints <= 4);

    // If we need to recompute the closest point
    if (mRecomputeClosestPoint) {

        mRecomputeClosestPoint = false;

        switch(mNbPoints) {

            case 0:

                // Cannot compute closest point when the simplex is empty
                mIsClosestPointValid = false;
                break;

            case 1:

                {
                    // There is a single point in the simplex, therefore, this point is
                    // the one closest to the origin
                    mClosestPoint = mPoints[0];
                    mClosestSuppPointA = mSuppPointsA[0];
                    mClosestSuppPointB = mSuppPointsB[0];
                    setBarycentricCoords(1, 0, 0, 0);
                    mIsClosestPointValid = checkClosestPointValid();
                }
                break;

            case 2:

                {
                    int bitsUsedPoints = 0;
                    float t;

                    // The simplex is a line AB (where A=mPoints[0] and B=mPoints[1].
                    // We need to find the point of that line closest to the origin
                    computeClosestPointOnSegment(mPoints[0], mPoints[1], bitsUsedPoints, t);

                    // Compute the closest point
                    mClosestSuppPointA = mSuppPointsA[0] + t * (mSuppPointsA[1] - mSuppPointsA[0]);
                    mClosestSuppPointB = mSuppPointsB[0] + t * (mSuppPointsB[1] - mSuppPointsB[0]);
                    mClosestPoint = mClosestSuppPointA - mClosestSuppPointB;
                    setBarycentricCoords(decimal(1.0) - t, t, 0, 0);
                    mIsClosestPointValid = checkClosestPointValid();

                    // Reduce the simplex (remove vertices that are not participating to
                    // the closest point
                    reduceSimplex(bitsUsedPoints);
                }
                break;

            case 3:
                {
                    // The simplex is a triangle. We need to find the point of that
                    // triangle that is closest to the origin

                    int bitsUsedVertices = 0;
                    Vector3 baryCoords;

                    // Compute the point of the triangle closest to the origin
                    computeClosestPointOnTriangle(mPoints[0], mPoints[1], mPoints[2], bitsUsedVertices, baryCoords);
                    mClosestSuppPointA = baryCoords[0] * mSuppPointsA[0] + baryCoords[1] * mSuppPointsA[1] +
                                         baryCoords[2] * mSuppPointsA[2];
                    mClosestSuppPointB = baryCoords[0] * mSuppPointsB[0] + baryCoords[1] * mSuppPointsB[1] +
                                         baryCoords[2] * mSuppPointsB[2];
                    mClosestPoint = mClosestSuppPointA - mClosestSuppPointB;

                    setBarycentricCoords(baryCoords.x, baryCoords.y, baryCoords.z, 0.0);
                    mIsClosestPointValid = checkClosestPointValid();

                    // Reduce the simplex (remove vertices that are not participating to
                    // the closest point
                    reduceSimplex(bitsUsedVertices);
                }
                break;

            case 4:

                {
                    // The simplex is a tetrahedron. We need to find the point of that
                    // tetrahedron that is closest to the origin

                    int bitsUsedVertices = 0;
                    Vector2 baryCoordsAB;
                    Vector2 baryCoordsCD;
                    bool isDegenerate;

                    // Compute the point closest to the origin on the tetrahedron
                    bool isOutside = computeClosestPointOnTetrahedron(mPoints[0], mPoints[1], mPoints[2], mPoints[3],
                                                                      bitsUsedVertices, baryCoordsAB, baryCoordsCD, isDegenerate);

                    // If the origin is outside the tetrahedron
                    if (isOutside) {

                        // Compute the point of the tetrahedron closest to the origin
                        mClosestSuppPointA = baryCoordsAB.x * mSuppPointsA[0] + baryCoordsAB.y * mSuppPointsA[1] +
                                             baryCoordsCD.x * mSuppPointsA[2] + baryCoordsCD.y * mSuppPointsA[3];
                        mClosestSuppPointB = baryCoordsAB.x * mSuppPointsB[0] + baryCoordsAB.y * mSuppPointsB[1] +
                                             baryCoordsCD.x * mSuppPointsB[2] + baryCoordsCD.y * mSuppPointsB[3];
                        mClosestPoint = mClosestSuppPointA - mClosestSuppPointB;

                        setBarycentricCoords(baryCoordsAB.x, baryCoordsAB.y, baryCoordsCD.x, baryCoordsCD.y);

                        // Reduce the simplex (remove vertices that are not participating to
                        // the closest point
                        reduceSimplex(bitsUsedVertices);
                    }
                    else {

                        // If it is a degenerate case
                        if (isDegenerate) {
                            mIsClosestPointValid = false;
                        }
                        else {

                            // The origin is inside the tetrahedron, therefore, the closest point
                            // is the origin

                            setBarycentricCoords(0.0, 0.0, 0.0, 0.0);

                            mClosestSuppPointA.setToZero();
                            mClosestSuppPointB.setToZero();
                            mClosestPoint.setToZero();

                            mIsClosestPointValid = true;
                        }
                        break;
                    }

                    mIsClosestPointValid = checkClosestPointValid();
                }
                break;
        }
    }

    return mIsClosestPointValid;
}

// Compute point of a line segment that is closest to the origin
void VoronoiSimplex::computeClosestPointOnSegment(const Vector3& a, const Vector3& b, int& bitUsedVertices,
                                                  float& t) const {

    Vector3 AP = -a;
    Vector3 AB = b - a;
    decimal APDotAB = AP.dot(AB);

    // If the closest point is on the side of A in the direction of B
    if (APDotAB > decimal(0.0)) {
        decimal lengthABSquare = AB.lengthSquare();

        // If the closest point is on the segment AB
        if (APDotAB < lengthABSquare) {
            t = APDotAB / lengthABSquare;

            bitUsedVertices = 3; // 0011 (both A and B are used)

        }
        else {  // If the origin is on the side of B that is not in the direction of A
            // Therefore, the closest point is B
            t = decimal(1.0);

            bitUsedVertices = 2; // 0010 (only B is used)
        }
    }
    else {  // If the origin is on the side of A that is not in the direction of B
        // Therefore, the closest point of the line is A
        t = decimal(0.0);

        bitUsedVertices = 1; // 0001 (only A is used)
    }
}

// Compute point on a triangle that is closest to the origin
/// This implementation is based on the one in the book
/// "Real-Time Collision Detection" by Christer Ericson.
void VoronoiSimplex::computeClosestPointOnTriangle(const Vector3& a, const Vector3& b, const Vector3& c,
                                                   int& bitsUsedVertices, Vector3& baryCoordsABC) const {

    // Check if the origin is in the Voronoi region of vertex A
    Vector3 ab = b - a;
    Vector3 ac = c - a;
    Vector3 ap = -a;
    decimal d1 = ab.dot(ap);
    decimal d2 = ac.dot(ap);
    if (d1 <= decimal(0.0) && d2 <= decimal(0.0)) {

        // The origin is in the Voronoi region of vertex A

        // Set the barycentric coords of the closest point on the triangle
        baryCoordsABC.setAllValues(1.0, 0, 0);

        bitsUsedVertices = 1;    // 0001 (only A is used)
        return;
    }

    // Check if the origin is in the Voronoi region of vertex B
    Vector3 bp = -b;
    decimal d3 = ab.dot(bp);
    decimal d4 = ac.dot(bp);
    if (d3 >= decimal(0.0) && d4 <= d3) {

        // The origin is in the Voronoi region of vertex B

        // Set the barycentric coords of the closest point on the triangle
        baryCoordsABC.setAllValues(0.0, 1.0, 0);

        bitsUsedVertices = 2;    // 0010 (only B is used)
        return;
    }

    // Check if the origin is in the Voronoi region of edge AB
    decimal vc = d1 * d4 - d3 * d2;
    if (vc <= decimal(0.0) && d1 >= decimal(0.0) && d3 <= decimal(0.0)) {

        // The origin is in the Voronoi region of edge AB
        // We return the projection of the origin on the edge AB
        assert(std::abs(d1 - d3) > MACHINE_EPSILON);
        decimal v = d1 / (d1 - d3);

        // Set the barycentric coords of the closest point on the triangle
        baryCoordsABC.setAllValues(decimal(1.0) - v, v, 0);

        bitsUsedVertices = 3;    // 0011 (A and B are used)
        return;
    }

    // Check if the origin is in the Voronoi region of vertex C
    Vector3 cp = -c;
    decimal d5 = ab.dot(cp);
    decimal d6 = ac.dot(cp);
    if (d6 >= decimal(0.0) && d5 <= d6) {

        // The origin is in the Voronoi region of vertex C

        // Set the barycentric coords of the closest point on the triangle
        baryCoordsABC.setAllValues(0.0, 0.0, 1.0);

        bitsUsedVertices = 4;    // 0100 (only C is used)
        return;
    }

    // Check if the origin is in the Voronoi region of edge AC
    decimal vb = d5 * d2 - d1 * d6;
    if (vb <= decimal(0.0) && d2 >= decimal(0.0) && d6 <= decimal(0.0)) {

        // The origin is in the Voronoi region of edge AC
        // We return the projection of the origin on the edge AC
        assert(std::abs(d2 - d6) > MACHINE_EPSILON);
        decimal w = d2 / (d2 - d6);

        // Set the barycentric coords of the closest point on the triangle
        baryCoordsABC.setAllValues(decimal(1.0) - w, 0, w);

        bitsUsedVertices = 5;    // 0101 (A and C are used)
        return;
    }

    // Check if the origin is in the Voronoi region of edge BC
    decimal va = d3 * d6 - d5 * d4;
    if (va <= decimal(0.0) && (d4 - d3) >= decimal(0.0) && (d5 - d6) >= decimal(0.0)) {

        // The origin is in the Voronoi region of edge BC
        // We return the projection of the origin on the edge BC
        assert(std::abs((d4 - d3) + (d5 - d6)) > MACHINE_EPSILON);
        decimal w = (d4 - d3) / ((d4 - d3) + (d5 - d6));

        // Set the barycentric coords of the closest point on the triangle
        baryCoordsABC.setAllValues(0.0, decimal(1.0) - w, w);

        bitsUsedVertices = 6;    // 0110 (B and C are used)
        return;
    }

    // The origin is in the Voronoi region of the face ABC
    decimal denom = decimal(1.0) / (va + vb + vc);
    decimal v = vb * denom;
    decimal w = vc * denom;

    // Set the barycentric coords of the closest point on the triangle
    baryCoordsABC.setAllValues(1 - v - w, v, w);

    bitsUsedVertices = 7;    // 0111 (A, B and C are used)
    return;
}

// Compute point of a tetrahedron that is closest to the origin
/// This implementation is based on the one in the book
/// "Real-Time Collision Detection" by Christer Ericson.
/// This method returns true if the origin is outside the tetrahedron.
bool VoronoiSimplex::computeClosestPointOnTetrahedron(const Vector3& a, const Vector3& b, const Vector3& c,
                                                      const Vector3& d, int& bitsUsedPoints, Vector2& baryCoordsAB,
                                                      Vector2& baryCoordsCD, bool& isDegenerate) const {

    isDegenerate = false;

    // Start as if the origin was inside the tetrahedron
    bitsUsedPoints = 15;        // 1111 (A, B, C and D are used)
    baryCoordsAB.setToZero();
    baryCoordsCD.setToZero();

    // Check if the origin is outside each tetrahedron face
    int isOriginOutsideFaceABC = testOriginOutsideOfPlane(a, b, c, d);
    int isOriginOutsideFaceACD = testOriginOutsideOfPlane(a, c, d, b);
    int isOriginOutsideFaceADB = testOriginOutsideOfPlane(a, d, b, c);
    int isOriginOutsideFaceBDC = testOriginOutsideOfPlane(b, d, c, a);

    // If we have a degenerate tetrahedron
    if (isOriginOutsideFaceABC < 0 || isOriginOutsideFaceACD < 0 ||
        isOriginOutsideFaceADB < 0 || isOriginOutsideFaceBDC < 0) {

        // The tetrahedron is degenerate
        isDegenerate = true;
        return false;
    }

    if (isOriginOutsideFaceABC == 0 && isOriginOutsideFaceACD == 0 &&
        isOriginOutsideFaceADB == 0 && isOriginOutsideFaceBDC == 0) {

        // The origin is inside the tetrahedron
        return true;
    }

    // We know that the origin is outside the tetrahedron, we now need to find
    // which of the four triangle faces is closest to it.

    decimal closestSquareDistance = DECIMAL_LARGEST;
    int tempUsedVertices;
    Vector3 triangleBaryCoords;

    // If the origin is outside face ABC
    if (isOriginOutsideFaceABC) {

        // Compute the closest point on this triangle face
        computeClosestPointOnTriangle(a, b, c, tempUsedVertices, triangleBaryCoords);
        Vector3 closestPoint = triangleBaryCoords[0] * a + triangleBaryCoords[1] * b +
                               triangleBaryCoords[2] * c;
        decimal squareDist = closestPoint.lengthSquare();

        // If the point on that face is the closest to the origin so far
        if (squareDist < closestSquareDistance) {

            // Use it as the current closest point
            closestSquareDistance = squareDist;
            baryCoordsAB.setAllValues(triangleBaryCoords[0], triangleBaryCoords[1]);
            baryCoordsCD.setAllValues(triangleBaryCoords[2], 0.0);
            bitsUsedPoints = tempUsedVertices;
        }
    }

    // If the origin is outside face ACD
    if (isOriginOutsideFaceACD) {

        // Compute the closest point on this triangle face
        computeClosestPointOnTriangle(a, c, d, tempUsedVertices, triangleBaryCoords);
        Vector3 closestPoint = triangleBaryCoords[0] * a + triangleBaryCoords[1] * c +
                               triangleBaryCoords[2] * d;
        decimal squareDist = closestPoint.lengthSquare();

        // If the point on that face is the closest to the origin so far
        if (squareDist < closestSquareDistance) {

            // Use it as the current closest point
            closestSquareDistance = squareDist;
            baryCoordsAB.setAllValues(triangleBaryCoords[0], 0.0);
            baryCoordsCD.setAllValues(triangleBaryCoords[1], triangleBaryCoords[2]);
            bitsUsedPoints = mapTriangleUsedVerticesToTetrahedron(tempUsedVertices, 0, 2, 3);
        }
    }

    // If the origin is outside face
    if (isOriginOutsideFaceADB) {

        // Compute the closest point on this triangle face
        computeClosestPointOnTriangle(a, d, b, tempUsedVertices, triangleBaryCoords);
        Vector3 closestPoint = triangleBaryCoords[0] * a + triangleBaryCoords[1] * d +
                               triangleBaryCoords[2] * b;
        decimal squareDist = closestPoint.lengthSquare();

        // If the point on that face is the closest to the origin so far
        if (squareDist < closestSquareDistance) {

            // Use it as the current closest point
            closestSquareDistance = squareDist;
            baryCoordsAB.setAllValues(triangleBaryCoords[0], triangleBaryCoords[2]);
            baryCoordsCD.setAllValues(0.0, triangleBaryCoords[1]);
            bitsUsedPoints = mapTriangleUsedVerticesToTetrahedron(tempUsedVertices, 0, 3, 1);
        }
    }

    // If the origin is outside face
    if (isOriginOutsideFaceBDC) {

        // Compute the closest point on this triangle face
        computeClosestPointOnTriangle(b, d, c, tempUsedVertices, triangleBaryCoords);
        Vector3 closestPoint = triangleBaryCoords[0] * b + triangleBaryCoords[1] * d +
                               triangleBaryCoords[2] * c;
        decimal squareDist = closestPoint.lengthSquare();

        // If the point on that face is the closest to the origin so far
        if (squareDist < closestSquareDistance) {

            // Use it as the current closest point
            baryCoordsAB.setAllValues(0.0, triangleBaryCoords[0]);
            baryCoordsCD.setAllValues(triangleBaryCoords[2], triangleBaryCoords[1]);
            bitsUsedPoints = mapTriangleUsedVerticesToTetrahedron(tempUsedVertices, 1, 3, 2);
        }
    }

    return true;
}

// 0111 (1,2,3) => 0111
// 0011 (2,1,3) =>
int VoronoiSimplex::mapTriangleUsedVerticesToTetrahedron(int triangleUsedVertices, int first, int second, int third) const {

    assert(triangleUsedVertices <= 7);
    int tetrahedronUsedVertices = (((1 & triangleUsedVertices) != 0) << first) |
                                  (((2 & triangleUsedVertices) != 0) << second) |
                                  (((4 & triangleUsedVertices) != 0) << third);

    assert(tetrahedronUsedVertices <= 14);
    return tetrahedronUsedVertices;
}

// Test if a point is outside of the plane given by the points of the triangle (a, b, c)
/// This method returns 1 if the point d and the origin are on opposite sides of
/// the triangle face (a, b, c). It returns 0 if they are on the same side.
/// This implementation is based on the one in the book
/// "Real-Time Collision Detection" by Christer Ericson.
int VoronoiSimplex::testOriginOutsideOfPlane(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d) const {

    // Normal of (a,b,c) triangle
    const Vector3 n = (b-a).cross(c-a);

    decimal signp = (-a).dot(n);
    decimal signd = (d-a).dot(n);

    // If the tetrahedron is degenerate (all points on the same plane)
    // This should never happen because after a point is added into the simplex, the user
    // of this class must check that the simplex is not affinely dependent using the
    // isAffinelyDependent() method
    if(signd * signd < epsilon * epsilon) {
        return -1;
    }

    return signp * signd < decimal(0.0);
}

// Backup the closest point
void VoronoiSimplex::backupClosestPointInSimplex(Vector3& v) {
    v = mClosestPoint;
}

// Return the maximum squared length of a point
decimal VoronoiSimplex::getMaxLengthSquareOfAPoint() const {

    decimal maxLengthSquare = decimal(0.0);

    for (int i=0; i<mNbPoints; i++) {
        decimal lengthSquare = mPoints[i].lengthSquare();
        if (lengthSquare > maxLengthSquare) {
            maxLengthSquare = lengthSquare;
        }
    }

    // Return the maximum squared length
    return maxLengthSquare;
}