/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
* Copyright (c) 2010-2015 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 <complex>
#include "configuration.h"
#include "ConvexMeshShape.h"

using namespace reactphysics3d;

// Constructor to initialize with an array of 3D vertices.
/// This method creates an internal copy of the input vertices.
/**
 * @param arrayVertices Array with the vertices of the convex mesh
 * @param nbVertices Number of vertices in the convex mesh
 * @param stride Stride between the beginning of two elements in the vertices array
 * @param margin Collision margin (in meters) around the collision shape
 */
ConvexMeshShape::ConvexMeshShape(const decimal* arrayVertices, uint nbVertices, int stride, decimal margin)
                : ConvexShape(CONVEX_MESH, margin), mNbVertices(nbVertices), mMinBounds(0, 0, 0),
                  mMaxBounds(0, 0, 0), mIsEdgesInformationUsed(false) {
    assert(nbVertices > 0);
    assert(stride > 0);

    const unsigned char* vertexPointer = (const unsigned char*) arrayVertices;

    // Copy all the vertices into the internal array
    for (uint i=0; i<mNbVertices; i++) {
        const decimal* newPoint = (const decimal*) vertexPointer;
        mVertices.push_back(Vector3(newPoint[0], newPoint[1], newPoint[2]));
        vertexPointer += stride;
    }

    // Recalculate the bounds of the mesh
    recalculateBounds();
}

// Constructor to initialize with a triangle mesh
/// This method creates an internal copy of the input vertices.
/**
 * @param triangleVertexArray Array with the vertices and indices of the vertices and triangles of the mesh
 * @param isEdgesInformationUsed True if you want to use edges information for collision detection (faster but requires more memory)
 * @param margin Collision margin (in meters) around the collision shape
 */
ConvexMeshShape::ConvexMeshShape(TriangleVertexArray* triangleVertexArray, bool isEdgesInformationUsed, decimal margin)
                : ConvexShape(CONVEX_MESH, margin), mMinBounds(0, 0, 0),
                  mMaxBounds(0, 0, 0), mIsEdgesInformationUsed(isEdgesInformationUsed) {

    TriangleVertexArray::VertexDataType vertexType = triangleVertexArray->getVertexDataType();
    TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType();
    unsigned char* verticesStart = triangleVertexArray->getVerticesStart();
    unsigned char* indicesStart = triangleVertexArray->getIndicesStart();
    int vertexStride = triangleVertexArray->getVerticesStride();
    int indexStride = triangleVertexArray->getIndicesStride();

    // For each vertex of the mesh
    for (int v = 0; v < triangleVertexArray->getNbVertices(); v++) {

        // Get the vertices components of the triangle
        if (vertexType == TriangleVertexArray::VERTEX_FLOAT_TYPE) {
            const float* vertices = (float*)(verticesStart + v * vertexStride);

            Vector3 vertex(vertices[0], vertices[1], vertices[2] );
            vertex = vertex * mScaling;
            mVertices.push_back(vertex);
        }
        else if (vertexType == TriangleVertexArray::VERTEX_DOUBLE_TYPE) {
            const double* vertices = (double*)(verticesStart + v * vertexStride);

            Vector3 vertex(vertices[0], vertices[1], vertices[2] );
            vertex = vertex * mScaling;
            mVertices.push_back(vertex);
        }
    }

    // If we need to use the edges information of the mesh
    if (mIsEdgesInformationUsed) {

        // For each triangle of the mesh
        for (int triangleIndex=0; triangleIndex<triangleVertexArray->getNbTriangles(); triangleIndex++) {

            void* vertexIndexPointer = (indicesStart + triangleIndex * 3 * indexStride);

            uint vertexIndex[3];

            // For each vertex of the triangle
            for (int k=0; k < 3; k++) {

                // Get the index of the current vertex in the triangle
                if (indexType == TriangleVertexArray::INDEX_INTEGER_TYPE) {
                    vertexIndex[k] = ((uint*)vertexIndexPointer)[k];
                }
                else if (indexType == TriangleVertexArray::INDEX_SHORT_TYPE) {
                    vertexIndex[k] = ((unsigned short*)vertexIndexPointer)[k];
                }
            }

            // Add information about the edges
            addEdge(vertexIndex[0], vertexIndex[1]);
            addEdge(vertexIndex[0], vertexIndex[2]);
            addEdge(vertexIndex[1], vertexIndex[2]);
        }
    }

    mNbVertices = mVertices.size();
    recalculateBounds();
}

// Constructor.
/// If you use this constructor, you will need to set the vertices manually one by one using
/// the addVertex() method.
ConvexMeshShape::ConvexMeshShape(decimal margin)
                : ConvexShape(CONVEX_MESH, margin), mNbVertices(0), mMinBounds(0, 0, 0),
                  mMaxBounds(0, 0, 0), mIsEdgesInformationUsed(false) {

}

// Destructor
ConvexMeshShape::~ConvexMeshShape() {

}

// Return a local support point in a given direction with the object margin
Vector3 ConvexMeshShape::getLocalSupportPointWithMargin(const Vector3& direction,
                                                        void** cachedCollisionData) const {

    // Get the support point without the margin
    Vector3 supportPoint = getLocalSupportPointWithoutMargin(direction, cachedCollisionData);

    // Get the unit direction vector
    Vector3 unitDirection = direction;
    if (direction.lengthSquare() < MACHINE_EPSILON * MACHINE_EPSILON) {
        unitDirection.setAllValues(1.0, 1.0, 1.0);
    }
    unitDirection.normalize();

    // Add the margin to the support point and return it
    return supportPoint + unitDirection * mMargin;
}

// Return a local support point in a given direction without the object margin.
/// If the edges information is not used for collision detection, this method will go through
/// the whole vertices list and pick up the vertex with the largest dot product in the support
/// direction. This is an O(n) process with "n" being the number of vertices in the mesh.
/// However, if the edges information is used, we can cache the previous support vertex and use
/// it as a start in a hill-climbing (local search) process to find the new support vertex which
/// will be in most of the cases very close to the previous one. Using hill-climbing, this method
/// runs in almost constant time.
Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
                                                           void** cachedCollisionData) const {

    assert(mNbVertices == mVertices.size());
    assert(cachedCollisionData != NULL);

    // Allocate memory for the cached collision data if not allocated yet
    if ((*cachedCollisionData) == NULL) {
        *cachedCollisionData = (int*) malloc(sizeof(int));
        *((int*)(*cachedCollisionData)) = 0;
    }

    // If the edges information is used to speed up the collision detection
    if (mIsEdgesInformationUsed) {

        assert(mEdgesAdjacencyList.size() == mNbVertices);

        uint maxVertex = *((int*)(*cachedCollisionData));
        decimal maxDotProduct = direction.dot(mVertices[maxVertex]);
        bool isOptimal;

        // Perform hill-climbing (local search)
        do {
            isOptimal = true;

            assert(mEdgesAdjacencyList.at(maxVertex).size() > 0);

            // For all neighbors of the current vertex
            std::set<uint>::const_iterator it;
            std::set<uint>::const_iterator itBegin = mEdgesAdjacencyList.at(maxVertex).begin();
            std::set<uint>::const_iterator itEnd = mEdgesAdjacencyList.at(maxVertex).end();
            for (it = itBegin; it != itEnd; ++it) {

                // Compute the dot product
                decimal dotProduct = direction.dot(mVertices[*it]);

                // If the current vertex is a better vertex (larger dot product)
                if (dotProduct > maxDotProduct) {
                    maxVertex = *it;
                    maxDotProduct = dotProduct;
                    isOptimal = false;
                }
            }

        } while(!isOptimal);

        // Cache the support vertex
        *((int*)(*cachedCollisionData)) = maxVertex;

        // Return the support vertex
        return mVertices[maxVertex] * mScaling;
    }
    else {  // If the edges information is not used

        double maxDotProduct = DECIMAL_SMALLEST;
        uint indexMaxDotProduct = 0;

        // For each vertex of the mesh
        for (uint i=0; i<mNbVertices; i++) {

            // Compute the dot product of the current vertex
            double dotProduct = direction.dot(mVertices[i]);

            // If the current dot product is larger than the maximum one
            if (dotProduct > maxDotProduct) {
                indexMaxDotProduct = i;
                maxDotProduct = dotProduct;
            }
        }

        assert(maxDotProduct >= decimal(0.0));

        // Return the vertex with the largest dot product in the support direction
        return mVertices[indexMaxDotProduct] * mScaling;
    }
}

// Recompute the bounds of the mesh
void ConvexMeshShape::recalculateBounds() {

    // TODO : Only works if the local origin is inside the mesh
    //        => Make it more robust (init with first vertex of mesh instead)

    mMinBounds.setToZero();
    mMaxBounds.setToZero();

    // For each vertex of the mesh
    for (uint i=0; i<mNbVertices; i++) {

        if (mVertices[i].x > mMaxBounds.x) mMaxBounds.x = mVertices[i].x;
        if (mVertices[i].x < mMinBounds.x) mMinBounds.x = mVertices[i].x;

        if (mVertices[i].y > mMaxBounds.y) mMaxBounds.y = mVertices[i].y;
        if (mVertices[i].y < mMinBounds.y) mMinBounds.y = mVertices[i].y;

        if (mVertices[i].z > mMaxBounds.z) mMaxBounds.z = mVertices[i].z;
        if (mVertices[i].z < mMinBounds.z) mMinBounds.z = mVertices[i].z;
    }

    // Apply the local scaling factor
    mMaxBounds = mMaxBounds * mScaling;
    mMinBounds = mMinBounds * mScaling;

    // Add the object margin to the bounds
    mMaxBounds += Vector3(mMargin, mMargin, mMargin);
    mMinBounds -= Vector3(mMargin, mMargin, mMargin);
}

// Raycast method with feedback information
bool ConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
    return proxyShape->mBody->mWorld.mCollisionDetection.mNarrowPhaseGJKAlgorithm.raycast(
                                     ray, proxyShape, raycastInfo);
}