#include "main.h"
#include <stdlib.h>
#include <stdio.h>
#include <iostream>
#include "geometry.h"
#include <math.h>


enum {
	M_QUIT = 0,
	M_HELP,
	M_READ,
	M_MORPH,
	M_MORPH_ALL,
	M_PLAY,
	M_DISPLAY
};


int  windowWidth;
int  windowHeight;
int imageWidth;
int imageHeight;
int intdisplay=2;

int numLines=0;
const int maxLines=1000;
Tuple lines[3][maxLines][2];




Image* sourceImage  = NULL;
Image* destImage = NULL;
Image* morphImage = NULL;
Image* warp0=NULL;
Image* warp1=NULL;
int sizeMovie=0;



void display();
void sourceDisplay();
void destDisplay();
void intDisplay();
void reshape(int width, int height);
void keyboard(unsigned char key, int x, int y);
void mouse(int button, int state, int x, int y);
void readMorphData();
void morph(double alpha);
void warp(int sourceNumber, Image* warpImage, Image* output);
void changeToCartesian();
void drawLines();

void makeMenu();
void menuFunc(int menuItem);

void makeMovie();
void playMovie();

int main (int argc, char** argv)
{



	// read in the input files
	sourceImage= new Image("source.bmp");
	destImage = new Image("destination.bmp");
	if (
		(sourceImage->getChannels() != destImage->getChannels()) || 
		(sourceImage->getWidth() != destImage->getWidth()) ||
		(sourceImage->getHeight() != destImage->getHeight())
		) {
			 cout << "Input images must be same size and number of channels." << endl;
			 exit(0);
		 }

	imageWidth = sourceImage->getWidth();
	imageHeight = sourceImage->getHeight();

	windowWidth=3*imageWidth+3;
	windowHeight=imageHeight+2;
	readMorphData();

    // set up the window
    glutInit(&argc, &argv[0]); 
    glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE);

	// set up main window
    glutInitWindowPosition(100,100);
    glutInitWindowSize(windowWidth, windowHeight);
    glutCreateWindow("hmc cs155 morphguts tool");
    glutDisplayFunc(display);
    glutReshapeFunc(reshape);
	glutKeyboardFunc(keyboard);
	glutMouseFunc(mouse);


	// set up opengl
	glClearColor(0.0,0.0,0.0,0.0);
    glDisable(GL_DEPTH_TEST);
 
	// make menu
	makeMenu();

    // wait for something to happen
    glutMainLoop();

  return 0;
}

void display ()
{


  // check if there have been any openGL problems
  GLenum errCode = glGetError();
  if (errCode != GL_NO_ERROR) 
  {
      const GLubyte* errString = gluErrorString(errCode);
      cout << "OpenGL error: " << errString << endl;
  }

  // clear the frame buffer
  glClear(GL_COLOR_BUFFER_BIT);

  // draw the image
  if (sourceImage) {
	  sourceDisplay();
  }

  if (destImage) {
	  destDisplay();
  }

  intDisplay();


  drawLines();

  // swap buffers
  glutSwapBuffers();  
}


void sourceDisplay()
{
	// we are drawing the image to the left half of the window (with a one pixel border)
	glRasterPos2i(1,1);
	if (sourceImage) {
		sourceImage->glDrawPixelsWrapper();
	}
}


void destDisplay()
{ 
	glRasterPos2i(3+2*imageWidth, 1);
	if (destImage) {
		destImage->glDrawPixelsWrapper();
	}
}

void intDisplay()
{
	
	glRasterPos2i(2+imageWidth,1);
	if (intdisplay==0 && warp0!=NULL)
		warp0->glDrawPixelsWrapper();
	else if (intdisplay==1 && warp1!=NULL)
		warp1->glDrawPixelsWrapper();
	else if (intdisplay==2 && morphImage!=NULL)
		morphImage->glDrawPixelsWrapper();
	return;
}

	


void drawLines()
{
	// draw source lines

	glColor3f(1,0,1);
	glBegin(GL_LINES);
	for (int i=0;i<numLines;i++)  // for each line
		for (int j=0; j<2; j++) // for each of source & destination
			for (int k=0;k<2;k++) // for each endpoint
				glVertex2i(lines[j][i][k][0]+ 1 + 2*(imageWidth+1)*j,lines[j][i][k][1] +1);
			
	glEnd();

		

	//draw points
	glColor3f(1,0,1);
	glPointSize(5);
	glBegin(GL_POINTS);
	for (int i=0;i<numLines;i++)  // for each line
		for (int j=0; j<2; j++)  // for source & destination
			for (int k=0; k<2; k++) // for each endpoint
				glVertex2i(lines[j][i][k][0]+1 + 2*(imageWidth+1)*j,lines[j][i][k][1]+1);
			
	glEnd();
	return;

}

void reshape (int width, int height) 
{
 
 // we don't let the user reshape the window;  its size is controlled by the image size

  // change the actual window's size
  glutReshapeWindow(windowWidth, windowHeight);

  
  // the lower left corner of the viewport is 0,0
  // the upper right corner is width, height
  glViewport(0, 0, (GLint) windowWidth, (GLint) windowHeight);  

  // setup orthographic projection matrix
  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  gluOrtho2D(0.0, windowWidth, 0.0, windowHeight);

  // default mode should be modelview
  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();
}




void keyboard(unsigned char key, int x, int y)
{ 

	switch(key) {
	

		

		case 'w':
		case 'W':

			cout << "Read the following lines for the source:" << endl;
			for (int i=0;i<numLines;i++)
				cout << lines[0][i][0] << " " << lines[0][i][1] << endl;
			cout << "Read the following lines for the destination:" << endl;
			for (int i=0;i<numLines;i++)
				cout << lines[1][i][0] << " " << lines[1][i][1] << endl;;
			break;

		

		case 'd':
		case 'D':
			menuFunc(M_DISPLAY);
			break;
			


	}

	
}

void mouse(int button, int state, int x, int y)
{

	
}

void motion(int x, int y)
{
	
}

void readMorphData() {
		fstream ifile("morph.dat", fstream::in);
		int numLineStrips;
		ifile >> numLineStrips;
		numLines=0;
		int sizeLines;

		for (int i=0;i<numLineStrips;i++) {
				ifile >> sizeLines;
				for (int j=0;j<sizeLines;j++) {
					if (j<sizeLines-1) {
	
						ifile >> lines[0][numLines][0][0];
						ifile >> lines[0][numLines][0][1];
						ifile >> lines[1][numLines][0][0];
						ifile >> lines[1][numLines][0][1];
						if (j>0) {
							lines[0][numLines-1][1]=lines[0][numLines][0];
							lines[1][numLines-1][1]=lines[1][numLines][0];

						}
						numLines++;  //we 've started a new line
					} //endif
					else {
						ifile >> lines[0][numLines-1][1][0];
						ifile >> lines[0][numLines-1][1][1];
						ifile >> lines[1][numLines-1][1][0];
						ifile >> lines[1][numLines-1][1][1];
					} // endelse
				

				}// end for

			}//end for
			ifile.close();
			//changeToCartesian();
			// add boundaries
			
			lines[0][numLines][0]= lines[1][numLines][0]=Tuple(0,0);
			lines[0][numLines][1]= lines[1][numLines][1]=Tuple(0,imageHeight-1);
			numLines++;
			lines[0][numLines][0]= lines[1][numLines][0]=Tuple(0,imageHeight-1);
			lines[0][numLines][1]= lines[1][numLines][1]=Tuple(imageWidth-1,imageHeight-1);
			numLines++;
			lines[0][numLines][0]= lines[1][numLines][0]=Tuple(imageWidth-1,imageHeight-1);
			lines[0][numLines][1]= lines[1][numLines][1]=Tuple(imageWidth-1,0);
			numLines++;
			lines[0][numLines][0]= lines[1][numLines][0]=Tuple(imageWidth-1,0);
			lines[0][numLines][1]= lines[1][numLines][1]=Tuple(0,0);
			numLines++;
}

void changeToCartesian()
{
	for (int i=0;i<2;i++) 
		for (int j=0; j< numLines; j++) 
			for (int k=0; k<2;k++) 
				lines[i][j][k][1] = imageHeight-1 - lines[i][j][k][1];
}

void morph(double alpha) {
	// compute first warp
	if (warp0==NULL)
		warp0=new Image(sourceImage->getWidth(), sourceImage->getHeight(), sourceImage->getChannels(), sourceImage->getBits());
	if (warp1==NULL)
		warp1=new Image(sourceImage->getWidth(), sourceImage->getHeight(), sourceImage->getChannels(), sourceImage->getBits());
	if (morphImage==NULL)
		morphImage=new Image(sourceImage->getWidth(), sourceImage->getHeight(), sourceImage->getChannels(), sourceImage->getBits());

	// compute intermediate lines

	for (int i=0; i<numLines; i++) {
		lines[2][i][0] = (1-alpha)*lines[0][i][0] + alpha*lines[1][i][0];
		lines[2][i][1] = (1-alpha)*lines[0][i][1] + alpha*lines[1][i][1];
	}

	// compute warp from source
		cout << "Computing first warp."<<endl;
		warp(0,sourceImage,warp0);
		cout << "Computing second war."<<endl;
		warp(1,destImage,warp1);

	// now blend to create morph
		for (int i=0;i<morphImage->getWidth(); i++)
			for (int j=0;j<morphImage->getHeight();j++) {
				Pixel tmp;
				tmp.r = (1-alpha)*warp0->getPixel(i,j,0) + alpha*warp1->getPixel(i,j,0);
				tmp.g = (1-alpha)*warp0->getPixel(i,j,1) + alpha*warp1->getPixel(i,j,1);
				tmp.b = (1-alpha)*warp0->getPixel(i,j,2) + alpha*warp1->getPixel(i,j,2);
				morphImage->setPixel(i,j,tmp);
			}
		return;
}

void warp(int source,Image* warpImage, Image* output) {

	// everything here should be in cartesian coordinates except retrieving & storing the pixel value
	// for each pixel in the output
	int numColumns=output->getWidth();
	int target=0;
	int inc=numColumns/10.0;
	for (int i=0;i<numColumns;i++) {
		//cout << "Working on line " << i<< endl;
		if (i>target) {
			cout << int(float(i)*100.0/float(numColumns)) << "% done " << endl;
			target+=inc;
		}
		for (int j=0;j<output->getHeight();j++) {
			// for each feature line
			Tuple ij=Tuple((float) i+0.5,(float) j+0.5);
			Tuple delta(0,0);
			float wsum=0;
			for (int k=0;k<numLines;k++) {
				// compute u,v
				Tuple pq = lines[2][k][1]-lines[2][k][0];
				float lengthpq=pq.length();
				// we're going to assume that the length is non-zero -- need to check somehwere
				Tuple pij = ij - lines[2][k][0];
				Tuple pqPerpendicular = pq.getPerpendicular();

				float u= pq.dot(pij)/(lengthpq*lengthpq);
				float v= pij.dot(pqPerpendicular)/lengthpq;

				// check it out
				Tuple newij = lines[2][k][0] + u*pq + v*pqPerpendicular*lengthpq;
				double ijerror = (newij-ij).length();


				//compute ijprime
				Tuple pqprime = lines[source][k][1] - lines[source][k][0];
				Tuple pqprimePerpendicular = pqprime.getPerpendicular();
				float lengthpqprime = pqprime.length();
				Tuple ijprime=lines[source][k][0] + u*pqprime + v * pqprimePerpendicular * lengthpqprime;

				// compute weight [length^p /  (a+dist)]^b
				// where length is the length of a line -- we use the average length
				// p is constant in [0,1],  at 0 all lines have same weight and at 1 then
				// longer lines have more weight -- we use 1
				// a is constant -- if close to 0 then the closer a pixel is to a line, the
				// more influence it has -- we use .01
				// dist is the distance from the pixel to the line
				// b is another constant in [.5,2] that affects how weights fall off with distance,
				// we use 1
				Tuple displacement = ijprime-ij;
				float dist;
				if (u>=0 && u<=1) { // if u is along the line, v is the distance to the line
					if (v>0)
						dist=v*lengthpq;
					else
						dist=-v*lengthpq;
				}
				else { // otherwise we need the distance to the closest endpoint
						// we already have the vector to one endpoint: pij
					Tuple tmp=ij-lines[2][k][1];  // this is the distance to the other endpoint
					if (tmp.length() > pij.length())
						dist=pij.length();
					else
						dist=tmp.length();
				}

				// weight is 

				//float weight= sqrt(sqrt(pq.length()+pqprime.length()/2.0))/(.1+dist*dist);
				float weight= 1.0/(1+dist*dist);
				delta += weight * displacement;
				wsum += weight;
			}
			Tuple endPixel = ij + delta/wsum;
			// we use bilinear interpolation to sample the image
			// we need to do this in image coordinates

			int sourceX0, sourceX1;
			double alphaX=0;
			if (endPixel[0] < 0) {
				sourceX0=sourceX1=0;
			}
			else if (endPixel[0]>=imageWidth) {
				sourceX0=sourceX1=imageWidth-1;
			}
			else if ((int)endPixel[0] == endPixel[0])
				sourceX0=sourceX1=endPixel[0];
			else {
				sourceX0=(int) endPixel[0];
				sourceX1=sourceX0+1;
				alphaX=endPixel[0]-sourceX0;
			}
			int sourceY0, sourceY1;
			double alphaY=0;
			double tmp=imageHeight-1-endPixel[1];
			if (tmp<0)
				sourceY0=sourceY1=0;
			else if (tmp>=imageHeight)
				sourceY0=sourceY1=imageHeight-1;
			else if (tmp == (int) tmp)
				sourceY0=sourceY1=tmp;
			else {
				sourceY0 = (int) tmp;
				sourceY1 = sourceY0+1;
				alphaY=tmp-sourceY0;
			}
	
			Pixel black;
			black.b=0;
			black.g=0;
			black.r=0;

			// when we write to the image we have to convert back to image coordinates
			Pixel p0 = warpImage->getPixel_(sourceX0,sourceY0);
			Pixel p1 = warpImage->getPixel_(sourceX0,sourceY1);
			Pixel p2 = warpImage->getPixel_(sourceX1,sourceY0);
			Pixel p3 = warpImage->getPixel_(sourceX1,sourceY1);
			Pixel newPixel;
			newPixel.r = (1-alphaY)*((1-alphaX)*p0.r + alphaX*p2.r)+
						  alphaY*((1-alphaX)*p1.r + alphaX*p3.r);
			newPixel.g = (1-alphaY)*((1-alphaX)*p0.g + alphaX*p2.g)+
						  alphaY*((1-alphaX)*p1.g + alphaX*p3.g);
			newPixel.b = (1-alphaY)*((1-alphaX)*p0.b + alphaX*p2.b)+
						  alphaY*((1-alphaX)*p1.b + alphaX*p3.b);
	

			output->setPixel(i,imageHeight-1-j,newPixel);


		}// end of pixel i,j
	}// end or row i
	cout << endl;
	return;
}


void makeMovie() {

	intdisplay=2;
	int startnum;
	cout << "What is the first frame number?";
	cin >> startnum;
	char filename[13]="morph000.bmp";
	for (int i=0;i<sizeMovie; i++) {
		cout << "Making image " << i << " of " << sizeMovie-1 << endl;
		morph((double)i /(double) (sizeMovie));
		char tmp[4];
		itoa(i+startnum,tmp,10);
		int j=0;
		if (i+startnum>99) {
			filename[5]=tmp[j];
			j++;
		}
		if (i+startnum>9) {
			filename[6]=tmp[j];
			j++;
		}
		filename[7]=tmp[j];
		morphImage->writeBMP(filename);
		display();
	}
}
void makeMenu ()
{



	glutCreateMenu(menuFunc);
	glutAddMenuEntry( "Read data",				M_READ);
	glutAddMenuEntry( "Morph",					M_MORPH);
	glutAddMenuEntry( "Display next <D>",		M_DISPLAY);
	glutAddMenuEntry( "Create morph sequence",			M_MORPH_ALL);
	glutAddMenuEntry( "Help",					M_HELP);
	glutAddMenuEntry( "Quit",					M_QUIT);


	glutAttachMenu(GLUT_RIGHT_BUTTON);

	return;
}

void menuFunc(int menuItem)
{
	double alpha;
	switch(menuItem) {

		case M_READ:
			readMorphData();
			break;
		case M_MORPH:
			case 'm':
		case 'M':
			cout << "Enter percentage of morph [0,1]: ";
			cin >> alpha;
			if (alpha < 0)
				alpha=0;
			if (alpha > 1)
				alpha=1;
			morph(alpha);
			glutPostRedisplay();
			break;
		case M_DISPLAY:
			intdisplay++;
			if (intdisplay>2)
				intdisplay=0;
			glutPostRedisplay();
			break;
		case M_MORPH_ALL:
			cout << "How many frames? ";
			sizeMovie=0;
			cin >> sizeMovie;
			makeMovie();
			break;
		case M_HELP:
			break;
		case M_QUIT:
			exit(0);
			break;
	}
}