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

	Program:		Range Imaging Toolkit (RITK)
	Module:			ritkOpenGLPointPickerPolygonEntity.cxx
	Version:		$Revision: 2774 $
	Date:			$Date: 2012-03-13 10:52:35 +0100 (Di, 13 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 <QDataStream>
#include <QVariant>
#include <QDockWidget>
#include <QApplication>
#include <QComboBox>

#define INPOLY_LOCAL
#include "ThirdParty/GLee.h"
#include "ThirdParty/inpoly.h"

#include "ritkOpenGLPointPickerPolygonEntity.h"
#include <time.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;		\
	}																	\
}



namespace ritk
{

//----------------------------------------------------------------------------
/// Constructor
OpenGLPointPickerPolygonEntity::OpenGLPointPickerPolygonEntity(){
	//TODO: control unit
	m_ControlUnit = new QDockWidget("PointPicker Polygon Mode");

	glGenTextures(1, &m_PolygonTexture);
	initPolyTexture();
	
	m_PolygonTextureDimensions = QPoint(-1,-1);

	set2DPickInvalidPoints(true);

	m_2DRelativeCoords = true;

	m_mouseOverPolygon = -1;

	//connect(this,	SIGNAL(Point1Changed(QVector3D)),	this->m_ControlUnit,	SLOT(UpdatePoint1(QVector3D)));
	//connect(this,	SIGNAL(Point2Changed(QVector3D)),	this->m_ControlUnit,	SLOT(UpdatePoint2(QVector3D)));
	//connect(this,	SIGNAL(DistanceChanged(float)),		this->m_ControlUnit,	SLOT(UpdateDistance(float)));
	//connect(this->m_ControlUnit, SIGNAL(SizeChanged(float)),	this,	SLOT(SetSphereSize(float)));


}

//----------------------------------------------------------------------------
/// Destructor
OpenGLPointPickerPolygonEntity::~OpenGLPointPickerPolygonEntity(){
	if(m_ControlUnit){
		delete m_ControlUnit;
		m_ControlUnit = NULL;
	}
	
	while(! m_PolygonList.empty()){
		Polygon *pol = m_PolygonList.first();
		m_PolygonList.removeFirst();
		delete pol;
	}

	glDeleteTextures(1, &m_PolygonTexture);

	m_ReadLock = false;

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



//----------------------------------------------------------------------------
/// Set Polygons
void OpenGLPointPickerPolygonEntity::SetPolygons(QList<QList<QVector2D> > &polyList){
	
	m_Mutex->Lock();
	
	
	for(QList<QList<QVector2D> >::iterator polyListIt = polyList.begin(); polyListIt != polyList.end(); polyListIt++){
		m_PolygonList.append(new Polygon());
		for(QList<QVector2D>::iterator polyIt = polyListIt->begin(); polyIt != polyListIt->end(); polyIt++){
			PickedPoint p;
			p.coord3D = QVector3D(0.0f,0.0f,0.0f);
			p.coord2D = *polyIt;
			m_PointList.append(p);
			m_PolygonList.last()->append(&m_PointList.last());
		}
	}

	updateLists();
	updatePolyTexture();
	

	m_Mutex->Unlock();


}

//----------------------------------------------------------------------------
bool
OpenGLPointPickerPolygonEntity::Event_DeletePoint(int index){
	PickedPoint *point = &m_PointList[index];
	Polygon* polygon;
	foreach(polygon, m_PolygonList){
		polygon->removeAll(point);
		if(polygon->empty()){
			m_PolygonList.removeOne(polygon);
			delete polygon;
		}
	}
	m_PointList.removeAt(index);
	updateLists();
	updatePolyTexture();

	return true;
}


//----------------------------------------------------------------------------
bool
OpenGLPointPickerPolygonEntity::Event_DeleteLine(int index){

	if(m_mouseOverPolygon < 0) return false;

	int i = 0;
	Polygon *poly = NULL;
	for(PolygonList::iterator it = m_PolygonList.begin(); it != m_PolygonList.end(); it++, i++){
		if(i == m_mouseOverPolygon){
			poly = *it;
			break;
		}
	}
	if(poly == NULL) return false;
	
	for(int i = 0; i < m_PointList.size(); i++){
		if(poly->contains(&(m_PointList[i]))){
			m_PointList.removeAt(i);
			i--;			
		}
	}
	
	m_PolygonList.removeOne(poly);

	updateLists();
	updatePolyTexture();

	return true;
}

//----------------------------------------------------------------------------
bool
OpenGLPointPickerPolygonEntity::Event_NewPoint(PickedPoint p){
	
	m_PointList.append(p);

	if(m_PolygonList.isEmpty())
		m_PolygonList.append(new Polygon());

	m_PolygonList.last()->append(&m_PointList.last());

	updateLists();

	return true;
}



//----------------------------------------------------------------------------
bool
OpenGLPointPickerPolygonEntity::Event_PointClicked(int index){

	return false;
}

//----------------------------------------------------------------------------
bool
OpenGLPointPickerPolygonEntity::Event_LineClicked(int index){

	PickedPoint newP;
	if(!PickPoint(m_MousePos, &newP)) return false;

	m_PointList.append(newP);

	PickedPoint *newPPtr = &m_PointList.last();

	
	//for(int i = 0; i < index; i++)
	PickedLine line = m_LineList[index];
	
	bool inserted = false;

	Polygon* polygon;
	foreach(polygon, m_PolygonList){
		for(Polygon::iterator it = polygon->begin(); it != polygon->end(); it++){
			if((*it) == line.p1){

				// Check if other point is previous point
				Polygon::iterator prevPoint;
				if(it == polygon->begin())
					prevPoint = polygon->end() - 1;
				else
					prevPoint = it - 1;

				if(*prevPoint == line.p2){
					polygon->insert(it, newPPtr);
					inserted = true;
					break;
				}


				// Check if other point is following point
				Polygon::iterator nextPoint;
				// Check for last item
				if(it == polygon->end() - 1)
					nextPoint = polygon->begin();
				else
					nextPoint = it + 1;
						
				if(*nextPoint == line.p2){
					polygon->insert(nextPoint, newPPtr);
					inserted = true;
					break;
				}
			}
		}
		if(inserted) break;
	}

	if(!inserted){
		m_PointList.removeLast();	
		return false;
	}

	updateLists();
	
	return true;

}


//----------------------------------------------------------------------------
bool
OpenGLPointPickerPolygonEntity::Event_PointReleased(int index){
	updatePolyTexture();

	return true;
}

//----------------------------------------------------------------------------
bool
OpenGLPointPickerPolygonEntity::Event_PointDragged(int index, PickedPoint newPos){
	PickedPoint p = newPos;
	MouseOverPointChanged(&p);
	
	m_PointList[index] = newPos;
	return true;
}


//----------------------------------------------------------------------------
bool
OpenGLPointPickerPolygonEntity::Event_ResetAll(){

	while(! m_PolygonList.empty()){
		Polygon *pol = m_PolygonList.first();
		m_PolygonList.removeFirst();
		delete pol;
	}

	m_PointList.clear();

	updateLists();

	updatePolyTexture();
	return true;
}

//----------------------------------------------------------------------------
void
OpenGLPointPickerPolygonEntity::updateLists(){

	// UPDATE LINE LIST
	m_LineList.clear();
	Polygon * polygon;


	foreach(polygon, m_PolygonList){
		if(polygon->size() > 2){
			m_LineList.append(PickedLine(polygon->first(), polygon->last()));
		}
		Polygon::iterator it = polygon->begin();
		it++;
		while(it != polygon->end()){
			PickedPoint* p1 = *(it-1);
			PickedPoint* p2 = *(it);
			m_LineList.append(PickedLine(p1, p2));
			it++;
		}
	}


	// Check for empty polygons

	PolygonList::iterator it = m_PolygonList.begin();

	while(it != m_PolygonList.end()){
		if((*it)->isEmpty()){
			m_PolygonList.removeAll(*it);
			it = m_PolygonList.begin();
			continue;
		}
		it++;
	}

}

//----------------------------------------------------------------------------
void OpenGLPointPickerPolygonEntity::initPolyTexture(){

	// select our current texture
    glBindTexture( GL_TEXTURE_2D, m_PolygonTexture );

    // no interpolation
    glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST );
	glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST );
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
	glTexEnvi(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_REPLACE);

	GLubyte data[] = {255, 190, 30, 128, 0, 190, 30, 255, 90, 0, 230, 128, 128, 190, 30, 128};

	// initialize texture with some value
	glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, 2, 2, 0, GL_RGBA, GL_UNSIGNED_BYTE, data);

	CHECK_GL_ERROR();

}



//----------------------------------------------------------------------------
void OpenGLPointPickerPolygonEntity::updatePolyTexture(){
	
	if(m_PolygonTextureDimensions == QPoint(-1,-1))
		return;

	int sizex = m_PolygonTextureDimensions.x();
	int sizey = m_PolygonTextureDimensions.y();
	
	// Allocate Texture Data
	GLubyte *data = new GLubyte[sizex*sizey*4];
	
	// Create List from Polygons
	int numPolys = m_PolygonList.count();
	int *polyLen = new int[numPolys];
	float **polyArray = new float *[numPolys];
	PolygonList::iterator polyListIt = m_PolygonList.begin();
	for(int i = 0; i < numPolys; i++){
		polyLen[i] = (*polyListIt)->count();
		// ignore polygon if it contains less than 3 points
		if(polyLen[i] < 3){
			i--;
			numPolys--;
			polyListIt++;
			continue;
		}
		polyArray[i] = new float [2*polyLen[i]];
		Polygon::iterator polyIt = (*polyListIt)->begin();
		for(int j = 0; j < polyLen[i]; j++){
			polyArray[i][2*j] = (*polyIt)->coord2D.x();
			polyArray[i][2*j+1] = (*polyIt)->coord2D.y();
			polyIt++;
		}
		polyListIt++;
	}
	
	

	
	
	for(int y = 0; y < sizey; y++){
		for(int x = 0; x < sizex; x++){
			
			bool inPoly = false;
			//if(numPolys == 0)
			//	inPoly = true;

			float abs_x;
			float abs_y;

			if(m_2DRelativeCoords){
				abs_x = (x + 0.5f) / (float)m_PolygonTextureDimensions.x();
				abs_y = (y + 0.5f) / (float)m_PolygonTextureDimensions.y();
			} else {
				abs_x = x + 0.5f;
				abs_y = y + 0.5f;
			}

			for(int i = 0; i < numPolys; i++){
				if (inpolyf(polyArray[i], polyLen[i], abs_x, abs_y) == 1){
					inPoly = !inPoly;
					//break;
				}
			}

			data[(y*sizex + x) * 4 + 0] = 0;
			data[(y*sizex + x) * 4 + 1] = 0;
			data[(y*sizex + x) * 4 + 2] = inPoly ? 0 : 255;
			data[(y*sizex + x) * 4 + 3] = inPoly ? 0 : 70;

			
		}
	}
	
	// Free memory of PolygonList
	for(int i = 0; i < numPolys; i++){
		delete[] polyArray[i];
	}
	delete[] polyArray;
	
	
	glActiveTexture(GL_TEXTURE0);
	glBindTexture( GL_TEXTURE_2D, m_PolygonTexture );
	glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, sizex, sizey, 0, GL_RGBA, GL_UNSIGNED_BYTE, data);

	CHECK_GL_ERROR();

}



//----------------------------------------------------------------------------
void OpenGLPointPickerPolygonEntity::Render2D(const OpenGLVisualizationWidget *Widget, QVector2D ImgSizeF){

	QPoint ImgSize = ImgSizeF.toPoint();

	m_Mutex->Lock();

	if(m_PolygonTextureDimensions != ImgSize){
		m_PolygonTextureDimensions = ImgSize;
		updatePolyTexture();
	}
	

	glDisable(GL_LIGHTING);
	glEnable(GL_TEXTURE_2D);
	glDepthMask(GL_FALSE);

	glActiveTexture(GL_TEXTURE0);
	glEnable(GL_BLEND);

	glBindTexture( GL_TEXTURE_2D, m_PolygonTexture );

	// dono why, but this line prevents a very annoying bug caused by OpenGLRImageEntity
	glTexEnvi(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_REPLACE);
	
	glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

	
	glBegin(GL_QUADS);
	glTexCoord2f(0.0f, 0.0f); glVertex3f(0.0f, 0.0f, 0.1f);
	glTexCoord2f(1.0f, 0.0f); glVertex3f(ImgSize.x(), 0.0f, 0.1f);
	glTexCoord2f(1.0f, 1.0f); glVertex3f(ImgSize.x(), ImgSize.y(), 0.1f);
	glTexCoord2f(0.0f, 1.0f); glVertex3f(0.0f, ImgSize.y(), 0.1f);
	glEnd();


	glDepthMask(GL_TRUE);
	glDisable(GL_BLEND);
	glDisable(GL_TEXTURE_2D);
	glEnable(GL_LIGHTING);

	m_Mutex->Unlock();

	CHECK_GL_ERROR();

	Superclass::Render2D(Widget, QVector2D(ImgSize));


}


//----------------------------------------------------------------------------
void OpenGLPointPickerPolygonEntity::ReadLock(){

	m_Mutex->Lock();

	m_ReadLockMutex.Lock();
	m_ReadLock = true;
	m_ReadLockMutex.Unlock();

}

//----------------------------------------------------------------------------
void OpenGLPointPickerPolygonEntity::ReadUnlock(){

	m_ReadLockMutex.Lock();
	m_ReadLock = false;
	m_ReadLockMutex.Unlock();

	m_Mutex->Unlock();

}


//----------------------------------------------------------------------------
OpenGLPointPickerPolygonEntity::PolygonList *
OpenGLPointPickerPolygonEntity::GetPolygonList(){

	m_ReadLockMutex.Lock();
	bool readLock = m_ReadLock;
	m_ReadLockMutex.Unlock();

	if(readLock == false)
		itkExceptionMacro(<<"readLock() necessary before calling this function!");
	

	return &m_PolygonList;

}

//---------------------------------------------------------------------------
QPoint OpenGLPointPickerPolygonEntity::GetDimensions(){

	m_ReadLockMutex.Lock();
	bool readLock = m_ReadLock;
	m_ReadLockMutex.Unlock();

	if(readLock == false)
		itkExceptionMacro(<<"readLock() necessary before calling this function!");
	
	if(m_PolygonTextureDimensions == QPoint(-1,-1))
		itkExceptionMacro(<<"m_PolygonTextureDimensions not set yet! Need one frame as input for initialization.");

	return m_PolygonTextureDimensions;

}


//---------------------------------------------------------------------------
void OpenGLPointPickerPolygonEntity::NewPolygon(){
	m_Mutex->Lock();
	m_PolygonList.append(new Polygon());
	m_Mutex->Unlock();
}

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

	int mouseOverPolygonPrev = m_mouseOverPolygon;
	int mouseOverPointPrev = m_mouseOverPoint;

	Superclass::calculateMouseOver();

	PickedPoint *pointOfDecision = NULL;

	if(m_mouseOverPoint != -1)
		pointOfDecision = &(m_PointList[m_mouseOverPoint]);
	else if(m_mouseOverLine != -1)
		pointOfDecision = m_LineList[m_mouseOverLine].p1;
	else
		pointOfDecision = NULL;

	m_mouseOverPolygon = -1;

	if(pointOfDecision != NULL){
		Polygon * poly;
		int i = 0;
		foreach(poly, m_PolygonList){
			if(poly->contains(pointOfDecision)){
				m_mouseOverPolygon = i;
				break;
			}
			i++;
		}
	}

	if(m_mouseOverPolygon != mouseOverPolygonPrev)
		emit MouseOverPolygonChanged(m_mouseOverPolygon);

	PickedPoint pNew;
	if(m_mouseOverPoint >= 0){
		pNew = m_PointList[m_mouseOverPoint];
	}


	if(mouseOverPointPrev != m_mouseOverPoint){
		if(m_mouseOverPoint < 0){
			emit MouseOverPointChanged(NULL);	
		}
		else
		{
			PickedPoint p = m_PointList[m_mouseOverPoint];
			emit MouseOverPointChanged(&p);	
		}
	}


	
}


} //namespace ritk
