#include "Helper/GL.h"

using namespace PROJ; 

PROJ::GL::GL()
{
}

PROJ::GL::~ GL()
{
}

void PROJ::GL::DrawChackerboard(int x, int y, int size,int rows,int cols)
{
	int x0 = x;
	//int y0 = y;
	int black = 1;
	for(int i = 0;i < rows;i++)
	{
		x = x0;
		black = i%2;
		for(int j = 0;j < cols;j++)
		{			
			GL::DrawRectangle(x,y,x+size,y+size,black);
			x+= size;
			black = black>0?0:1;
		}
		y+= size;
	}
}

void PROJ::GL::DrawChackerboard(int rows,int cols,std::vector<geometry::Vector2Df> corners)
{
	int black = 1;
	for(int i = 0;i < rows;i++)
	{

		black = i%2;
		for(int j = 0;j < cols;j++)
		{
			int index = (i*rows)+cols;
			GL::DrawRectangle(corners.at(index).GetX(),corners.at(index).GetY(),corners.at(index+1).GetX(),corners.at(index+1).GetY(),black);
			black = black>0?0:1;
		}
	}
}

void PROJ::GL::DrawRectangle(int x1,int y1,int x2,int y2,int black)
{
	if(black)
		glColor3f(0.0f, 0.0f, 0.0f); 
	else
		glColor3f(1.0f, 1.0f, 1.0f); 
	glRecti(x1,y1,x2,y2);
}

/**
 * Loads texture from input OpenCV image.
 * @param image Texture source image.
 * @param texId Texture id.
 * @return True, when texture is loaded.
 */
bool PROJ::GL::LoadTextureFromCvImage(IplImage *image,GLuint * textId,GLint internalFormat,GLenum format)
{
	if (image==NULL) 
		return false;
   	
	glGenTextures(1, textId);

	glBindTexture( GL_TEXTURE_2D, *textId ); //bind the texture to it's array
	glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MAG_FILTER,GL_NEAREST);
	glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_NEAREST);
	//glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP);
	//glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP);

	//glHint(GL_PERSPECTIVE_CORRECTION_HINT,GL_NICEST);
	//glPixelStorei(GL_UNPACK_ALIGNMENT, 1);

	glTexImage2D(GL_TEXTURE_2D, 0, internalFormat, image->width, image->height,0,format, GL_UNSIGNED_BYTE, image->imageData);
	 
	return true;
}

/**
 * Draws a textured quad. 
 * @param x1 Upper left corner x coordinate.
 * @param y1 Upper left corner y coordinate.
 * @param x2 Bottom right corner x coordinate.
 * @param y2 Bottom right corner y coordinate.
 */
void PROJ::GL::DrawQuad(float x1,float y1,float x2,float y2)
{
	glPushMatrix();
	glTranslatef(0.0, 0.0, zOffset); 
		
	glBegin(GL_QUADS);
	glTexCoord2d(0.0f, 0.0f); glVertex2d(x1, y1);
	glTexCoord2d(1.0f, 0.0f); glVertex2d(x2, y1);
	glTexCoord2d(1.0f, 1.0f); glVertex2d(x2, y2);
	glTexCoord2d(0.0f, 1.0f); glVertex2d(x1, y2);
	glEnd();

	glPopMatrix(); 
}; 

/**
 * Draws a textured quad. 
 * @param points Four points in order Upper left,Upper right,Bottom right and Bottom left corner
 */
void PROJ::GL::DrawQuad(std::vector<geometry::Vector2Df>* points)
{
	glPushMatrix();
	glTranslatef(0.0, 0.0, zOffset);
		
	glBegin(GL_POLYGON);

		int i;
		for(i = 0;i < points->size();i++)
		{
			switch(i)
			{
				case 0:
					glTexCoord2d(0.0f, 0.0f);
					break;
				case 1:
					glTexCoord2d(1.0f, 0.0f);
					break;
				case 2:
					glTexCoord2d(1.0f, 1.0f);
					break;
				case 3:
					glTexCoord2d(0.0f, 1.0f);
					break;
				default:
					break;
			}			
			geometry::Vector2Df point = points->at(i);
			glVertex2d(point.GetX(), point.GetY());
		}
	glEnd();

	glPopMatrix(); 
}; 

/**
 * Draws texture given by parameter on the screen
 * @param filename Name of image source file.
 * @param x1 Upper left corner x coordinate.
 * @param y1 Upper left corner y coordinate.
 * @param x2 Bottom right corner x coordinate.
 * @param y2 Bottom right corner y coordinate.
 */
void PROJ::GL::DrawTexture(GLuint *texId,float x1, float y1, float x2,float y2)
{
	if(texId)
	{
		glEnable(GL_TEXTURE_2D);
		glBindTexture( GL_TEXTURE_2D, *texId); //bind the texture
		GL::DrawQuad(x1,y1,x2,y2);
		glDisable(GL_TEXTURE_2D);
	}
	else
	{
		//if the display is red, there is something wrong with capturing 
		std::cerr << "Can't display texture." << std::endl;
		glColor3f(1.0f, 0.0f, 0.0f); 
	}
	return;  
}

/**
 * Draws texture given by parameter on the screen
 * @param filename Name of image source file.
 * @param points Corner points coordinates.
 */
void PROJ::GL::DrawTexture(GLuint *texId,std::vector<geometry::Vector2Df>* points)
{
	if(texId)
	{
		glEnable(GL_TEXTURE_2D);
		glBindTexture( GL_TEXTURE_2D, *texId); //bind the texture
		GL::DrawQuad(points);
		glDisable(GL_TEXTURE_2D);
	}
	else
	{
		//if the display is red, there is something wrong with capturing
		std::cerr << "Can't display texture." << std::endl;
		glColor3f(1.0f, 0.0f, 0.0f);
	}
	return;
}

void PROJ::GL::PrintGLMatrix(GLenum pname,std::string name)
{
	GLfloat mat[matrixMembers];
	glGetFloatv (pname, (float*)mat);

	GL::PrintGLMatrix(mat,name);
}

void PROJ::GL::PrintGLMatrix(GLfloat * mat,std::string name/*bool transposed*/)
{
	if(!mat)
		return;
	std::cout << "************************************" << std::endl;
	std::cout << name << std::endl;
	std::cout << "[" << std::endl;
	//print the matrix

	for(int i=0; i< matrixCols; i++)
	{
		int j = 0;
		std::cout << mat[i] << '\t';
		j += matrixRows;
		std::cout << mat[i+j] << '\t'; //i+4
		j += matrixRows;
		std::cout << mat[i+j] << '\t'; //i+8
		j += matrixRows;
		std::cout << mat[i+j] << std::endl; //i+12
	}

	std::cout << "];" << std::endl;
	std::cout << "##################" << std::endl;
}

GLfloat * PROJ::GL::CreateModelviewMatrix(CvMat * externalMatrix)
{		
	//matrix for axis transformation from OpenCV to OpenGL camera
	CvMat * tmp = cvCreateMat(matrixRows,matrixCols,CV_64FC1);
	cvSetIdentity(tmp);
	cvmSet(tmp,CV_DIAGONAL_1_INDEX,CV_DIAGONAL_1_INDEX,INVERT_VALUE);
	cvmSet(tmp,CV_DIAGONAL_2_INDEX,CV_DIAGONAL_2_INDEX,INVERT_VALUE);

	CvMat * modelviewMatrix =  CALIB::Cvh::MatMul(tmp,externalMatrix);	
	GLfloat * glExternal = GL::MatrixCvToGL(modelviewMatrix);
	

	//OpenCvHelper::PrintCvMatrix(modelviewMatrix,"GL - MODELVIEW");

	cvReleaseMat(&tmp);
	cvReleaseMat(&modelviewMatrix);
	return glExternal;
}

GLfloat * PROJ::GL::MatrixCvToGL(CvMat * mat)
{	
	if(!mat|| mat->cols != matrixCols-1 || mat->rows != matrixRows-1)
	{
		std::cerr << "MatrixCvToGL: Invalid input matrix!" << std::endl;
		return 0;
	}
	
	//get the matrix as a list of floats
	float *matrix = mat->data.fl;
	GLfloat * glMat = new GLfloat[matrixMembers];
	//GLfloat glMat[16];
	//we set it to the default - 0 translation
	//and 1.0 scale for x y z and w
	for(int i = 0; i < matrixMembers; i++){
		if(i % 5 != 0) glMat[i] = 0.0;
		else glMat[i] = 1.0;
	}

	//http://forum.openframeworks.cc/index.php/topic,509.0.html
	//we need to copy these values
	//from the 3x3 2D openCV matrix which is row ordered
	//
	// ie:   [0][1][2] x
	//       [3][4][5] y
	//       [6][7][8] w

	//to openGL's 4x4 3D column ordered matrix
	//        x  y  z  w
	// ie:   [0][3][ ][6]
	//       [1][4][ ][7]
	//		 [ ][ ][ ][ ]
	//       [2][5][ ][8]
	//

	glMat[0]	= (GLfloat)cvmGet(mat,0,0); //0
	glMat[4]	= (GLfloat)cvmGet(mat,0,1); //1

	float aa = (GLfloat)cvmGet(mat,0,2);
	glMat[12]	= (GLfloat)cvmGet(mat,0,2); //2

	glMat[1]	= (GLfloat)cvmGet(mat,1,0); //3
	glMat[5]	= (GLfloat)cvmGet(mat,1,1); //4
	glMat[13]	= (GLfloat)cvmGet(mat,1,2); //5

	glMat[3]	= (GLfloat)cvmGet(mat,2,0); //6
	glMat[7]	= (GLfloat)cvmGet(mat,2,1); //7
	glMat[15]	= (GLfloat)cvmGet(mat,2,2); //8

	//GL::PrintGLMatrix(glMat,"glMat");
	return glMat;
}

CvMat * PROJ::GL::MatrixGLToCv(GLfloat * glMat)
{		
	if(!glMat)
	{
		std::cerr << "MatrixGLToCv: Invalid input matrix!" << std::endl;
		return 0;
	}

	CvMat * mat = cvCreateMat(matrixRows,matrixCols,CV_64FC1);
	
	for(int i = 0;i < matrixRows;i++)
	{
		for(int j = 0;j < matrixCols;j++)
		{
			cvmSet(mat,j,i,glMat[(i*matrixRows)+j]);
		}
	}
	return mat;
}

void PROJ::GL::GetColumn(Matrix4x4 mat,int colNum,geometry::Vector3Dd * vec)
{		
	int index = colNum*matrixRows;
	vec->SetX(mat[index]);
	index++;
	vec->SetY(mat[index]);
	index++;
	vec->SetZ(mat[index]);
}

bool PROJ::GL::MatrixCvToGL(CvMat * mat,GLfloat * m)//Matrix4x4 & m)
{	
	if(!mat|| mat->cols != matrixCols || mat->rows != matrixRows)
	{
		std::cerr << "MatrixCvToGL: Invalid input matrix!" << std::endl;
		return false;
	}
	
	for(int i = 0;i < matrixRows;i++)
	{
		for(int j = 0;j < matrixCols;j++)
		{
			m[(i*matrixRows)+j] = (GLfloat)cvmGet(mat,j,i);
		}
	}
	return true;
}

geometry::Planed * PROJ::GL::CreatePlane(Matrix4x4 m)
{
	geometry::Vector3Dd positionVec,normalVec;
	GL::GetColumn(m,POSITION_VEC_COLUMN_INDEX,&positionVec);
	GL::GetColumn(m,NORMAL_VEC_COLUMN_INDEX,&normalVec);
	
	normalVec.Normalize();
	return new geometry::Planed(normalVec,normalVec.GetDotProduct(positionVec));
}

void PROJ::GL::InitTransformMatrixGLtoCV(Matrix4x4& m)
{
	SetIdentityMatrix(m);
	//set values for transformation between OpenCv and GL coordinate system
	m[GL_DIAGONAL_1_INDEX] = INVERT_VALUE;
	m[GL_DIAGONAL_2_INDEX] = INVERT_VALUE;
}

void PROJ::GL::LoadTransformationMatrix(GLenum mode,Matrix4x4& m)
{
		int matGlMode = 0;
		
		//save matrix mode
		glGetIntegerv(GL_MATRIX_MODE,&matGlMode);
	
		glMatrixMode(mode);		

		//glPushMatrix();
		//glLoadMatrixf(m);
		glMultMatrixf(m);
		//glMultTransposeMatrixd(m);
		
		//reset matrix mode
		glMatrixMode(matGlMode);
}

void PROJ::GL::DrawMarks(std::vector<geometry::Vector2Df>* points)
{
	for(int i = 0;i < points->size();i++)
	{
//		double r = 1.0, g = 1.0, b = 0.0;
//		r = (double)(i % 4);
//		g = (double)((i + 1) % 4);
//		b = (double)((i + 2) % 4);
//		if(r > 1.0) r = 1.0;
//		if(g > 1.0) g = 1.0;
//		if(b > 1.0) b = 1.0;
//		glColor3f(r, g, b);
		GL::DrawMark(points->at(i).GetX(),points->at(i).GetY(),defaultMarkSize);
	}
}

/**
 * Draws a 2D cross at the given position with given size.
 * @param x X position.
 * @param y Y position.
 * @param size Size of the cross.
 */
void PROJ::GL::DrawMark(double x, double y, double size)
{
	//glPushMatrix();
	//glTranslatef(0.0, 0.0, zOffset + 0.01);

	glBegin(GL_LINES);
	glVertex2d((x-size), y);
	glVertex2d((x+size), y);
	glEnd();

	glBegin(GL_LINES);
	glVertex2d(x, (y-size));
	glVertex2d(x, (y+size));
	glEnd();

	//glPopMatrix();
}

void PROJ::GL::DrawPoints(std::vector<geometry::Vector2Df>* points)
{
	for(int i = 0;i < points->size();i++)
	{
		GL::DrawPoint(points->at(i).GetX(),points->at(i).GetY(),defaultPointSize);
	}
}

void PROJ::GL::DrawPoint(double x, double y, double size)
{
	glPointSize(size);
	glBegin(GL_POINTS);
		glVertex2d(x, y);
	glEnd();
}

void PROJ::GL::DrawLine(double x1, double y1,double x2,double y2)
{
	glBegin(GL_LINES);
		glVertex2d(x1, y1);
		glVertex2d(x2, y2);
	glEnd();
}

void PROJ::GL::DrawVector(std::vector<geometry::Vector2Df> points,geometry::Vector2Df imgCamScale,GLenum mode,bool scaleByCameraImg)
{
	if(points.empty())
		return;
	glBegin(mode);
		for(int i = 0;i < points.size();i++)
		{
			float x = points.at(i).GetX();
			float y =  points.at(i).GetY();

			if(scaleByCameraImg)
			{
				x *= imgCamScale.GetX();
				y *= imgCamScale.GetY();
			}
			glVertex2d(x,y);
		}
	glEnd();
	glColor3f(1.0f,1.0f,1.0f);
};

void PROJ::GL::DrawIntensityMap(int x,int y,int w,int h,float maxIntensity)
{
	std::vector<geometry::Vector2Df> points;
	GL::RectToPoins(x,y,w,h,&points);
	GL::DrawIntensityMap(&points,maxIntensity);
}

void PROJ::GL::DrawIntensityMap(std::vector<geometry::Vector2Df>* cornerPoints,float maxIntensity)
{
	if(cornerPoints->size() < CORNERS_COUNT)
		return;
	geometry::Vector2Df middlePoint = cornerPoints->at(2)-cornerPoints->at(0);
	middlePoint = middlePoint * 0.5f;
	middlePoint = middlePoint + cornerPoints->at(0);

	glEnable(GL_BLEND);
    //glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
	glBlendFunc(GL_SRC_ALPHA, GL_ONE);
	//glBlendFunc(GL_ONE, GL_ONE);

    for(int i = 0;i <= CORNERS_COUNT-1;i++)
    {
    	geometry::Vector2Df p1 = cornerPoints->at(i);
    	int index = i+1;
    	if(index >= CORNERS_COUNT)
    		index = 0;
    	geometry::Vector2Df p2 = cornerPoints->at(index);
    	glBegin(GL_TRIANGLES);
			glColor4f(0.0f, 0.0f, 0.0f,1.0f);
			glVertex2d(p1.GetX(),p1.GetY());
			glVertex2d(p2.GetX(),p2.GetY());
			glColor4f(maxIntensity,maxIntensity,maxIntensity,1.0f);//alphaChannel);
			glVertex2d(middlePoint.GetX(),middlePoint.GetY());
		glEnd();
    }
	glDisable(GL_BLEND);
	//glColor3f(1.0f, 1.0f, 1.0f);
	//glColor4f(1.0f, 1.0f, 1.0f,0.0f);

	//glEnable(GL_BLEND);
    //glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

//    glBegin(GL_TRIANGLE_STRIP);
//    glColor3f(0.0f, 0.0f, 0.0f);
//    for(int i = 0;i < CORNERS_COUNT;i++)
//    {
//    	geometry::Vector2Df p = cornerPoints->at(i);
//    	glVertex2d(p.GetX(),p.GetY());
//
//    	if(i == 1)
//    	{
//    		glColor3f(alphaChannel,alphaChannel,alphaChannel);
//    		glVertex2d(middlePoint.GetX(),middlePoint.GetY());
//    		glColor3f(0.0f, 0.0f, 0.0f);
//    	}
//    }
//    glVertex2d(cornerPoints->at(0).GetX(),cornerPoints->at(0).GetY());
//
//    glEnd();
//	glDisable(GL_BLEND);

}

void PROJ::GL::RectToPoins(float x1,float y1,float w,float h,std::vector<geometry::Vector2Df>* points)
{
	points->clear();
	points->push_back(geometry::Vector2Df(x1,y1));
	points->push_back(geometry::Vector2Df(x1+w,y1));
	points->push_back(geometry::Vector2Df(x1+w,y1+h));
	points->push_back(geometry::Vector2Df(x1,y1+h));
}

IplImage * PROJ::GL::TakeScreenshot(float x,float y,float w,float h,bool asGrayscale)
{
	int size = RGBAchannels*w*h;
	unsigned char *  data = new unsigned char[size];
	glReadPixels(x,y,w,h,GL_RGBA,GL_UNSIGNED_BYTE,data);
	IplImage * image = CALIB::Cvh::CreateIplimage(w,h,data,RGBAchannels,IPL_DEPTH_8U,true);
	if(asGrayscale)
	{
		IplImage * gray = CALIB::Cvh::convertRGBtoGray(image);
		cvReleaseImage( &image );
		image = gray;
	}
	return image;
}
