#include "RenderUtils.h"



int delta = 0;

void *font = GLUT_BITMAP_TIMES_ROMAN_24;

char s[1000];



char buff1[8];

void SaveAllBuffers(int w, int h, int num) {



	int thesize = sizeof(char) * w * h * 3;

	char* pixels = (char*)malloc( thesize );

	CvMat* img = cvCreateMat(h,w,CV_8UC3);



	int initWindowID = glutGetWindow();

	for( int idx = 0; idx < num; idx++ ) {

		_itoa_s(idx, buff1,8, 10 );

		string end(buff1);

		glutSetWindow(idx+1);

		glReadPixels( 0, 0, w, h, GL_RGB, GL_UNSIGNED_BYTE, pixels );

		int k = 0;

		for( int i = h-1; i >= 0; i-- ) {

			for( int j = 0; j < w; j++ ) {

				uchar r = (uchar) pixels[k];

				uchar g = (uchar) pixels[k+1];

				uchar b = (uchar) pixels[k+2];

				CvScalar scalar = cvScalar(b,g,r);

				cvSet2D(img,i,j,scalar);

				k+=3;

			}

		}

		string name = "ReadBufferWindow";

		name.append(end);

		string fname = ".png";

		name.append(fname);

		cvSaveImage( name.c_str(), img );

		glFinish();

		int breakhere = 1;



	}

	cvReleaseMat( &img );

	free( pixels );

	glutSetWindow(initWindowID);



}



void TestReadBuffer(int h, int w)

{

	int thesize = sizeof(char) * w * h * 3;

	char* pixels = (char*)malloc( thesize );

	glReadPixels( 0, 0, w, h, GL_RGB, GL_UNSIGNED_BYTE, pixels );

	IplImage* img = cvCreateImage( cvSize(w,h), IPL_DEPTH_8U, 3 );

	cvZero(img);

	int step       = img->widthStep/sizeof(uchar);

	int channels   = img->nChannels;

	uchar* data    = (uchar *)img->imageData;

	

	for( int k = 0; k < thesize; k+=3 ) {

		int i = h - k / 3 / h - 1;

		int j = (k / 3) % h;

		int r = (int) pixels[k];

		int g = (int) pixels[k+1];

		int b = (int) pixels[k+2];

		data[i*step+j*channels+0]=b; // B

		data[i*step+j*channels+1]=g; // G

		data[i*step+j*channels+2]=r; // R

		int breakehre = 1;

	}

	cvSaveImage( "TestReadBuffer.png", img );

	cvReleaseImage( &img );

	free( pixels );

	int breakhere = 1;

}



void DrawAxes(vector<float>& size)

{

	glPushMatrix();

	// Draw and Label Axes

	glScalef(size[0],size[1],size[2]);

    glColor3f (1.0, 1.0, 1.0);

	glBegin(GL_LINES);

                glVertex3f(-1.8f, 0.0f,0.0f);

                glVertex3f(1.8f, 0.0f,0.0f);

    glEnd();

    glBegin(GL_LINES);

                glVertex3f(0.0f, -1.8f,0.0f);

                glVertex3f(0.0f, 1.8f,0.0f);

    glEnd();

    glBegin(GL_LINES);

                glVertex3f(0.0f, 0.0f, -1.8f);

                glVertex3f(0.0f, 0.0f, 1.8f);

    glEnd();



    sprintf(s,"+z");

    outputCharacter(0.0f, 0.0f, 1.98f, s);

    sprintf(s,"-z");

    outputCharacter(0.0f, 0.0f, -1.98f, s);

    sprintf(s,"+x");

    outputCharacter(1.9f, 0.0f, 0.0f, s);

    sprintf(s,"-x");

    outputCharacter(-1.9f, 0.0f, 0.0f, s);

    sprintf(s,"+y");

    outputCharacter(0.0f, 1.9f, 0.0f, s);

    sprintf(s,"-y");

    outputCharacter(0.0f, -1.99f, 0.0f, s);

	glPopMatrix();

}



void DrawCalibrationPoints(const vector<float>& ptsxyz) {

	glPushMatrix();

	glColor3f (1.0, 0.0, 1.0);

	

	int len = ptsxyz.size();



	for( int i = 0; i < len; i+=3 ) {

		sprintf(s,"*");

		float ptx = ptsxyz[i];

		float pty = ptsxyz[i+1];

		float ptz = ptsxyz[i+2];

		outputCharacter(ptx, pty, ptz, s);

	}

	glPopMatrix();

}



void outputCharacter(float x, float y, float z, char *string) {

  int len, i;

  glRasterPos3f(x, y, z);

  len = (int) strlen(string);

  for (i = 0; i < len; i++) {

    glutBitmapCharacter(font, string[i]);

  }

}



char buff[32];

char frm[8];

IplImage* ReadImgByNum(const string& basename, const string& end, int iFrame, int numChars) {

	

		

	_itoa_s( iFrame, buff, 20 );

	int num = 0; // number of nonzero entries

	char val = buff[num];

	while( val != 0 && num < numChars) {

		num++;

		val = buff[num];

	}

	int idx = 0;

	for( int i = 0; i < numChars; i++ ) { frm[i] = '0'; }

	for( int i = numChars-num; i<numChars; i++) { 

		frm[i] = buff[i-numChars+num]; 

	}	

	string csFrame;

	csFrame.assign( frm, numChars );



	string filename = basename;

	filename.append(csFrame);

	filename.append(end);



	IplImage* img = cvLoadImage( filename.c_str() );

	return img;



}