/*   gcc -Wall -lglut -lGL -lGLU GLproj.c -o GLproj   */

#include <GL/glut.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include "PNMImageOffsetable.h"
#include "boundingbox.h"
#include "dst_to_src_pchirp2.h"
#include "motion.h"

static GLenum errCode;
const GLubyte *errString;

/* the only way to communicate to callback?? */
  int M,N;
int ImageWidth=0, ImageHeight=0; 
int TexWidth=0, TexHeight=0, TexBorder=0;
double *ppmIn_buffer=NULL;
float *imgbuffer=NULL;
float angle=0;
float angledelta=2.5;
double ml,nl, mh, nh;
float rot180y[16] = {-1, 0, 0, 0, 0, 1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1};
float rot180x[16] = { 1, 0, 0, 0, 0,-1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1};
float flipx[16] =   { 1, 0, 0, 0, 0,-1, 0, 0, 0, 0,  1, 0, 0, 0, 0, 1}; 
float flipy[16] =   {-1, 0, 0, 0, 0, 1, 0, 0, 0, 0,  1, 0, 0, 0, 0, 1}; 
float rot90CW[16]= {0,-1, 0, 0,  1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 };
float rot90CCW[16]={0,-1, 0, 0,  1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 };
float mymat[16] = {0};

static float scalefactor=1.0;
static float scaleincrement=0.02;
static float aspectratio=0;
static float ud=0;
static float lr=0;
static float io=0;
static float translate_increment=0.005;
static Var params;

void init(void);
void display(void);
void reshape(int w, int h);
void keyboard(unsigned char key, int x, int y);
void spinDisplay(void);
void dbl_buffer_to_float(double *buffer, int width, int height, int nchans,
                         float *out);
void drawboundingbox(void);
void drawidentitybox(void);
void GLboundingbox(float *leftbb,float *rightbb,float *bottombb,float *topbb,
                   float nearplane, float farplane);
  
double round(double x) {
  return floor(x + 0.5);
}

void usage()
{
  fprintf(stderr, "Usage: GLproj [input filename] [output filename]"
                  "[8 projective chirp parameters]");
}

void errcheck() 
{
  if ((errCode = glGetError()) != GL_NO_ERROR) {
    errString = gluErrorString(errCode);
    fprintf (stderr, "OpenGL Error: %s\n", errString);
    exit(1);
  }
}


int main(int argc, char** argv)
{
  char *inFile; 
  char *outFile;
  int i, errCode;
  int max_texture_size = 0;
  int mOut, nOut;
  //double params[8];
  PNMImageOffsetable ppmIn;
  Var p_trans, pout;

  /* parse parameters */
  if( argc != 11 ) {
    usage();
    return 1;
  }
  inFile = argv[1];
  outFile= argv[2];

  /* read input file */
  if( (errCode = PNM_InitFromFile( &ppmIn, inFile ))!=PNM_SUCCESS ) {
    fprintf(stderr, "GLproj: Error reading file %s as input\n", inFile );
    return 1; 
  }
  ImageWidth = ppmIn.xSize; 
  ImageHeight=ppmIn.ySize;
  aspectratio=(float)ImageWidth/(float)ImageHeight;

  fprintf(stderr,"input image is width=%d, height=%d, aspect ratio=%g\n",
          ImageWidth, ImageHeight,aspectratio );

  init_var( &params, "params", 8, 1, INTERNAL_VARIABLE, GREY_SCALE );
  for (i = 0; i < 8; i++) {
    params.data[i] = atof(argv[3+i]);
    if( i==5 ) fprintf(stderr, "params.data[5]=%g\n",params.data[5]);
  }
  /* Compute bounding box of outgoing image */
  if (boundingbox(&params, &ml, &nl, &mh, &nh) < 0) {
    perror( "GLproj: Error computing bounding box" );
    return 1;
  }
  fprintf( stderr, "BBox: (%f, %f) to (%f, %f)\n", ml, nl, mh, nh );

  /* determine size of output image */
  M = PNM_GetYSize( &ppmIn );
  N = PNM_GetXSize( &ppmIn);

  ml = round(M*ml) / (double)M;
  mh = round(M*mh) / (double)M;
  nl = round(N*nl) / (double)N;
  nh = round(N*nh) / (double)N;

  mOut = M * (mh - ml);
  nOut = N * (nh - nl);
  fprintf(stderr, "Output Image will be w=%d h=%d\n", nOut, mOut);

 
  /* get parameters from command line, converted format for OpenGL */ 
  mymat[0] = 1.0/params.data[4]; //a11
  mymat[4] = -params.data[3]; //a12
  mymat[8] = params.data[5]; //b1
  mymat[12] = 0;
  mymat[1] = -params.data[1]; //a12
  mymat[5] = 1.0/params.data[0]; //a22
  mymat[9] = params.data[2]; //b2
  mymat[13] = 0;
  mymat[2] = params.data[7]; //c1
  mymat[6] = params.data[6]; //c2
  mymat[10]= 1;
  mymat[14]= 0;
  mymat[3] = 0;
  mymat[7] = 0;
  mymat[11]= 0;
  mymat[15]= 1;

  /* translate by Ml and Nl in the inverse space */

  init_var( &p_trans, "p_trans", 1, 8, INTERNAL_VARIABLE, GREY_SCALE );
  init_var( &pout, "pout", 1, 8, INTERNAL_VARIABLE, GREY_SCALE );
  p_trans.data[0] = 1;
  p_trans.data[1] = 0;
  p_trans.data[2] = ml;
  p_trans.data[3] = 0;
  p_trans.data[4] = 1;
  p_trans.data[5] = nl;
  p_trans.data[6] = 0;
  p_trans.data[7] = 0;
  pcompose(&params, &p_trans, &pout);
  //Var_Destructor( &params );
  Var_Destructor( &p_trans );
  fprintf( stderr, "final params: " );
  for (i = 0; i < 8; i++)
      fprintf( stderr, "%f ", pout.data[i] );
  fprintf( stderr, "\n" );

  mymat[0] = 1.0/pout.data[4]; //a11
  mymat[4] = -pout.data[3]; //a12
  mymat[8] = pout.data[5]; //b1
  mymat[12] = 0;
  mymat[1] = -pout.data[1]; //a12
  mymat[5] = 1.0/pout.data[0]; //a22
  mymat[9] = pout.data[2]; //b2
  mymat[13] = 0;
  mymat[2] = pout.data[7]; //c1
  mymat[6] = pout.data[6]; //c2
  mymat[10]= 1;
  mymat[14]= 0;
  mymat[3] = 0;
  mymat[7] = 0;
  mymat[11]= 0;
  mymat[15]= 1;

  // Compute bounding box of outgoing image 

  if (boundingbox(&pout, &ml, &nl, &mh, &nh) < 0) {
    perror( "GLproj: Error computing bounding box" );
    return 1;
  }
  fprintf( stderr, "BBox: (%f, %f) to (%f, %f)\n", ml, nl, mh, nh );
  M = PNM_GetYSize( &ppmIn );
  N = PNM_GetXSize( &ppmIn);
/*
  ml = round(M*ml) / (double)M;
  mh = round(M*mh) / (double)M;
  nl = round(N*nl) / (double)N;
  nh = round(N*nh) / (double)N;
*/

  mOut = M * (mh - ml);
  nOut = N * (nh - nl);
  fprintf(stderr, "Output Image will be w=%d h=%d\n", nOut, mOut);


  glutInit(&argc, argv);
  glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB );
  //glutInitWindowSize(ppmIn.xSize, ppmIn.ySize);
  glutInitWindowSize(nOut, mOut);
  //glutInitWindowSize(600, 600);
  glutInitWindowPosition(0, 0);
  glutCreateWindow(argv[0]);
  glGetIntegerv(GL_MAX_TEXTURE_SIZE, &max_texture_size);
  fprintf(stderr, "Maximum texture dimensions is %d\n", max_texture_size);

  /* check if we can just make a large texture for the image. die if not */
  if( ImageWidth > max_texture_size || ImageHeight > max_texture_size ) {
    fprintf(stderr, "Max allowable texture smaller than image\n");
    return 1;
  }
  else {
    /* these must be 2^m+2^b where b={0,1} */
    TexWidth = 1026;
    TexHeight = 1026;
    TexBorder = 1;
  }

  imgbuffer = (float *) malloc(sizeof(float)*4*1024*1024);
  for( i=0;  i<ImageWidth*ImageHeight ; i++ ) {
    imgbuffer[i*3 + 0] = 0.0;
    imgbuffer[i*3 + 1] = 0.0;
    imgbuffer[i*3 + 2] = 1.0;
  }
  dbl_buffer_to_float( ppmIn.data, ppmIn.xSize, ppmIn.ySize, 3, imgbuffer);

  init();
  glutReshapeFunc(reshape);
  glutDisplayFunc(display);
  glutKeyboardFunc (keyboard);
  glutIdleFunc(spinDisplay);

  glutMainLoop();

  return 0;
}

void display(void) 
{
  int i;
  float texdist=0.0;
  float quaddist=1.0;
  /* -1,-1 makes orbits like image */
  float quadxsize=-1.0;  
  float quadysize=-1.0;

  glClear(GL_COLOR_BUFFER_BIT); 
  //this is how MPlayer does it
  /* subImage can be any arbitrary size (not necessarily 2^m) */
  for( i=0; i<ImageHeight; i++ ){
      glTexSubImage2D( GL_TEXTURE_2D,  // target
              0,              // level
              0,              // x offset
              i,              // y offset
              ImageWidth,    // width
              1,              // height
              //(BYTES_PP==4)?GL_RGBA:GL_RGB,        // format
              //GL_LUMINANCE,        // format
              GL_RGB,        // format
              GL_FLOAT, // type
              //&(imgbuffer[(ImageHeight-i)*ImageWidth*3])); // *pixels
              &(imgbuffer[(i)*ImageWidth*3])); // *pixels
              //fprintf(stderr, "%d\r", i);
              //errcheck();
  }
  /* map only the image part of the large texture */
  /* seems to preserve aspect ratio? */

  glBegin(GL_QUADS);
  glTexCoord3f(0.0, 0.0, texdist); 
  glVertex3f(0.0, 0.0, quaddist);

  glTexCoord3f(0.0, (float)((float)ImageHeight/(float)TexHeight),texdist); 
  glVertex3f(0.0, quadysize, quaddist);

  glTexCoord3f((float)((float)ImageWidth/(float)TexWidth), 
               (float)((float)ImageHeight/(float)TexHeight),texdist); 
  glVertex3f(quadxsize, quadysize,quaddist);

  glTexCoord3f((float)((float)ImageWidth/(float)TexWidth), 0.0, texdist); 
  glVertex3f(quadxsize, 0.0,quaddist);
  glEnd();
  glFinish();
 

  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();

  //glTranslatef(0,0,-1);
  //glRotatef(15,1,0,0); //looks like v00r2
  //glRotatef(15,0,1,0); //looks like v00r1
  //glMultMatrixf(mymat);
  //glTranslatef(0,0,1);

  // need to rotate it to display nicely 
  // this seems to relate orbits coordinates to GL coordinates 
  //drawboundingbox();
  //drawidentitybox();
  glScalef(scalefactor,scalefactor,1);

  glTranslatef(lr,ud,io);
  errcheck();
  glutSwapBuffers();
}

void reshape(int w, int h)
{
  float nearplane = 0.5, farplane=1.5;
  float frustscale = 1/nearplane;
  float frustleft,frustright,frustbottom,frusttop;
  glViewport(0, 0, w, h);
  fprintf(stderr, "w=%d, h=%d\n", w, h);
  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  glRotatef(15,1,0,0);
  printf("\nglRotate3f(15,1,0,0)\n"); printProjectionMatrix();
  glLoadIdentity();

  GLboundingbox(&frustleft, &frustright, &frustbottom, 
                &frusttop,nearplane,farplane);

  //nh = nh+0.03;
  fprintf(stderr, "nl=%g nh=%g ml=%g mh=%g\n", nl,nh,ml,mh);
  /* frustum must be shifted when shifts occur.(due to final params trans?)*/
  /* not yet 100% working need to compose all params or zoom???*/

  printf("\nBefore Frustum call:\n");printProjectionMatrix();
/*
  glFrustum(nl/frustscale, nh/frustscale,  
            -mh/frustscale,-ml/frustscale, nearplane, 1.5);
*/
  //glFrustum(-1,1,-1,1, 0.5, 1.5);
  glFrustum(frustleft,frustright,frustbottom,frusttop,nearplane,farplane);
  fprintf(stderr, "Frustum = %g, %g, %g, %g\n",nl/2,nh/2,-mh/2,-ml/2);
  printf("\nAfter Frustum Call\n");printProjectionMatrix();

  //glOrtho(nl, nh, -mh,-ml, nearplane, 1.5); //??

  gluLookAt(0,0,0,0,0, 1,0, 1,0);
  printf("\nAfter LookAt Call\n");printProjectionMatrix();
  glMultMatrixf(mymat);
  printf("\nAfter MulMatrix Call\n");printProjectionMatrix();


  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();
}

void spinDisplay(void)
{
    glutPostRedisplay();
}

void keyboard(unsigned char key, int x, int y)
{
    switch (key) {
        case 27:
        case 'q':
            exit(0);
            break;
        case '+':
            scalefactor=scalefactor+scaleincrement;
            fprintf(stderr, "Scale Factor=%f\n", scalefactor);
            break;
        case '-':
            scalefactor=scalefactor-scaleincrement;
            fprintf(stderr, "Scale Factor=%f\n", scalefactor);
            break;
        case '[':
            angle+=angledelta;
            break;
        case ']':
            angle-=angledelta;
            break;

        case 'h':
            lr+=translate_increment;
            break;
        case 'j':
            ud+=-translate_increment;
            break;
        case 'k':
            ud+=translate_increment;
            break;
        case 'l':
            lr+=-translate_increment;
            break;
    }
    glutPostRedisplay();
}

void init(void)
{
    glClearColor (0.0, 0.0, 1.0, 1.0);
    glShadeModel(GL_FLAT);
    //glShadeModel(GL_SMOOTH);
    glPixelStorei(GL_UNPACK_ALIGNMENT, 1);

    glDisable(GL_BLEND);
    glDisable(GL_DEPTH_TEST);
    glDepthMask(GL_FALSE);
    glDisable(GL_CULL_FACE);
    /* turn on texture mapping */
    glEnable(GL_TEXTURE_2D);

    //glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
    //glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
    /* specify texture wrapping */
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
    //glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_DECAL);

    /* create texture */
    /* 2nd param is addtional levels for mipmapping */
    /* 3rd param is components for blending/modulating */
    /* width and height must have the form 2^m+2b */
    fprintf(stderr, "Creating Texture %dx%d...", TexWidth, TexHeight);
    glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, TexWidth,
            TexHeight, TexBorder, 
            GL_RGB, 
            GL_FLOAT,
            imgbuffer);
    errcheck();
    fprintf(stderr, "Done\n");
    fprintf(stderr, "Initialiazed OpenGL OK\n"); 
}

void dbl_buffer_to_float(double *buffer, int width, int height, int nchans,
                         float *out)
{
  int i;
  for(i=0; i<width*height*nchans; i++ ) {
    out[i] = (float)(buffer[i]/(float)255);
    //out[i] = 1.0;
  }
  return;
}

void drawboundingbox()
{
  float linez=1.0;
  static int printedflag=0;
  glLineWidth(3.0);
  /* top border */
  glBegin(GL_LINES);glVertex3f(nl,ml,linez);glVertex3f(nh,ml,linez); glEnd();
  /* left border */
  glBegin(GL_LINES);glVertex3f(nl,ml,linez);glVertex3f(nl,mh,linez); glEnd();
  /* bottom */
  glBegin(GL_LINES);glVertex3f(nl,mh,linez);glVertex3f(nh,mh,linez); glEnd();
  /* right */
  glBegin(GL_LINES);glVertex3f(nh,ml,linez);glVertex3f(nh,mh,linez); glEnd();
  if( printedflag == 0) {
    fprintf(stderr, "Bounding box GL corners:tl(%g,%g), " 
                    "tr(%g,%g), br(%g,%g), bl(%g,%g)",
                   nl, -ml, nh,-ml, nh,-mh, nl,-mh);
    printedflag=1;
  }
  
}

void drawidentitybox()
{
  float linez=1.0;
  glLineWidth(3.0);
  glBegin(GL_LINES);glVertex3f(0,0,linez);glVertex3f(0,1,linez); glEnd();
  glBegin(GL_LINES);glVertex3f(0,1,linez);glVertex3f(1,1,linez); glEnd();
  glBegin(GL_LINES);glVertex3f(1,1,linez);glVertex3f(1,0,linez); glEnd();
  glBegin(GL_LINES);glVertex3f(1,0,linez);glVertex3f(0,0,linez); glEnd();
}

void printProjectionMatrix()
{
  float pmatrix[16]={0};
  glGetFloatv( GL_PROJECTION_MATRIX, pmatrix);  
  printf("%5f %5f %5f %5f\n", pmatrix[0], pmatrix[4], pmatrix[8], pmatrix[12]);
  printf("%5f %5f %5f %5f\n", pmatrix[1], pmatrix[5], pmatrix[9], pmatrix[13]);
  printf("%5f %5f %5f %5f\n", pmatrix[2], pmatrix[6], pmatrix[10], pmatrix[14]);
  printf("%5f %5f %5f %5f\n", pmatrix[3], pmatrix[7], pmatrix[11], pmatrix[15]);
}


/* matrix multiplication and normalization, tuned especially for the 
   way GL returns matrices and tuned especially to normalize each 
   parameter by W */
void findcorner(float *pmat, float *vec, float *retX, float *retY)
{
  float X=0,Y=0,Z=0,W=0;
  X=pmat[0]*vec[0] + pmat[4]*vec[1] + pmat[8]*vec[2] + pmat[12]*vec[3];
  Y=pmat[1]*vec[0] + pmat[5]*vec[1] + pmat[9]*vec[2] + pmat[13]*vec[3];
  Z=pmat[2]*vec[0] + pmat[6]*vec[1] + pmat[10]*vec[2] + pmat[14]*vec[3];
  W=pmat[3]*vec[0] + pmat[7]*vec[1] + pmat[11]*vec[2] + pmat[15]*vec[3];
  *retX = X/W;
  *retY = Y/W;
  if( Z/W < -1 || Z/W > 1 ) 
    fprintf(stderr,"Warning, frustum clipping detected\n");
}

/* this function uses the projection matrix to determine the bounding box.
 * The orbits bounding box function doesn't work as expected with GL.
 *   (this could just be because I was using it wrong - but believe you
 *    me, I tried - oh how I did try)
 * However, what does seem to work to bound the image is to have a frustum
 * which is [-1 1 1 -1 0.5 1.5] and then figure out where the image
 * plane corners (0,0),...,(-1,-1) are projected to according to 
 * this frustum, then use these coords (normalized device coordinates) to 
 * determine the new Frustum.  the bounding box values are then X/W,Y/W. 
 * Hopefully this will --exactly-- look like pchirp2nocrop.
 */
void GLboundingbox(float *leftbb, float *rightbb, float *bottombb, 
                   float *topbb, float nearplane, float farplane)
{
  float pmatrix[16]={0};
  float vec[4]={0};
  float leftbound=0, rightbound=0, bottombound=0, topbound=0;
  
  *leftbb  = 0.0; 
  *bottombb= 0.0; 
  *topbb   = 0.0;
  *rightbb = 0.0;

  /*save current matrix, make a new one for calcs, then restore the matrix*/
  glPushMatrix();
  glLoadIdentity();
  glFrustum(-1,1,-1,1, nearplane, farplane);
  gluLookAt(0,0,0,0,0, 1,0, 1,0);
  glMultMatrixf(mymat);
  glGetFloatv( GL_PROJECTION_MATRIX, pmatrix);  
  printProjectionMatrix();
  glPopMatrix();

  /* on a sane day, upperleft corner will either bound top or left,
     lowerright corner will either bound bottom or right,
     and so on, so just go through CW making decisions when we can*/ 
  /* upper left corner */
  vec[0] = 0; vec[1] = 0;  vec[2] = 1; vec[3] = 1;
  findcorner(pmatrix, vec, leftbb, topbb );

  /* upper right corner */
  vec[0] = -1; vec[1] =0;  vec[2] = 1; vec[3] = 1;
  findcorner(pmatrix, vec, &rightbound, &topbound);

  /* decide top bound now since know both upperleft/upperright corners now */
  if( topbound > *topbb ) *topbb = topbound;

  /* lower right corner */
  vec[0] = -1; vec[1] =-1;  vec[2] = 1; vec[3] = 1;
  findcorner(pmatrix, vec, rightbb, bottombb);

  /* decide right side boundary now */
  if( rightbound > *rightbb ) *rightbb = rightbound;
  
  /* lower left */ 
  vec[0] = 0; vec[1] =-1;  vec[2] = 1; vec[3] = 1;
  findcorner(pmatrix, vec, &leftbound, &bottombound);

  /* decide bottom */
  if( bottombound < *bottombb ) *bottombb = bottombound;

  /* decide left */
  if( leftbound < *leftbb ) *leftbb = leftbound;

  fprintf(stderr, "Bounding frustum: left/right:(%f %f) bottom/top:(%f %f)\n",
    *leftbb, *rightbb, *bottombb, *topbb);
  return;
}