/***************************************************************************
*                                                                          *
*  Copyright (C) Golaem S.A.  All Rights Reserved.                         *
*                                                                          *
***************************************************************************/

#pragma once

#include "glmAngle.h"
#include "glmQuaternion.h"
#include "glmMathBase.h"
#include "glmVector3.h"
#include "glmVector4.h"
#include "glmMatrix4.h"

namespace glm 
{ 

	struct Frustum  
	{
	public:
		void    Update(const Matrix4 &view, const Matrix4& projection);
		void 	Update(const double view[16], const double projection[16]);

		bool    PointInFrustum( const Vector4 & vt ) const
		{
			// If you remember the plane equation (A*x + B*y + C*z + D = 0), then the rest
			// of this code should be quite obvious and easy to figure out yourself.
			// In case don't know the plane equation, it might be a good idea to look
			// at our Plane Collision tutorial at www.GameTutorials.com in OpenGL Tutorials.
			// I will briefly go over it here.  (A,B,C) is the (X,Y,Z) of the normal to the plane.
			// They are the same thing... but just called ABC because you don't want to say:
			// (x*x + y*y + z*z + d = 0).  That would be wrong, so they substitute them.
			// the (x, y, z) in the equation is the point that you are testing.  The D is
			// The distance the plane is from the origin.  The equation ends with "= 0" because
			// that is true when the point (x, y, z) is ON the plane.  When the point is NOT on
			// the plane, it is either a negative number (the point is behind the plane) or a
			// positive number (the point is in front of the plane).  We want to check if the point
			// is in front of the plane, so all we have to do is go through each point and make
			// sure the plane equation goes out to a positive number on each side of the frustum.
			// The result (be it positive or negative) is the distance the point is front the plane.

			// Go through all the sides of the frustum
			for(int i = 0; i < 6; i++ )
			{
				// Calculate the plane equation and check if the point is behind a side of the frustum
				if(m_Frustum[i][A] * vt.x + m_Frustum[i][B] * vt.y + m_Frustum[i][C] * vt.z + m_Frustum[i][D] <= 0)
				{
					// The point was behind a side, so it ISN'T in the frustum
					return false;
				}
			}

			// The point was inside of the frustum (In front of ALL the sides of the frustum)
			return true;
		}

		bool    SphereInFrustum( const Vector4 & vt) const
		{
			for(int i = 0; i < 6; i++ )    
			{
				// If the center of the sphere is farther away from the plane than the radius
				if( m_Frustum[i][A] * vt.x + m_Frustum[i][B] * vt.y + m_Frustum[i][C] * vt.z + m_Frustum[i][D] <= -vt.w )
				{
					// The distance was greater than the radius so the sphere is outside of the frustum
					return false;
				}
			}

			// The sphere was inside of the frustum!
			return true;
		}

		int SphereInFrustumVis(const Vector4& v) const
		{

			float distance;
			int result = 2;

			for(int i=0; i < 6; i++) {
				distance = m_Frustum[i][A] * v.x + m_Frustum[i][B] * v.y + m_Frustum[i][C] * v.z + m_Frustum[i][D];//pl[i].distance(p);
				if (distance < -v.w)
					return 0;
				else if (distance < v.w)
					result =  1;
			}
			return(result);
		}

        bool BoxInFrustum(const Vector3& vt, const Vector3& size) const
        {
            for(int i = 0; i < 6; i++ )
			{
				if(m_Frustum[i][A] * (vt.x - size.x) + m_Frustum[i][B] * (vt.y - size.y) + m_Frustum[i][C] * (vt.z - size.z) + m_Frustum[i][D] > 0)
					continue;
				if(m_Frustum[i][A] * (vt.x + size.x) + m_Frustum[i][B] * (vt.y - size.y) + m_Frustum[i][C] * (vt.z - size.z) + m_Frustum[i][D] > 0)
					continue;
				if(m_Frustum[i][A] * (vt.x - size.x) + m_Frustum[i][B] * (vt.y + size.y) + m_Frustum[i][C] * (vt.z - size.z) + m_Frustum[i][D] > 0)
					continue;
				if(m_Frustum[i][A] * (vt.x + size.x) + m_Frustum[i][B] * (vt.y + size.y) + m_Frustum[i][C] * (vt.z - size.z) + m_Frustum[i][D] > 0)
					continue;
				if(m_Frustum[i][A] * (vt.x - size.x) + m_Frustum[i][B] * (vt.y - size.y) + m_Frustum[i][C] * (vt.z + size.z) + m_Frustum[i][D] > 0)
					continue;
				if(m_Frustum[i][A] * (vt.x + size.x) + m_Frustum[i][B] * (vt.y - size.y) + m_Frustum[i][C] * (vt.z + size.z) + m_Frustum[i][D] > 0)
					continue;
				if(m_Frustum[i][A] * (vt.x - size.x) + m_Frustum[i][B] * (vt.y + size.y) + m_Frustum[i][C] * (vt.z + size.z) + m_Frustum[i][D] > 0)
					continue;
				if(m_Frustum[i][A] * (vt.x + size.x) + m_Frustum[i][B] * (vt.y + size.y) + m_Frustum[i][C] * (vt.z + size.z) + m_Frustum[i][D] > 0)
					continue;

				// If we get here, it isn't in the frustum
				return false;
			}

			return true;
        }

        // matrix is an orthonormalized matrix. only orientation is used.
		bool OBBInFrustum( const Matrix4 &mt, const Vector3 &pos, const Vector3& size) const;

		void Inflate ( float distance )
		{
			for (int i=0;i<6;i++)
				m_Frustum[i][3] += distance;
		}
	public:

		float m_Frustum[6][4];
		void NormalizePlane(float frustum[6][4], int side);
		void NormalizePlane(double frustum[6][4], int side);

		enum FrustumSide
		{
			RIGHT    = 0,        // The RIGHT side of the frustum
			LEFT    = 1,        // The LEFT     side of the frustum
			BOTTOM    = 2,        // The BOTTOM side of the frustum
			TOP        = 3,        // The TOP side of the frustum
			BACK    = 4,        // The BACK    side of the frustum
			FRONT    = 5            // The FRONT side of the frustum
		}; 

		// Like above, instead of saying a number for the ABC and D of the plane, we
		// want to be more descriptive.
		enum PlaneData
		{
			A = 0,                // The X value of the plane's normal
			B = 1,                // The Y value of the plane's normal
			C = 2,                // The Z value of the plane's normal
			D = 3                // The distance the plane is from the origin
		};


	};


	//-------------------------------------------------------------------------
	// NormalizePlane
	//-------------------------------------------------------------------------
	inline void 
	Frustum::NormalizePlane(float frustum[6][4], int side)
	{
		// Here we calculate the magnitude of the normal to the plane (point A B C)
		// Remember that (A, B, C) is that same thing as the normal's (X, Y, Z).
		// To calculate magnitude you use the equation:  magnitude = sqrtf( x^2 + y^2 + z^2)
		float magnitude = 1.0f / sqrtf( frustum[side][A] * frustum[side][A] +
			frustum[side][B] * frustum[side][B] +
			frustum[side][C] * frustum[side][C] );

		// Then we divide the plane's values by it's magnitude.
		// This makes it easier to work with.
		frustum[side][A] *= magnitude;
		frustum[side][B] *= magnitude;
		frustum[side][C] *= magnitude;
		frustum[side][D] *= magnitude;
	}

	inline void 
	Frustum::NormalizePlane(double frustum[6][4], int side)
	{
		double magnitude = 1.0 / sqrt( frustum[side][A] * frustum[side][A] +
			frustum[side][B] * frustum[side][B] +
			frustum[side][C] * frustum[side][C] );

		// Then we divide the plane's values by it's magnitude.
		// This makes it easier to work with.
		frustum[side][A] *= magnitude;
		frustum[side][B] *= magnitude;
		frustum[side][C] *= magnitude;
		frustum[side][D] *= magnitude;
	}
	inline void 
	Frustum::Update(const double modl[16], const double proj[16])
	{
		double   clip[16];
		//   float   t;

		double dFrustum[6][4];
		
		// Now that we have our modelview and projection matrix, if we combine these 2 matrices,
		// it will give us our clipping planes.  To combine 2 matrices, we multiply them.

		clip[ 0] = modl[ 0] * proj[ 0] + modl[ 1] * proj[ 4] + modl[ 2] * proj[ 8] + modl[ 3] * proj[12];
		clip[ 1] = modl[ 0] * proj[ 1] + modl[ 1] * proj[ 5] + modl[ 2] * proj[ 9] + modl[ 3] * proj[13];
		clip[ 2] = modl[ 0] * proj[ 2] + modl[ 1] * proj[ 6] + modl[ 2] * proj[10] + modl[ 3] * proj[14];
		clip[ 3] = modl[ 0] * proj[ 3] + modl[ 1] * proj[ 7] + modl[ 2] * proj[11] + modl[ 3] * proj[15];

		clip[ 4] = modl[ 4] * proj[ 0] + modl[ 5] * proj[ 4] + modl[ 6] * proj[ 8] + modl[ 7] * proj[12];
		clip[ 5] = modl[ 4] * proj[ 1] + modl[ 5] * proj[ 5] + modl[ 6] * proj[ 9] + modl[ 7] * proj[13];
		clip[ 6] = modl[ 4] * proj[ 2] + modl[ 5] * proj[ 6] + modl[ 6] * proj[10] + modl[ 7] * proj[14];
		clip[ 7] = modl[ 4] * proj[ 3] + modl[ 5] * proj[ 7] + modl[ 6] * proj[11] + modl[ 7] * proj[15];

		clip[ 8] = modl[ 8] * proj[ 0] + modl[ 9] * proj[ 4] + modl[10] * proj[ 8] + modl[11] * proj[12];
		clip[ 9] = modl[ 8] * proj[ 1] + modl[ 9] * proj[ 5] + modl[10] * proj[ 9] + modl[11] * proj[13];
		clip[10] = modl[ 8] * proj[ 2] + modl[ 9] * proj[ 6] + modl[10] * proj[10] + modl[11] * proj[14];
		clip[11] = modl[ 8] * proj[ 3] + modl[ 9] * proj[ 7] + modl[10] * proj[11] + modl[11] * proj[15];

		clip[12] = modl[12] * proj[ 0] + modl[13] * proj[ 4] + modl[14] * proj[ 8] + modl[15] * proj[12];
		clip[13] = modl[12] * proj[ 1] + modl[13] * proj[ 5] + modl[14] * proj[ 9] + modl[15] * proj[13];
		clip[14] = modl[12] * proj[ 2] + modl[13] * proj[ 6] + modl[14] * proj[10] + modl[15] * proj[14];
		clip[15] = modl[12] * proj[ 3] + modl[13] * proj[ 7] + modl[14] * proj[11] + modl[15] * proj[15];

		// Now we actually want to get the sides of the frustum.  To do this we take
		// the clipping planes we received above and extract the sides from them.

		// This will extract the RIGHT side of the frustum
		dFrustum[RIGHT][A] = clip[ 3] - clip[ 0];
		dFrustum[RIGHT][B] = clip[ 7] - clip[ 4];
		dFrustum[RIGHT][C] = clip[11] - clip[ 8];
		dFrustum[RIGHT][D] = clip[15] - clip[12];

		// Now that we have a normal (A,B,C) and a distance (D) to the plane,
		// we want to normalize that normal and distance.

		// Normalize the RIGHT side
		NormalizePlane(dFrustum, RIGHT);

		// This will extract the LEFT side of the frustum
		dFrustum[LEFT][A] = clip[ 3] + clip[ 0];
		dFrustum[LEFT][B] = clip[ 7] + clip[ 4];
		dFrustum[LEFT][C] = clip[11] + clip[ 8];
		dFrustum[LEFT][D] = clip[15] + clip[12];

		// Normalize the LEFT side
		NormalizePlane(dFrustum, LEFT);

		// This will extract the BOTTOM side of the frustum
		dFrustum[BOTTOM][A] = clip[ 3] + clip[ 1];
		dFrustum[BOTTOM][B] = clip[ 7] + clip[ 5];
		dFrustum[BOTTOM][C] = clip[11] + clip[ 9];
		dFrustum[BOTTOM][D] = clip[15] + clip[13];

		// Normalize the BOTTOM side
		NormalizePlane(dFrustum, BOTTOM);

		// This will extract the TOP side of the frustum
		dFrustum[TOP][A] = clip[ 3] - clip[ 1];
		dFrustum[TOP][B] = clip[ 7] - clip[ 5];
		dFrustum[TOP][C] = clip[11] - clip[ 9];
		dFrustum[TOP][D] = clip[15] - clip[13];

		// Normalize the TOP side
		NormalizePlane(dFrustum, TOP);

		// This will extract the BACK side of the frustum
		dFrustum[BACK][A] = clip[ 3] - clip[ 2];
		dFrustum[BACK][B] = clip[ 7] - clip[ 6];
		dFrustum[BACK][C] = clip[11] - clip[10];
		dFrustum[BACK][D] = clip[15] - clip[14];

		// This will extract the FRONT side of the frustum
		dFrustum[FRONT][A] = clip[ 3] + clip[ 2];
		dFrustum[FRONT][B] = clip[ 7] + clip[ 6];
		dFrustum[FRONT][C] = clip[11] + clip[10];
		dFrustum[FRONT][D] = clip[15] + clip[14];

		if (sqrt(dFrustum[FRONT][A] * dFrustum[FRONT][A] + dFrustum[FRONT][B] * dFrustum[FRONT][B] + dFrustum[FRONT][C] * dFrustum[FRONT][C]) < DBL_EPSILON)
		{
			dFrustum[FRONT][A] = -dFrustum[BACK][A];
			dFrustum[FRONT][B] = -dFrustum[BACK][B];
			dFrustum[FRONT][C] = -dFrustum[BACK][C];
			//m_Frustum[FRONT][D] = 1000000.f;
		}

		// Normalize the FRONT side
		NormalizePlane(dFrustum, FRONT);

		// This will extract the BACK side of the frustum
		if (sqrt(dFrustum[BACK][A] * dFrustum[BACK][A] + dFrustum[BACK][B] * dFrustum[BACK][B] + dFrustum[BACK][C] * dFrustum[BACK][C]) < DBL_EPSILON)
		{
			dFrustum[BACK][A] = -dFrustum[FRONT][A];
			dFrustum[BACK][B] = -dFrustum[FRONT][B];
			dFrustum[BACK][C] = -dFrustum[FRONT][C];
			//m_Frustum[BACK][D] = 1000000.f;
		}
		// Normalize the BACK side
		NormalizePlane(dFrustum, BACK);

		for (int i = 0;i<6;i++)
		{
			for (int j = 0;j<4;j++)
			{
				m_Frustum[i][j] = static_cast<float>( dFrustum[i][j] );
			}
		}
	}
	//-------------------------------------------------------------------------
	// Update
	//-------------------------------------------------------------------------
	inline void 
	Frustum::Update(const Matrix4 &view, const Matrix4& projection)
	{
		const float   *proj( projection.const_floats() );
		const float   *modl( view.const_floats() );
		float   clip[16];
		//   float   t;


		// Now that we have our modelview and projection matrix, if we combine these 2 matrices,
		// it will give us our clipping planes.  To combine 2 matrices, we multiply them.

		clip[ 0] = modl[ 0] * proj[ 0] + modl[ 1] * proj[ 4] + modl[ 2] * proj[ 8] + modl[ 3] * proj[12];
		clip[ 1] = modl[ 0] * proj[ 1] + modl[ 1] * proj[ 5] + modl[ 2] * proj[ 9] + modl[ 3] * proj[13];
		clip[ 2] = modl[ 0] * proj[ 2] + modl[ 1] * proj[ 6] + modl[ 2] * proj[10] + modl[ 3] * proj[14];
		clip[ 3] = modl[ 0] * proj[ 3] + modl[ 1] * proj[ 7] + modl[ 2] * proj[11] + modl[ 3] * proj[15];

		clip[ 4] = modl[ 4] * proj[ 0] + modl[ 5] * proj[ 4] + modl[ 6] * proj[ 8] + modl[ 7] * proj[12];
		clip[ 5] = modl[ 4] * proj[ 1] + modl[ 5] * proj[ 5] + modl[ 6] * proj[ 9] + modl[ 7] * proj[13];
		clip[ 6] = modl[ 4] * proj[ 2] + modl[ 5] * proj[ 6] + modl[ 6] * proj[10] + modl[ 7] * proj[14];
		clip[ 7] = modl[ 4] * proj[ 3] + modl[ 5] * proj[ 7] + modl[ 6] * proj[11] + modl[ 7] * proj[15];

		clip[ 8] = modl[ 8] * proj[ 0] + modl[ 9] * proj[ 4] + modl[10] * proj[ 8] + modl[11] * proj[12];
		clip[ 9] = modl[ 8] * proj[ 1] + modl[ 9] * proj[ 5] + modl[10] * proj[ 9] + modl[11] * proj[13];
		clip[10] = modl[ 8] * proj[ 2] + modl[ 9] * proj[ 6] + modl[10] * proj[10] + modl[11] * proj[14];
		clip[11] = modl[ 8] * proj[ 3] + modl[ 9] * proj[ 7] + modl[10] * proj[11] + modl[11] * proj[15];

		clip[12] = modl[12] * proj[ 0] + modl[13] * proj[ 4] + modl[14] * proj[ 8] + modl[15] * proj[12];
		clip[13] = modl[12] * proj[ 1] + modl[13] * proj[ 5] + modl[14] * proj[ 9] + modl[15] * proj[13];
		clip[14] = modl[12] * proj[ 2] + modl[13] * proj[ 6] + modl[14] * proj[10] + modl[15] * proj[14];
		clip[15] = modl[12] * proj[ 3] + modl[13] * proj[ 7] + modl[14] * proj[11] + modl[15] * proj[15];

		// Now we actually want to get the sides of the frustum.  To do this we take
		// the clipping planes we received above and extract the sides from them.

		// This will extract the RIGHT side of the frustum
		m_Frustum[RIGHT][A] = clip[ 3] - clip[ 0];
		m_Frustum[RIGHT][B] = clip[ 7] - clip[ 4];
		m_Frustum[RIGHT][C] = clip[11] - clip[ 8];
		m_Frustum[RIGHT][D] = clip[15] - clip[12];

		// Now that we have a normal (A,B,C) and a distance (D) to the plane,
		// we want to normalize that normal and distance.

		// Normalize the RIGHT side
		NormalizePlane(m_Frustum, RIGHT);

		// This will extract the LEFT side of the frustum
		m_Frustum[LEFT][A] = clip[ 3] + clip[ 0];
		m_Frustum[LEFT][B] = clip[ 7] + clip[ 4];
		m_Frustum[LEFT][C] = clip[11] + clip[ 8];
		m_Frustum[LEFT][D] = clip[15] + clip[12];

		// Normalize the LEFT side
		NormalizePlane(m_Frustum, LEFT);

		// This will extract the BOTTOM side of the frustum
		m_Frustum[BOTTOM][A] = clip[ 3] + clip[ 1];
		m_Frustum[BOTTOM][B] = clip[ 7] + clip[ 5];
		m_Frustum[BOTTOM][C] = clip[11] + clip[ 9];
		m_Frustum[BOTTOM][D] = clip[15] + clip[13];

		// Normalize the BOTTOM side
		NormalizePlane(m_Frustum, BOTTOM);

		// This will extract the TOP side of the frustum
		m_Frustum[TOP][A] = clip[ 3] - clip[ 1];
		m_Frustum[TOP][B] = clip[ 7] - clip[ 5];
		m_Frustum[TOP][C] = clip[11] - clip[ 9];
		m_Frustum[TOP][D] = clip[15] - clip[13];

		// Normalize the TOP side
		NormalizePlane(m_Frustum, TOP);

		// This will extract the FRONT side of the frustum
		m_Frustum[FRONT][A] = clip[ 3] + clip[ 2];
		m_Frustum[FRONT][B] = clip[ 7] + clip[ 6];
		m_Frustum[FRONT][C] = clip[11] + clip[10];
		m_Frustum[FRONT][D] = clip[15] + clip[14];

		m_Frustum[BACK][A] = clip[ 3] - clip[ 2];
		m_Frustum[BACK][B] = clip[ 7] - clip[ 6];
		m_Frustum[BACK][C] = clip[11] - clip[10];
		m_Frustum[BACK][D] = clip[15] - clip[14];


		if ( sqrtf( m_Frustum[FRONT][A] * m_Frustum[FRONT][A] + m_Frustum[FRONT][B] * m_Frustum[FRONT][B] + m_Frustum[FRONT][C] * m_Frustum[FRONT][C] ) < FLT_EPSILON)
		{
			m_Frustum[FRONT][A] = -m_Frustum[BACK][A];
			m_Frustum[FRONT][B] = -m_Frustum[BACK][B];
			m_Frustum[FRONT][C] = -m_Frustum[BACK][C];
			m_Frustum[FRONT][D] = 1000000.f;
		}

		// Normalize the FRONT side
		NormalizePlane(m_Frustum, FRONT);

		// This will extract the BACK side of the frustum
		
		
		if ( sqrtf( m_Frustum[BACK][A] * m_Frustum[BACK][A] + m_Frustum[BACK][B] * m_Frustum[BACK][B] + m_Frustum[BACK][C] * m_Frustum[BACK][C] ) < FLT_EPSILON)
		{
			m_Frustum[BACK][A] = -m_Frustum[FRONT][A];
			m_Frustum[BACK][B] = -m_Frustum[FRONT][B];
			m_Frustum[BACK][C] = -m_Frustum[FRONT][C];
			m_Frustum[BACK][D] = 1000000.f;
		}
		// Normalize the BACK side
		NormalizePlane(m_Frustum, BACK);
	}


	//-------------------------------------------------------------------------
	// OBBInFrustum
	//-------------------------------------------------------------------------
	inline bool 
	Frustum::OBBInFrustum( const Matrix4 &mt, const Vector3 &pos, const Vector3& size) const
	{
		Frustum tmpfrus;
		for (int i=0;i<6;i++)
		{
			Vector4 r, v(m_Frustum[i][0], m_Frustum[i][1], m_Frustum[i][2], 0.f );
			r = transformVector(v, mt);
			tmpfrus.m_Frustum[i][0] = r.x;
			tmpfrus.m_Frustum[i][1] = r.y;
			tmpfrus.m_Frustum[i][2] = r.z;
			tmpfrus.m_Frustum[i][3] = m_Frustum[i][3];
		}

		Vector4 npos = transformPoint( Vector4(pos.x, pos.y, pos.z, 1.f), mt);
        bool bRet = tmpfrus.BoxInFrustum(Vector3(npos.getFloatValues()), size);
        return bRet;
	}

}
