/**
 * @file
 * @brief View-frustum data types and related operations.
 */

/*
 Copyright (C) 2001-2006, William Joseph.
 All Rights Reserved.

 This file is part of GtkRadiant.

 GtkRadiant is free software; you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation; either version 2 of the License, or
 (at your option) any later version.

 GtkRadiant is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.

 You should have received a copy of the GNU General Public License
 along with GtkRadiant; if not, write to the Free Software
 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 */

#if !defined(INCLUDED_MATH_FRUSTUM_H)
#define INCLUDED_MATH_FRUSTUM_H

#include "math/matrix.h"
#include "math/Plane3.h"
#include "math/aabb.h"
#include "math/line.h"
#include "VolumeIntersectionValue.h"

inline Matrix4 matrix4_frustum (float left, float right, float bottom, float top, float nearval, float farval)
{
	return Matrix4(static_cast<float> ((2 * nearval) / (right - left)), 0, 0, 0, 0, static_cast<float> ((2 * nearval)
			/ (top - bottom)), 0, 0, static_cast<float> ((right + left) / (right - left)), static_cast<float> ((top
			+ bottom) / (top - bottom)), static_cast<float> (-(farval + nearval) / (farval - nearval)), -1, 0, 0,
			static_cast<float> (-(2 * farval * nearval) / (farval - nearval)), 0);
}

typedef unsigned char ClipResult;
const ClipResult c_CLIP_PASS = 0x00; // 000000
const ClipResult c_CLIP_LT_X = 0x01; // 000001
const ClipResult c_CLIP_GT_X = 0x02; // 000010
const ClipResult c_CLIP_LT_Y = 0x04; // 000100
const ClipResult c_CLIP_GT_Y = 0x08; // 001000
const ClipResult c_CLIP_LT_Z = 0x10; // 010000
const ClipResult c_CLIP_GT_Z = 0x20; // 100000
const ClipResult c_CLIP_FAIL = 0x3F; // 111111

template<typename Index>
class Vector4ClipLT
{
	public:
		static bool compare (const Vector4& self)
		{
			return self[Index::VALUE] < self[3];
		}
		static double scale (const Vector4& self, const Vector4& other)
		{
			return (self[Index::VALUE] - self[3]) / (other[3] - other[Index::VALUE]);
		}
};

template<typename Index>
class Vector4ClipGT
{
	public:
		static bool compare (const Vector4& self)
		{
			return self[Index::VALUE] > -self[3];
		}
		static double scale (const Vector4& self, const Vector4& other)
		{
			return (self[Index::VALUE] + self[3]) / (-other[3] - other[Index::VALUE]);
		}
};

template<typename ClipPlane>
class Vector4ClipPolygon
{
	public:
		typedef Vector4* iterator;
		typedef const Vector4* const_iterator;

		static std::size_t apply (const_iterator first, const_iterator last, iterator out)
		{
			const_iterator next = first, i = last - 1;
			iterator tmp(out);
			bool b0 = ClipPlane::compare(*i);
			while (next != last) {
				bool b1 = ClipPlane::compare(*next);
				if (b0 ^ b1) {
					*out = *next - *i;

					double scale = ClipPlane::scale(*i, *out);

					(*out)[0] = static_cast<float> ((*i)[0] + scale * ((*out)[0]));
					(*out)[1] = static_cast<float> ((*i)[1] + scale * ((*out)[1]));
					(*out)[2] = static_cast<float> ((*i)[2] + scale * ((*out)[2]));
					(*out)[3] = static_cast<float> ((*i)[3] + scale * ((*out)[3]));

					++out;
				}

				if (b1) {
					*out = *next;
					++out;
				}

				i = next;
				++next;
				b0 = b1;
			}

			return out - tmp;
		}
};

#define CLIP_X_LT_W(p) (Vector4ClipLT< IntegralConstant<0> >::compare(p))
#define CLIP_X_GT_W(p) (Vector4ClipGT< IntegralConstant<0> >::compare(p))
#define CLIP_Y_LT_W(p) (Vector4ClipLT< IntegralConstant<1> >::compare(p))
#define CLIP_Y_GT_W(p) (Vector4ClipGT< IntegralConstant<1> >::compare(p))
#define CLIP_Z_LT_W(p) (Vector4ClipLT< IntegralConstant<2> >::compare(p))
#define CLIP_Z_GT_W(p) (Vector4ClipGT< IntegralConstant<2> >::compare(p))

inline ClipResult homogenous_clip_point (const Vector4& clipped)
{
	ClipResult result = c_CLIP_FAIL;
	if (CLIP_X_LT_W(clipped))
		result &= ~c_CLIP_LT_X; // X < W
	if (CLIP_X_GT_W(clipped))
		result &= ~c_CLIP_GT_X; // X > -W
	if (CLIP_Y_LT_W(clipped))
		result &= ~c_CLIP_LT_Y; // Y < W
	if (CLIP_Y_GT_W(clipped))
		result &= ~c_CLIP_GT_Y; // Y > -W
	if (CLIP_Z_LT_W(clipped))
		result &= ~c_CLIP_LT_Z; // Z < W
	if (CLIP_Z_GT_W(clipped))
		result &= ~c_CLIP_GT_Z; // Z > -W
	return result;
}

/// \brief Clips \p point by canonical matrix \p self.
/// Stores the result in \p clipped.
/// Returns a bitmask indicating which clip-planes the point was outside.
inline ClipResult matrix4_clip_point (const Matrix4& self, const Vector3& point, Vector4& clipped)
{
	clipped[0] = point[0];
	clipped[1] = point[1];
	clipped[2] = point[2];
	clipped[3] = 1;
	matrix4_transform_vector4(self, clipped);
	return homogenous_clip_point(clipped);
}

inline std::size_t homogenous_clip_triangle (Vector4 clipped[9])
{
	Vector4 buffer[9];
	std::size_t count = 3;
	count = Vector4ClipPolygon<Vector4ClipLT<IntegralConstant<0> > >::apply(clipped, clipped + count, buffer);
	count = Vector4ClipPolygon<Vector4ClipGT<IntegralConstant<0> > >::apply(buffer, buffer + count, clipped);
	count = Vector4ClipPolygon<Vector4ClipLT<IntegralConstant<1> > >::apply(clipped, clipped + count, buffer);
	count = Vector4ClipPolygon<Vector4ClipGT<IntegralConstant<1> > >::apply(buffer, buffer + count, clipped);
	count = Vector4ClipPolygon<Vector4ClipLT<IntegralConstant<2> > >::apply(clipped, clipped + count, buffer);
	return Vector4ClipPolygon<Vector4ClipGT<IntegralConstant<2> > >::apply(buffer, buffer + count, clipped);
}

/// \brief Transforms and clips the triangle formed by \p p0, \p p1, \p p2 by the canonical matrix \p self.
/// Stores the resulting polygon in \p clipped.
/// Returns the number of points in the resulting polygon.
inline std::size_t matrix4_clip_triangle (const Matrix4& self, const Vector3& p0, const Vector3& p1, const Vector3& p2,
		Vector4 clipped[9])
{
	clipped[0][0] = p0[0];
	clipped[0][1] = p0[1];
	clipped[0][2] = p0[2];
	clipped[0][3] = 1;
	clipped[1][0] = p1[0];
	clipped[1][1] = p1[1];
	clipped[1][2] = p1[2];
	clipped[1][3] = 1;
	clipped[2][0] = p2[0];
	clipped[2][1] = p2[1];
	clipped[2][2] = p2[2];
	clipped[2][3] = 1;

	matrix4_transform_vector4(self, clipped[0]);
	matrix4_transform_vector4(self, clipped[1]);
	matrix4_transform_vector4(self, clipped[2]);

	return homogenous_clip_triangle(clipped);
}

inline std::size_t homogenous_clip_line (Vector4 clipped[2])
{
	const Vector4& p0 = clipped[0];
	const Vector4& p1 = clipped[1];

	// early out
	{
		ClipResult mask0 = homogenous_clip_point(clipped[0]);
		ClipResult mask1 = homogenous_clip_point(clipped[1]);

		if ((mask0 | mask1) == c_CLIP_PASS) // both points passed all planes
			return 2;

		if (mask0 & mask1) // both points failed any one plane
			return 0;
	}

	{
		const bool index = CLIP_X_LT_W(p0);
		if (index ^ CLIP_X_LT_W(p1)) {
			Vector4 clip(p1 - p0);

			double scale = (p0[0] - p0[3]) / (clip[3] - clip[0]);

			clip[0] = static_cast<float> (p0[0] + scale * clip[0]);
			clip[1] = static_cast<float> (p0[1] + scale * clip[1]);
			clip[2] = static_cast<float> (p0[2] + scale * clip[2]);
			clip[3] = static_cast<float> (p0[3] + scale * clip[3]);

			clipped[index] = clip;
		} else if (index == 0)
			return 0;
	}

	{
		const bool index = CLIP_X_GT_W(p0);
		if (index ^ CLIP_X_GT_W(p1)) {
			Vector4 clip(p1 - p0);

			double scale = (p0[0] + p0[3]) / (-clip[3] - clip[0]);

			clip[0] = static_cast<float> (p0[0] + scale * clip[0]);
			clip[1] = static_cast<float> (p0[1] + scale * clip[1]);
			clip[2] = static_cast<float> (p0[2] + scale * clip[2]);
			clip[3] = static_cast<float> (p0[3] + scale * clip[3]);

			clipped[index] = clip;
		} else if (index == 0)
			return 0;
	}

	{
		const bool index = CLIP_Y_LT_W(p0);
		if (index ^ CLIP_Y_LT_W(p1)) {
			Vector4 clip(p1 - p0);

			double scale = (p0[1] - p0[3]) / (clip[3] - clip[1]);

			clip[0] = static_cast<float> (p0[0] + scale * clip[0]);
			clip[1] = static_cast<float> (p0[1] + scale * clip[1]);
			clip[2] = static_cast<float> (p0[2] + scale * clip[2]);
			clip[3] = static_cast<float> (p0[3] + scale * clip[3]);

			clipped[index] = clip;
		} else if (index == 0)
			return 0;
	}

	{
		const bool index = CLIP_Y_GT_W(p0);
		if (index ^ CLIP_Y_GT_W(p1)) {
			Vector4 clip(p1 - p0);

			double scale = (p0[1] + p0[3]) / (-clip[3] - clip[1]);

			clip[0] = static_cast<float> (p0[0] + scale * clip[0]);
			clip[1] = static_cast<float> (p0[1] + scale * clip[1]);
			clip[2] = static_cast<float> (p0[2] + scale * clip[2]);
			clip[3] = static_cast<float> (p0[3] + scale * clip[3]);

			clipped[index] = clip;
		} else if (index == 0)
			return 0;
	}

	{
		const bool index = CLIP_Z_LT_W(p0);
		if (index ^ CLIP_Z_LT_W(p1)) {
			Vector4 clip(p1 - p0);

			double scale = (p0[2] - p0[3]) / (clip[3] - clip[2]);

			clip[0] = static_cast<float> (p0[0] + scale * clip[0]);
			clip[1] = static_cast<float> (p0[1] + scale * clip[1]);
			clip[2] = static_cast<float> (p0[2] + scale * clip[2]);
			clip[3] = static_cast<float> (p0[3] + scale * clip[3]);

			clipped[index] = clip;
		} else if (index == 0)
			return 0;
	}

	{
		const bool index = CLIP_Z_GT_W(p0);
		if (index ^ CLIP_Z_GT_W(p1)) {
			Vector4 clip(p1 - p0);

			double scale = (p0[2] + p0[3]) / (-clip[3] - clip[2]);

			clip[0] = static_cast<float> (p0[0] + scale * clip[0]);
			clip[1] = static_cast<float> (p0[1] + scale * clip[1]);
			clip[2] = static_cast<float> (p0[2] + scale * clip[2]);
			clip[3] = static_cast<float> (p0[3] + scale * clip[3]);

			clipped[index] = clip;
		} else if (index == 0)
			return 0;
	}

	return 2;
}

/// \brief Transforms and clips the line formed by \p p0, \p p1 by the canonical matrix \p self.
/// Stores the resulting line in \p clipped.
/// Returns the number of points in the resulting line.
inline std::size_t matrix4_clip_line (const Matrix4& self, const Vector3& p0, const Vector3& p1, Vector4 clipped[2])
{
	clipped[0][0] = p0[0];
	clipped[0][1] = p0[1];
	clipped[0][2] = p0[2];
	clipped[0][3] = 1;
	clipped[1][0] = p1[0];
	clipped[1][1] = p1[1];
	clipped[1][2] = p1[2];
	clipped[1][3] = 1;

	matrix4_transform_vector4(self, clipped[0]);
	matrix4_transform_vector4(self, clipped[1]);

	return homogenous_clip_line(clipped);
}

struct Frustum
{
		Plane3 right, left, bottom, top, back, front;

		Frustum ()
		{
		}
		Frustum (const Plane3& _right, const Plane3& _left, const Plane3& _bottom, const Plane3& _top,
				const Plane3& _back, const Plane3& _front) :
			right(_right), left(_left), bottom(_bottom), top(_top), back(_back), front(_front)
		{
		}
};

inline bool viewproj_test_point (const Matrix4& viewproj, const Vector3& point)
{
	Vector4 hpoint(matrix4_transformed_vector4(viewproj, Vector4(point, 1.0f)));
	if (fabs(hpoint[0]) < fabs(hpoint[3]) && fabs(hpoint[1]) < fabs(hpoint[3]) && fabs(hpoint[2]) < fabs(hpoint[3]))
		return true;
	return false;
}

inline bool viewproj_test_transformed_point (const Matrix4& viewproj, const Vector3& point, const Matrix4& localToWorld)
{
	return viewproj_test_point(viewproj, matrix4_transformed_point(localToWorld, point));
}

inline Frustum frustum_from_viewproj (const Matrix4& viewproj)
{
	  return Frustum(Plane3(viewproj[3] - viewproj[0], viewproj[7] - viewproj[4], viewproj[11]
			- viewproj[8], viewproj[15] - viewproj[12]).getNormalised(), Plane3(viewproj[3]
			+ viewproj[0], viewproj[7] + viewproj[4], viewproj[11] + viewproj[8], viewproj[15]
			+ viewproj[12]).getNormalised(),
			Plane3(viewproj[3] + viewproj[1], viewproj[7] + viewproj[5],
					viewproj[11] + viewproj[9], viewproj[15] + viewproj[13]).getNormalised(),
			Plane3(viewproj[3] - viewproj[1], viewproj[7] - viewproj[5],
					viewproj[11] - viewproj[9], viewproj[15] - viewproj[13]).getNormalised(),
			Plane3(viewproj[3] - viewproj[2], viewproj[7] - viewproj[6], viewproj[11]
					- viewproj[10], viewproj[15] - viewproj[14]).getNormalised(), Plane3(
					viewproj[3] + viewproj[2], viewproj[7] + viewproj[6], viewproj[11]
							+ viewproj[10], viewproj[15] + viewproj[14]).getNormalised());
}

inline VolumeIntersectionValue frustum_test_aabb (const Frustum& frustum, const AABB& aabb)
{
	VolumeIntersectionValue result = VOLUME_INSIDE;

	switch (aabb_classify_plane(aabb, frustum.right)) {
	case 2:
		return VOLUME_OUTSIDE;
	case 1:
		result = VOLUME_PARTIAL;
	}

	switch (aabb_classify_plane(aabb, frustum.left)) {
	case 2:
		return VOLUME_OUTSIDE;
	case 1:
		result = VOLUME_PARTIAL;
	}

	switch (aabb_classify_plane(aabb, frustum.bottom)) {
	case 2:
		return VOLUME_OUTSIDE;
	case 1:
		result = VOLUME_PARTIAL;
	}

	switch (aabb_classify_plane(aabb, frustum.top)) {
	case 2:
		return VOLUME_OUTSIDE;
	case 1:
		result = VOLUME_PARTIAL;
	}

	switch (aabb_classify_plane(aabb, frustum.back)) {
	case 2:
		return VOLUME_OUTSIDE;
	case 1:
		result = VOLUME_PARTIAL;
	}

	switch (aabb_classify_plane(aabb, frustum.front)) {
	case 2:
		return VOLUME_OUTSIDE;
	case 1:
		result = VOLUME_PARTIAL;
	}

	return result;
}

inline double plane_distance_to_point (const Plane3& plane, const Vector3& point)
{
	return plane.normal().dot(point) + plane.dist();
}

inline double plane_distance_to_oriented_extents (const Plane3& plane, const Vector3& extents,
		const Matrix4& orientation)
{
	return fabs(extents[0] * plane.normal().dot(orientation.x().getVector3())) + fabs(extents[1] * plane.normal().dot(
			orientation.y().getVector3())) + fabs(extents[2] * plane.normal().dot(orientation.z().getVector3()));
}

/// \brief Return false if \p aabb with \p orientation is partially or completely outside \p plane.
inline bool plane_contains_oriented_aabb (const Plane3& plane, const AABB& aabb, const Matrix4& orientation)
{
	double dot = plane_distance_to_point(plane, aabb.origin);
	return !(dot > 0 || -dot < plane_distance_to_oriented_extents(plane, aabb.extents, orientation));
}

inline VolumeIntersectionValue frustum_intersects_transformed_aabb (const Frustum& frustum, const AABB& aabb,
		const Matrix4& localToWorld)
{
	AABB aabb_world(aabb);
	matrix4_transform_point(localToWorld, aabb_world.origin);

	if (plane_contains_oriented_aabb(frustum.right, aabb_world, localToWorld) || plane_contains_oriented_aabb(
			frustum.left, aabb_world, localToWorld) || plane_contains_oriented_aabb(frustum.bottom, aabb_world,
			localToWorld) || plane_contains_oriented_aabb(frustum.top, aabb_world, localToWorld)
			|| plane_contains_oriented_aabb(frustum.back, aabb_world, localToWorld) || plane_contains_oriented_aabb(
			frustum.front, aabb_world, localToWorld))
		return VOLUME_OUTSIDE;
	return VOLUME_INSIDE;
}

inline bool plane3_test_point (const Plane3& plane, const Vector3& point)
{
	return point.dot(plane.normal()) + plane.dist() <= 0;
}

inline bool plane3_test_line (const Plane3& plane, const Segment& segment)
{
	return segment_classify_plane(segment, plane) == 2;
}

inline bool frustum_test_point (const Frustum& frustum, const Vector3& point)
{
	return !plane3_test_point(frustum.right, point) && !plane3_test_point(frustum.left, point) && !plane3_test_point(
			frustum.bottom, point) && !plane3_test_point(frustum.top, point) && !plane3_test_point(frustum.back, point)
			&& !plane3_test_point(frustum.front, point);
}

inline bool frustum_test_line (const Frustum& frustum, const Segment& segment)
{
	return !plane3_test_line(frustum.right, segment) && !plane3_test_line(frustum.left, segment) && !plane3_test_line(
			frustum.bottom, segment) && !plane3_test_line(frustum.top, segment) && !plane3_test_line(frustum.back,
			segment) && !plane3_test_line(frustum.front, segment);
}

inline bool viewer_test_plane (const Vector4& viewer, const Plane3& plane)
{
	return ((plane.normal().x() * viewer[0]) + (plane.normal().y() * viewer[1]) + (plane.normal().z() * viewer[2]) + (plane.dist() * viewer[3])) > 0;
}

inline Vector3 triangle_cross (const Vector3& p0, const Vector3& p1, const Vector3& p2)
{
	return (p1 - p0).crossProduct(p1 - p2);
}

inline bool viewer_test_triangle (const Vector4& viewer, const Vector3& p0, const Vector3& p1, const Vector3& p2)
{
	Vector3 cross(triangle_cross(p0, p1, p2));
	return ((viewer[0] * cross[0]) + (viewer[1] * cross[1]) + (viewer[2] * cross[2]) + (viewer[3] * 0)) > 0;
}

inline Vector4 viewer_from_transformed_viewer (const Vector4& viewer, const Matrix4& transform)
{
	if (viewer[3] == 0) {
		return Vector4(matrix4_transformed_direction(transform, viewer.getVector3()), 0);
	} else {
		return Vector4(matrix4_transformed_point(transform, viewer.getVector3()), viewer[3]);
	}
}

inline bool viewer_test_transformed_plane (const Vector4& viewer, const Plane3& plane, const Matrix4& localToWorld)
{
#if 0
	return viewer_test_plane(viewer_from_transformed_viewer(viewer, matrix4_affine_inverse(localToWorld)), plane);
#else
	return viewer_test_plane(viewer, localToWorld.transform(plane));
#endif
}

inline Vector4 viewer_from_viewproj (const Matrix4& viewproj)
{
	// get viewer pos in object coords
	Vector4 viewer(matrix4_transformed_vector4(matrix4_full_inverse(viewproj), Vector4(0, 0, -1, 0)));
	if (viewer[3] != 0) // non-affine matrix
	{
		viewer[0] /= viewer[3];
		viewer[1] /= viewer[3];
		viewer[2] /= viewer[3];
		viewer[3] /= viewer[3];
	}
	return viewer;
}

#endif
