/***************************************************************************
*                                                                          *
*  Copyright (C) Golaem S.A. & Mikros Image - All Rights Reserved.         *
*                                                                          *
***************************************************************************/

#pragma once

#include <glmCoreDefinitions.h>
#include <glmMathBase.h>
#include <float.h>

namespace glm
{
	template <typename pointT>
	struct AABB
	{
		AABB()
			: _left(-1)
			, _right(-1)
			, _firstIndex(0)
			, _indexCount(0)
		{
		}
		pointT _AABBmin, _AABBmax;
		int _left, _right;
		int _firstIndex, _indexCount; // _indexCount == 0 if none
	};

	//-----------------------------------------------------------------------------
	template <typename vertexT, typename pointT>
	void computeAABBBounds(const vertexT* vts, int* indices, int indexCount, pointT& AABBmin, pointT& AABBmax)
	{
		AABBmin.setValues(FLT_MAX, FLT_MAX, FLT_MAX);
		AABBmax.setValues(-FLT_MAX, -FLT_MAX, -FLT_MAX);

		for (int i = 0; i < indexCount; i++)
		{
			float* v = (float*)&vts[(*indices++)];

			//AABBmin[0] = minNoComparison(AABBmin[0], v[0]);
			//AABBmin[1] = minNoComparison(AABBmin[1], v[1]);
			//AABBmin[2] = minNoComparison(AABBmin[2], v[2]);

			//AABBmax[0] = maxNoComparison(AABBmax[0], v[0]);
			//AABBmax[1] = maxNoComparison(AABBmax[1], v[1]);
			//AABBmax[2] = maxNoComparison(AABBmax[2], v[2]);

			AABBmin[0] = (AABBmin[0] < v[0]) ? AABBmin[0] : v[0];
			AABBmin[1] = (AABBmin[1] < v[1]) ? AABBmin[1] : v[1];
			AABBmin[2] = (AABBmin[2] < v[2]) ? AABBmin[2] : v[2];
			AABBmax[0] = (AABBmax[0] > v[0]) ? AABBmax[0] : v[0];
			AABBmax[1] = (AABBmax[1] > v[1]) ? AABBmax[1] : v[1];
			AABBmax[2] = (AABBmax[2] > v[2]) ? AABBmax[2] : v[2];
		}
	}

	//-----------------------------------------------------------------------------
	template <typename vertexT, typename pointT>
	void recomputeAABBBoundsOnAxis(const vertexT* vts, int* indices, int indexCount, pointT& AABBmin, pointT& AABBmax, int axis)
	{
		AABBmin[axis] = FLT_MAX;
		AABBmax[axis] = -FLT_MAX;

		for (int i = 0; i < indexCount; i++)
		{
			float* v = (float*)&vts[(*indices++)];

			AABBmin[axis] = glm::min(AABBmin[axis], v[axis]);
			AABBmax[axis] = glm::max(AABBmax[axis], v[axis]);
		}
	}

	//-----------------------------------------------------------------------------
	template <typename vertexT, typename pointT>
	int computeBestAxis(const vertexT* vts, int* indices, int indexCount, const pointT& AABBmin, const pointT& AABBmax)
	{
		if (indexCount < 16 * 3)
			return -1;

		int score[3] = { 0, 0, 0 }; // range up to +-INT_MAX should be enough
		for (int i = 0; i < 3; i++)
		{
			float axisMid = (AABBmin[i] + AABBmax[i]) * 0.5f * 3.f; //multiply by 3 instead of dividing all 3 vertices later
																	/*int left(0), right(0);*/

			for (int k = 0; k < indexCount; k += 3)
			{
				float faceMiddle = ((*(pointT*)&vts[indices[k + 0]])[i] + (*(pointT*)&vts[indices[k + 1]])[i] + (*(pointT*)&vts[indices[k + 2]])[i]);
				score[i] += (int)sign(faceMiddle - axisMid); // -1 if on one side, +1 if on the other, 0 if on the axis

				//if (faceMiddle < axisMid)
				//	left++;
				//else
				//	right++;
			}
			score[i] = abs(score[i]);
		}
		int best(0);
		for (int i = 1; i < 3; i++)
			if (score[i] < score[best])
				best = i;

		return best;
	}

	//-----------------------------------------------------------------------------
	// reorder indices in the array. low part is before split axis(on the left). high part, on the right.
	template <typename vertexT, typename pointT>
	int splitIndices(const vertexT* vts, int* indices, int indexCount, int axis, const pointT& AABBmin, const pointT& AABBmax)
	{
		float axisMid = (AABBmin[axis] + AABBmax[axis]) * 0.5f;
		int* ind = indices;
		int tailIndex = indexCount - 3;
		for (int k = 0; k < indexCount;)
		{
			float faceMiddle = ((*(pointT*)&vts[ind[k]])[axis] + (*(pointT*)&vts[ind[k + 1]])[axis] + (*(pointT*)&vts[ind[k + 2]])[axis]) / 3.f;
			if (faceMiddle < axisMid)
			{
				k += 3;
			}
			else
			{
				std::swap(ind[k], ind[tailIndex]);
				std::swap(ind[k + 1], ind[tailIndex + 1]);
				std::swap(ind[k + 2], ind[tailIndex + 2]);
				tailIndex -= 3;
			}
			if (tailIndex <= k)
				break;
		}
		return tailIndex;
	}

	//-----------------------------------------------------------------------------
	template <typename vertexT, typename pointT, typename AABContainer>
	int computeAABBHierarchy(const vertexT* vts, int* indices, int firstIndex, int indexCount, int level, AABContainer& container, pointT& aabbMin, pointT& aabbMax)
	{
		if (indexCount <= 0)
			return -1;

		AABB<pointT> aabb;
		aabb._AABBmin = aabbMin;
		aabb._AABBmax = aabbMax;
		int bestAxis = computeBestAxis<vertexT, pointT>(vts, indices + firstIndex, indexCount, aabb._AABBmin, aabb._AABBmax);
		if (level < 20 && bestAxis >= 0)
		{
			int indicesInLeft(splitIndices(vts, indices + firstIndex, indexCount, bestAxis, aabb._AABBmin, aabb._AABBmax));

			pointT leftMin = aabbMin;
			pointT leftMax = aabbMax;
			recomputeAABBBoundsOnAxis<vertexT, pointT>(vts, indices + firstIndex, indicesInLeft, leftMin, leftMax, bestAxis);

			pointT rightMin = aabbMin;
			pointT rightMax = aabbMax;
			recomputeAABBBoundsOnAxis<vertexT, pointT>(vts, indices + firstIndex + indicesInLeft, indexCount - indicesInLeft, rightMin, rightMax, bestAxis);

			aabb._left = computeAABBHierarchy<vertexT, pointT, AABContainer>(vts, indices, firstIndex, indicesInLeft, level + 1, container, leftMin, leftMax);
			aabb._right = computeAABBHierarchy<vertexT, pointT, AABContainer>(vts, indices, firstIndex + indicesInLeft, indexCount - indicesInLeft, level + 1, container, rightMin, rightMax);
		}
		else
		{
			aabb._firstIndex = firstIndex;
			aabb._indexCount = indexCount;
		}
		container.push_back(aabb);
		return (int)(container.size() - 1);
	}

	//-----------------------------------------------------------------------------
	template <typename vertexT, typename pointT, typename AABContainer>
	int computeAABBHierarchy(const vertexT* vts, int* indices, int firstIndex, int indexCount, int level, AABContainer& container)
	{
		if (indexCount <= 0)
			return -1;

		pointT aabbMin;
		pointT aabbMax;

		// compute whole aabb bbox at start
		AABB<pointT> aabb;
		computeAABBBounds<vertexT, pointT>(vts, indices + firstIndex, indexCount, aabbMin, aabbMax);

		// recursive call once we computed the aabb
		return computeAABBHierarchy(vts, indices, firstIndex, indexCount, level, container, aabbMin, aabbMax);
	}


#if !defined(RayTriangleEPSILON)

#define RayTriangleEPSILON 0.000001
#define RayTriangleCROSS(dest, v1, v2)       \
    dest[0] = v1[1] * v2[2] - v1[2] * v2[1]; \
    dest[1] = v1[2] * v2[0] - v1[0] * v2[2]; \
    dest[2] = v1[0] * v2[1] - v1[1] * v2[0];
#define RayTriangleDOT(v1, v2) (v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2])
#define RayTriangleSUB(dest, v1, v2) \
    dest[0] = v1[0] - v2[0];         \
    dest[1] = v1[1] - v2[1];         \
    dest[2] = v1[2] - v2[2];
#endif

	//-----------------------------------------------------------------------------
	inline int rayTriangle(float orig[3], float dir[3],
		float vert0[3], float vert1[3], float vert2[3],
		float* t, float* u, float* v)
	{
		float edge1[3], edge2[3], tvec[3], pvec[3], qvec[3];
		float det, inv_det;

		/* find vectors for two edges sharing vert0 */
		RayTriangleSUB(edge1, vert1, vert0);
		RayTriangleSUB(edge2, vert2, vert0);

		/* begin calculating determinant - also used to calculate U parameter */
		RayTriangleCROSS(pvec, dir, edge2);

		/* if determinant is near zero, ray lies in plane of triangle */
		det = RayTriangleDOT(edge1, pvec);

#ifdef TEST_CULL /* define TEST_CULL if culling is desired */
		if (det < RayTriangleEPSILON)
			return 0;

		/* calculate distance from vert0 to ray origin */
		RayTriangleSUB(tvec, orig, vert0);

		/* calculate U parameter and test bounds */
		*u = RayTriangleDOT(tvec, pvec);
		if (*u < 0.0 || *u > det)
			return 0;

		/* prepare to test V parameter */
		RayTriangleCROSS(qvec, tvec, edge1);

		/* calculate V parameter and test bounds */
		*v = RayTriangleDOT(dir, qvec);
		if (*v < 0.0 || *u + *v > det)
			return 0;

		/* calculate t, scale parameters, ray intersects triangle */
		*t = DOT(edge2, qvec);
		inv_det = 1.0 / det;
		*t *= inv_det;
		*u *= inv_det;
		*v *= inv_det;
#else /* the non-culling branch */
		if (det > -RayTriangleEPSILON && det < RayTriangleEPSILON)
			return 0;
		inv_det = 1.0f / det;

		/* calculate distance from vert0 to ray origin */
		RayTriangleSUB(tvec, orig, vert0);

		/* calculate U parameter and test bounds */
		*u = RayTriangleDOT(tvec, pvec) * inv_det;
		if (*u < 0.0 || *u > 1.0)
			return 0;

		/* prepare to test V parameter */
		RayTriangleCROSS(qvec, tvec, edge1);

		/* calculate V parameter and test bounds */
		*v = RayTriangleDOT(dir, qvec) * inv_det;
		if (*v < 0.0 || *u + *v > 1.0)
			return 0;

		/* calculate t, ray intersects triangle */
		*t = RayTriangleDOT(edge2, qvec) * inv_det;
#endif
		return 1;
	}

	//-----------------------------------------------------------------------------
	template <typename pointT>
	bool intersectRayAABoxT(const pointT& rO, const pointT& rV, const pointT& min, const pointT& max)
	{
		pointT T_1, T_2; // vectors to hold the T-values for every direction
		float t_near = -FLT_MAX;
		float t_far = FLT_MAX;

		for (int i = 0; i < 3; i++)
		{
			//we test slabs in every direction
			if (fabsf(rV[i]) <= FLT_EPSILON)
			{
				// ray parallel to planes in this direction
				if ((rO[i] < min[i]) || (rO[i] > max[i]))
				{
					return false; // parallel AND outside box : no intersection possible
				}
			}
			else
			{
				// ray not parallel to planes in this direction
				T_1[i] = (min[i] - rO[i]) / rV[i];
				T_2[i] = (max[i] - rO[i]) / rV[i];

				if (T_1[i] > T_2[i])
				{
					// we want T_1 to hold values for intersection with near plane
					//swap(T_1,T_2);
					float temp = T_1[i];
					T_1[i] = T_2[i];
					T_2[i] = temp;
				}
				if (T_1[i] > t_near)
				{
					t_near = T_1[i];
				}
				if (T_2[i] < t_far)
				{
					t_far = T_2[i];
				}
				if ((t_near > t_far) || (t_far < 0.f))
				{
					return false;
				}
			}
		}
		return true; // if we made it here, there was an intersection - YAY
	}

	//-----------------------------------------------------------------------------
	template <typename vertexT, typename pointT>
	bool hierarchicalRaycast(const pointT& sourcePt, const pointT& direction, /*const pointT& directionInverse,*/ int& triIndex, float& tt, int aabbIndex, const AABB<pointT>* aabbArray, const vertexT* vertices, const int* indices)
	{
		const AABB<pointT>& aabb(aabbArray[aabbIndex]);

		//#if GLM_IS_COMPILER_MSVC && ( defined(_M_X64) || defined(_M_IA64) )
		//    if (intersectRayAABoxBranchlessT<pointT>(sourcePt, directionInverse, aabb._AABBmin, aabb._AABBmax))
		//#else
		if (intersectRayAABoxT<pointT>(sourcePt, direction, aabb._AABBmin, aabb._AABBmax))
			//#endif
		{
			bool hasHit(false);

			if (aabb._left != -1)
				hasHit |= hierarchicalRaycast<vertexT, pointT>(sourcePt, direction, triIndex, tt, aabb._left, aabbArray, vertices, indices);

			if (aabb._right != -1)
				hasHit |= hierarchicalRaycast<vertexT, pointT>(sourcePt, direction, triIndex, tt, aabb._right, aabbArray, vertices, indices);

			for (int i = aabb._firstIndex; i < (aabb._firstIndex + aabb._indexCount); i += 3)
			{
				float tu, tv, t;
				float* v1 = (float*)&vertices[indices[i]];
				float* v2 = (float*)&vertices[indices[i + 1]];
				float* v3 = (float*)&vertices[indices[i + 2]];
				if (rayTriangle((float*)&sourcePt.x, (float*)&direction.x,
					v1, v2, v3,
					&t, &tu, &tv))
				{
					if ((t < tt) && (t >= 0.f))
					{
						triIndex = i;
						tt = t;
						hasHit = true;
					}
				}
			}
			return hasHit;
		}
		return false;
    }
} // namespace glm
