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

	Program:		Range Imaging Toolkit (RITK)
	Module:			ritkOpenGLPointPickerEntity.cxx
	Version:		$Revision: 2867 $
	Date:			$Date: 2012-03-30 15:52:11 +0200 (Fr, 30 Mrz 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 <QDataStream>
#include <QVariant>
#include <QDockWidget>
#include <QApplication>
#include <QComboBox>

#include "ritkOpenGLVisualizationWidget.h"
#include "ritkOpenGLPointPickerEntity.h"
#include "ritkOpenGLRImageEntity.h"

#define CHECK_GL_ERROR()												\
{																		\
	GLenum errCode = glGetError();										\
	if ( errCode != GL_NO_ERROR )										\
	{																	\
	std::cout << "An OpenGL error occured: " << errCode << std::endl;	\
	std::cout << gluErrorString(errCode) << std::endl;					\
	std::cout << __FILE__ << " Line " << __LINE__  << std::endl;		\
	}																	\
}

#pragma comment(lib, "opengl32.lib")
#pragma comment(lib, "glu32.lib")

//#define PERFORMANCE_CHECK
#define USE_SUB_TEX
//#define USE_INDEX_VBO


namespace ritk
{

//----------------------------------------------------------------------------
OpenGLPointPickerEntity::OpenGLPointPickerEntity()
{
	m_SphereSize = 5.0f;

	m_QuadricPointer = gluNewQuadric();

	m_MousePos = QPoint(-1,-1);

	m_Mutex = itk::FastMutexLock::New();

	m_mouseOverPoint = -1;
	m_mouseOverLine = -1;
	m_mousePointGrabbed = -1;

	m_2DPickInvalidPoints = false;
	m_2DRelativeCoords = false;

	m_ImgDimensions = QVector2D(1,1);

#ifdef PERFORMANCE_CHECK
	// Initialize timers
	for ( int i = 0; i < 10; i++ )
	{
		m_Counters[i] = 0;
		m_Timers[i] = 0;
	}
#endif
}

//----------------------------------------------------------------------------
OpenGLPointPickerEntity::~OpenGLPointPickerEntity()
{
	gluDeleteQuadric(m_QuadricPointer);

	/*if(m_Mutex)
		m_Mutex->Delete();
		m_Mutex = NULL;*/

	/*if ( m_ControlUnit ){
		delete m_ControlUnit;
		m_ControlUnit = NULL;
	}*/
}

//----------------------------------------------------------------------------
void
OpenGLPointPickerEntity::set2DPickInvalidPoints(bool value)
{
	m_Mutex->Lock();
	m_2DPickInvalidPoints = value;
	m_Mutex->Unlock();
}

//----------------------------------------------------------------------------
void
OpenGLPointPickerEntity::Render2D(const OpenGLVisualizationWidget *Widget, QVector2D ImgDimensions)
{
	m_Mutex->Lock();

	m_ImgDimensions = ImgDimensions;

	glDisable(GL_LIGHTING);
	glDepthMask(GL_FALSE);


	// Draw Lines
	int i = 0;
	PickedLine line;
	foreach ( line, m_LineList ){
		
		if(i == m_mouseOverLine){
			glColor3f(0.0f, 0.5f, 1.0f);
		} else {
			glColor3f(0.0f, 0.0f, 1.0f);
		}
		
		PickedPoint *p1 = line.p1;
		PickedPoint *p2 = line.p2;
		glBegin(GL_LINES);
		if(m_2DRelativeCoords){
			glVertex3f(p1->coord2D.x() * ImgDimensions.x(), p1->coord2D.y() * ImgDimensions.y(), 0.49f);
			glVertex3f(p2->coord2D.x() * ImgDimensions.x(), p2->coord2D.y() * ImgDimensions.y(), 0.49f);
		} else {
			glVertex3f(p1->coord2D.x(), p1->coord2D.y(), 0.49f);
			glVertex3f(p2->coord2D.x(), p2->coord2D.y(), 0.49f);
		}
		glEnd();
		i++;
	}



	calculateMouseOver();


	// Draw Points
	i = 0;
	PickedPoint vec;
	foreach ( vec, m_PointList ){

		if (i == m_mouseOverPoint){
			glColor4f(1.0f, 0.0f, 0.0f,1.0f);
			glPointSize(7);
		} else {
			glColor4f(1.0f, 0.0f, 1.0f,0.5f);
			glPointSize(5);
		}
		//glEnable(GL_BLEND);
		//glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
		glBegin(GL_POINTS);
		if(m_2DRelativeCoords){
			glVertex3f(vec.coord2D.x() * ImgDimensions.x(), vec.coord2D.y() * ImgDimensions.y(), 0.5f);
		} else {
			glVertex3f(vec.coord2D.x(), vec.coord2D.y(), 0.5f);
		}
		glEnd();
		//glDisable(GL_BLEND);
		i++;
	}

	
	glDepthMask(GL_TRUE);
	glEnable(GL_LIGHTING);
	
	m_Mutex->Unlock();

}


	
//----------------------------------------------------------------------------
void
OpenGLPointPickerEntity::Render3D(const OpenGLVisualizationWidget *Widget)
{
	m_Mutex->Lock();

	glDisable(GL_LIGHTING);
	glDepthMask(GL_FALSE);

	// Draw line
	glColor3f(0.0f, 0.0f, 1.0f);
	PickedLine line;
	foreach ( line, m_LineList ){
		PickedPoint *p1 = line.p1;
		PickedPoint *p2 = line.p2;
		glBegin(GL_LINES);
		glVertex3f(p1->coord3D.x(), p1->coord3D.y(), p1->coord3D.z());
		glVertex3f(p2->coord3D.x(), p2->coord3D.y(), p2->coord3D.z());
		glEnd();
	}

	// Draw spheres
	glMatrixMode(GL_MODELVIEW); 
	
	PickedPoint vec;

	foreach ( vec, m_PointList ){

		glPushMatrix();
	
		glTranslatef(vec.coord3D.x(), vec.coord3D.y(), vec.coord3D.z());
		glColor3f(1.0f, 0.0f, 1.0f);
		gluSphere(m_QuadricPointer, m_SphereSize, 10, 10);

		glPopMatrix();
	}

	
	glDepthMask(GL_TRUE);

	// Check for GL errors
	CHECK_GL_ERROR();

	m_Mutex->Unlock();
}

//----------------------------------------------------------------------------
void
OpenGLPointPickerEntity::setMousePos(QPoint pos){

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

	GLint viewport[4];
	
	// read matrix
	glGetIntegerv( GL_VIEWPORT, viewport );
		
	if(pos.x() < viewport[0] || pos.x() >= viewport[2] || pos.y() < viewport[1] || pos.y() >= viewport[3]){
		m_MousePos = QPoint(-1,-1);
	} else {
		m_MousePos = pos;
	}

	calculateMouseOver();

}


//----------------------------------------------------------------------------
void
OpenGLPointPickerEntity::calculateMouseOver(){

	if(m_MousePos == QPoint(-1,-1)){
		m_mouseOverLine = -1;
		m_mouseOverPoint = -1;
		return;
	}

	if(m_Render3D){


		//TODO


	} else {


		float maxDist = 15;
		float maxLineDist = 13;

		int mouseOverPoint = -1;
		float mouseOverDist = maxDist*maxDist;
		int i = 0;

		// calculate the distances of each point to the mouse; pick the closest one (if smaller than maxdist)
		PickedPoint vec;
		foreach ( vec, m_PointList ){
			QVector2D res2D;
			
			// Project point
			if(m_2DRelativeCoords){
				Project2D(QVector2D(vec.coord2D.x() * m_ImgDimensions.x(), vec.coord2D.y() * m_ImgDimensions.y()), &res2D);
			} else {
				Project2D(vec.coord2D, &res2D);
			}
			
			
			// compare point position to mouse position
			QVector2D dist = res2D - QVector2D(m_MousePos);
			float lenSquared = dist.lengthSquared();
			
			if(lenSquared < mouseOverDist){
				mouseOverPoint = i;
				mouseOverDist = lenSquared;
			}

			i++;
		}

		if(mouseOverPoint != -1){
			m_mouseOverLine = -1;
			m_mouseOverPoint = mouseOverPoint;
			return;
		}

		// if no points found, look for lines
		

		QVector2D mousePos = QVector2D(m_MousePos);
		
		int mouseOverLine = -1;
		int mouseOverLineDist = maxLineDist*maxLineDist;

		PickedLine line;

		i = -1;
		foreach ( line, m_LineList ){
			i++;

			// calculate the projection from the point to each line
			QVector2D a, b, ab;
			
			// project points into absolute coordinates
			if(m_2DRelativeCoords){
				Project2D(QVector2D(line.p1->coord2D.x() * m_ImgDimensions.x(), line.p1->coord2D.y() * m_ImgDimensions.y()), &a);
				Project2D(QVector2D(line.p2->coord2D.x() * m_ImgDimensions.x(), line.p2->coord2D.y() * m_ImgDimensions.y()), &b);
			} else {
				Project2D(line.p1->coord2D,&a);
				Project2D(line.p2->coord2D,&b);
			}
			ab = b - a;

			if (a == b)
				continue;
			

			// n = 1 -> closest point on line is b; n = 0 -> closest point on line is a
			float n = (ab.x()*mousePos.x() + ab.y() * mousePos.y() - ab.x() * a.x() - ab.y() * a.y()) / (ab.x() * ab.x() + ab.y() * ab.y());

			if(n < 0.0f || n > 1.0f)
				continue;

			// calculate closest point on line
			QVector2D cp = a + n * ab;

			QVector2D d = cp - mousePos;

			float dSquared = d.lengthSquared();

			//printf("d: %f, %f\n", dSquared, mouseOverLineDist)
			if(dSquared < mouseOverLineDist){
				mouseOverLine = i;
				mouseOverLineDist = dSquared;

			}

			
		}


		if(mouseOverLine != -1){
			m_mouseOverPoint = -1;
			m_mouseOverLine = mouseOverLine;
			return;
		}

		
		// nothing found
		m_mouseOverLine = -1;
		m_mouseOverPoint = -1;
		
	}


}

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


//----------------------------------------------------------------------------
void
OpenGLPointPickerEntity::PrintSelf(std::ostream& os, itk::Indent indent) const
{
	std::cout << "OpenGLPointPickerEntity" << std::endl;	
}


//----------------------------------------------------------------------------
int
OpenGLPointPickerEntity::GetNumPoints()
{
	int erg = m_PointList.length();
	return erg;
}

//----------------------------------------------------------------------------
float
OpenGLPointPickerEntity::GetDistance(int p0, int p1)
{
	m_Mutex->Lock();
	float erg;
	if (p0 < 0 || p0 >= m_PointList.length() || p1 < 0 || p1 >= m_PointList.length())
	{
		erg = -1.0f;
	}
	else
	{
		QVector3D vec0 = m_PointList[p0].coord3D;
		QVector3D vec1 = m_PointList[p1].coord3D;
		QVector3D dvec = vec0 - vec1;
		erg = dvec.length();
	}		
	m_Mutex->Unlock();
	return erg;
}


//----------------------------------------------------------------------------
QVector3D
OpenGLPointPickerEntity::GetPoint(int p)
{

	return m_PointList[p].coord3D;

}

//----------------------------------------------------------------------------
QVector2D
OpenGLPointPickerEntity::GetPoint2D(int p)
{

	return m_PointList[p].coord2D;
}


//----------------------------------------------------------------------------
void
OpenGLPointPickerEntity::SetSphereSize(float size)
{
	m_Mutex->Lock();
	m_SphereSize = size;
	m_Mutex->Unlock();
}


//----------------------------------------------------------------------------
void 
OpenGLPointPickerEntity::DeletePoints()
{
	m_Mutex->Lock();
	m_PointList.clear();
	m_Mutex->Unlock();

	emit PointPickerChangedSignal();
}


//----------------------------------------------------------------------------
bool
OpenGLPointPickerEntity::MouseMoved(QMouseEvent *e)
{

	m_Mutex->Lock();

	// calculate mouseover and return true (=request rerender) if mouseover changed
	
	int overPoint_bup = m_mouseOverPoint;
	int overLine_bup = m_mouseOverLine;

	setMousePos(e->pos());

	bool changed = false;
	if(overPoint_bup != m_mouseOverPoint || overLine_bup != m_mouseOverLine)
		changed = true;
	

	if(m_mousePointGrabbed != -1){
		PickedPoint newPos;
		if(PickPoint(m_MousePos, &newPos))
			if(Event_PointDragged(m_mousePointGrabbed, newPos))
				changed = true;
		
	}



	m_Mutex->Unlock();

	return changed;
}



//----------------------------------------------------------------------------
bool
OpenGLPointPickerEntity::RightMousePressed(QMouseEvent *e)
{
	m_Mutex->Lock();

	setMousePos(e->pos());

	bool erg = false;

	if(e->modifiers() & Qt::ControlModifier){
		if(m_mouseOverPoint != -1)
		{
			erg = Event_DeletePoint(m_mouseOverPoint);
			calculateMouseOver();
		}
		else if (m_mouseOverLine != -1)
		{
			erg = Event_DeleteLine(m_mouseOverLine);
			calculateMouseOver();
		}
	}
	else
	{
		if(m_mouseOverLine == -1 && m_mouseOverPoint == -1){

			PickedPoint pos;
			if(!PickPoint(e->pos(), &pos))
				erg = false;
			else{
				// Send Event to subclasses
				erg = Event_NewPoint(pos);

				calculateMouseOver();
				
			}
		}

		else if(m_mouseOverPoint != -1){
			erg = Event_PointClicked(m_mouseOverPoint);
		}

		else if(m_mouseOverLine != -1){
			erg = Event_LineClicked(m_mouseOverLine);
		}

		calculateMouseOver();

		if(m_mouseOverPoint != -1){
			m_mousePointGrabbed = m_mouseOverPoint;
		}
	}	


	m_Mutex->Unlock();

	return true;
}

//----------------------------------------------------------------------------
bool
OpenGLPointPickerEntity::RightMouseReleased(QMouseEvent *e)
{
	m_Mutex->Lock();

	
	setMousePos(e->pos());

	bool changed = false;
	if(m_mousePointGrabbed != -1)
		if(Event_PointReleased(m_mousePointGrabbed))
			changed = true;
	
	
	m_mousePointGrabbed = -1;

	m_Mutex->Unlock();
	return changed;
}

//----------------------------------------------------------------------------
void
OpenGLPointPickerEntity::Reset(){
	m_Mutex->Lock();
	Event_ResetAll();
	m_Mutex->Unlock();
}


//----------------------------------------------------------------------------
bool
OpenGLPointPickerEntity::Event_NewPoint(PickedPoint point){
	m_PointList.append(point);
	int len = m_PointList.length();
	if(len >= 2){
		m_LineList.append(PickedLine(&m_PointList[len-1], &m_PointList[len-2]));
	}
	emit PointPickerChangedSignal();
	return true;
}


//----------------------------------------------------------------------------
bool
OpenGLPointPickerEntity::unProject(QPoint mousePos, QVector3D *res){
	
	if(res == NULL){
		itkExceptionMacro(<<"ERROR: unApply: nullpointerexception");
	}
	
	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)mousePos.x();
	winY = (float)viewport[3] - (float)mousePos.y();

	
	
	// read depth from depth buffer
	if(!m_Render3D && m_2DPickInvalidPoints)
		winZ = 0.5f;
	else
		glReadPixels( int(winX), int(winY), 1, 1, GL_DEPTH_COMPONENT, GL_FLOAT, &winZ );

	

	// if depth not infinity, calculate point
	if (winZ == 1.0f){
		return false;
	} 

	//printf("winZ: %f\n", winZ);

	if (GLU_TRUE != gluUnProject( winX, winY, winZ, modelview, projection, viewport, &posX, &posY, &posZ))
		itkExceptionMacro(<<"gluUnproject");

	res->setX(posX);
	res->setY(posY);
	res->setZ(posZ);

	return true;
}



//----------------------------------------------------------------------------
bool
OpenGLPointPickerEntity::Project(QVector3D vec, QVector3D *res){
	if(res == NULL){
		itkExceptionMacro(<<"ERROR: unApply: nullpointerexception");
	}
	
	if(parentWidget == NULL){
		itkExceptionMacro(<<"ERROR: parentWidget not set!");
	}
	parentWidget->makeCurrent();

	GLint viewport[4];
	GLdouble modelview[16];
	GLdouble projection[16];
	
	// read matrices
	glGetDoublev( GL_MODELVIEW_MATRIX, modelview );
	glGetDoublev( GL_PROJECTION_MATRIX, projection );
	glGetIntegerv( GL_VIEWPORT, viewport );

	GLdouble winX, winY, winZ;

	if (GLU_TRUE != gluProject(vec.x(), vec.y(), vec.z(), modelview, projection, viewport,  &winX, &winY, &winZ))
		itkExceptionMacro(<<"gluProject");

	res->setX(winX);
	res->setY((float)viewport[3] - winY);
	res->setZ(winZ);

	return true;
}

//----------------------------------------------------------------------------
bool
OpenGLPointPickerEntity::Project2D(QVector2D vec, QVector2D *res){
	QVector3D res3D;
	if(!Project(QVector3D(vec.x(), vec.y(), 0.0f), &res3D)) return false;
	res->setX(res3D.x());
	res->setY(res3D.y());
	return true;
}

//----------------------------------------------------------------------------
bool
OpenGLPointPickerEntity::PickPoint(QPoint PointPickCoordinates, PickedPoint *res){
	
	if(PointPickCoordinates == QPoint(-1, -1)) return false;
	
	QVector3D unProjected;
	
	if(!unProject(PointPickCoordinates, &unProjected)) return false;

	QVector2D pos2D;
	QVector3D pos3D;

	const ritk::OpenGLRImageEntity *RImageWidget = dynamic_cast<const ritk::OpenGLRImageEntity *> (parentWidget->GetPrimaryEntity());
	ritk::RImageF2::ConstPointer rImg = NULL;
	
	if(RImageWidget != NULL){
		rImg = RImageWidget->getCurrentFrame();
	}

	if(m_Render3D){
		pos3D = unProjected;

		//TODO: calculate 2D Point
		pos2D = QVector2D(0.0f, 0.0f);
		
	} else {
		if(m_2DRelativeCoords){
			pos2D.setX(unProjected.x() / m_ImgDimensions.x());
			pos2D.setY(unProjected.y() / m_ImgDimensions.y());
		} else {
			pos2D.setX(unProjected.x());
			pos2D.setY(unProjected.y());
		}		

		if(rImg.IsNotNull()){
			
			// calculate 3D coords from 2D position and WorldCoordimage
			
			int SizeX = rImg->GetWorldCoordImage()->GetBufferedRegion().GetSize()[0];
			int SizeY = rImg->GetWorldCoordImage()->GetBufferedRegion().GetSize()[1];

			// interpolate
			QPoint p[4];
			p[0] = QPoint((int)(unProjected.x() - 0.5f),(int)(unProjected.y() - 0.5f));
			p[1] = QPoint((int)(unProjected.x() - 0.5f),(int)(unProjected.y() + 0.5f));
			p[2] = QPoint((int)(unProjected.x() + 0.5f),(int)(unProjected.y() - 0.5f));
			p[3] = QPoint((int)(unProjected.x() + 0.5f),(int)(unProjected.y() + 0.5f));
			

			pos3D = QVector3D(0,0,0);
			float sumWeights = 0.0f;

			for(int i = 0; i < 4; i++){
				
				if(p[i].x() < 0 || p[i].y() < 0 || p[i].x() >= SizeX || p[i].y() >= SizeY)
					continue;
				
				RImageF2::WorldCoordType wc = rImg->GetWorldCoordImage()->GetBufferPointer()[(SizeY - 1 - p[i].y()) * SizeX + p[i].x()];
			
				float dx = 1.0f - abs(p[i].x() + 0.5f - unProjected.x());
				float dy = 1.0f - abs(p[i].y() + 0.5f - unProjected.y());
				
				float d = dx * dy;
				
				pos3D += QVector3D(d * wc[0], d * wc[1], d * wc[2]);

				sumWeights += d;
				
			}

			if(sumWeights != 0.0f){

				pos3D.setX(pos3D.x() / sumWeights);
				pos3D.setY(pos3D.y() / sumWeights);
				pos3D.setZ(pos3D.z() / sumWeights);

			}

		} else {
			//TODO: calculate 3D Point
			pos3D = QVector3D(0.0f, 0.0f, 0.0f);
		}
	}
	
	res->coord2D = pos2D;
	res->coord3D = pos3D;

	return true;

}


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


} // namespace ritk
