/* -----------------------------------------------------------------------------
	GSFramework
	Copyright 2001-2013 Emmanuel Julien. All Rights Reserved.
----------------------------------------------------------------------------- */


		#include <cmath>
		#include "geometry/frustum.h"
		#include "geometry/bounding_box.h"
		#include "geometry/sat.h"
		#include "shape/shape.h"
		#include "math/matrix4.h"

		using namespace GS;


//------------------------------------------------------------------------------
void				Frustum::SetPerspective(float fov, float znear, float zfar, const Matrix4 *matrix, float h_ar, float v_ar)
{
	fov *= 0.5f;
	const float	hyp = tan(fov);
	const float	hfov = atan(hyp / h_ar), vfov = atan(hyp / v_ar);

	Vector4 n;

	const float	sinv = sin(vfov);
	const float	cosv = cos(vfov);
	n.Set(0, cosv, -sinv);
	plane[Top].Set(NULL, n, matrix);
	n.Set(0, -cosv, -sinv);
	plane[Bottom].Set(NULL, n, matrix);

	const float	sinh = sin(hfov);
	const float	cosh = cos(hfov);
	n.Set(-cosh, 0, -sinh);
	plane[Left].Set(NULL, n, matrix);
	n.Set(cosh, 0, -sinh);
	plane[Right].Set(NULL, n, matrix);

	Vector4 s;
	s.Set(0, 0, znear);
	n.Set(0, 0, -1);
	plane[Near].Set(&s, n, matrix);
	s.Set(0, 0, zfar);
	n.Set(0, 0, 1);
	plane[Far].Set(&s, n, matrix);

	// Model vertices.
	Vector4 bvtx[8];
	Vector4 *_vtx = matrix ? bvtx : vtx;

	// Compute near plane corners.
	float k = znear / -cosv;
	_vtx[0].y = -sinv * k;
	_vtx[0].z = znear;//-cosv * k;
	k = znear / cosh;
	_vtx[0].x = -sinh * k;
	_vtx[0].w = 1;

	_vtx[1].Set(-_vtx[0].x, _vtx[0].y, _vtx[0].z);
	_vtx[2].Set(-_vtx[0].x, -_vtx[0].y, _vtx[0].z);
	_vtx[3].Set(_vtx[0].x, -_vtx[0].y, _vtx[0].z);

	// Compute far plane corners.
	k = zfar / -cosv;
	_vtx[4].y = -sinv * k;
	_vtx[4].z = zfar;//-cosv * k;
	k = zfar / cosh;
	_vtx[4].x = -sinh * k;
	_vtx[4].w = 1;

	_vtx[5].Set(-_vtx[4].x, _vtx[4].y, _vtx[4].z);
	_vtx[6].Set(-_vtx[4].x, -_vtx[4].y, _vtx[4].z);
	_vtx[7].Set(_vtx[4].x, -_vtx[4].y, _vtx[4].z);

	if	(matrix)
		matrix->Apply(vtx, bvtx, 8);
}
void				Frustum::SetOrthographic(float width, float height, float znear, float zfar, const Matrix4 *matrix, float h_ar, float v_ar)
{
	Vector4 s, n;

	width *= h_ar;
	height *= v_ar;

	s.Set(0, height * 0.5f, 0);
	n.Set(0, 1, 0);
	plane[Top].Set(&s, n, matrix);
	s.Set(0, -height * 0.5f, 0);
	n.Set(0, -1, 0);
	plane[Bottom].Set(&s, n, matrix);
	s.Set(-width * 0.5f, 0, 0);
	n.Set(-1, 0, 0);
	plane[Left].Set(&s, n, matrix);
	s.Set(width * 0.5f, 0, 0);
	n.Set(1, 0, 0);
	plane[Right].Set(&s, n, matrix);
	s.Set(0, 0, znear);
	n.Set(0, 0, -1);
	plane[Near].Set(&s, n, matrix);
	s.Set(0, 0, zfar);
	n.Set(0, 0, 1);
	plane[Far].Set(&s, n, matrix);

	// Model vertices.
	Vector4 bvtx[8];
	Vector4 *_vtx = matrix ? bvtx : vtx;

	// Compute near plane corners.
	_vtx[0].Set(-width * 0.5f,  height * 0.5f, znear);
	_vtx[1].Set( width * 0.5f,  height * 0.5f, znear);
	_vtx[2].Set( width * 0.5f, -height * 0.5f, znear);
	_vtx[3].Set(-width * 0.5f, -height * 0.5f, znear);
	_vtx[4].Set(-width * 0.5f,  height * 0.5f, zfar);
	_vtx[5].Set( width * 0.5f,  height * 0.5f, zfar);
	_vtx[6].Set( width * 0.5f, -height * 0.5f, zfar);
	_vtx[7].Set(-width * 0.5f, -height * 0.5f, zfar);

	if	(matrix)
		matrix->Apply(vtx, bvtx, 8);
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
Frustum::Visibility	Frustum::ClassifyShape(const Shape &s, const Matrix4 *m) const
{
	Visibility v = Inside;

	Matrix3 rm;
	if	(m)
		rm = Matrix3::FromMatrix4(*m).Transposed();

	for	(uint n = 0; n < 6; ++n)
	{
		float d, r;

		if	(m)
		{
			d = plane[n].DistanceToPlane(s.GetCenter() * m[0]);
			r = s.GetSupportDistance(plane[n].GetNormal() * rm);
		}
		else
		{
			d = plane[n].DistanceToPlane(s.GetCenter());
			r = s.GetSupportDistance(plane[n].GetNormal());
		}

		if	(d > r)
			return Outside;
		if	(d > -r)
			v = Clipped;
	}
	return v;
}
Frustum::Visibility	Frustum::ClassifySphere(const Vector4 &p, float r) const
{
	Visibility v = Inside;
	for	(uint n = 0; n < 6; ++n)
	{
		if	(plane[n].DistanceToPlane(p) > r)
			return Outside;
		if	(plane[n].DistanceToPlane(p) > -r)
			v = Clipped;
	}
	return v;
}
Frustum::Visibility	Frustum::ClassifySet(uint count, const Vector4 * const GSRESTRICT set, const float offset) const
{
	Visibility v = Inside;
	for	(uint n = 0; n < 6; ++n)
	{
		uint out = 0;
		for	(uint i = 0; i < count; ++i)
			if	(plane[n].DistanceToPlane(set[i]) > offset)
				++out;

		if	(out == count)
			return Outside;
		if	(out > 0)
			v = Clipped;
	}
	return v;
}
//------------------------------------------------------------------------------

#if		(__PLATFORM_NINTENDO_WII__ == 0)
//#define		FRUSTUM_TEST_USE_SAT
#endif

//--------------------------------------------
#define		SAT_TEST(_N_, _U_, _A_, _V_, _B_)\
{\
	SAT::Overlap _v = SAT::TestOverlap(_N_, _U_, _A_, _V_, _B_);\
	if	(_v == SAT::Outside)\
		return Outside;\
	if	(_v == SAT::Clipped)\
		v = Clipped;\
}
//--------------------------------------------

//------------------------------------------------------------------------------
Frustum::Visibility	Frustum::ClassifyMinMax(const MinMax &mm, const Matrix4 *matrix) const
{
	// TODO Please, use AABB half width and implicit interval projection... will you?
	Vector4 s[8], d[8], *p;

	s[0].Set(mm.mn.x, mm.mn.y, mm.mn.z);
	s[1].Set(mm.mx.x, mm.mn.y, mm.mn.z);
	s[2].Set(mm.mx.x, mm.mx.y, mm.mn.z);
	s[3].Set(mm.mn.x, mm.mx.y, mm.mn.z);
	s[4].Set(mm.mn.x, mm.mn.y, mm.mx.z);
	s[5].Set(mm.mx.x, mm.mn.y, mm.mx.z);
	s[6].Set(mm.mx.x, mm.mx.y, mm.mx.z);
	s[7].Set(mm.mn.x, mm.mx.y, mm.mx.z);

	if	(matrix)
	{
		matrix->Apply(d, s, 8);
		p = d;
	}
	else
		p = s;

#ifndef	FRUSTUM_TEST_USE_SAT
	// Faster but much coarser test.
	return ClassifySet(8, p);
#else
	// Frustum/AABB SAT.
	Visibility v = Inside;

	// Test face/{face/edge} contact.
	SAT_TEST(plane[Top].GetNormal(), 8, vtx, 8, p);
	SAT_TEST(plane[Bottom].GetNormal(), 8, vtx, 8, p);
	SAT_TEST(plane[Left].GetNormal(), 8, vtx, 8, p);
	SAT_TEST(plane[Right].GetNormal(), 8, vtx, 8, p);
	SAT_TEST(plane[Near].GetNormal(), 8, vtx, 8, p);
	SAT_TEST(plane[Far].GetNormal(), 8, vtx, 8, p);

	Vector4 _edge[3];

	_edge[0] = matrix ? matrix->GetRow(0) : Vector4(1, 0, 0);
	SAT_TEST(_edge[0], 8, vtx, 8, p);
	_edge[1] = matrix ? matrix->GetRow(1) : Vector4(0, 1, 0);
	SAT_TEST(_edge[1], 8, vtx, 8, p);
	_edge[2] = matrix ? matrix->GetRow(2) : Vector4(0, 0, 1);
	SAT_TEST(_edge[2], 8, vtx, 8, p);

	// Test edge/edge contact.
	Vector4 edge[6];
	for	(uint n = 0; n < 4; ++n)
		edge[n] = vtx[n + 4] - vtx[n];
	edge[4] = vtx[1] - vtx[0];
	edge[5] = vtx[3] - vtx[0];

	for (uint n = 0; n < 6; ++n)
		for	(uint m = 0; m < 3; ++m)
		{
			Vector4 axis = edge[n].Cross(_edge[m]);
			if	(Math::EqualZero(axis.Len2()))
				continue;
			SAT_TEST(axis, 8, vtx, 8, p);
		}
	return v;
#endif
}
Frustum::Visibility	Frustum::ClassifyFrustrum(const Frustum &frustum) const
{
#ifndef	FRUSTUM_TEST_USE_SAT
	// Faster but much coarser test.
	return ClassifySet(8, frustum.vtx);
#else
	// Frustum/frustum SAT.
	Visibility v = Inside;

	// Test face/{face/edge} contact.
	SAT_TEST(plane[Top].GetNormal(), 8, vtx, 8, frustum.vtx);
	SAT_TEST(plane[Bottom].GetNormal(), 8, vtx, 8, frustum.vtx);
	SAT_TEST(plane[Left].GetNormal(), 8, vtx, 8, frustum.vtx);
	SAT_TEST(plane[Right].GetNormal(), 8, vtx, 8, frustum.vtx);
	SAT_TEST(plane[Far].GetNormal(), 8, vtx, 8, frustum.vtx);
	SAT_TEST(frustum.plane[Top].GetNormal(), 8, vtx, 8, frustum.vtx);
	SAT_TEST(frustum.plane[Bottom].GetNormal(), 8, vtx, 8, frustum.vtx);
	SAT_TEST(frustum.plane[Left].GetNormal(), 8, vtx, 8, frustum.vtx);
	SAT_TEST(frustum.plane[Right].GetNormal(), 8, vtx, 8, frustum.vtx);
	SAT_TEST(frustum.plane[Far].GetNormal(), 8, vtx, 8, frustum.vtx);

	// Test edge/edge contact.
	Vector4 edge[6], _edge[6];
	for	(uint n = 0; n < 4; ++n)
	{
		edge[n] = vtx[n + 4] - vtx[n];
		_edge[n] = frustum.vtx[n + 4] - frustum.vtx[n];
	}
	edge[4] = vtx[1] - vtx[0];
	edge[5] = vtx[3] - vtx[0];
	_edge[4] = frustum.vtx[1] - frustum.vtx[0];
	_edge[5] = frustum.vtx[3] - frustum.vtx[0];

	for (uint n = 0; n < 6; ++n)
		for	(uint m = 0; m < 6; ++m)
		{
			Vector4 axis = edge[n].Cross(_edge[m]);
			if	(!Math::EqualZero(axis.Len2()))
				SAT_TEST(axis, 8, vtx, 8, frustum.vtx);
		}
	return v;
#endif
}
//------------------------------------------------------------------------------