#include "image2d.h"

image2d::image2d() : image( NULL )
{
  width = height = 0;
}

image2d::~image2d()
{
  del();
}

double image2d::sum()
{
  double s = 0;
  for( int i = 0; i < width*height; i++ )
    s = image[i] + s;
  return s;
}

int image2d::checkint()
{
  double fpart, ipart;
  for( int i = 0; i < height*width; i++ )
    {
      fpart =  modf( image[i], &ipart );
      if( fabs(fpart) > 0.000001 )
        {
          std::cout << "int check failed\n";
          std::cout << image[i] << "\n";
          exit(0);
        }
    }
  return 0;
}

int image2d::logscale()
{
  double min, max;
  min = max = 0;
  for( int i = 0; i < height*width; i++ )
    {
      if( image[i] >= 0 )
        image[i] = log( image[i] + 1 );
      else
        image[i] = -log( 1 - image[i] );
      if( image[i] < min )
        min = image[i];
      if( image[i] > max )
        max = image[i];
    }
  std::cout << "logscale: min max" << min << " " << max << "\n";
  return 0;
}

int image2d::setall( double v )
{
  for( int i = 0; i < height*width; i++ )
    image[i] = v;
  return 0;
}

int image2d::logicaland( image2d &a )
{
  int c;
  c = 0;
  for( int i = 0; i < height; i++ )
    {
      for(int j = 0; j < width; j++ )
        {
          if( a.getpixel(i,j) > 0.5 && getpixel(i,j) > 0.5 )
            {
              setpixel(i,j, 1 );
              c = c + 1;
            }
          else
            setpixel(i,j,0);
        }
    }
  return c;
}

int image2d::logicaland( subimage &a, subimage &b )
{
  create( a.getsizex(), a.getsizey() );
  int c;
  c = 0;
  for( int i = 0; i < height; i++ )
    {
      for(int j = 0; j < width; j++ )
        {
          if( a.getpixel(i,j) > 0.5 && b.getpixel(i,j) > 0.5 )
            {
              setpixel(i,j, 1 );
              c = c + 1;
            }
        }
    }
  return c;
}

int image2d::median( int fsize )
{
  double *values;
  double *data;
  data = new double [height*width];
  values = new double [ (2*fsize+1)*(2*fsize+1) ];
  for( int i = 0; i < height; i++ )
    {
      for( int j = 0; j < width; j++ )
        {
          int c;
          c = getpixelbox( i, j, fsize, values );
          std::sort( values, values+c );
          data[i*width+j] = values[c/2];
        }
    }
  delete [] values;
  delete [] image;
  image = data;
  return 0;
}

int image2d::getpixelbox(int cx, int cy, int r, double *values)
{
  int startx, starty;
  int endx, endy, leny;
  startx = cx-r;
  starty = cy-r;
  endx = cx+r;
  endy = cy+r;
  if( startx < 0 )
    startx = 0;
  if( starty < 0 )
    starty = 0;
  if( endx >= height )
    endx = height-1;
  if( endy >= width )
    endy = width-1;
  leny = endy-starty+1;
  for( int i = startx; i <= endx; i++ )
    memcpy( values+((i-startx)*leny), image+(i*width+starty), leny*sizeof(double) );
  return (endx-startx+1)*leny;
}

int image2d::logscale( double s )
{
  for( int i = 0; i < height*width; i++ )
    {
      if( image[i] >= 0 )
        image[i] = log( (image[i]*s)+1);
      else
        image[i] = -log(1-(image[i]*s));
    }
  return 0;
}

int image2d::logmult( double s )
{
  for( int i = 0; i < height*width; i++ )
    {
      if( image[i] >= 0 )
        image[i] = log( (image[i]*s)+1);
      else
        image[i] = -log(1-(image[i]*s));
    }
  normalizeminmax();
  return 0;
}

double image2d::max()
{
  double v;
  v = image[0];
  for( int i = 1; i < height*width; i++ )
    if( image[i] > v )
      v = image[i];
  return v;
}

int image2d::add( double v )
{
  for( int i = 0; i < height*width; i++ )
    image[i] = image[i] + v;
  return 0;
}

int image2d::multiply( double v )
{
  for( int i = 0; i < height*width; i++ )
    image[i] = image[i] * v;
  return 0;
}

double image2d::min()
{
  double v;
  v = image[0];
  for( int i = 1; i < height*width; i++ )
    if( image[i] < v )
      v = image[i];
  return v;
}

int image2d::gamma(double g)
{
  for( int i = 0; i < height*width; i++ )
    image[i] = pow(image[i], g );
  return 0;
}

int image2d::getshortint( int *d )
{
  for( int i = 0; i < height*width; i++ )
    {
      double fpart, ipart;
      fpart = modf( image[i], &ipart );
      if( fpart > 0.0000001 )
        {
          std::cout << "conversion to integers warning " << image[i] << "\n";
        }
      if( ipart > 16777216 )
        {
          std::cout << "cliping error " << ipart << " " << i << "\n";
          ipart = 16777216;
        }
      d[i] = (int)ipart;
    }
  return 0;
}

int image2d::edgedetect( image2d *im )
{
  image2d smoothed;
  im->create( height, width );
  smoothed.load( this );
  smoothed.gaussian( 2 );
  // apply the sobel filter
  for( int i = 1; i < height-1; i++)
    {
      for( int j = 1; j < width-1; j++)
        {
          double gx, gy;
          gx = ( smoothed.getpixel( i-1, j-1 ) +
                 smoothed.getpixel( i, j-1 ) * 2 + 
                 smoothed.getpixel( i+1, j-1) ) - 
            ( smoothed.getpixel( i-1, j+1 ) +
              smoothed.getpixel( i, j+1 ) * 2 + 
              smoothed.getpixel( i+1, j+1) );
          gy = ( smoothed.getpixel( i-1, j-1 ) +
                 smoothed.getpixel( i-1, j ) * 2 + 
                 smoothed.getpixel( i-1, j+1) ) - 
            ( smoothed.getpixel( i+1, j-1 ) +
              smoothed.getpixel( i+1, j ) * 2 + 
              smoothed.getpixel( i+1, j+1) );
          double g;
          g = sqrt( pow(gx,2) + pow(gy,2) );
          im->setpixel( i, j, g );
        }
    }
  for(int i = 0; i < height; i++)
    {
      im->setpixel(i, 0, 0);
      im->setpixel(i, width-1, 0);
      im->setpixel(i, 1, 0);
      im->setpixel(i, width-2, 0);
    }
  for(int i = 0; i < width; i++)
    {
      im->setpixel(0, i, 0 );
      im->setpixel(height-1, i, 0 );
      im->setpixel(1, i, 0 );
      im->setpixel(height-2, i, 0 );
    }
  std::cout << "done\nbluring edges\n";
  im->gaussian( 7 );
  std::cout << "done\n";
#ifdef __DEBUG__
#ifndef _NOMAGICK_
  Magick::Image imim;
  im->getimage_normalized( imim );
  imim.write( "edges.jpg" );
#endif
#endif
  return 0;
}

int image2d::creategauss( double *g, double sigma1,
                          int pszx, int pszy )
{
  double s1 = sigma1;
  for( double i = 0; i < pszx; i = i + 1 )
    for( double j = 0; j < pszy; j = j + 1 )
      g[ (int)(i * pszy + j) ] = 1/(2*PI*s1)*exp(-(pow(i,2)+pow(j,2))/(2*s1));

  image2d tim;
  tim.loadcopy( g, pszx, pszy );
  Magick::Image mim;
  tim.getimage_normalized( mim );
  char buffer[128];
  sprintf( buffer, "gaussian_%.0f.png", sigma1 );
  mim.write( buffer );
  pszx = pszy;
  return 0;
}


int image2d::creategaussf( double *g, double sigma1, int szx, int szy )
{
  double s1 = -2*pow(sigma1, 2);
  double ii,jj;
  double x, y;
  x = szx;
  y = szy;
  for( double i = 0; i < szx; i = i + 1 )
    {
      for( double j = 0; j < szy; j = j + 1 )
        {
          if( i > szx/2 )
            ii = szx-i;
          else
            ii = i;
          if( j > szy/2 )
            jj = szy-j;
          else 
            jj = j;
          g[ (int)(i * szy + j) ] = exp( s1*x*y*(pow(ii/x,2)+pow(jj/y,2)) );
          if( g[(int)(i * szy + j)] > 1 )
            {
              std::cout << "ERROR gauss function " << i << " " << j << " " <<
                g[(int)(i * szy + j)] << "\n";
            }
        }
    }
  return 0;
}

int image2d::print( fftw_complex *d, int height, int width, double sigma )
{
  image2d tim;
  tim.create( height, width/2 );
  for( int i = 0; i < height; i++ )
    for( int j = 0; j < (width/2+1); j++ )
      tim.setpixel(i, j, log( log(fabs(d[i*(width/2+1)+j][0])*1000+1)+1) );
  Magick::Image mim;
  std::cout << "real part\n";
  tim.getimage_normalized( mim );
  char buffer[512];
  sprintf( buffer, "gaussreal%.0f.png", sigma );
  mim.write( buffer );
  for( int i = 0; i < height; i++ )
    for( int j = 0; j < (width/2+1); j++ )
      tim.setpixel(i, j, log(log(fabs(d[i*(width/2+1)+j][1])*1000+1)+1) );
  std::cout << "img part\n";
  tim.getimage_normalized( mim );
  sprintf( buffer, "gausscomplex%.0f.png", sigma );
  mim.write( buffer );
  return 0;
}

int image2d::dog(double sigma1, double sigma2)
{
  double *dog;
  fftw_complex *dftdog;
  std::cout << "calculating dog " << sigma1 << " " << sigma2
            << " image sz: " << height << "x" << width 
            << "\n";
  dog = (double*) fftw_malloc( sizeof(double)*4*width*height );
  dftdog = (fftw_complex*) fftw_malloc( sizeof(fftw_complex)*4*width*height );

  createdog( dog, sigma1, sigma2, 2*width, 2*height );

  fftw_plan dftg;
  dftg = fftw_plan_dft_r2c_2d( 2*height, 2*width,
                               dog, dftdog, FFTW_ESTIMATE);
  fftw_execute( dftg );

  convolute( dftdog );

  fftw_destroy_plan(dftg);

  fftw_free( dftdog );
  fftw_free( dog );
  for( int i = 0; i < height*width; i++ )
    {
      image[i] = fabs(image[i]);
    }
  return 0;
}

int image2d::gaussian( double *sigmas, image2d **outim, int num )
{
  int pheight, pwidth;
  fftw_complex *OM, *NOM;
  double *g;
  double *oim;

  fftw_plan dftp, idftp;

  pheight = height * 2;
  pwidth = width * 2;
  OM = (fftw_complex*) fftw_malloc( sizeof(fftw_complex)*(pwidth/2+1)*pheight );
  NOM = (fftw_complex*) fftw_malloc( sizeof(fftw_complex)*(pwidth/2+1)*pheight );
  oim = (double*) fftw_malloc( sizeof(double)*pwidth*pheight );
  g = new double[pwidth*pheight];
  std::cout << "padding to " << pheight << " " << pwidth << "\n";
  padzeros(pheight, pwidth);

  for( int i = 0; i < (pheight*(pwidth/2+1)); i++ )
    OM[i][0] = OM[i][1] = NOM[i][0] = NOM[i][1] = 0;
  dftp = fftw_plan_dft_r2c_2d( pheight, pwidth,
                               image, OM, FFTW_ESTIMATE );
  fftw_execute( dftp );
  fftw_destroy_plan( dftp );
  double v = height*width;
  v = 1.0/v;

  crop( pheight/2, pwidth/2);
  for( int i = 0; i < num; i++ )
    {
      std::cout << "gauss: " << sigmas[i] << "\n";
      creategaussf( g, sigmas[i], pheight, pwidth );
      for( int x = 0; x < pheight; x++ )
        {
          for( int y = 0; y < pwidth/2+1; y++ )
            {
              NOM[x*(pwidth/2+1)+y][0] = OM[x*(pwidth/2+1)+y][0]*g[x*pwidth+y];
              NOM[x*(pwidth/2+1)+y][1] = OM[x*(pwidth/2+1)+y][1]*g[x*pwidth+y];
            }
        }
      idftp = fftw_plan_dft_c2r_2d( pheight, pwidth,
                                    NOM, oim, FFTW_ESTIMATE );
      fftw_execute( idftp );
      fftw_destroy_plan( idftp );

      outim[i]->copysubim( oim, 0, 0, height, width, pheight, pwidth );
      outim[i]->multiply( v );
      for( int j = 0; j < height*width; j++ )
        {
          if( outim[i]->image[j] > 1.0001 )
            {
              std::cout << "ERROR range: " << outim[i]->image[j] << "\n";
              exit(0);
            }
        }
    }

  delete [] g;
  fftw_free( oim );
  fftw_free( OM );
  fftw_free( NOM );

  return 0;
}


int image2d::gaussian(double sigma)
{
  double *dog;
  int pheight, pwidth;

  for( int i = 0; i < height*width; i++ )
    {
      if( image[i] > 1.0001 )
        {
          std::cout << "ERROR range1: " << image[i] << "\n";
          exit(0);
        }
      else if( image[i] > 1 )
        image[i] = 1.0;
    }

  pheight = height * 2;
  pwidth = width * 2;
  std::cout << "calculating gaussian " << sigma 
            << " image sz: " << height << "x" << width << "\n";

  dog = (double*) fftw_malloc( sizeof(double)*pwidth*pheight );

  creategaussf( dog, sigma, pheight, pwidth );

  convolute( dog );
  
  fftw_free( dog );
  
  for( int i = 0; i < height*width; i++ )
    {
      if( image[i] > 1.004 )
        {
          std::cout << "ERROR range: " << image[i] << "\n";
          exit(0);
        }
      else if( image[i] > 1 )
        image[i] = 1.0;
    }
  
  return 0;
}

int image2d::convolute( double *F )
{
  fftw_complex *I;
  int pwidth, pheight;
  pheight = height *2;
  pwidth = width*2;
  I = (fftw_complex*) fftw_malloc( sizeof(fftw_complex)*pwidth*pheight );
  fftw_plan dftp;
  padzeros( pheight, pwidth );
  dftp = fftw_plan_dft_r2c_2d( pheight, pwidth,
                               image, I, FFTW_ESTIMATE );
  fftw_execute( dftp );
  for( int i = 0; i < height; i++ )
    {
      for( int j = 0; j < (width/2+1); j++)
        {
          I[i*(width/2+1)+j][0] = (I[i*(width/2+1)+j][0]*F[i*width+j]);
          I[i*(width/2+1)+j][1] = (I[i*(width/2+1)+j][1]*F[i*width+j]);
        }
    }
  fftw_destroy_plan( dftp );
  dftp = fftw_plan_dft_c2r_2d( height, width, I, image,
                               FFTW_ESTIMATE );
  fftw_execute( dftp );
  fftw_destroy_plan( dftp );
  fftw_free( I );
  double v = height*width;
  for( int i = 0; i < height*width; i++ )
    image[i] = image[i]/v;
  crop( height/2, width/2);
  return 0;
}

int image2d::convolute( fftw_complex *F )
{
  int pwidth, pheight;
  pheight = height *2;
  pwidth = width*2;
  fftw_complex *I;
  I = (fftw_complex*) fftw_malloc( sizeof(fftw_complex)*pwidth*pheight );
  fftw_plan dftp;
  padzeros( pheight, pwidth );
  dftp = fftw_plan_dft_r2c_2d( pheight, pwidth,
                               image, I, FFTW_ESTIMATE );
  fftw_execute( dftp );

  for( int i = 0; i < height; i++ )
    {
      for( int j = 0; j < (width/2)+1; j++ )
        {
          double a, b;
          int idx;
          idx = i*(width/2+1)+j;
  
          a = (I[idx][0]*F[idx][0]) - (I[idx][1]*F[idx][1]);
          b = (I[idx][0]*F[idx][1]) + (I[idx][1]*F[idx][0]);
          I[idx][0] = a;
          I[idx][1] = b;
          
        }
    }
  fftw_destroy_plan( dftp );
  dftp = fftw_plan_dft_c2r_2d( height, width, I, image,
                               FFTW_ESTIMATE );
  fftw_execute( dftp );
  fftw_destroy_plan( dftp );
  fftw_free( I );
  double v = height*width;
  
  for( int i = 0; i < height*width; i++ )
    {
      image[i] = fabs(image[i])/v;
      
      if( image[i] > 1.00001 || image[i] < 0 ) 
        {
          std::cout << "ERROR 1 range: " << image[i] << "\n";
          exit(0);
        }
      
    }
  crop( height/2, width/2);
  return 0;
}

double image2d::mutualinformation( image2d *o, int minx, int maxx, int miny, int maxy )
{
  double res;
  res = 0;
  // calculate pdfs
  // calculate joint pdf
  std::cout << "mi: " << minx << " " << maxx << " : " 
            << miny << " " << maxy << "\n";
  double mypdf[64];
  double opdf[64];
  double jpdf[64][64];

  double mypdf1[64];
  double opdf1[64];
  double jpdf1[64][64];
  for( int i = 0; i < 64; i++)
    {
      mypdf[i] = opdf[i] = 0;
      for( int j = 0; j < 64; j++ )
        {
          jpdf[i][j] = 0;
        }
    }

  for( int i = minx; i < maxx; i++ )
    {
      for( int j = miny;j < maxy; j++ )
        {
          int a, b;
          a = (int)(getpixel( i, j ) * 64);
          b = (int)(o->getpixel( i, j ) * 64);
          if( a >= 64 )
            a = 63;
          if( b >= 64 )
            b = 63;
          mypdf[a] += 1;
          opdf[b] += 1;
          jpdf[a][b] += 1;
        }
    }
  pmfblur( mypdf, mypdf1 );
  pmfblur( opdf, opdf1 );
  jointpmfblur( jpdf, jpdf1 );
  res = 0;
  /// sum up the data
  for( int i = 0; i < 64; i++)
    {
      for( int j = 0; j < 64; j++)
        {
          double t;
          if( jpdf1[i][j] > 0 && mypdf1[i] > 0 && opdf1[j] > 0 )
            {
              t = jpdf1[i][j] * log( jpdf1[i][j]/( mypdf1[i]*opdf1[j] ) );
              res = res + t;
            }
        }
    }

  return res;
}

int image2d::jointpmfblur( double a[64][64], double b[64][64] )
{
  double kernel[5][5];
  for( int i = 0; i < 5; i++)
    {
      kernel[i][0] = 0.025;
      kernel[0][i] = 0.025;
      kernel[i][4] = 0.025;
      kernel[4][i] = 0.025;
    }
  // 16 * 0.025 = 0.40
  for( int i = 1; i < 4; i++ )
    {
      kernel[i][1] = 0.05;
      kernel[1][i] = 0.05;
      kernel[i][3] = 0.05;
      kernel[3][i] = 0.05;
    }
  kernel[2][2] = 0.2;

  double sum;
  sum = 0;
  for(int i = 0; i < 64; i++ )
    {
      for( int j = 0; j < 64; j++ )
        {
          double c, d;
          c = d = 0;
          for( int k = 0; k < 5; k++ )
            {
              for(int l = 0; l < 5; l++ )
                {
                  int x, y;
                  x = i+k-2;
                  y = j+l-2;
                  if( x >= 0 && x < 64 &&
                      y >= 0 && y < 64 )
                    {
                      c = c + a[x][y]*kernel[k][l];
                      d = d + kernel[k][l];
                    }
                }
            }
          b[i][j] = c/d;
          sum = sum + b[i][j];
        }
    }
  if( sum > 0 )
    {
      for(int i = 0; i < 64; i++ )
        {
          for( int j = 0; j < 64; j++ )
            {
              b[i][j] = b[i][j]/sum;
              if( isnan(b[i][j]) != 0 )
                {
                  std::cout << "ERROR jpdf " << b[i][j] << "\n";
                }
            }
        }
    }

#ifdef __DEBUG_MI__
  sum = 0;
  for(int i = 0; i < 64; i++ )
    for( int j = 0; j < 64; j++ )
      sum += b[i][j];
  std::cout << "jpdf sum: " << sum << "\n";
  if( fabs(1-sum) > 0.0001 )
    {
      std::cout << "ERROR MI JPDF\n";
      exit(0);
    }
#endif

  return 0;
}

int image2d::pmfblur( double a[64], double b[64] )
{
  double kernel[5];
  double sum;
  sum = 0;
  kernel[0] = kernel[4] = 0.1;
  kernel[1] = kernel[3] = 0.2;
  kernel[2] = 0.4;
  for( int i = 0; i < 64; i++ )
    {
      double c;
      double d;
      c = d = 0;
      for( int j = 0; j < 5; j++ )
        {
          int k;
          k = i + j - 2;
          if( !( k < 0 || k >= 64 ) )
            {
              c = c + kernel[j]*a[k];
              d = d + kernel[j];
            }
        }
      b[i] = c/d;
      sum = sum + b[i];
    }
  for( int i = 0; i < 64; i++ )
    {
      b[i] = b[i]/sum;
      if( isnan(b[i]) != 0 )
        {
          std::cout << "ERROR jpdf " << b[i] << "\n";
        }
    }
#ifdef __DEBUG_MI__
  sum = 0;
  for(int i = 0; i < 64; i++ )
    sum += b[i];
  std::cout << "pdf sum: " << sum << "\n";
  if( fabs(1-sum) > 0.0001 )
    {
      std::cout << "ERROR MI PDF\n";
      exit(0);
    }
#endif

  return 0;
}

int image2d::convfilter(double *filter, int x, int y)
{
  double accum;
  int h, w;
  h = getheight();
  w = getwidth();
  double *newim = new double [h*w];
  for(int i = 0; i < h; i++)
    {
      for(int j = 0; j < w; j++)
	{
	  accum = 0;
	  for(int k = i-x/2; k <= i+x/2; k++)
            {
              for(int l = j-y/2; l <= j+y/2; l++)
                {
                  int kk, ll;
                  kk = (k-(i-x/2));
                  ll = (l-(j-y/2));
                  accum = accum + getpixel(k, l)*
                    filter[kk*y+ll];
                  if( isnan( filter[kk*y+ll] ) != 0 )
                    {
                      std::cout << "filter error\n";
                      std::cout << kk << " " << ll << "\n";
                      std::cout << x << " " << y << "\n";
                      exit(0);
                    }
                  if( isnan(accum) != 0 )
                    {
                      std::cout << "gaussian filter invalid 4\n";
                      exit(0);
                    }
                }
            }
	  newim[i*w+j] = accum;
          if( isnan(accum) != 0 )
            {
              std::cout << "gaussian filter invalid 3\n";
              exit(0);
            }
	}
    }
  delete [] image;
  image = newim;
  return 0;
}

int image2d::del()
{
  if(image != NULL)
    delete [] image;
  image = NULL;
  return 0;
}

int image2d::create(int h, int w)
{
  del();
  width = w;
  height = h;
  image = new double [width * height];
  clear();
  return 0;
}

int image2d::clear()
{
  if(image == NULL)
    return 0;
  for( int i = 0; i < width*height; i++)
    image[i] = 0;
  return 0;
}

int image2d::loadlowres( image2d *im, int ratio )
{
  del();
  width = im->getwidth()/ratio;
  height = im->getheight()/ratio;
  image = new double[width*height];
  int radius = ratio/2;
  for( int i = 0; i < height; i++ )
    {
      for(int j = 0; j < width; j++ )
        {
          int li, lj, hi, hj;
          li = i*ratio - radius;
          hi = i*ratio + radius;
          lj = j*ratio - radius;
          hj = j*ratio + radius;
          if( li < 0 )
            li = 0;
          if( lj < 0 )
            lj = 0;
          if( hi >= im->getheight() )
            hi = im->getheight()-1;
          if( hj >= im->getwidth() )
            hj = im->getwidth()-1;
          double avg;
          double c;
          c = 0;
          avg = 0;
          for( int ii = li; ii <= hi; ii++ )
            {
              for( int jj = lj; jj <= lj; jj++ )
                {
                  avg = avg + im->getpixel(ii, jj);
                  c = c + 1;
                }
            }
          avg = avg / c;
          setpixel(i, j, avg);
        }
    }
  return 0;
}

int image2d::load(image2d *im)
{
  del();
  width = im->getwidth();
  height = im->getheight();
  image = new double [width*height];
  im->getimage(image);
  return 0;
}

image2d::image2d( const image2d &in )
{
  width = in.width;
  height = in.height;
  image = new double [width*height];
  memcpy( image, in.image, sizeof(double)*width*height );
}

image2d& image2d::operator=(const image2d &in)
{
  del();
  width = in.width;
  height = in.height;
  image = new double( width*height );
  memcpy( image, in.image, sizeof(double)*width*height );
  return *this;
}

int image2d::load(double *data, int h, int w)
{
  del();
  width = w;
  height = h;
  image = data;
  return 0;
}

int image2d::load(int *data, int h, int w)
{
  create(h, w);
  for( int i = 0; i < h*w; i++ )
    image[i] = data[i];
  return 0;
}

int image2d::loadcopy(double *data, int h, int w)
{
  del();
  width = w;
  height = h;
  image = new double[w*h];
  memcpy(image, data, sizeof(double)*w*h);
  return 0;
}

int image2d::load(Complex *data, int h, int w)
{
#ifdef __DEBUG__
  std::cout << "loading complex data\n";
#endif
  del();
  width = w;
  height = h;
  image = new double [width*height];
  for(int i = 0; i < h; i++)
    {
      for(int j = 0; j < w; j++)
        {
          double t = data[i*w+j].abs();
          //          std::cout << t << "\n";
          setpixel(i, j, t);
        }
    }
  return 0;
}

int image2d::padzeros( int nszh, int nszw )
{
  if( nszh < height || nszw < width )
    return 0;
  double *nd;
  nd = new double[nszh*nszw];
  for( int i = 0; i < nszh; i++ )
    {
      for( int j = 0; j < nszw; j++ )
        {
          if( i < height && j < width )
            nd[i*nszw+j] = getpixel(i,j);
          else
            nd[i*nszw+j] = 0;
        }
    }
  delete [] image;
  image = nd;
  height = nszh;
  width = nszw;
  return 0;
}

int image2d::copysubim( double *oim, int lx, int ly,
                        int h, int w, int ph, int pw )
{
  if( ph < h || pw < w )
    {
      std::cout << "sub image copy size error\n";
      exit(0);
    }
  create(h, w);
  for( int i = 0; i < h; i++ )
    {
      memcpy(image+(i*w), oim+( (i+lx)*pw ), sizeof(double)*w );
      //check should wrap
      for( int j = 0; j < w; j++ )
        {
          //          image[i*w+j] = oim[ (i+lx)*pw+ly+j ];
          if( image[i*w+j] != oim[ (i+lx)*pw+ly+j ] )
            {
              std::cout << "subcopy failed\n";
              exit(0);
            }
        }
    }
  return 0;
}

int image2d::crop( int nszh, int nszw )
{
  if( nszh > height || nszw > width )
    return 0;
  double *nd;
  nd = new double[nszh*nszw];
  for( int i = 0; i < nszh; i++ )
    {
      for( int j = 0; j < nszw; j++ )
        {
          nd[i*nszw+j] = getpixel(i,j);
        }
    }
  delete [] image;
  image = nd;
  height = nszh;
  width = nszw;
  return 0;
}

int image2d::getimage(double *ptr)
{
  memcpy(ptr, image, sizeof(double)*(width*height));
  return 0;
}

int image2d::createdog( double *g, double sigma1,
                        double sigma2, int szx, int szy )
{
  double s1 = sigma1;
  double s2 = sigma2;
  for( double i = 0; i < szx; i = i + 1 )
    for( double j = 0; j < szy; j = j + 1 )
      {
        g[ (int)(i * szx + j) ] = 1/(2*PI*s1)*exp(-(pow(i,2)+pow(j,2))/(2*s1)) -
          1/(2*PI*s2)*exp(-( pow(i, 2) + pow(j, 2) )/(2*s2) );
      }
  
  return 0;
}

#ifndef _NOMAGICK_
int image2d::load(Magick::Image &im, int channel)
{
  del();
  Magick::Geometry geo = im.size();
  width = geo.width();
  height = geo.height();
  std::cout << "loading image: " 
            << height << "x" << width << "\n";
  Magick::ColorRGB rgb;
  Magick::ColorGray gray;
  image = new double [width* height];
  Magick::PixelPacket *pixel_cache = im.getPixels(0,0,width,height);
  for(int i = 0; i < height; i++)
    {
      for(int j = 0; j < width; j++)
	{
	  if(channel == CH_RED)
	    {
	      rgb = pixel_cache[i*width+j];
	      setpixel(i, j, rgb.red());
	    }
	  else if(channel == CH_GREEN)
	    {
	      rgb = pixel_cache[i*width+j];
	      //rgb = im.pixelColor(j, i);
	      setpixel(i, j, rgb.green());
	    }
	  else if(channel == CH_BLUE)
	    {
	      rgb = pixel_cache[i*width+j];
	      //rgb = im.pixelColor(j, i);
	      setpixel(i, j, rgb.blue());
	    }
          else if(channel == CH_VS )
            {
              rgb = pixel_cache[i*width+j];
              gray = pixel_cache[i*width+j];
              double v;
              double min, max;
              max = min = rgb.red();
              if( rgb.green() < rgb.blue() )
                {
                  if( rgb.green() < min )
                    min = rgb.green();
                }
              else
                {
                  if( rgb.blue() < min )
                    min = rgb.blue();
                }
              if( rgb.green() > rgb.blue () )
                {
                  if( rgb.green() > max )
                    max = rgb.green();
                }
              else
                if( rgb.blue() > max )
                  max = rgb.blue();

              if( min > max )
                {
                  std::cout << "ERROR\n";
                }

              if( min == max )
                v = gray.shade();
              else
                v = gray.shade() - ( 1 - min/max );
              if( v < 0 )
                v = 0;
              else if(v > 1)
                v = 1;
              setpixel(i,j, v );
            }
	  else
	    {
	      gray = im.pixelColor(j, i);
	      setpixel(i, j, gray.shade());
	    }
	}
    }
  return 0;
}
#endif

#ifndef _NOMAGICK_
int image2d::getimage(Magick::Image &im1)
{
  if(image == NULL)
    return -1;
  Magick::Geometry geo;
  geo.width(width);
  geo.height(height);
  //im.size(geo);
  Magick::Image im(geo, Magick::Color("red"));

#ifdef __DEBUG__
  std::cout << height << " " << width << "\n";
#endif
  Magick::ColorGray gray(0);
  Magick::PixelPacket *pixel_cache = im.getPixels(0,0,width,height);
  for(int i = 0; i < height; i++)
    {
      for(int j = 0; j < width; j++)
	{
	  gray.isValid(true);
          double value;
          value = getpixel(i,j);
          if( getpixel(i,j) > 1 )
            {
              value = 1;
            }
          if( getpixel(i,j) < 0 )
            {
              value = 0;
            }
	  gray.shade( value );
	  pixel_cache[i*width+j] = gray;
	  //im.pixelColor(j, i, gray);
	    
	}
    }
  im.syncPixels();
  im1 = im;
  return 0;
}
#endif

#ifndef _NOMAGICK_
int image2d::getimage(Magick::Image &im1, int minx, int miny, int maxx, int maxy)
{
  std::cout << "getting cropped image\n";
  if(image == NULL)
    return -1;
  Magick::Geometry geo;
  minx -= 2;
  miny -= 2;
  maxx += 2;
  maxy += 2;
  geo.width(maxy-miny);
  geo.height(maxx-minx);
  int mwidth=maxy-miny;
  //im.size(geo);
  Magick::Image im(geo, Magick::Color("black"));
#ifdef __DEBUG__
  std::cout << height << " " << width << "\n";
  std::cout << "(" << minx << " " << miny << ") "
            << "(" << maxx << " " << maxy << ")\n";

#endif
  Magick::ColorGray gray(0);
  Magick::PixelPacket *pixel_cache = im.getPixels(0,0,mwidth, maxx-minx);
  for(int i = minx; i <= maxx; i++)
    {
      for(int j = miny; j <= maxy; j++)
	{

	  gray.isValid(true);
	  gray.shade(getpixel(i, j));
	  pixel_cache[(i-minx)*mwidth+(j-miny)] = gray;
	  //im.pixelColor(j, i, gray);
	    
	}
    }
  im.syncPixels();
  im1 = im;
#ifdef __DEBUG__
  std::cout << "done crop\n";
#endif
  return 0;
}
#endif

int image2d::normalize( double min, double max )
{
  double scale = 1/(max-min);
  std::cout << "normalizing: " << min << " " << max << "\n"
            << height << "x" << width << "\n";
  for(int i = 0; i < height; i++)
    {
      for(int j = 0; j < width; j++)
	{
	  double t = getpixel(i, j);
	  t = t -min;
	  t = t*scale;
          if( t < 0 )
            t = 0;
          if( t > 1 )
            t = 1;
	  setpixel(i, j, t);
	}
    }
  return 0;
}

#ifndef _NOMAGICK_
int image2d::getimage_normalized(Magick::Image &im1)
{
  if(image == NULL)
    return -1;
  double min = getpixel(0, 0);
  double max = getpixel(0, 0);
  for(int i = 0; i < height; i++)
    {
      for(int j = 0; j < width; j++)
	{
	  double t = getpixel(i, j);
	  if(min > t)
	    min = t;
	  if(max < t)
	    max = t;
	}
    }
  double scale = 1/(max-min);
  Magick::Geometry geo;
  geo.width(width);
  geo.height(height);
  //im.size(geo);
  Magick::Image im(geo, Magick::Color("black"));
  im.depth(8);
  im.type( Magick::GrayscaleType );
  im1.depth(8);
  im1.type( Magick::GrayscaleType );
#ifdef __DEBUG__
  std::cout << height << " " << width << "\n";
#endif
  Magick::ColorGray gray(0);
  Magick::PixelPacket *pixel_cache
    = im.getPixels(0,0,width,height);
  std::cout << "image size: "
            << height << " " << width << " min: " 
            << min << " scale: " 
            << scale  << "\n";

  for(int i = 0; i < height; i++)
    {
      for(int j = 0; j < width; j++)
	{
	  gray.isValid(true);
	  double t = getpixel(i, j);
	  t = t -min;
	  t = t*scale;
          gray.shade(t);
	  pixel_cache[i*width+j] = gray;
	}
    }
  im.syncPixels();
  im1 = im;
  return 0;
}
#endif

double image2d::getpixel(int x, int y)
{
  if(image == NULL)
    return 0;
  if( x < 0 )
    x = 0;
  else if( x >= height )
    x = height - 1;

  if(y < 0)
    y = 0;
  else if(y >= width)
    y = width-1;

#ifdef __DEBUG__
  if( isnan( image[x*width+y] ) != 0 )
    {
      std::cout << "IMAGE contains NAN: " << x << " " << y << "\n";
      exit(0);
    }
#endif

  return image[x*width+y];
}

double image2d::getpixelb( double x, double y )
{  
  double hx, hy, lx, ly;
  double v1, vh, vl;
  
  hx = ceil( x );
  hy = ceil( y );
  lx = floor( x );
  ly = floor( y );

  if(hx == lx)
    hx = hx + 1;
  if(hy == ly)
    hy = hy + 1;

  vh = (hx - x)*getpixel( (int)lx, (int)hy )
    +  (x - lx)*getpixel( (int)hx, (int)hy );

  vl = (hx - x)*getpixel( (int)lx, (int)ly )
    +  (x - lx)*getpixel( (int)hx, (int)ly );

  v1 = (hy - y)*vl + (y - ly)*vh;
  
  return v1;
}

double image2d::getpixeld( double x, double y )
{  
  double hx, hy, lx, ly;
  double sx, sy;
  double v1, v2;
  
  hx = ceil( x * ((double)height) );
  hy = ceil( y * ((double)width) );
  lx = floor( x * ((double)height) );
  ly = floor( y * ((double)width) );

  if(hx == lx)
    hx = hx + 1;
  if(hy == ly)
    hy = hy + 1;

  sx = x * height;
  sy = y * width;

  v1 = fabs(sx - lx)*getpixel( (int)hx, (int)hy )
    +  fabs(hx - sx)*getpixel( (int)lx, (int)hy );

  v2 = fabs(sx - lx)*getpixel( (int)hx, (int)ly )
    +  fabs(hx - sx)*getpixel( (int)lx, (int)ly );

  v1 = fabs(sy - ly)*v1 + fabs(hy - sy)*v2;
  
  return v1;
}

int image2d::setpixel(int x, int y, double value)
{
  if(image == NULL)
    return 0;

  if(x < 0)
    x = 0;
  if(y < 0)
    y = 0;
  if(x >= height)
    x = height-1;
  if(y >= width)
    y = width-1;
  if( isnan(value) != 0)
    {
      std::cout << "ERROR IMAGE trying to set nan value to image\n";
      exit(0);
    }
  image[x*width+y] = value;
  return 0;
}

int image2d::normalizeminmax()
{
  double min = getpixel(0, 0);
  double max = getpixel(0, 0);
  for(int i = 0; i < height; i++)
    {
      for(int j = 0; j < width; j++)
	{
	  double t = getpixel(i, j);
	  if(min > t)
	    min = t;
	  if(max < t)
	    max = t;
	}
    }
  std::cout << "min max: " << min << " " << max << "\n";
  double scale = 1/(max-min);
  for(int i = 0; i < height; i++)
    {
      for(int j = 0; j < width; j++)
	{
	  double t = getpixel(i, j);
	  t = t -min;
	  t = t*scale;
	  setpixel(i, j, t);
	}
    }
  return 0;
}

int image2d::sub( image2d *im )
{
  int w = im->getwidth();
  int h = im->getheight();
  if(image == NULL)
    create( h, w);
  if(width < w)
    w = width;
  if(height < w)
    h = height;
  for(int i = 0; i < h; i++)
    {
      for(int j = 0; j < w; j++)
        {
          double v;
          v = getpixel(i,j)-im->getpixel(i, j);
          setpixel(i, j, v);
        }
    }
  return 0;
}

int image2d::add(image2d *im)
{
  int w = im->getwidth();
  int h = im->getheight();
  if(image == NULL)
    create( h, w);
  if(width < w)
    w = width;
  if(height < w)
    h = height;
  for(int i = 0; i < h; i++)
    for(int j = 0; j < w; j++)
      setpixel(i, j, getpixel(i,j)+im->getpixel(i, j));

  return 0;
}

int image2d::load(subimage *im)
{
  create(im->getsizex(), im->getsizey());
  for(int i = 0; i < height; i++)
    {
      for(int j = 0; j < width; j++)
        {
	  double nv = im->getpixel(i, j);
          setpixel(i, j, nv);
        }
    }
  return 0;
}

int image2d::add(subimage *im)
{
  for(int i = 0; i < height; i++)
    {
      for(int j = 0; j < width; j++)
        {
	  double nv = getpixel(i,j)+im->getpixel(i, j);
          setpixel(i, j, nv);
        }
    }
  return 0;
}

int image2d::multquantize( int factor )
{
  for(int i = 0; i < height*width; i++)
    {
      image[i] = round(image[i] * factor);
      if(image[i] < 0)
        {
          image[i] = 0;
        }
      if(image[i] >= factor)
        {
          image[i] = factor-1;
        }
    }
  return 0;
}

int image2d::divide( double factor )
{
  std::cout << "dividing by " << factor << "\n";
  for(int i = 0; i < height*width; i++)
    image[i] = image[i] / factor;
  return 0;
}

int image2d::dividefactor( int factor )
{
  for(int i = 0; i < height*width; i++)
    image[i] = image[i] / ((double) factor);
  return 0;
}

double image2d::stdvar()
{
  double avg, std;
  std = 0;
  avg = average();
  for(int i = 0; i < height*width; i++)
    std = std + pow(avg - image[i], 2);
  return sqrt( std/( (double)(height*width) ) );
}

double image2d::average()
{
  double avg;
  avg = 0;
  for(int i = 0; i < height*width; i++)
    avg = avg + image[i];
  return avg/( (double)(height*width) );
}

double image2d::variance( double avg)
{
  double var;
  var = 0;
  for(int i = 0; i < height*width; i++)
    var = var + pow( image[i] - avg, 2 );
  return var/( (double)(height*width) );
}

int image2d::subtract_lowerbound(double cut)
{
  for(int i = 0; i < height*width; i++)
    {
      image[i] = image[i] - cut;
      if(image[i] < 0)
        image[i] = 0;
    }
  return 0;
}
/*
int image2d::logscale()
{
  for(int i = 0; i < height*width; i++)
    {
      if(image[i] > 0)
        image[i] = log(image[i]+1);
    }
  return 0;
}
*/
