/*======================================================================================

Program:		Range Imaging Toolkit (RITK)
Module:			ritkOpenGLCamera.cxx
Version:		$Revision: 2956 $
Date:			$Date: 2012-04-09 19:44:46 +0200 (Mo, 09 Apr 2012) $

Copyright (c) 2010-2011
Pattern Recognition Lab, 
University Erlangen-Nuremberg, Germany
All rights reserved.

This program 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 RITKCopyright.txt for more information.

http://www5.informatik.uni-erlangen.de/research/software/ritk

========================================================================================*/

#include "ritkDebugManager.h"
#include "ritkOpenGLCamera.h"

#include <math.h>

#include <GL/gl.h>
#include <GL/glu.h>

#include <itkVersorTransform.h>
#include "ritkOpenGLVisualizationWidget.h"

#include "ritkDebugManager.h"

#define DEG2RAD(x) (3.1415*x)/180.0
#define RAD2DEG(x) (180.0*x)/3.1415

namespace ritk
{

//----------------------------------------------------------------------------
OpenGLCamera::OpenGLCamera()
{

	// Initialize the camera coordinate system
	m_EyePos.Fill(0);
	m_ViewCenter.Fill(0);
	m_ViewUp.Fill(0);

	// Initialize clipping planes, viewport and FOV
	m_ClippingPlanes.Fill(0);
	m_ViewPort.Fill(0);
	m_FOV = 75;

	// Initialize the transformation
	//m_Zoom = 0.5f*log(0.5f/1.5f);
	m_Zoom = 0;
	m_TranslationVector.Fill(0);
	m_RotationMatrix.SetIdentity();
	m_OpenGLRotationMatrix.SetIdentity();
	
	
	ResetCamera2D();
	
	m_ControlUnit = new OpenGLCameraControlUnit();

	// Invalid state by default
	if(m_Render3D)
		m_InitFlag = false;
	else
		m_InitFlag = true;
}


//----------------------------------------------------------------------------
OpenGLCamera::~OpenGLCamera()
{
	// @todo Check
	/*if ( m_ControlUnit )
		delete m_ControlUnit;*/
}


//----------------------------------------------------------------------------
bool
OpenGLCamera::IsInitialized() const
{
	m_Mutex.Lock();
	bool b = m_InitFlag;
	m_Mutex.Unlock();
	return b;
}


//----------------------------------------------------------------------------
void
OpenGLCamera::SetEyePos(const Vec3 &v)
{
	m_Mutex.Lock();
	m_EyePos = v;
	m_Mutex.Unlock();

	emit StatusChanged();
}


//----------------------------------------------------------------------------
OpenGLCamera::Vec3
OpenGLCamera::GetEyePos() const
{
	m_Mutex.Lock();
	Vec3 v = m_EyePos;
	m_Mutex.Unlock();
	return v;
}


//----------------------------------------------------------------------------
void
OpenGLCamera::SetViewCenter(const Vec3 &v)
{
	m_Mutex.Lock();
	m_ViewCenter = v;
	m_Mutex.Unlock();

	emit StatusChanged();
}


//----------------------------------------------------------------------------
OpenGLCamera::Vec3
OpenGLCamera::GetViewCenter() const
{
	m_Mutex.Lock();
	Vec3 v = m_ViewCenter;
	m_Mutex.Unlock();
	return v;
}


//----------------------------------------------------------------------------
void
OpenGLCamera::SetViewUp(const Vec3 &v)
{
	m_Mutex.Lock();
	m_ViewUp = v;
	m_Mutex.Unlock();

	emit StatusChanged();
}


//----------------------------------------------------------------------------
OpenGLCamera::Vec3
OpenGLCamera::GetViewUp() const
{
	m_Mutex.Lock();
	Vec3 v = m_ViewUp;
	m_Mutex.Unlock();
	return v;
}


//----------------------------------------------------------------------------
void
OpenGLCamera::SetViewPort(const Vec2 &v)
{
	m_Mutex.Lock();
	m_ViewPort = v;
	m_Mutex.Unlock();

	emit StatusChanged();
}


//----------------------------------------------------------------------------
OpenGLCamera::Vec2
OpenGLCamera::GetViewPort() const
{
	m_Mutex.Lock();
	Vec2 v = m_ViewPort;
	m_Mutex.Unlock();
	return v;
}

//----------------------------------------------------------------------------
void
OpenGLCamera::ResetCamera2D()
{
	m_Mutex.Lock();

	m_ViewCenter2D[0] = 0.5f;
	m_ViewCenter2D[1] = 0.5f;
	m_Zoom2D = 1.0f;
	m_RatioTransformationFactor = 1.0f;
	m_ImgDimensions[0] = 1.0f;
	m_ImgDimensions[1] = 1.0f;

	m_Mutex.Unlock();

	emit StatusChanged();
}


//----------------------------------------------------------------------------
OpenGLCamera::Vec2
OpenGLCamera::GetClippingPlanes() const
{
	m_Mutex.Lock();
	Vec2 v = m_ClippingPlanes;
	m_Mutex.Unlock();
	return v;
}

//----------------------------------------------------------------------------
float
OpenGLCamera::GetFieldOfView() const
{
	m_Mutex.Lock();
	float FOV = m_FOV;
	m_Mutex.Unlock();
	return FOV;
}


//----------------------------------------------------------------------------
void
OpenGLCamera::ResetCamera3D(const Vec4 &BoundingSphere)
{
	m_Mutex.Lock();

	m_BoundingSphere = BoundingSphere;

	std::cout << "Resetting camera: " << m_BoundingSphere << std::endl;

	// Set up our viewing direction
	double d = 0.6f*(m_FOV/180.0)*3.14159265358979323846;

	//float Radius = m_BoundingSphere[3];
	m_ViewCenter[0] = m_BoundingSphere[0];
	m_ViewCenter[1] = m_BoundingSphere[1];
	m_ViewCenter[2] = m_BoundingSphere[2];
	m_EyePos[0] = m_ViewCenter[0];
	m_EyePos[1] = m_ViewCenter[1];
	m_EyePos[2] = m_ViewCenter[2] + 0.6f * ((m_BoundingSphere[3] / tan(d)) + m_BoundingSphere[3]);
	m_ViewUp[0] = 0;
	m_ViewUp[1] = 1;
	m_ViewUp[2] = 0;

	// Update clipping planes
	m_ClippingPlanes[0] = abs(m_ViewCenter[2] - m_EyePos[2]) - m_BoundingSphere[3];
	m_ClippingPlanes[1] = abs(m_ViewCenter[2] - m_EyePos[2]) + m_BoundingSphere[3];
	if(m_ClippingPlanes[0] < 0.1f) m_ClippingPlanes[0] = 0.1f;
//	printf("Reset 3D: %f, %f\n", m_ClippingPlanes[0], m_ClippingPlanes[1]);
	

	// Reset transformations
	//m_Zoom = 0.5f*log(0.5f/1.5f);
	m_Zoom = 0;
	m_TranslationVector.Fill(0);
	m_RotationMatrix.SetIdentity();
	m_OpenGLRotationMatrix.SetIdentity();

	// Update the initialization flag
	m_InitFlag = true;

	m_Mutex.Unlock();

	emit StatusChanged();
}

//----------------------------------------------------------------------------
ritk::OpenGLCamera::Vec2
OpenGLCamera::Unapply2D(const Vec2 &v)
{
	
	m_Mutex.Lock();
	Vec2 erg = Unapply2D_internal(v);
	m_Mutex.Unlock();

	return erg;
}

//----------------------------------------------------------------------------
ritk::OpenGLCamera::Vec2
OpenGLCamera::Unapply2D_internal(const Vec2 &v)
{
		if(parentWidget == NULL){
			itkExceptionMacro(<<"ERROR: parentWidget not set!");
		}
		parentWidget->makeCurrent();
		
		GLint viewport[4];
		GLdouble modelview[16];
		GLdouble projection[16];
		GLfloat winX, winY, winZ;
		GLdouble posX, posY, posZ;
	
		// read matrices
		glGetDoublev( GL_MODELVIEW_MATRIX, modelview );
		glGetDoublev( GL_PROJECTION_MATRIX, projection );
		glGetIntegerv( GL_VIEWPORT, viewport );

		// calculate 2d click coordinates
		winX = (float)v[0];
		winY = (float)viewport[3] - (float)v[1];
		winZ = 0.5f;
		
		gluUnProject( winX, winY, winZ, modelview, projection, viewport, &posX, &posY, &posZ);

		

		Vec2 erg;
		erg[0] = (float)posX;
		erg[1] = (float)posY;

		//printf("Vec: %f, %f\n", erg[0], erg[1]);

		return erg;
}

//----------------------------------------------------------------------------
void
OpenGLCamera::Rotate(const Vec2 &d)
{
	if (!m_Render3D)
		return;

	m_Mutex.Lock();

	// The axis/angle transformation type
	typedef itk::VersorTransform<RealType> TransformType;

	// Compute the normalized viewport direction and its norm
	Vec2 dn = d;
	dn.Normalize();
	RealType ViewPortMag = d.GetNorm();

	// Compute the in-plane axis
	Vec3 ViewDirection = m_ViewCenter-m_EyePos;
	ViewDirection.Normalize();
	Vec3 InPlaneAxis = itk::CrossProduct(m_ViewUp,ViewDirection);
	InPlaneAxis.Normalize();

	// Compute the direction vector in the viewport
	Vec3 ViewPortDir;
	ViewPortDir = dn[0]*InPlaneAxis + dn[1]*m_ViewUp;

	// The rotation axis is now defined as the vector perpendicular to our viewing direction and the direction vector in the viewport
	Vec3 RotAxis = itk::CrossProduct(ViewPortDir,ViewDirection);

	// Set up the rotation transform
	TransformType::Pointer Transform = TransformType::New();
	Transform->SetRotation(RotAxis, -0.1f*DEG2RAD(ViewPortMag));

	// Update the current rotation
	m_RotationMatrix = Transform->GetMatrix() * m_RotationMatrix;
	for ( unsigned int r = 0; r < 3; r++ )
		for ( unsigned int c = 0; c < 3; c++ )
			m_OpenGLRotationMatrix[r][c] = m_RotationMatrix[c][r];

	m_Mutex.Unlock();

	emit StatusChanged();
}




//----------------------------------------------------------------------------
void
OpenGLCamera::Translate(const Vec2 &d)
{

	if(m_Render3D){
		m_Mutex.Lock();
		// x-movement will translate along the in-plane axis
		Vec3 ViewDirection = m_ViewCenter-m_EyePos;
		ViewDirection.Normalize();
		Vec3 InPlaneAxis = itk::CrossProduct(m_ViewUp,ViewDirection);
		Vec3 TranslationX = d[0]*InPlaneAxis;

		// y-movement will translate along the view-up axis
		Vec3 TranslationY = d[1]*m_ViewUp;

		// Update the translation vector
		m_TranslationVector += TranslationX + TranslationY;
		m_Mutex.Unlock();
	}
	else
	{
		m_Mutex.Lock();

		if(parentWidget == NULL){
			itkExceptionMacro(<<"ERROR: parentWidget not set!");
		}
		parentWidget->makeCurrent();
		

		Vec2 d_unapplied;
		// Calculate absolute mouse movement from relative movement
		d_unapplied = Unapply2D_internal(m_ViewPort * .5f + d) -  Unapply2D_internal(m_ViewPort * .5f);
		d_unapplied[0] /= m_ImgDimensions[0];
		d_unapplied[1] /= m_ImgDimensions[1];
		
		// move ViewCenter according to absolute mouse movement
		m_ViewCenter2D -= d_unapplied;

		// prevent that the texture gets moved out of the visible area
		CheckTransformBoundaries2D();
				
		m_Mutex.Unlock();

	}



	emit StatusChanged();
}



//----------------------------------------------------------------------------

void
OpenGLCamera::CheckTransformBoundaries2D()
{
	
	if(m_ViewCenter2D[0] > 0.5f){
		if ((1.0f - m_ViewCenter2D[0]) * m_Zoom2D < 0.5f)
			m_ViewCenter2D[0] = 1.0f - 0.5f/m_Zoom2D;
	}
	else
	{
		if ((m_ViewCenter2D[0]) * m_Zoom2D < 0.5f)
			m_ViewCenter2D[0] = 0.5f/m_Zoom2D;
	}
	if(m_ViewCenter2D[1] > 0.5f){
		if ((1.0f - m_ViewCenter2D[1]) * m_Zoom2D < 0.5f)
			m_ViewCenter2D[1] = 1.0f - 0.5f/m_Zoom2D;
	}
	else
	{
		if ((m_ViewCenter2D[1]) * m_Zoom2D < 0.5f)
			m_ViewCenter2D[1] = 0.5f/m_Zoom2D;
	}




}

//----------------------------------------------------------------------------
void
OpenGLCamera::Zoom(RealType Zoom, QPoint mousePos_qpoint)
{
	m_Mutex.Lock();

	if(m_Render3D){

		m_Zoom += 0.1*Zoom;


	} else {

		if(parentWidget == NULL){
			itkExceptionMacro(<<"ERROR: parentWidget not set!");
		}
		parentWidget->makeCurrent();

		RealType zoom_old = m_Zoom2D;
		m_Zoom2D *= pow(1.1f, -Zoom);
		if(m_Zoom2D < 1.0f) m_Zoom2D = 1.0f;
		//printf("Mousepos: %i, %i\n", mousePos.x(), mousePos.y());
		Vec2 mousePos;
		mousePos[0] = mousePos_qpoint.x();
		mousePos[1] = mousePos_qpoint.y();
		mousePos = Unapply2D_internal(mousePos);
		mousePos[0] /= m_ImgDimensions[0];
		mousePos[1] /= m_ImgDimensions[1];
		Vec2 dist = m_ViewCenter2D - mousePos;
		dist *= zoom_old / m_Zoom2D;
		m_ViewCenter2D = dist + mousePos;

		CheckTransformBoundaries2D();
	
	}

	m_Mutex.Unlock();
	emit StatusChanged();
}


//----------------------------------------------------------------------------
OpenGLCamera::BoundingBoxType
OpenGLCamera::GetBoundingBox() const
{
	// Dummy atm
	BoundingBoxType BB;
	BB.Fill(0);
	return BB;
}

//----------------------------------------------------------------------------
void
OpenGLCamera::Render2D(const OpenGLVisualizationWidget *Widget, QVector2D ImgDimensions)
{

	m_Mutex.Lock();

	m_ImgDimensions[0] = ImgDimensions.x();
	m_ImgDimensions[1] = ImgDimensions.y();


	glMatrixMode(GL_MODELVIEW);							
	glLoadIdentity();									


	// Zoom
	float zoom = 1.0f/m_Zoom2D;
	glOrtho(-zoom, zoom, -zoom, zoom, 1, -1);

	// Translation
	glTranslatef(- (2*m_ViewCenter2D[0] - 1), 2 * m_ViewCenter2D[1] - 1, 0);
	
	
	// Resize the Image to the payload size
	glOrtho(0,ImgDimensions.x(),ImgDimensions.y(), 0, -1, 1);
	

	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	
	// Calculate the horizontal to vertical scaling ration caused by m_ViewPort
	m_RatioTransformationFactor = m_ViewPort[1] * ImgDimensions.x()/(m_ViewPort[0] * ImgDimensions.y());
	
	// Rescale and translate to the correct position
	if(m_RatioTransformationFactor > 1.0f)
	{
		glOrtho(-1, 1, -m_RatioTransformationFactor, m_RatioTransformationFactor, 1, -1);
	}
	else
	{
		float ratiof = 1.0f/m_RatioTransformationFactor;
		glOrtho(-ratiof, ratiof, -1, 1, 1, -1);
	}
	
	m_Mutex.Unlock();

	
}

//----------------------------------------------------------------------------
void
OpenGLCamera::Render3D(const OpenGLVisualizationWidget *Widget)
{
	m_Mutex.Lock();

	// Sanity check
	if ( !m_InitFlag )
	{
		m_Mutex.Unlock();
		return;
	}

	// Compute zoom direction and distance from the eye to the view center
	Vec3 ZoomVector = m_ViewCenter-m_EyePos;
	float Distance = ZoomVector.GetNorm();
	ZoomVector.Normalize();
	
	// Compute the zoom
	float RequestedZoom = (Distance/10)*m_Zoom;
	float alpha = 1.0/(2*Distance) * log((1.f+0.999f)/(1.f-0.99f));
	float Weight = tanh(alpha*abs(RequestedZoom));
	float ActualZoom = (Weight)*Distance;
	ActualZoom = RequestedZoom < 0 ? ActualZoom*-1 : ActualZoom;
	/*std::cout << "Zoom" << std::endl;
	std::cout << "	Distance=" << Distance << std::endl;
	std::cout << "	RequestedZoom=" << RequestedZoom << std::endl;
	std::cout << "	Weight=" << Weight << std::endl;
	std::cout << "	ActualZoom=" << ActualZoom << std::endl;*/
	ZoomVector = ActualZoom*ZoomVector;
	
	// Calculate new clipping planes
	Vec2 ActualClippingPlanes = m_ClippingPlanes;
	Vec3 distRealVec = m_ViewCenter - m_EyePos;
	Vec3 distZoomedVec = distRealVec + ZoomVector;
	RealType ClippingDiff = distRealVec.GetNorm() - distZoomedVec.GetNorm();
	ActualClippingPlanes[0] -= ClippingDiff;
	ActualClippingPlanes[1] -= ClippingDiff;
	if( ActualClippingPlanes[1] < 1.0f ) ActualClippingPlanes[1] = 1.0f;
	if( ActualClippingPlanes[0] < ActualClippingPlanes[1] * 0.001f ) ActualClippingPlanes[0] = ActualClippingPlanes[1] * 0.001f;
	
	// Set up viewing frustum 
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	//gluPerspective((GLfloat)45*(tanh(m_Zoom)+1.f)*2, (GLfloat)(m_ViewPort[0])/(GLfloat)(m_ViewPort[1]), (GLfloat)m_ClippingPlanes[0], (GLfloat)m_ClippingPlanes[1]);
	gluPerspective((GLfloat)m_FOV, (GLfloat)(m_ViewPort[0])/(GLfloat)(m_ViewPort[1]), (GLfloat)ActualClippingPlanes[0], (GLfloat)ActualClippingPlanes[1]);

	// Camera coordinate system
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();

	// Set up our viewing direction
	gluLookAt(m_EyePos[0],m_EyePos[1],m_EyePos[2], m_ViewCenter[0],m_ViewCenter[1],m_ViewCenter[2], m_ViewUp[0],m_ViewUp[1],m_ViewUp[2]);
	glTranslatef(ZoomVector[0], ZoomVector[1], ZoomVector[2]);

	// Translation 
	glTranslatef(-m_TranslationVector[0],-m_TranslationVector[1],-m_TranslationVector[2]);

	// Rotation (around the viewing center)
	glTranslatef(m_ViewCenter[0],m_ViewCenter[1],m_ViewCenter[2]);
	glMultMatrixf(m_OpenGLRotationMatrix[0]);
	glTranslatef(-m_ViewCenter[0],-m_ViewCenter[1],-m_ViewCenter[2]);


	/*glDisable(GL_LIGHTING);
	glPushMatrix();
	glTranslatef(m_ViewCenter[0], m_ViewCenter[1], m_ViewCenter[2]);
	GLUquadric *QuadricPointer = gluNewQuadric();
	glColor4f(1.0f, 0.0f, 1.0f, 0.9f);
	gluSphere(QuadricPointer, m_BoundingSphere[3], 100, 100);
	glPopMatrix();
	glEnable(GL_LIGHTING);*/

	
	m_Mutex.Unlock();
}


//----------------------------------------------------------------------------
void
OpenGLCamera::PrintSelf(std::ostream& os, itk::Indent indent) const
{
	m_Mutex.Lock();

	Superclass::PrintSelf(os,indent);

	os << "OpenGLCamera" << std::endl;
	os << "	CoordSystem" << std::endl;
	os << "		EyePos: " << m_EyePos << std::endl;
	os << "		ViewCenter: " << m_ViewCenter << std::endl;
	os << "		ViewUp: " << m_ViewUp << std::endl;
	os << "	ViewParams" << std::endl;
	os << "		ViewPort: " << m_ViewPort << std::endl;
	os << "		ClippingPlanes: " << m_ClippingPlanes << std::endl;
	os << "	Transformation" << std::endl;
	os << "		Zoom: " << m_Zoom << std::endl;
	os << "		Translation: " << m_TranslationVector << std::endl;
	os << "		RotationMatrix: " << m_RotationMatrix << std::endl;
	os << "		OpenGLRotationMatrix: " << m_OpenGLRotationMatrix << std::endl;

	m_Mutex.Unlock();
}


//----------------------------------------------------------------------------
QDockWidget*
OpenGLCamera::GetControlUnit() const
{
	return m_ControlUnit;	
}

} // namespace ritk
