#include "BstmCamera.h"

namespace Brainstem
{
	Camera::Camera(IKernel* kernel, IDirect3DDevice9* device, float fov, float znear, float zfar) 
	{ 
		Kernel = kernel; 
		Device = device;

		FOV = fov;
		CullingFOV = fov;
		zNear = znear;
		zFar = zfar;
	}
	Camera::~Camera() { }

	void Camera::Init() { }
	void Camera::Update() { }

	void Camera::UpdateFrustumPlanes()
	{
		Matrix4<float> PerspFOV;
		Matrix4<float> proj = PerspFOV.CreateProjectionMatrixPerspectiveFovLH(CullingFOV, AspectRatio, zNear, zFar);

		Matrix4<float> vpMatrix; 
		vpMatrix = proj * View;

		FrustumPlanes[0].A = vpMatrix.GetAt(0, 3) + vpMatrix.GetAt(0, 0);
		FrustumPlanes[0].B = vpMatrix.GetAt(1, 3) + vpMatrix.GetAt(1, 0);
		FrustumPlanes[0].C = vpMatrix.GetAt(2, 3) + vpMatrix.GetAt(2, 0);
		FrustumPlanes[0].D = vpMatrix.GetAt(3, 3) + vpMatrix.GetAt(3, 0);

		FrustumPlanes[1].A = vpMatrix.GetAt(0, 3) - vpMatrix.GetAt(0, 0);
		FrustumPlanes[1].B = vpMatrix.GetAt(1, 3) - vpMatrix.GetAt(1, 0);
		FrustumPlanes[1].C = vpMatrix.GetAt(2, 3) - vpMatrix.GetAt(2, 0);
		FrustumPlanes[1].D = vpMatrix.GetAt(3, 3) - vpMatrix.GetAt(3, 0);

		FrustumPlanes[2].A = vpMatrix.GetAt(0, 3) - vpMatrix.GetAt(0, 1);
		FrustumPlanes[2].B = vpMatrix.GetAt(1, 3) - vpMatrix.GetAt(1, 1);
		FrustumPlanes[2].C = vpMatrix.GetAt(2, 3) - vpMatrix.GetAt(2, 1);
		FrustumPlanes[2].D = vpMatrix.GetAt(3, 3) - vpMatrix.GetAt(3, 1);

		FrustumPlanes[3].A = vpMatrix.GetAt(0, 3) + vpMatrix.GetAt(0, 1);
		FrustumPlanes[3].B = vpMatrix.GetAt(1, 3) + vpMatrix.GetAt(1, 1);
		FrustumPlanes[3].C = vpMatrix.GetAt(2, 3) + vpMatrix.GetAt(2, 1);
		FrustumPlanes[3].D = vpMatrix.GetAt(3, 3) + vpMatrix.GetAt(3, 1);

		FrustumPlanes[4].A = vpMatrix.GetAt(0, 3) + vpMatrix.GetAt(0, 2);
		FrustumPlanes[4].B = vpMatrix.GetAt(1, 3) + vpMatrix.GetAt(1, 2);
		FrustumPlanes[4].C = vpMatrix.GetAt(2, 3) + vpMatrix.GetAt(2, 2);
		FrustumPlanes[4].D = vpMatrix.GetAt(3, 3) + vpMatrix.GetAt(3, 2);

		FrustumPlanes[5].A = vpMatrix.GetAt(0, 3) - vpMatrix.GetAt(0, 2);
		FrustumPlanes[5].B = vpMatrix.GetAt(1, 3) - vpMatrix.GetAt(1, 2);
		FrustumPlanes[5].C = vpMatrix.GetAt(2, 3) - vpMatrix.GetAt(2, 2);
		FrustumPlanes[5].D = vpMatrix.GetAt(3, 3) - vpMatrix.GetAt(3, 2);

		for(int i = 0;i < 6; i++)
			FrustumPlanes[i].Normalize();
	}

	void Camera::UpdateFrustumVertices()
	{
		Vector3<float> vZ = Target - Position;
		vZ.Normalize();

		Vector3<float> Up = Vector3<float>(0, 1, 0);

		Vector3<float> vX = Up.CrossProduct(vZ);
		vX.Normalize();

		Vector3<float> vY = vZ.CrossProduct(vX);
	
		float fNearPlaneHeight = tanf(FOV * 0.5f) * zNear;
		float fNearPlaneWidth = fNearPlaneHeight * AspectRatio;

		float fFarPlaneHeight = tanf(FOV * 0.5f) * zFar;
		float fFarPlaneWidth = fFarPlaneHeight * AspectRatio;

		Vector3<float> vNearPlaneCenter = Position + vZ * zNear;
		Vector3<float> vFarPlaneCenter = Position + vZ * zFar;

		// Near
		FrustumVertices[0] = Vector3<float>(vNearPlaneCenter - vX * fNearPlaneWidth - vY * fNearPlaneHeight); // LR
		FrustumVertices[1] = Vector3<float>(vNearPlaneCenter - vX * fNearPlaneWidth + vY * fNearPlaneHeight); // UR
		FrustumVertices[2] = Vector3<float>(vNearPlaneCenter + vX * fNearPlaneWidth + vY * fNearPlaneHeight); // UL
		FrustumVertices[3] = Vector3<float>(vNearPlaneCenter + vX * fNearPlaneWidth - vY * fNearPlaneHeight); // LL

		// Far
		FrustumVertices[4] = Vector3<float>(vFarPlaneCenter - vX * fFarPlaneWidth - vY * fFarPlaneHeight); // LR
		FrustumVertices[5] = Vector3<float>(vFarPlaneCenter - vX * fFarPlaneWidth + vY * fFarPlaneHeight); // UR
		FrustumVertices[6] = Vector3<float>(vFarPlaneCenter + vX * fFarPlaneWidth + vY * fFarPlaneHeight); // UL
		FrustumVertices[7] = Vector3<float>(vFarPlaneCenter + vX * fFarPlaneWidth - vY * fFarPlaneHeight); // LL

		Vector3<float> vCenter(0.0f, 0.0f, 0.0f);
		for( int i = 0 ; i < 8; i++ ) 
			vCenter += FrustumVertices[i];

		vCenter /= 8;

		float fScale = 1.1f;

		for(int i = 0; i < 8; i++ )
			FrustumVertices[i] += (FrustumVertices[i] - vCenter) * (fScale - 1);
	}

	bool Camera::FrustumCubeTest(Vector3<float> min, Vector3<float> max)
	{
		for( int i = 0; i < 6; i++ )
		{
			if (FrustumPlanes[i].DotCoord(min) >= 0.0f)
				continue;
			if (FrustumPlanes[i].DotCoord(max) >= 0.0f)
				continue;

			if (FrustumPlanes[i].DotCoord(Vector3<float>(max.X, min.Y, min.Z)) >= 0.0f)
				continue;
			if (FrustumPlanes[i].DotCoord(Vector3<float>(min.X, max.Y, min.Z)) >= 0.0f)
				continue;
			if (FrustumPlanes[i].DotCoord(Vector3<float>(max.X, max.Y, min.Z)) >= 0.0f)
				continue;
			if (FrustumPlanes[i].DotCoord(Vector3<float>(min.X, min.Y, max.Z)) >= 0.0f)
				continue;
			if (FrustumPlanes[i].DotCoord(Vector3<float>(max.X, min.Y, max.Z)) >= 0.0f)
				continue;
			if (FrustumPlanes[i].DotCoord(Vector3<float>(min.X, max.Y, max.Z)) >= 0.0f)
				continue;

			return false;
		}

		return true;
	}

	bool Camera::FrustumSphereTest(Vector3<float> center, float radius)
	{
		for( int i = 0; i < 6; i++ ) 
		{
			if (FrustumPlanes[i].DotCoord(center) < -radius)
				return false;
		}

		return true;
	}

	bool Camera::FrustumBoxTest(Vector3<float> center, float size)
	{
		for( int i = 0; i < 6; i++ )
		{
			if(FrustumPlanes[i].DotCoord(Vector3<float>(center.X - size, center.Y - size, center.Z - size)) >= 0.0f)
				continue;
			if(FrustumPlanes[i].DotCoord(Vector3<float>(center.X + size, center.Y - size, center.Z - size)) >= 0.0f)
				continue;
			if(FrustumPlanes[i].DotCoord(Vector3<float>(center.X - size, center.Y + size, center.Z - size)) >= 0.0f)
				continue;
			if(FrustumPlanes[i].DotCoord(Vector3<float>(center.X + size, center.Y + size, center.Z - size)) >= 0.0f)
				continue;
			if(FrustumPlanes[i].DotCoord(Vector3<float>(center.X - size, center.Y - size, center.Z + size)) >= 0.0f)
				continue;
			if(FrustumPlanes[i].DotCoord(Vector3<float>(center.X + size, center.Y - size, center.Z + size)) >= 0.0f)
				continue;
			if(FrustumPlanes[i].DotCoord(Vector3<float>(center.X - size, center.Y + size, center.Z + size)) >= 0.0f)
				continue;
			if(FrustumPlanes[i].DotCoord(Vector3<float>(center.X + size, center.Y + size, center.Z + size)) >= 0.0f)
				continue;

			return false;
		}
		return true;
	}

	bool Camera::FrustumTest(Vector3<float> position)
	{
		for (int i = 0; i < 6; i++) 
			if (FrustumPlanes[i].DotCoord(position) < 0)
				return false;

		return true;
	}
}