#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;
	float intensity = 0.0f;
	int black = 1;
	for(int i = 0;i < rows;i++)
	{
		x = x0;
		black = i%2;
		intensity = black > 0?0.0f:1.0f;

		for(int j = 0;j < cols;j++)
		{
			GL::DrawRectangle(x,y,x+size,y+size,intensity);
			x+= size;
			intensity = intensity>0?0.0f:1.0f;
		}
		y+= size;
	}
}

void PROJ::GL::DrawChackerboard(int rows,int cols,std::vector<geometry::Vector2Df> corners)
{
	int black = 1;
	float intensity = 0.0f;
	for(int i = 0;i < rows;i++)
	{
		black = i%2;
		intensity = black > 0?0.0f:1.0f;
		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(),intensity);
			intensity = intensity>0?0.0f:1.0f;
		}
	}
}

void PROJ::GL::DrawRectangle(int x1,int y1,int x2,int y2,float intensity)
{
	glColor3f(intensity,intensity,intensity);
	glRecti(x1,y1,x2,y2);
}

void PROJ::GL::DrawRectangle(int x1,int y1,int x2,int y2,float r,float g,float b)
{
	glColor3f(r,g,b);
	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_MAG_FILTER,GL_LINEAR);// Lineární filtrování
	//glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_NEAREST);
	glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_LINEAR);
	//glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_LINEAR_MIPMAP_LINEAR);
	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 a textured mesh.
 * @param points Four points in order Upper left,Upper right,Bottom right and Bottom left corner
 */
void PROJ::GL::DrawMesh(int rows, int cols,std::vector<geometry::Vector2Df>* meshPoints,std::vector<geometry::Vector2Df>* texturePoints)
{
	glPushMatrix();
	glTranslatef(0.0, 0.0, zOffset);
	glColor3f(1.0f,1.0f,1.0f);

	glBegin(GL_QUADS);

		for(int i = 0;i < rows-1;i++)
		{
			for(int j = 0;j < cols-1;j++)
			{
				int index1 = i*cols+j;
				int index2 = index1 + 1;

				int index3 = (i+1)*cols+j;
				int index4 = index3 + 1;

				geometry::Vector2Df texturePoint1 = texturePoints->at(index1);
				geometry::Vector2Df texturePoint2 = texturePoints->at(index2);
				geometry::Vector2Df texturePoint3 = texturePoints->at(index3);
				geometry::Vector2Df texturePoint4 = texturePoints->at(index4);

				geometry::Vector2Df meshPoint1 = meshPoints->at(index1);
				geometry::Vector2Df meshPoint2 = meshPoints->at(index2);
				geometry::Vector2Df meshPoint3 = meshPoints->at(index3);
				geometry::Vector2Df meshPoint4 = meshPoints->at(index4);

				glTexCoord2d(texturePoint1.GetX(), texturePoint1.GetY());
				glVertex2d(meshPoint1.GetX(), meshPoint1.GetY());

				glTexCoord2d(texturePoint2.GetX(), texturePoint2.GetY());
				glVertex2d(meshPoint2.GetX(), meshPoint2.GetY());

				glTexCoord2d(texturePoint4.GetX(), texturePoint4.GetY());
				glVertex2d(meshPoint4.GetX(), meshPoint4.GetY());

				glTexCoord2d(texturePoint3.GetX(), texturePoint3.GetY());
				glVertex2d(meshPoint3.GetX(), meshPoint3.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);
		glColor3f(1.0f,1.0f,1.0f); //white color
	}
	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::DrawTextureOnMesh(GLuint *texId,int rows,int cols,std::vector<geometry::Vector2Df>* meshPoints,std::vector<geometry::Vector2Df>* texturePoints)
{
	if(texId)
	{
		glEnable(GL_TEXTURE_2D);
		glBindTexture( GL_TEXTURE_2D, *texId); //bind the texture
		GL::DrawMesh(rows,cols,meshPoints,texturePoints);
		glDisable(GL_TEXTURE_2D);
		glColor3f(1.0f,1.0f,1.0f); //white color
	}
	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::DrawStrips(int x,int y,int width,int height,bool vertical,int stripsInStep,int stripsCount)
{
	float intensityStep = 1.0f/(float)stripsInStep;
	float size = 0;
	if(!vertical)
		size = width/stripsCount;
	else
		size = height/stripsCount;

	float intensity = 1.0f;
	for(int i = 0;i < stripsCount;i++)
	{
		intensity = 1.0f;

		intensity -= (i%stripsInStep)*intensityStep;
		int intensityInt = intensity*255;

		glColor3f(intensity, intensity, intensity);
		if(!vertical)
			glRecti(i*size,y,(i+1)*size,y+height);
		else
			glRecti(x,i*size,x+width,(i+1)*size);
	}
}


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

void PROJ::GL::SetupTransformMatrix(GLfloat * mat)
{
	glMultMatrixf((Matrix4x4&)*mat);
}

void PROJ::GL::DrawIntensityMap(std::vector<geometry::Vector2Df>* cornerPoints,float maxIntensity,float projectorWeight)
{
	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,projectorWeight,0.0f,1.0f);
			glVertex2d(p1.GetX(),p1.GetY());
			glVertex2d(p2.GetX(),p2.GetY());
			glColor4f(maxIntensity,projectorWeight,0.0f,1.0f);//alphaChannel);
			glVertex2f(middlePoint.GetX(),middlePoint.GetY());
		glEnd();
    }
	glDisable(GL_BLEND);
}

//void PROJ::GL::DrawIntensityMap(std::vector<geometry::Vector2Df>* cornerPoints,float maxIntensity,float projectorWeight)
//{
//	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 indexNext = i+1;
//    	if(indexNext >= CORNERS_COUNT)
//    		indexNext = 0;
//    	geometry::Vector2Df p2 = cornerPoints->at(indexNext);
//
//    	int indexPrev = i-1;
//    	if(indexPrev < 0)
//    		indexPrev = CORNERS_COUNT-1;
//    	geometry::Vector2Df p0 = cornerPoints->at(indexPrev);
//
//    	glBegin(GL_QUADS);
//    		//glColor4f(maxIntensity,projectorWeight,0.0f,1.0f);//alphaChannel);
//    		glColor4f(maxIntensity,maxIntensity,maxIntensity,1.0f);//alphaChannel);
//    	    glVertex2f(middlePoint.GetX(),middlePoint.GetY());
//
//
//    	    geometry::Vector2Df pT1 = ((p1-p0)*0.5)+p0;
////    	    if(pT1.GetX() < 0 || pT1.GetY() <0)
////    	    	pT1 = pT1+p0;
////    	    else
////    	    	pT1 = pT1+p0;
//    	    glColor4f(0.5f,0.5f,0.5f,1.0f);
//    	    glVertex2f(pT1.GetX(),pT1.GetY());
//    	    glColor4f(0.0f,0.0f,0.0f,1.0f);
//    	    glVertex2f(p1.GetX(),p1.GetY());
//
//    	    geometry::Vector2Df pT2 = ((p2-p1)*0.5)+p1;
////    	    if(pT2.GetX() < 0 || pT2.GetY() <0)
////				pT2 = pT2+p1;
////			else
////				pT1 = pT1+p0;
////			if(p2 > p1)
////				pT2 =((p2-p1)/2)+p1;
////			else
////				pT2 =((p1-p1)/2)+p2;
//    	    glColor4f(0.5f,0.5f,0.5f,1.0f);
//			glVertex2f(pT2.GetX(),pT2.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;
}

void PROJ::GL::DrawGrid(float x1,float x2,float y1,float y2,int step,GLfloat r,GLfloat g,GLfloat b)
{
	std::vector<geometry::Vector2Df> points;
	for(int k = step;k<=x2;k=k+step)
	{
		points.push_back(geometry::Vector2Df(k,y1));
		points.push_back(geometry::Vector2Df(k,y2));
	}

	for(int j = step;j<=y2;j=j+step)
	{
		points.push_back(geometry::Vector2Df(x1,j));
		points.push_back(geometry::Vector2Df(x2,j));
	}

	glColor3f(r,g,b); //blue color
	for(int t=0;t < points.size();t=t+2)
	{
		geometry::Vector2Df point1 = points.at(t);
		geometry::Vector2Df point2 = points.at(t+1);
		GL::DrawLine(point1.GetX(),point1.GetY(),point2.GetX(),point2.GetY());
	}
	glColor3f(1.0f,1.0f,1.0f); //white
}
