//=============================================================================

#include "correlator.h"

#include "bwImage.h"
#include "rgbImage.h"
#include <GL/glu.h>
#include <GL/glx.h>
#include "xwindow.c"

correlator::correlator( const movieIO &src, bool visible, bool clearFrames )
{
  ::correlator( src.getWidth(), src.getHeight(), visible, clearFrames );
  rgbImage *rgbBuf;
  for ( int i=0; i<src.getLength(); i++ )
  {
    rgbBuf = src.deepRGBFrame( i );
    addImageFrame( i, rgbBuf );
    src.delRGBFrame( rgbBuf );
  }
}

correlator::correlator( int width, int height, bool visible, bool clearFrames )
{
  numFrames_ = 0;
  visible_ = visible;
  clearFrames_ = visible && clearFrames;
  imgWidth_  = width;
  imgHeight_ = height;
  texWidth_  = nextPowerOfTwo(width);
  texHeight_ = nextPowerOfTwo(height);
  int viewportSize = width>height ? width : height;
  
  int visualAttr[] = { GLX_RGBA,//  GLX_DOUBLEBUFFER,
		       GLX_RED_SIZE,          8,
		       GLX_GREEN_SIZE,        8,
		       GLX_BLUE_SIZE,         8,
		       GLX_ALPHA_SIZE,        0,
		       GLX_DEPTH_SIZE,        0,
		       GLX_ACCUM_RED_SIZE,    0,
		       GLX_ACCUM_GREEN_SIZE,  0,
		       GLX_ACCUM_BLUE_SIZE,   0,
		       GLX_AUX_BUFFERS,       0,
		       GLX_SAMPLES_SGIS,      0,
		       None };
  
  createWindowAndContext( &dpy_, &window_, &ctx_, 50, 0,
			  viewportSize, viewportSize,
			  GL_FALSE, NULL, visualAttr, NULL, 
			  "Image Correlation" );

  localCompBuf_ =
    (unsigned char *)malloc( viewportSize*viewportSize*3*
			     sizeof(unsigned char) );
  
  if ( visible )
  {
    glViewport( 0, 0, viewportSize, viewportSize );
    glMatrixMode( GL_PROJECTION );
    glLoadIdentity();
    glOrtho( 0, viewportSize, 0, viewportSize, -1, 1 );
    
    glMatrixMode( GL_MODELVIEW );
    glLoadIdentity();
    
    glTexEnvi( GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_DECAL );

    if ( !clearFrames_ ) glClear( GL_COLOR_BUFFER_BIT );
  }
}


correlator::~correlator()
{
}

bool 
correlator::addImageFrame( int idx, bwImage *frm )
{
  return false;
  numFrames_++;
  glEnable( GL_TEXTURE_2D );

  glGenTextures( 1, &frameHandles_[idx] );
  glBindTexture( GL_TEXTURE_2D, frameHandles_[idx] );
  
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);

  unsigned char *scaledImg =
    (unsigned char*)malloc( texWidth_*texHeight_*sizeof(unsigned char) );
  
  gluScaleImage( GL_LUMINANCE,
		 frm->getWidth(), frm->getHeight(),
		 GL_UNSIGNED_BYTE,
                 frm->rawData(),
		 texWidth_, texHeight_,
		 GL_UNSIGNED_BYTE, scaledImg );
  
  glTexImage2D( GL_TEXTURE_2D, 0, GL_LUMINANCE,
		texWidth_, texHeight_,
		0, GL_RGB, GL_UNSIGNED_BYTE,
		scaledImg );

  free( scaledImg );
}

bool 
correlator::addImageFrame( int idx, rgbImage *frm )
{
  numFrames_++;
  glEnable( GL_TEXTURE_2D );

  glGenTextures( 1, &frameHandles_[idx] );
  glBindTexture( GL_TEXTURE_2D, frameHandles_[idx] );
  
  glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR );
  glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR );

  unsigned char *scaledImg =
    (unsigned char*)malloc( texWidth_*texHeight_*3*sizeof(unsigned char) );
  
  gluScaleImage( GL_RGB,
		 frm->getWidth(), frm->getHeight(),
		 GL_UNSIGNED_BYTE,
                 frm->rawData(),
		 texWidth_, texHeight_,
		 GL_UNSIGNED_BYTE, scaledImg );
  
  glTexImage2D( GL_TEXTURE_2D, 0, GL_RGB,
		texWidth_, texHeight_,
		0, GL_RGB, GL_UNSIGNED_BYTE,
		scaledImg );

  free( scaledImg );

  return true;
}

void correlator::clearWindow()
{
  if ( visible_ ) glClear( GL_COLOR_BUFFER_BIT );
}

float 
correlator::correlation( int frame1,
			 int tri1x1, int tri1y1,
			 int tri1x2, int tri1y2,
			 int tri1x3, int tri1y3,
			 int frame2,
			 int tri2x1, int tri2y1,
			 int tri2x2, int tri2y2,
			 int tri2x3, int tri2y3 ) const
{
  /*
  int twoArea1 =
    (tri1x2*tri1y3 - tri1y2*tri1x3) +
    (tri1x1*tri1y2 - tri1y1*tri1x2) +
    (tri1y1*tri1x3 - tri1x1*tri1y3);
  int twoArea2 =
    (tri2x2*tri2y3 - tri2y2*tri2x3) +
    (tri2x1*tri2y2 - tri2y1*tri2x2) +
    (tri2y1*tri2x3 - tri2x1*tri2y3);
  */
  int max1 = tri1x1;
  int min1 = tri1x1;
  
  if ( max1<tri1x2 ) max1=tri1x2;
  else if ( min1>tri1x2 ) min1=tri1x2;
  if ( max1<tri1y2 ) max1=tri1y2;
  else if ( min1>tri1y2 ) min1=tri1y2;
  
  if ( max1<tri1x3 ) max1=tri1x3;
  else if ( min1>tri1x3 ) min1=tri1x3;
  if ( max1<tri1y3 ) max1=tri1y3;
  else if ( min1>tri1y3 ) min1=tri1y3;

  int max2 = tri2x1;
  int min2 = tri2x1;
  
  if ( max2<tri2x2 ) max2=tri2x2;
  else if ( min2>tri2x2 ) min2=tri2x2;
  if ( max2<tri2y2 ) max2=tri2y2;
  else if ( min2>tri2y2 ) min2=tri2y2;
  
  if ( max2<tri2x3 ) max2=tri2x3;
  else if ( min2>tri2x3 ) min2=tri2x3;
  if ( max2<tri2y3 ) max2=tri2y3;
  else if ( min2>tri2y3 ) min2=tri2y3;

  int winSize;
  if ( max1-min1>max2-min2 )
    winSize = max1-min1;
  else
    winSize = max2-min2;

  // Clear the window
  if ( clearFrames_ ) glClear( GL_COLOR_BUFFER_BIT );
  
  glBindTexture( GL_TEXTURE_2D, frameHandles_[frame1] );
  glBegin( GL_TRIANGLES );
  glTexCoord2f( (float)tri1x1/(float)imgWidth_,
		(float)tri1y1/(float)imgHeight_ );
  glVertex2i( 1, 0 );
  glTexCoord2f( (float)tri1x2/(float)imgWidth_,
		(float)tri1y2/(float)imgHeight_ );
  glVertex2i( winSize, 0 );
  glTexCoord2f( (float)tri1x3/(float)imgWidth_,
		(float)tri1y3/(float)imgHeight_ );
  glVertex2i( winSize, winSize-1 );
  glEnd();
  
  glBindTexture( GL_TEXTURE_2D, frameHandles_[frame2] );
  glBegin( GL_TRIANGLES );
  glTexCoord2f( (float)tri2x1/(float)imgWidth_,
		(float)tri2y1/(float)imgHeight_ );
  glVertex2i( 0, 0 );
  glTexCoord2f( (float)tri2x2/(float)imgWidth_,
		(float)tri2y2/(float)imgHeight_ );
  glVertex2i( 0, winSize );
  glTexCoord2f( (float)tri2x3/(float)imgWidth_,
		(float)tri2y3/(float)imgHeight_ );
  glVertex2i( winSize, winSize );
  glEnd();

  glFlush();
  
  glPixelTransferf( GL_RED_SCALE, 0.3333f );
  glPixelTransferf( GL_GREEN_SCALE, 0.3333f );
  glPixelTransferf( GL_BLUE_SCALE, 0.3333f );
  glReadPixels( 0, 0, winSize, winSize,
		GL_LUMINANCE, GL_UNSIGNED_BYTE,
		localCompBuf_ );
  
  int sum;
  int sumSqr=0;
  for ( int y=1; y<winSize; y++ )
  {
    for ( int x=0; x<y; x++ )
    {
      sum = 
	(int)localCompBuf_[y*winSize+x] -
	(int)localCompBuf_[x*winSize+y];
      sumSqr -= sum*sum;
    }
  }
  /*
  for ( int y=0; y<winSize; y++ )
  {
    for ( int x=0; x<winSize; x++ )
    {
      if (localCompBuf_[y*winSize+x]==0)
	printf("0");else printf("1");
    }
    printf("\n");
  }
  */
  return sumSqr / (winSize*winSize/2.0f) / 65536.0f + 1.0f;
}

// float 
// correlator::correlation( int frame1, int ulx, int uly, int lrx, int lry,
// 			 int frame2, int dx, int dy ) const
// {
//   int width  = lrx-ulx;
//   int height = lry-uly;
  
//   // Clear the window
//   if ( clearFrames_ ) glClear( GL_COLOR_BUFFER_BIT );
  
//   glBindTexture( GL_TEXTURE_2D, frameHandles_[frame1] );
//   glBegin( GL_QUADS );
//   glTexCoord2f( (float)ulx/(float)imgWidth_,
// 		(float)uly/(float)imgHeight_ );
//   glVertex2i( 0, 0 );
//   glTexCoord2f( (float)lrx/(float)imgWidth_,
// 		(float)uly/(float)imgHeight_ );
//   glVertex2i( width, 0 );
//   glTexCoord2f( (float)lrx/(float)imgWidth_,
// 		(float)lry/(float)imgHeight_ );
//   glVertex2i( width, height );
//   glTexCoord2f( (float)ulx/(float)imgWidth_,
// 		(float)lry/(float)imgHeight_ );
//   glVertex2i( 0, height );
//   glEnd();
  
//   glBindTexture( GL_TEXTURE_2D, frameHandles_[frame2] );
//   glBegin( GL_QUADS );
//   ulx += dx;
//   uly += dy;
//   lrx += dx;
//   lry += dy;
//   glTexCoord2f( (float)ulx/(float)imgWidth_,
// 		(float)uly/(float)imgHeight_ );
//   glVertex2i( width, 0 );
//   glTexCoord2f( (float)lrx/(float)imgWidth_,
// 		(float)uly/(float)imgHeight_ );
//   glVertex2i( 2*width, 0 );
//   glTexCoord2f( (float)lrx/(float)imgWidth_,
// 		(float)lry/(float)imgHeight_ );
//   glVertex2i( 2*width, height );
//   glTexCoord2f( (float)ulx/(float)imgWidth_,
// 		(float)lry/(float)imgHeight_ );
//   glVertex2i( width, height );
//   glEnd();

//   glFlush();

  
//   glPixelTransferf( GL_RED_SCALE, 0.3333f );
//   glPixelTransferf( GL_GREEN_SCALE, 0.3333f );
//   glPixelTransferf( GL_BLUE_SCALE, 0.3333f );
//   glReadPixels( 0, 0, 2*width, height,
// 		GL_LUMINANCE, GL_UNSIGNED_BYTE,
// 		localCompBuf_ );
  
//   int sum;
//   int sumSqr=0;
//   int width2 = 2*width;
//   for ( int y=0; y<height; y++ )
//   {
//     for ( int x=0; x<width; x++ )
//     {
//       sum =
// 	(int)localCompBuf_[y*width2+x] -
// 	(int)localCompBuf_[y*width2+width+x];
//       sumSqr -= sum*sum;
//     }
//   }
  
//   return sumSqr / (width*height) / 65536.0f + 1.0f;
// }

void 
correlator::setCorrelationSrc( int frame1,
			       int ulx, int uly, int lrx, int lry )
{
  srcUlx_=ulx;
  srcUly_=uly;
  srcLrx_=lrx;
  srcLry_=lry;
  
  srcWidth_  = lrx-ulx;
  srcHeight_ = lry-uly;
  
  // Clear the window
  if ( clearFrames_ ) glClear( GL_COLOR_BUFFER_BIT );
  
  glBindTexture( GL_TEXTURE_2D, frameHandles_[frame1] );
  glBegin( GL_QUADS );
  glTexCoord2f( (float)ulx/(float)imgWidth_,
		(float)uly/(float)imgHeight_ );
  glVertex2i( 0, 0 );
  glTexCoord2f( (float)lrx/(float)imgWidth_,
		(float)uly/(float)imgHeight_ );
  glVertex2i( srcWidth_, 0 );
  glTexCoord2f( (float)lrx/(float)imgWidth_,
		(float)lry/(float)imgHeight_ );
  glVertex2i( srcWidth_, srcHeight_ );
  glTexCoord2f( (float)ulx/(float)imgWidth_,
		(float)lry/(float)imgHeight_ );
  glVertex2i( 0, srcHeight_ );
  glEnd();
}

float 
correlator::correlateWith( int frame2, int dx, int dy )
{
  glBindTexture( GL_TEXTURE_2D, frameHandles_[frame2] );
  glBegin( GL_QUADS );
  
  glTexCoord2f( (float)(srcUlx_+dx)/(float)imgWidth_,
		(float)(srcUly_+dy)/(float)imgHeight_ );
  glVertex2i( srcWidth_, 0 );
  glTexCoord2f( (float)(srcLrx_+dx)/(float)imgWidth_,
		(float)(srcUly_+dy)/(float)imgHeight_ );
  glVertex2i( 2*srcWidth_, 0 );
  glTexCoord2f( (float)(srcLrx_+dx)/(float)imgWidth_,
		(float)(srcLry_+dy)/(float)imgHeight_ );
  glVertex2i( 2*srcWidth_, srcHeight_ );
  glTexCoord2f( (float)(srcUlx_+dx)/(float)imgWidth_,
		(float)(srcLry_+dy)/(float)imgHeight_ );
  glVertex2i( srcWidth_, srcHeight_ );
  glEnd();

  glFlush();

  
  glPixelTransferf( GL_RED_SCALE, 0.3333f );
  glPixelTransferf( GL_GREEN_SCALE, 0.3333f );
  glPixelTransferf( GL_BLUE_SCALE, 0.3333f );
  glReadPixels( 0, 0, 2*srcWidth_, srcHeight_,
		GL_LUMINANCE, GL_UNSIGNED_BYTE,
		localCompBuf_ );
  
  int sum;
  int sumSqr=0;
  int width2 = 2*srcWidth_;
  for ( int y=0; y<srcHeight_; y++ )
  {
    for ( int x=0; x<srcWidth_; x++ )
    {
      sum =
	(int)localCompBuf_[y*width2+x] -
	(int)localCompBuf_[y*width2+srcWidth_+x];
      sumSqr -= sum*sum;
    }
  }
  
  return sumSqr / (srcWidth_*srcHeight_) / 65536.0f + 1.0f;
}