#include "common.h"
#include "Camera.h"
#include "glInterface.h"
#include "Polygon3.h"
#include "Matrix4x4.h"
#include "Polyhedron.h"

namespace CHCDemoEngine 
{

using namespace std;

// our coordinate system is left-handed
// we look into positive y and have the positive z axis pointing up
static const Vector3 baseDir = -Vector3::UNIT_Y();



/**********************************************************/
/*            class Frustum implementation                */
/**********************************************************/


Frustum::Frustum(const Matrix4x4 &trafo)
{
	//////////
	//-- extract the plane equations

	for (int i = 0; i < 4; ++ i)
	{
		mClipPlanes[Frustum::RIGHT_PLANE][i] = trafo.x[i][3] - trafo.x[i][0];
		mClipPlanes[Frustum::LEFT_PLANE][i] = trafo.x[i][3] + trafo.x[i][0]; 
		
		mClipPlanes[Frustum::BOTTOM_PLANE][i] = trafo.x[i][3] + trafo.x[i][1];
		mClipPlanes[Frustum::TOP_PLANE][i] = trafo.x[i][3] - trafo.x[i][1]; 

		mClipPlanes[Frustum::FAR_PLANE][i] = trafo.x[i][3] - trafo.x[i][2]; 
		mClipPlanes[Frustum::NEAR_PLANE][i] = trafo.x[i][3] + trafo.x[i][2];
	}
}


void Frustum::EnclosePolyhedron(const Polyhedron &polyhedron)
{
	VertexArray vertices;
	polyhedron.CollectVertices(vertices);

	for (int i = 0; i < 6; ++ i)
	{
		Plane3 &plane = mClipPlanes[i];

		VertexArray::const_iterator it, it_end = vertices.end();
		float minDist = 1e20;

		for (it = vertices.begin(); it != it_end; ++ it)
		{
			float dist = plane.Distance(*it);

			if (dist < minDist)
			{
				//cout << "minDist: " << dist << endl;
				minDist = dist;
			}
		}

		plane.mD += minDist;
	}
}


void Frustum::ExtractTransformation(Matrix4x4 &m) const
{
	for (int i = 0; i < 4; ++ i)
	{
		m.x[i][0] = (mClipPlanes[LEFT_PLANE][i] - mClipPlanes[RIGHT_PLANE][i]) * 0.5f;
		m.x[i][1] = (mClipPlanes[BOTTOM_PLANE][i] - mClipPlanes[TOP_PLANE][i]) * 0.5f;
		m.x[i][2] = (mClipPlanes[NEAR_PLANE][i] - mClipPlanes[FAR_PLANE][i]) * 0.5f;
		m.x[i][3] = (mClipPlanes[LEFT_PLANE][i] + mClipPlanes[RIGHT_PLANE][i]) * 0.5f;
	}
}



/*********************************************************/
/*            class Camera implementation                */
/*********************************************************/


Camera::Camera() 
{
	SetPosition(Vector3(0, 0, 0));

	mPitch = mYaw = 0;
	Precompute(baseDir);

	CalculateFromPitchAndYaw();
}


void Camera::Precompute(const Vector3 &dir) 
{
	Vector3 up = Vector3::UNIT_Z();
	
	mBaseOrientation = LookAt(Vector3::ZERO(), dir, up);
	mViewOrientation = mBaseOrientation;
}


void Camera::SetPosition(const Vector3 &pos) 
{
	mPosition = pos;
}


void Camera::SetNear(float nearDist) 
{
	mNear = nearDist;
	UpdateProjectionMatrix();
}


void Camera::SetFar(float farDist) 
{ 
	mFar = farDist; 
	UpdateProjectionMatrix();
}


void Camera::GetModelViewMatrix(Matrix4x4 &mat) const
{
	mat = mViewOrientation;

	// note: left handed system => we go into positive z
	mat.x[3][0] = -DotProd(GetRightVector(), mPosition);
	mat.x[3][1] = -DotProd(GetUpVector(), mPosition);
	mat.x[3][2] =  DotProd(GetDirection(), mPosition);
}


void Camera::GetViewOrientationMatrix(Matrix4x4 &mat) const
{
	mat = mViewOrientation;
}


void Camera::SetupViewProjection()
{
	glMatrixMode(GL_PROJECTION);
	SetupProjection();

	glMatrixMode(GL_MODELVIEW);
	// set up the view matrix
	SetupCameraView();
}


void Camera::SetupProjection()
{
	Matrix4x4 m;

	GetProjectionMatrix(m);
	glLoadMatrixf((float *)m.x);
}


void Camera::SetupCameraView()
{
	Matrix4x4 m;
	GetModelViewMatrix(m);

	glLoadMatrixf((float *)m.x);
}


void Camera::Yaw(float angle)
{
	mYaw += angle;
	CalculateFromPitchAndYaw();
}


void Camera::Pitch(float angle)
{
	mPitch += angle;
	CalculateFromPitchAndYaw();
}


void Camera::SetDirection(const Vector3 &dir)
{
	Vector3 ndir = -Normalize(dir);

	mPitch = -atan2(ndir.x, ndir.y);
	mYaw = atan2(ndir.z, sqrt((ndir.x * ndir.x) + (ndir.y * ndir.y)));

	CalculateFromPitchAndYaw();
}


void Camera::CalculateFromPitchAndYaw()
{
	mViewOrientation = mBaseOrientation;

	Matrix4x4 roty = RotationYMatrix(mPitch);
	Matrix4x4 rotx = RotationXMatrix(mYaw);

	mViewOrientation *= roty;
	mViewOrientation *= rotx;
}


Vector3 Camera::GetDirection() const
{ 
	return -Vector3(mViewOrientation.x[0][2], mViewOrientation.x[1][2], mViewOrientation.x[2][2]);
}


Vector3 Camera::GetUpVector() const 
{ 
	return Vector3(mViewOrientation.x[0][1], mViewOrientation.x[1][1], mViewOrientation.x[2][1]);
}


Vector3 Camera::GetRightVector() const 
{ 
	return Vector3(mViewOrientation.x[0][0], mViewOrientation.x[1][0], mViewOrientation.x[2][0]);		
}


Vector3 Camera::GetBaseDirection() const
{ 
	return -Vector3(mBaseOrientation.x[0][2], mBaseOrientation.x[1][2], mBaseOrientation.x[2][2]);
}


Vector3 Camera::GetBaseUpVector() const 
{ 
	return Vector3(mBaseOrientation.x[0][1], mBaseOrientation.x[1][1], mBaseOrientation.x[2][1]);
}


Vector3 Camera::GetBaseRightVector() const 
{ 
	return Vector3(mBaseOrientation.x[0][0], mBaseOrientation.x[1][0], mBaseOrientation.x[2][0]);
}


void Camera::CalcFrustum(Frustum &frustum)
{
	// we grab the plane equations of the six clipplanes of the viewfrustum
	Matrix4x4 matViewing, matProjectionView;

	GetModelViewMatrix(matViewing);
	GetProjectionMatrix(matProjectionView);

	matProjectionView = matViewing * matProjectionView;

	frustum = Frustum(matProjectionView);


	////////////
	//-- normalize the coefficients

	for (int i = 0; i < 6; ++ i)
	{
		// the clipping planes look outward the frustum,
		// so distances > 0 mean that a point is outside
		const float invLength = -1.0f / Magnitude(frustum.mClipPlanes[i].mNormal);
	
		frustum.mClipPlanes[i].mD *= invLength;
		frustum.mClipPlanes[i].mNormal *= invLength;
	}
}


void Camera::GetProjectionMatrix(Matrix4x4 &m)const
{
	//glGetFloatv(GL_PROJECTION_MATRIX, (float *)m.x);
	m = mProjection;
}



/*********************************************************/
/*       Class PerspectiveCamera implementation          */
/*********************************************************/


PerspectiveCamera::PerspectiveCamera() 
{
	mFOVy = 60.0f * M_PI / 180.0f;
	mAspect = 1.0f;

	UpdateProjectionMatrix();
}


PerspectiveCamera::PerspectiveCamera(float aspect, float fieldOfView) 
{
	mFOVy = fieldOfView * M_PI / 180.0f;
	mAspect = aspect;

	UpdateProjectionMatrix();
}


Polyhedron *PerspectiveCamera::ComputeFrustum(float farthestVisibleDistance) const
{
	Vector3 ftl, ftr, fbl, fbr;
	Vector3 ntl, ntr, nbl, nbr;

	VertexArray sides[6];

	ComputePoints(ftl, ftr, fbl, fbr, ntl, ntr, nbl, nbr, farthestVisibleDistance);

	for (int i = 0; i < 6; ++ i)
		sides[i].resize(4);
	
	// left, right
	sides[0][0] = ftl; sides[0][1] = fbl; sides[0][2] = nbl; sides[0][3] = ntl;
	sides[1][0] = fbr; sides[1][1] = ftr; sides[1][2] = ntr; sides[1][3] = nbr;
	// bottom, top
	sides[2][0] = fbl; sides[2][1] = fbr; sides[2][2] = nbr; sides[2][3] = nbl;
	sides[3][0] = ftr; sides[3][1] = ftl; sides[3][2] = ntl; sides[3][3] = ntr;
	// near, far
	sides[4][0] = ntr; sides[4][1] = ntl; sides[4][2] = nbl; sides[4][3] = nbr; 
	sides[5][0] = ftl; sides[5][1] = ftr; sides[5][2] = fbr; sides[5][3] = fbl; 


	//////////
	//-- compute polyhedron

	PolygonContainer polygons;

	for (int i = 0; i < 6; ++ i)
	{
		Polygon3 *poly = new Polygon3(sides[i]);
		polygons.push_back(poly);
	}

	return new Polyhedron(polygons);
}


void PerspectiveCamera::ComputePointsInternal(Vector3 &ftl, Vector3 &ftr, Vector3 &fbl, Vector3 &fbr,
											  Vector3 &ntl, Vector3 &ntr, Vector3 &nbl, Vector3 &nbr,
											  const Vector3 &view, const Vector3 &right, const Vector3 &up,
											  const Vector3 &pos,
											  float farthestVisibleDistance) const
{
	const float z_near = mNear;
	const float z_far = min(mFar, farthestVisibleDistance);

	const float fov = mFOVy;

	const float aspectRatio = GetAspect();

	const float h_near = tan(fov * 0.5f) * z_near;
	const float w_near = h_near * aspectRatio;

	const float h_far = tan(fov * 0.5f) * z_far;
	const float w_far = h_far * aspectRatio;

	const Vector3 fc = pos + view * z_far; 
	

	Vector3 t1, t2;

	t1 = h_far * up;
	t2 = w_far * right;

	ftl = fc + t1 - t2;
	ftr = fc + t1 + t2;
	fbl = fc - t1 - t2;
	fbr = fc - t1 + t2;

	const Vector3 nc = pos + view * z_near;
	
	t1 = h_near * up;
	t2 = w_near * right;

	ntl = nc + t1 - t2;
	ntr = nc + t1 + t2;
	nbl = nc - t1 - t2;
	nbr = nc - t1 + t2;
}


void PerspectiveCamera::UpdateProjectionMatrix()
{
	mProjection = GetPerspective(mFOVy, 1.0f / mAspect, mNear, mFar);
}



void PerspectiveCamera::ComputePoints(Vector3 &ftl, Vector3 &ftr, Vector3 &fbl, Vector3 &fbr,
									  Vector3 &ntl, Vector3 &ntr, Vector3 &nbl, Vector3 &nbr,
									  float farthestVisibleDistance) const
{
	ComputePointsInternal(ftl, ftr, fbl, fbr, 
		                  ntl, ntr, nbl, nbr, 
						  GetDirection(), GetRightVector(), GetUpVector(), 
						  GetPosition(),
						  farthestVisibleDistance);
}



/*********************************************************/
/*          Class OrthoCamera implementation             */
/*********************************************************/


OrthoCamera::OrthoCamera(): 
mLeft(0), mRight(1), mBottom(0), mTop(1)
{
	mNear = 0; mFar = 1;
	UpdateProjectionMatrix();
}


OrthoCamera::OrthoCamera(float l, float r, float b, float t): 
mLeft(l), mRight(r), mBottom(b), mTop(t)
{
	mNear = 0; mFar = 1;
	UpdateProjectionMatrix();
}


OrthoCamera::OrthoCamera(float l, float r, float b, float t, float n, float f): 
mLeft(l), mRight(r), mBottom(b), mTop(t)
{
	mNear = n; mFar = f;
	UpdateProjectionMatrix();
}


void OrthoCamera::UpdateProjectionMatrix()
{
	mProjection = GetOrtho(mLeft, mRight, mBottom, mTop, mNear, mFar);
}


}