#include <iostream>
#include <math.h>
#include <map>
#include <time.h>
#include <complex>

#if defined(__APPLE__) && defined(__MACH__)
#include <GLUT/glut.h>
#else
#include <GL/glut.h>
#endif

#include "Eigen/Dense"

#include "lodepng.h"

#ifndef M_PI
#define M_PI 3.141592653589793
#endif


void Initialize
(const std::string& fname_png,
unsigned int& width, unsigned int& height,
double& u3, double& v3)
{
  // Load file and decode image
  std::vector<unsigned char> image;
  unsigned error = lodepng::decode(image, width, height, fname_png.c_str());
  std::cout << fname_png << " " << error << std::endl;
  if( error != 0 ) return;
//  std::cout<<width<<" "<<height<<std::endl;
  // Texture size must be power of two for the primitive OpenGL version this is written for. Find next power of two.
  int u2 = 1; while (u2 < width){ u2 *= 2; }
  int v2 = 1; while (v2 < height){ v2 *= 2; }
  u3 = (double)width/u2;
  v3 = (double)height/v2;
//  std::cout<<u2<<" "<<v2<<"    "<<u3<<" "<<v3<<std::endl;
  // Make power of two version of the image.
  std::vector<unsigned char> image2;
  image2.resize(u2 * v2*4);
  for (size_t y = 0; y < height; y++){
    for (size_t x = 0; x < width; x++){
      for (size_t c = 0; c < 4; c++){
        image2[4*u2 * y+4*x+c] = image[4*width * y+4*x+c];
      }
    }
  }
  // Enable the texture for OpenGL.
  glEnable(GL_TEXTURE_2D);
//  glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); //GL_NEAREST = no smoothing
//  glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
  glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); //GL_NEAREST = no smoothing
  glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
  glTexImage2D(GL_TEXTURE_2D, 0, 4, u2, v2, 0, GL_RGBA, GL_UNSIGNED_BYTE, &image2[0]);
}



///////////////////////////////////////////////////////////////////////////////

unsigned width, height;
double u3,v3;

int iwin0, iwin1;
double spx, spy;

double Trans[9] = { 1,0,0, 0,1,0, 0,0,1 };
double aP[4][2];
int ipick_pane = -1;
int ipick_corner = -1;

void Translate(double pt[2], const double pf[2])
{
  double s[3] = {pf[0], pf[1], 1};
  double t[3] = {0,0,0};
  for(int i=0;i<3;++i){
    for(int j=0;j<3;++j){
      t[i] += Trans[i*3+j]*s[j];
    }
  }
  pt[0] = t[0]/t[2];
  pt[1] = t[1]/t[2];
}

void heat(double val, double min, double max){
  double avg = (min+max)*0.5;
  double hspan = (max-min)*0.5;
  double r,g,b;
  if( val > max ){
    r = 1;
    g = 0;
    b = 0;
  }
  else if( val < min ){
    r = 0;
    g = 0;
    b = 1;
  }
  else if( val < avg ){
    double t = (val-min)/hspan;
    r = (1-t)*0 + t*0.5;
    g = (1-t)*0 + t*0.5;
    b = (1-t)*1 + t*0.5;
  }
  else{
    double t = (val-avg)/hspan;
    r = (1-t)*0.5 + t*1;
    g = (1-t)*0.5 + t*0;
    b = (1-t)*0.5 + t*0;
  }
  ::glColor3d(r,g,b);
}

/////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////


void myGlutDisplay_Tex(void)
{
// ::glClearColor(0.2f, 0.7f, 0.7f ,1.0f);
	::glClearColor(1.0f, 1.0f, 1.0f ,1.0f);
	::glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);
	::glEnable(GL_DEPTH_TEST);
  
	::glEnable(GL_POLYGON_OFFSET_FILL );
	::glPolygonOffset( 1.1f, 4.0f );
  
  ::glMatrixMode(GL_PROJECTION);
  ::glPushMatrix();
  ::glLoadIdentity();
  {
    int viewport[4];
    ::glGetIntegerv(GL_VIEWPORT, viewport);
    double asp = (double)viewport[2]/viewport[3];
    ::glOrtho(-asp, asp, -1,1, -10, +10);
  }
  ::glMatrixMode(GL_MODELVIEW);
  ::glPushMatrix();
  ::glLoadIdentity();
  ////
  ::glDisable(GL_DEPTH_TEST);
  ::glDisable(GL_LIGHTING);
  ::glDisable(GL_CULL_FACE);
  ::glDisable(GL_BLEND);
  ::glEnable(GL_TEXTURE_2D);
  ////
  /*  
  ::glColor4d(1,1,1, 1.0);
  glBegin(GL_QUADS);
  glTexCoord2d( 0,  0); glVertex2d(aQ[0][0], aQ[0][1]);
  glTexCoord2d(u3,  0); glVertex2d(aQ[1][0], aQ[1][1]);
  glTexCoord2d(u3, v3); glVertex2d(aQ[2][0], aQ[2][1]);
  glTexCoord2d( 0, v3); glVertex2d(aQ[3][0], aQ[3][1]);
  glEnd();
   */
  double aQ[4][2];
  for(int ip=0;ip<4;++ip){ Translate(aQ[ip],aP[ip]); }
  const int ndiv = 16;
  ::glColor4d(1,1,1, 1.0);
  glBegin(GL_QUADS);
  for(int i=0;i<ndiv;++i){
    for(int j=0;j<ndiv;++j){
      double tx0 = (i+0)*u3/ndiv;
      double tx1 = (i+1)*u3/ndiv;
      double ty0 = (j+0)*v3/ndiv;
      double ty1 = (j+1)*v3/ndiv;
      double px0 = (i+0)*(aP[1][0]-aP[0][0])/ndiv + aP[0][0];
      double px1 = (i+1)*(aP[1][0]-aP[0][0])/ndiv + aP[0][0];
      double py0 = (j+0)*(aP[2][1]-aP[1][1])/ndiv + aP[1][1];
      double py1 = (j+1)*(aP[2][1]-aP[1][1])/ndiv + aP[1][1];
      double p0[2] = {px0,py0}; double q0[2]; Translate(q0, p0);
      double p1[2] = {px1,py0}; double q1[2]; Translate(q1, p1);
      double p2[2] = {px1,py1}; double q2[2]; Translate(q2, p2);
      double p3[2] = {px0,py1}; double q3[2]; Translate(q3, p3);
      glTexCoord2d(tx0, ty0); glVertex2dv(q0);
      glTexCoord2d(tx1, ty0); glVertex2dv(q1);
      glTexCoord2d(tx1, ty1); glVertex2dv(q2);
      glTexCoord2d(tx0, ty1); glVertex2dv(q3);
    }
  }
  glEnd();
  ////
  ::glDisable(GL_TEXTURE_2D);
  ::glPointSize(10);
  ::glBegin(GL_POINTS);
  for(int ip=0;ip<4;++ip){
    if( ipick_corner == ip ){ ::glColor3d(1,0,0); }
    else{ ::glColor3d(0,0,0); }
    glVertex2d(aQ[ip][0], aQ[ip][1]);
  }
  ::glEnd();
  ////
  ::glDisable(GL_BLEND);
  ::glDisable(GL_LIGHTING);
  ////
  ::glMatrixMode(GL_PROJECTION);
  ::glPopMatrix();
  ::glMatrixMode(GL_MODELVIEW);
  ::glPopMatrix();
  
  
  ::glBegin(GL_LINES);
  ::glColor3d(1,0,0);
  ::glVertex3d(-100,0,0);
  ::glVertex3d(+100,0,0);
  ::glColor3d(0,1,0);
  ::glVertex3d(0,-100,0);
  ::glVertex3d(0,+100,0);
  ::glEnd();
  
  
  ::glutSwapBuffers();
}


void myGlutDisplay_Mat(void)
{
  // ::glClearColor(0.2f, 0.7f, 0.7f ,1.0f);
  ::glClearColor(1.0f, 1.0f, 1.0f ,1.0f);
  ::glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);
  ::glEnable(GL_DEPTH_TEST);
  
  ::glEnable(GL_POLYGON_OFFSET_FILL );
  ::glPolygonOffset( 1.1f, 4.0f );
  
  ::glMatrixMode(GL_PROJECTION);
  ::glPushMatrix();
  ::glLoadIdentity();
  ::glMatrixMode(GL_MODELVIEW);
  ::glPushMatrix();
  ::glLoadIdentity();
  
  ::glDisable(GL_TEXTURE_2D);

  
  ::glLineWidth(1);
  ::glBegin(GL_LINES);
  ::glColor3d(0,0,0);
  ::glVertex3d(-1,+1/3.0, 0);
  ::glVertex3d(+1,+1/3.0, 0);
  ::glVertex3d(-1,-1/3.0, 0);
  ::glVertex3d(+1,-1/3.0, 0);
  ::glVertex3d(+1/3.0, -1,0);
  ::glVertex3d(+1/3.0, +1,0);
  ::glVertex3d(-1/3.0, -1,0);
  ::glVertex3d(-1/3.0, +1,0);
  ::glEnd();
  
  if( ipick_pane != -1 ){
    int i0 = ipick_pane / 3;
    int j0 = ipick_pane - 3*i0;
    ::glColor3d(1,1,0);
    ::glLineWidth(5);
    double tx = +j0*2.0/3;
    double ty = -i0*2.0/3;
    double aC[4][2] = {
      {tx+-1.0,    ty+1.0/3.0},
      {tx+-1.0/3.0,ty+1.0/3.0},
      {tx+-1.0/3.0,ty+1.0},
      {tx+-1.0,    ty+1.0} };
    ::glBegin(GL_LINE_LOOP);
    for(int ip=0;ip<4;++ip){ ::glVertex2dv(aC[ip]); }
    ::glEnd();
  }
  
  for(int i=0;i<3;++i){
    for(int j=0;j<3;++j){
      double tx = +j*2.0/3;
      double ty = -i*2.0/3;
      double aC[4][2] = {
        {tx+-1.0,    ty+1.0/3.0},
        {tx+-1.0/3.0,ty+1.0/3.0},
        {tx+-1.0/3.0,ty+1.0},
        {tx+-1.0,    ty+1.0} };
      heat(Trans[i*3+j],-2,2);
      ::glBegin(GL_QUADS);
      for(int ip=0;ip<4;++ip){ ::glVertex2dv(aC[ip]); }
      ::glEnd();
    }
  }
  
  ////
  ::glMatrixMode(GL_PROJECTION);
  ::glPopMatrix();
  ::glMatrixMode(GL_MODELVIEW);
  ::glPopMatrix();
  
  ::glutSwapBuffers();
}

void myGlutIdle(){
  ::glutPostRedisplay();
}


void myGlutResize(int w, int h)
{
  glViewport(0, 0, w, h);
	::glutPostRedisplay();
}


void myGlutSpecial(int Key, int x, int y)
{
  switch( Key )
  {
    case GLUT_KEY_UP:
    {
      if( ipick_pane >= 0 && ipick_pane < 9 ){
        Trans[ipick_pane] += 0.1;
      }
      break;
    }
    case GLUT_KEY_DOWN:
    {
      if( ipick_pane >= 0 && ipick_pane < 9 ){
        Trans[ipick_pane] -= 0.1;
      }
      break;
    }
  }
  
  ::glutSetWindow(iwin0);
  ::glutPostRedisplay();
  ::glutSetWindow(iwin1);
  ::glutPostRedisplay();
}

static void CalcInvMat(double* a, const int n, int& info )
{
  double tmp1;
  
  info = 0;
  int i,j,k;
  for(i=0;i<n;i++){
    if( fabs(a[i*n+i]) < 1.0e-30 ){
      std::cout << "zero diagonal " << i << " " << a[i*n+i] << std::endl;
      info = 1;
      return;
    }
    if( a[i*n+i] < 0.0 ){
      info--;
    }
    tmp1 = 1.0 / a[i*n+i];
    a[i*n+i] = 1.0;
    for(k=0;k<n;k++){
      a[i*n+k] *= tmp1;
    }
    for(j=0;j<n;j++){
      if( j!=i ){
        tmp1 = a[j*n+i];
        a[j*n+i] = 0.0;
        for(k=0;k<n;k++){
          a[j*n+k] -= tmp1*a[i*n+k];
        }
      }
    }
  }
}

void setIdentity(double R[9]){
  for(int i=0;i<9;++i){ R[i] = 0; }
  R[0] = 1;
  R[4] = 1;
  R[8] = 1;
}


void Solve(double* x, int n, const double* a, const double* b){
  Eigen::MatrixXd A(n,n);
  for(int i=0;i<n;++i){
    for(int j=0;j<n;++j){
      A(i,j) = a[i*n+j];
    }
  }
  Eigen::VectorXd B(n);
  for(int i=0;i<n;++i){
    B(i) = b[i];
  }
  Eigen::FullPivLU< Eigen::MatrixXd > lu(A);
  Eigen::VectorXd X = lu.solve(B);
  for(int i=0;i<n;++i){
    x[i] = X(i);
  }
}


void myGlutMotion_Tex( int x, int y ){
  GLint viewport[4];
  ::glGetIntegerv(GL_VIEWPORT,viewport);
  const int win_w = viewport[2];
  const int win_h = viewport[3];
  double spx1 = (2.0*x-win_w)/win_h;
  double spy1 = (win_h-2.0*y)/win_h;
  
  if( ipick_corner >= 0 && ipick_corner < 4 ){
    double aQ[4][2];
    for(int ip=0;ip<4;++ip){ Translate(aQ[ip],aP[ip]); }
    aQ[ipick_corner][0] = spx1;
    aQ[ipick_corner][1] = spy1;
    double A[8*8];
    double b[8];
    for(int ip=0;ip<4;++ip){
//      std::cout << ip << " " << aQ[ip][0] << " " << aP[ip][0] << "  " << aQ[ip][1] << " " << aP[ip][1] << std::endl;
      A[(ip*2+0)*8+0] = +aP[ip][0];
      A[(ip*2+0)*8+1] = +aP[ip][1];
      A[(ip*2+0)*8+2] = 1;
      A[(ip*2+0)*8+3] = 0;
      A[(ip*2+0)*8+4] = 0;
      A[(ip*2+0)*8+5] = 0;
      A[(ip*2+0)*8+6] = -aP[ip][0]*aQ[ip][0];
      A[(ip*2+0)*8+7] = -aP[ip][1]*aQ[ip][0];
      ////
      A[(ip*2+1)*8+0] = 0;
      A[(ip*2+1)*8+1] = 0;
      A[(ip*2+1)*8+2] = 0;
      A[(ip*2+1)*8+3] = +aP[ip][0];
      A[(ip*2+1)*8+4] = +aP[ip][1];
      A[(ip*2+1)*8+5] = 1;
      A[(ip*2+1)*8+6] = -aP[ip][0]*aQ[ip][1];
      A[(ip*2+1)*8+7] = -aP[ip][1]*aQ[ip][1];
      ////
      b[ip*2+0] = aQ[ip][0];
      b[ip*2+1] = aQ[ip][1];
    }
    /*
    int info; CalcInvMat(A, 8, info);
    std::cout << info << std::endl;
    for(int i=0;i<8;++i){
      Trans[i] = 0;
      for(int j=0;j<8;++j){
        Trans[i] += A[i*8+j]*b[j];
      }
      std::cout << i << " " << Trans[i] << std::endl;
    }
     */
    Solve(Trans,8,A,b);
    Trans[8] = 1;
  }
  
  spx = spx1;
  spy = spy1;
  
  ::glutSetWindow(iwin0);
  ::glutPostRedisplay();
  ::glutSetWindow(iwin1);
  ::glutPostRedisplay();
}

void myGlutMouse_Tex(int button, int state, int x, int y)
{
//  win.glutMouse(button,state,x,y);
  GLint viewport[4]; ::glGetIntegerv(GL_VIEWPORT,viewport);
  const int win_w = viewport[2];
  const int win_h = viewport[3];
  spx = (2.0*x-win_w)/win_h;
  spy = (win_h-2.0*y)/win_h;
  
  ipick_corner = -1;
  for(int ip=0;ip<4;++ip){
    double q[2];
    Translate(q,aP[ip]);
    double dx = spx-q[0];
    double dy = spy-q[1];
    double len = sqrt(dx*dx+dy*dy);
    if( len < 0.02 ){
      ipick_corner = ip;
      break;
    }
  }

  ipick_pane = -1;
  
  std::cout << "pick corner " << ipick_corner << std::endl;
  
  ::glutSetWindow(iwin0);
  ::glutPostRedisplay();
  ::glutSetWindow(iwin1);
  ::glutPostRedisplay();
}

void myGlutMouse_Mat(int button, int state, int x, int y)
{
  GLint viewport[4]; ::glGetIntegerv(GL_VIEWPORT,viewport);
  const int win_w = viewport[2];
  const int win_h = viewport[3];
  spx = (2.0*x-win_w)/win_h;
  spy = (win_h-2.0*y)/win_h;
  int i0 = (1.0-spy)*1.5;
  int j0 = (spx+1.0)*1.5;
  ipick_pane = i0*3+j0;
  ipick_corner = -1;
  std::cout << ipick_pane << std::endl;
  
  ::glutSetWindow(iwin0);
  ::glutPostRedisplay();
  ::glutSetWindow(iwin1);
  ::glutPostRedisplay();
}

void myGlutKeyboard(unsigned char Key, int x, int y)
{
	switch(Key)
	{
    case 'q':
    case 'Q':
    case '\033':
      exit(0);  /* '\033' ? ESC ? ASCII ??? */
      break;
      ::glutPostRedisplay();
    case 'i':
    {
      setIdentity(Trans);
      break;
    }
    case ' ':
    {
      static int iprob = 0;
      setIdentity(Trans);
      if( iprob == 0  ){ Initialize("mona.png",width,height, u3,v3); }
      if( iprob == 1  ){ Initialize("uot.png",width,height, u3,v3); }
      if( iprob == 2  ){ Initialize("painting.png",width,height, u3,v3); }
      iprob++;
      if( iprob == 3) { iprob = 0; }
      {
        double asp_img = (double)width/height;
        double s = 0.5;
        /*
         aP[0][0] = -asp_img*s;  aP[0][1] = +s;
         aP[1][0] = +asp_img*s;  aP[1][1] = +s;
         aP[2][0] = +asp_img*s;  aP[2][1] = -s;
         aP[3][0] = -asp_img*s;  aP[3][1] = -s;
         */
        aP[0][0] = -asp_img*0;  aP[0][1] = +s;
        aP[1][0] = +asp_img*s;  aP[1][1] = +s;
        aP[2][0] = +asp_img*s;  aP[2][1] = -0;
        aP[3][0] = -asp_img*0;  aP[3][1] = -0;
      }
      break;
    }
  }
  ::glutSetWindow(iwin0);
  ::glutPostRedisplay();
  ::glutSetWindow(iwin1);
  ::glutPostRedisplay();

}



int main(int argc,char* argv[])
{
  glutInit(&argc, argv);
  
  glutInitWindowPosition(100,200);
  glutInitWindowSize(200, 200);
 	glutInitDisplayMode(GLUT_DOUBLE|GLUT_RGBA|GLUT_DEPTH);
  iwin1 = glutCreateWindow("Matrix");
	glutDisplayFunc(myGlutDisplay_Mat);
	glutMouseFunc(myGlutMouse_Mat);
	glutIdleFunc(myGlutIdle);
	glutReshapeFunc(myGlutResize);
	glutKeyboardFunc(myGlutKeyboard);
	glutSpecialFunc(myGlutSpecial);
  
  
  // Initialize GLUT window 3D
  glutInitWindowPosition(300,200);
  glutInitWindowSize(800, 600);
 	glutInitDisplayMode(GLUT_DOUBLE|GLUT_RGBA|GLUT_DEPTH);
  iwin0 = glutCreateWindow("Transformation");
  glutDisplayFunc(myGlutDisplay_Tex);
	glutMouseFunc(myGlutMouse_Tex);
	glutMotionFunc(myGlutMotion_Tex);
	glutKeyboardFunc(myGlutKeyboard);
	glutSpecialFunc(myGlutSpecial);
  
  ////////////////////////
  Initialize("mona.png",width,height, u3,v3);
  {
    double asp_img = (double)width/height;
    double s = 0.5;
    /*
    aP[0][0] = -asp_img*s;  aP[0][1] = +s;
    aP[1][0] = +asp_img*s;  aP[1][1] = +s;
    aP[2][0] = +asp_img*s;  aP[2][1] = -s;
    aP[3][0] = -asp_img*s;  aP[3][1] = -s;
     */
    aP[0][0] = -asp_img*0;  aP[0][1] = +s;
    aP[1][0] = +asp_img*s;  aP[1][1] = +s;
    aP[2][0] = +asp_img*s;  aP[2][1] = -0;
    aP[3][0] = -asp_img*0;  aP[3][1] = -0;
  }

  glutMainLoop();
	return 0;
}


