/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
* Copyright (c) 2010-2022 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.    *
*                                                                               *
********************************************************************************/

#ifndef REACTPHYSICS3D_AABB_H
#define REACTPHYSICS3D_AABB_H

// Libraries
#include <reactphysics3d/mathematics/mathematics.h>

/// ReactPhysics3D namespace
namespace reactphysics3d {
    
// Class AABB
/**
 * This class represents a bounding volume of type "Axis Aligned
 * Bounding Box". It's a box where all the edges are always aligned
 * with the world coordinate system. The AABB is defined by the
 * minimum and maximum world coordinates of the three axis.
 */
class AABB {

    private :

        // -------------------- Attributes -------------------- //

        /// Minimum world coordinates of the AABB on the x,y and z axis
        Vector3 mMinCoordinates;

        /// Maximum world coordinates of the AABB on the x,y and z axis
        Vector3 mMaxCoordinates;

    public :

        // -------------------- Methods -------------------- //

        /// Constructor
        AABB() = default;

        /// Constructor
        AABB(const Vector3& minCoordinates, const Vector3& maxCoordinates);

        /// Return the center point
        Vector3 getCenter() const;

        /// Return the minimum coordinates of the AABB
        const Vector3& getMin() const;

        /// Set the minimum coordinates of the AABB
        void setMin(const Vector3& min);

        /// Return the maximum coordinates of the AABB
        const Vector3& getMax() const;

        /// Set the maximum coordinates of the AABB
        void setMax(const Vector3& max);

        /// Return the size of the AABB in the three dimension x, y and z
        Vector3 getExtent() const;

        /// Inflate each side of the AABB by a given size
        void inflate(decimal dx, decimal dy, decimal dz);

        /// Return true if the current AABB is overlapping with the AABB in argument
        bool testCollision(const AABB& aabb) const;

        /// Return the volume of the AABB
        decimal getVolume() const;

        /// Merge the AABB in parameter with the current one
        void mergeWithAABB(const AABB& aabb);

        /// Replace the current AABB with a new AABB that is the union of two AABBs in parameters
        void mergeTwoAABBs(const AABB& aabb1, const AABB& aabb2);

        /// Return true if the current AABB contains the AABB given in parameter
        bool contains(const AABB& aabb) const;

        /// Return true if a point is inside the AABB
        bool contains(const Vector3& point) const;

        /// Return true if the AABB of a triangle intersects the AABB
        bool testCollisionTriangleAABB(const Vector3* trianglePoints) const;

        /// Return true if the ray intersects the AABB
        bool testRayIntersect(const Vector3& rayOrigin, const Vector3& rayDirectionInv, decimal rayMaxFraction) const;

        /// Compute the intersection of a ray and the AABB
        bool raycast(const Ray& ray, Vector3& hitPoint) const;

        /// Apply a scale factor to the AABB
        void applyScale(const Vector3& scale);

        /// Create and return an AABB for a triangle
        static AABB createAABBForTriangle(const Vector3* trianglePoints);

        // -------------------- Friendship -------------------- //

        friend class DynamicAABBTree;
};

// Return the center point of the AABB in world coordinates
RP3D_FORCE_INLINE Vector3 AABB::getCenter() const {
    return (mMinCoordinates + mMaxCoordinates) * decimal(0.5);
}

// Return the minimum coordinates of the AABB
RP3D_FORCE_INLINE const Vector3& AABB::getMin() const {
    return mMinCoordinates;
}

// Set the minimum coordinates of the AABB
RP3D_FORCE_INLINE void AABB::setMin(const Vector3& min) {
    mMinCoordinates = min;
}

// Return the maximum coordinates of the AABB
RP3D_FORCE_INLINE const Vector3& AABB::getMax() const {
    return mMaxCoordinates;
}

// Set the maximum coordinates of the AABB
RP3D_FORCE_INLINE void AABB::setMax(const Vector3& max) {
    mMaxCoordinates = max;
}

// Return the size of the AABB in the three dimension x, y and z
RP3D_FORCE_INLINE Vector3 AABB::getExtent() const {
  return  mMaxCoordinates - mMinCoordinates;
}

// Inflate each side of the AABB by a given size
RP3D_FORCE_INLINE void AABB::inflate(decimal dx, decimal dy, decimal dz) {
    mMaxCoordinates += Vector3(dx, dy, dz);
    mMinCoordinates -= Vector3(dx, dy, dz);
}

// Return true if the current AABB is overlapping with the AABB in argument.
/// Two AABBs overlap if they overlap in the three x, y and z axis at the same time
RP3D_FORCE_INLINE bool AABB::testCollision(const AABB& aabb) const {
    if (mMaxCoordinates.x < aabb.mMinCoordinates.x ||
        aabb.mMaxCoordinates.x < mMinCoordinates.x) return false;
    if (mMaxCoordinates.y < aabb.mMinCoordinates.y ||
        aabb.mMaxCoordinates.y < mMinCoordinates.y) return false;
    if (mMaxCoordinates.z < aabb.mMinCoordinates.z||
        aabb.mMaxCoordinates.z < mMinCoordinates.z) return false;
    return true;
}

// Return the volume of the AABB
RP3D_FORCE_INLINE decimal AABB::getVolume() const {
    const Vector3 diff = mMaxCoordinates - mMinCoordinates;
    return (diff.x * diff.y * diff.z);
}

// Return true if the AABB of a triangle intersects the AABB
RP3D_FORCE_INLINE bool AABB::testCollisionTriangleAABB(const Vector3* trianglePoints) const {

    if (min3(trianglePoints[0].x, trianglePoints[1].x, trianglePoints[2].x) > mMaxCoordinates.x) return false;
    if (min3(trianglePoints[0].y, trianglePoints[1].y, trianglePoints[2].y) > mMaxCoordinates.y) return false;
    if (min3(trianglePoints[0].z, trianglePoints[1].z, trianglePoints[2].z) > mMaxCoordinates.z) return false;

    if (max3(trianglePoints[0].x, trianglePoints[1].x, trianglePoints[2].x) < mMinCoordinates.x) return false;
    if (max3(trianglePoints[0].y, trianglePoints[1].y, trianglePoints[2].y) < mMinCoordinates.y) return false;
    if (max3(trianglePoints[0].z, trianglePoints[1].z, trianglePoints[2].z) < mMinCoordinates.z) return false;

    return true;
}

// Return true if a point is inside the AABB
RP3D_FORCE_INLINE bool AABB::contains(const Vector3& point) const {

    return (point.x >= mMinCoordinates.x - MACHINE_EPSILON && point.x <= mMaxCoordinates.x + MACHINE_EPSILON &&
            point.y >= mMinCoordinates.y - MACHINE_EPSILON && point.y <= mMaxCoordinates.y + MACHINE_EPSILON &&
            point.z >= mMinCoordinates.z - MACHINE_EPSILON && point.z <= mMaxCoordinates.z + MACHINE_EPSILON);
}

// Apply a scale factor to the AABB
RP3D_FORCE_INLINE void AABB::applyScale(const Vector3& scale) {
    mMinCoordinates = mMinCoordinates * scale;
    mMaxCoordinates = mMaxCoordinates * scale;
}

// Merge the AABB in parameter with the current one
RP3D_FORCE_INLINE void AABB::mergeWithAABB(const AABB& aabb) {
    mMinCoordinates.x = std::min(mMinCoordinates.x, aabb.mMinCoordinates.x);
    mMinCoordinates.y = std::min(mMinCoordinates.y, aabb.mMinCoordinates.y);
    mMinCoordinates.z = std::min(mMinCoordinates.z, aabb.mMinCoordinates.z);

    mMaxCoordinates.x = std::max(mMaxCoordinates.x, aabb.mMaxCoordinates.x);
    mMaxCoordinates.y = std::max(mMaxCoordinates.y, aabb.mMaxCoordinates.y);
    mMaxCoordinates.z = std::max(mMaxCoordinates.z, aabb.mMaxCoordinates.z);
}

// Replace the current AABB with a new AABB that is the union of two AABBs in parameters
RP3D_FORCE_INLINE void AABB::mergeTwoAABBs(const AABB& aabb1, const AABB& aabb2) {
    mMinCoordinates.x = std::min(aabb1.mMinCoordinates.x, aabb2.mMinCoordinates.x);
    mMinCoordinates.y = std::min(aabb1.mMinCoordinates.y, aabb2.mMinCoordinates.y);
    mMinCoordinates.z = std::min(aabb1.mMinCoordinates.z, aabb2.mMinCoordinates.z);

    mMaxCoordinates.x = std::max(aabb1.mMaxCoordinates.x, aabb2.mMaxCoordinates.x);
    mMaxCoordinates.y = std::max(aabb1.mMaxCoordinates.y, aabb2.mMaxCoordinates.y);
    mMaxCoordinates.z = std::max(aabb1.mMaxCoordinates.z, aabb2.mMaxCoordinates.z);
}

// Return true if the current AABB contains the AABB given in parameter
RP3D_FORCE_INLINE bool AABB::contains(const AABB& aabb) const {

    bool isInside = true;
    isInside = isInside && mMinCoordinates.x <= aabb.mMinCoordinates.x;
    isInside = isInside && mMinCoordinates.y <= aabb.mMinCoordinates.y;
    isInside = isInside && mMinCoordinates.z <= aabb.mMinCoordinates.z;

    isInside = isInside && mMaxCoordinates.x >= aabb.mMaxCoordinates.x;
    isInside = isInside && mMaxCoordinates.y >= aabb.mMaxCoordinates.y;
    isInside = isInside && mMaxCoordinates.z >= aabb.mMaxCoordinates.z;
    return isInside;
}

// Create and return an AABB for a triangle
RP3D_FORCE_INLINE AABB AABB::createAABBForTriangle(const Vector3* trianglePoints) {

    Vector3 minCoords(trianglePoints[0].x, trianglePoints[0].y, trianglePoints[0].z);
    Vector3 maxCoords(trianglePoints[0].x, trianglePoints[0].y, trianglePoints[0].z);

    if (trianglePoints[1].x < minCoords.x) minCoords.x = trianglePoints[1].x;
    if (trianglePoints[1].y < minCoords.y) minCoords.y = trianglePoints[1].y;
    if (trianglePoints[1].z < minCoords.z) minCoords.z = trianglePoints[1].z;

    if (trianglePoints[2].x < minCoords.x) minCoords.x = trianglePoints[2].x;
    if (trianglePoints[2].y < minCoords.y) minCoords.y = trianglePoints[2].y;
    if (trianglePoints[2].z < minCoords.z) minCoords.z = trianglePoints[2].z;

    if (trianglePoints[1].x > maxCoords.x) maxCoords.x = trianglePoints[1].x;
    if (trianglePoints[1].y > maxCoords.y) maxCoords.y = trianglePoints[1].y;
    if (trianglePoints[1].z > maxCoords.z) maxCoords.z = trianglePoints[1].z;

    if (trianglePoints[2].x > maxCoords.x) maxCoords.x = trianglePoints[2].x;
    if (trianglePoints[2].y > maxCoords.y) maxCoords.y = trianglePoints[2].y;
    if (trianglePoints[2].z > maxCoords.z) maxCoords.z = trianglePoints[2].z;

    return AABB(minCoords, maxCoords);
}

// Return true if the ray intersects the AABB
RP3D_FORCE_INLINE bool AABB::testRayIntersect(const Vector3& rayOrigin, const Vector3& rayDirectionInverse, decimal rayMaxFraction) const {

    // This algorithm relies on the IEE floating point properties (division by zero). If the rayDirection is zero, rayDirectionInverse and
    // therfore t1 and t2 will be +-INFINITY. If the i coordinate of the ray's origin is inside the AABB (mMinCoordinates[i] < rayOrigin[i] < mMaxCordinates[i)), we have
    // t1 = -t2 = +- INFINITY. Since max(n, -INFINITY) = min(n, INFINITY) = n for all n, tMin and tMax will stay unchanged. Secondly, if the i
    // coordinate of the ray's origin is outside the box (rayOrigin[i] < mMinCoordinates[i] or rayOrigin[i] > mMaxCoordinates[i]) we have
    // t1 = t2 = +- INFINITY and therefore either tMin = +INFINITY or tMax = -INFINITY. One of those values will stay until the end and make the
    // method to return false. Unfortunately, if the ray lies exactly on a slab (rayOrigin[i] = mMinCoordinates[i] or rayOrigin[i] = mMaxCoordinates[i]) we
    // have t1 = (mMinCoordinates[i] - rayOrigin[i]) * rayDirectionInverse[i] = 0 * INFINITY = NaN which is a problem for the remaining of the algorithm.
    // This will cause the method to return true when the ray is not intersecting the AABB and therefore cause to traverse more nodes than necessary in
    // the BVH tree. Because this should be rare, it is not really a big issue.
    // Reference: https://tavianator.com/2011/ray_box.html

    decimal t1 = (mMinCoordinates[0] - rayOrigin[0]) * rayDirectionInverse[0];
    decimal t2 = (mMaxCoordinates[0] - rayOrigin[0]) * rayDirectionInverse[0];

    decimal tMin = std::min(t1, t2);
    decimal tMax = std::max(t1, t2);
    tMax = std::min(tMax, rayMaxFraction);

    for (int i = 1; i < 3; i++) {

        t1 = (mMinCoordinates[i] - rayOrigin[i]) * rayDirectionInverse[i];
        t2 = (mMaxCoordinates[i] - rayOrigin[i]) * rayDirectionInverse[i];

        tMin = std::max(tMin, std::min(t1, t2));
        tMax = std::min(tMax, std::max(t1, t2));
    }

    return tMax >= std::max(tMin, decimal(0.0));
}

// Compute the intersection of a ray and the AABB
RP3D_FORCE_INLINE bool AABB::raycast(const Ray& ray, Vector3& hitPoint) const {

    decimal tMin = decimal(0.0);
    decimal tMax = DECIMAL_LARGEST;

    const decimal epsilon = decimal(0.00001);

    const Vector3 rayDirection = ray.point2 - ray.point1;

    // For all three slabs
    for (int i=0; i < 3; i++) {

        // If the ray is parallel to the slab
        if (std::abs(rayDirection[i]) < epsilon) {

            // If origin of the ray is not inside the slab, no hit
            if (ray.point1[i] < mMinCoordinates[i] || ray.point1[i] > mMaxCoordinates[i]) return false;
        }
        else {

            decimal rayDirectionInverse = decimal(1.0) / rayDirection[i];
            decimal t1 = (mMinCoordinates[i] - ray.point1[i]) * rayDirectionInverse;
            decimal t2 = (mMaxCoordinates[i] - ray.point1[i]) * rayDirectionInverse;

            if (t1 > t2) {

                // Swap t1 and t2
                decimal tTemp = t2;
                t2 = t1;
                t1 = tTemp;
            }
            
            tMin = std::max(tMin, t1);
            tMax = std::min(tMax, t2);
            
            // Exit with no collision 
            if (tMin > tMax) return false;
        }
    }

    // Compute the hit point
    hitPoint = ray.point1 + tMin * rayDirection;

    return true;
}

}

#endif