#include "BinTreesCulling.h"

namespace bTree
{
	/////////////////// Frustum intersection
	
	void CullingPlanes::FromMatrixVP_T(const Matrix &viewproj_t, const VRUT::AABB &clip_volume)
	{
		// TODO Although this could be optimized (there are a lot of ones and zeros and also the transposition could be replaced
		//		by reordering variables in TransformCoord), this is not called that often and this is more readable and safer.
		//		The result would be similar to FromMatrixVinv_P_centeredCam.
		AABBToClipPlanes(planes, clip_volume);
		viewproj_t.TransformCoordArray<Vector4>(planes.begin(), planes.end(), planes.begin());
	}

	void CullingPlanes::FromMatrixVPinv(const Matrix &viewproj_inv)
	{
		Vector3 TopLeftBack			= viewproj_inv.TransformCoordHom(Vector3(-1, 1,-1));
		Vector3 BottomLeftBack		= viewproj_inv.TransformCoordHom(Vector3(-1,-1,-1));
		Vector3 BottomRightBack		= viewproj_inv.TransformCoordHom(Vector3( 1,-1,-1));
		Vector3 TopRightForward		= viewproj_inv.TransformCoordHom(Vector3( 1, 1, 1));
		Vector3 TopLeftForward		= viewproj_inv.TransformCoordHom(Vector3(-1, 1, 1));
		Vector3 BottomRightForward	= viewproj_inv.TransformCoordHom(Vector3( 1,-1, 1));

		//These points are some of the corners of the cube, and by using the cross product, we can reconstruct all the planes from just the six of them
		planes.resize(6);
		planes[FRUSTUM_NEAR]	= Vector4::PlaneFromPoints(TopLeftBack,BottomRightBack, BottomLeftBack);
		planes[FRUSTUM_FAR]		= Vector4::PlaneFromPoints(BottomRightForward,TopRightForward,TopLeftForward);
		planes[FRUSTUM_LEFT]	= Vector4::PlaneFromPoints(TopLeftBack,BottomLeftBack,TopLeftForward);
		planes[FRUSTUM_RIGHT]	= Vector4::PlaneFromPoints(BottomRightBack,TopRightForward,BottomRightForward);
		planes[FRUSTUM_TOP]		= Vector4::PlaneFromPoints(TopLeftBack,TopLeftForward,TopRightForward);
		planes[FRUSTUM_BOTTOM]	= Vector4::PlaneFromPoints(BottomLeftBack,BottomRightBack,BottomRightForward);
	}

	CullingPlanes::VFC_RESULT CullingPlanes::InFrustum(const CullingPlanes &planes, const VRUT::AABB &aabb, unsigned char *last_plane, unsigned char *plane_mask)
	{
		size_t planesCount=planes.planes.size();
		if (planesCount==0)
			return OUTSIDE;
		VFC_RESULT ret = INSIDE;

		unsigned char plane = (last_plane) ? (*last_plane % planesCount) : 0;
		unsigned char mask = (plane_mask) ? (*plane_mask) : 0;
		for (size_t i = 0; i < planesCount; plane = (plane + 1) % planesCount, ++i)
		{
			// Does this plane really need to be tested?
			if (mask & (1 << plane))
				continue;

			float nv, pv;
			aabb.ComputeNPDistances(planes.planes[plane], &nv, &pv);

			if (pv < 0.0f)
			{
				if (last_plane)
					*last_plane = plane;
				return OUTSIDE;
			}

			if (nv < 0.0f)
				ret = INTERSECT;		// We intersect a plane, let's try whether there is another plane which culls the object
			else
				mask |= (1 << plane);	// We are inside the frustum, this plane needs not to be tested in children of aabb
		}

		if (plane_mask)
			*plane_mask = mask;
		return ret;
	}

	CullingPlanes::VFC_RESULT CullingPlanes::InFrustumSimpler(const CullingPlanes &planes, const VRUT::AABB &aabb)
	{
		VFC_RESULT ret = INSIDE;
		for (size_t i = 0; i < planes.planes.size(); ++i)
		{
			float nv, pv;
			aabb.ComputeNPDistances(planes.planes[i], &nv, &pv);

			if (pv < 0.0f)	return OUTSIDE;		// Fully outside
			if (nv < 0.0f)	ret = INTERSECT;	// Check another plane
		}
		return ret;
	}

	bool CullingPlanes::InFrustumSimplest(const CullingPlanes &planes, const VRUT::AABB &aabb)
	{
		for (size_t i = 0; i < planes.planes.size(); ++i)
		{
			float pv;
			aabb.ComputeNPDistances(planes.planes[i], nullptr, &pv);
			if (pv < 0.0f)	return false;
		}
		return true;
	}

	bool CullingPlanes::InFrustumSimplest(const CullingPlanes &planes, const Matrix &plane_transf, const VRUT::AABB &aabb)
	{
		for (size_t i = 0; i < planes.planes.size(); ++i)
		{
			float pv;
			aabb.ComputeNPDistances(plane_transf.TransformCoord(planes.planes[i]), nullptr, &pv);
			if (pv < 0.0f)	return false;
		}
		return true;
	}

	void CullingPlanes::AABBToClipPlanes(std::vector<Vector4> &clip_planes, const VRUT::AABB &clip_volume)
	{
		clip_planes.resize(6);
		clip_planes[CullingPlanes::FRUSTUM_NEAR]	= Vector4( 0.0f, 0.0f,+1.0f,-clip_volume.MinBound.z);
		clip_planes[CullingPlanes::FRUSTUM_FAR]		= Vector4( 0.0f, 0.0f,-1.0f,+clip_volume.MaxBound.z);
		clip_planes[CullingPlanes::FRUSTUM_LEFT]	= Vector4(+1.0f, 0.0f, 0.0f,-clip_volume.MinBound.x);
		clip_planes[CullingPlanes::FRUSTUM_RIGHT]	= Vector4(-1.0f, 0.0f, 0.0f,+clip_volume.MaxBound.x);
		clip_planes[CullingPlanes::FRUSTUM_TOP]		= Vector4( 0.0f,-1.0f, 0.0f,+clip_volume.MaxBound.y);
		clip_planes[CullingPlanes::FRUSTUM_BOTTOM]	= Vector4( 0.0f,+1.0f, 0.0f,-clip_volume.MinBound.y);
	}

	std::vector<Vector4> CullingPlanes::AABBToClipPlanes(const VRUT::AABB &clip_volume)
	{
		std::vector<Vector4> result;
		AABBToClipPlanes(result, clip_volume);
		return result;
	}

}
