#include "image3d.h"

int pthread_sort( double *data, double *buffer,
                  int start, int end );

int pthread_getpixels( double *data, int i, int j, 
                       int k, int s, int h, int w,
                       int d, double *v );

void *pthread_medianftr(double *data, double *dst,
                        int h, int w, int d, int s,
                        int sd, int ed );

void *pthread_median( void *arg );

int pthread_sort( double *data, double *buffer,
                  int start, int end )
{
  if( end - start > 1 )
    {
      int se2;
      se2 = (start+end)/2;
      pthread_sort( buffer, data, start, se2 );
      pthread_sort( buffer, data, se2, end );
      int p1, p2;
      p1 = start;
      p2 = se2;
      int c = 0;
      while( p1 < se2 || p2 < end )
        {
          if( p1 == se2 )
            {
              buffer[c+start] = data[p2];
              c++;
              p2++;
            }
          else if( p2 == end )
            {
              buffer[c+start] = data[p1];
              c++;
              p1++;
            }
          else
            {
              if( data[p1] < data[p2] )
                {
                  buffer[c+start] = data[p1];
                  p1++;
                }
              else
                {
                  buffer[c+start] = data[p2];
                  p2++;
                }
              c++;
            }
        }
    }
  return 0;
}

int pthread_getpixels( double *data, int i, int j, 
                       int k, int s, int h, int w,
                       int d, double *v )
{
  int c = 0;
  int si, sj, sk, ei, ej, ek;
  si = i-(s/2);
  sj = j-(s/2);
  sk = k-(s/2);
  ei = si + s;
  ej = sj + s;
  ek = sk + s;
  if( si < 0 )
    si = 0;
  if( sj < 0 ) 
    sj = 0;
  if( sk < 0 )
    sk = 0;
  if( ei > h )
    ei = h;
  if( ej > w )
    ej = w;
  if( ek > d )
    ek = d;
  for( k = sk; k < ek; k++ )
    {
      for( i = si; i < ei; i++ )
        {
          for( j = sj; j < ej; j++ )
            {
              v[c] = data[ k*h*w+i*w+j  ];
              c++;
            }
        }
    }
  return c;
}

void *pthread_medianftr(double *data, double *dst,
                        int h, int w, int d, int s,
                        int sd, int ed )
{
  double *values;
  values = new double [s*s*s];
  //  buffer = new double [s*s*s];
  //  buffer = new double [s*s*s];
  std::cout << sd << ">" << ed << "\n";
  for( int k = sd; k < ed; k++ )
    {
      std::cout << k << "\n";
      for( int i = 0; i < h; i++ )
        {
          for( int j = 0; j < w; j++ )
            {
              int c;
              c = pthread_getpixels( data, i, j, k,
                                     s, h, w, d,
                                     values );
              //memcpy( buffer, values, sizeof(double)*c );
              //pthread_sort( values, buffer, 0, c );
              std::sort( values, values+c );
              dst[ k*h*w+i*w+j  ] = values[c/2];
            }
        }
    }
  //  delete [] buffer;
  delete [] values;
  return NULL;
}

void *pthread_median( void *arg )
{
  filterjob *fj = (filterjob*)arg;
  pthread_medianftr(fj->data, fj->dst,
                    fj->h, fj->w, fj->d, fj->s,
                    fj->sd, fj->ed );
  return NULL;
}

image3d::image3d()
{
  data = NULL;
}

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

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

int image3d::create( int h, int w, int d)
{
  del();
  std::cout << "allocating " 
            << ((double)(h*w*d*sizeof(double)))/1024.0 
            << " kbytes of data\n";
  data = new double [h*w*d];
  memset( data, 0, sizeof(double)*h*w*d );
  height = h;
  width = w;
  depth = d;
  return 0;
}

double image3d::getpixel( int h, int w, int d)
{
  if( data == NULL )
    return 0;
  int o;
  o = calcoffset(h, w, d );
  if( o >= 0 )
    return data[o];
  else 
    {
      std::cout << "ERROR getpixel\n";
      return 0;
    }
}

int image3d::loadd( image2d &im, int d )
{
  if( im.getwidth() != width ||
      im.getheight() != height )
    {
      std::cout << "ERROR size\n";
      std::cout << im.getwidth() << "x"
                << im.getheight() << "\n";
      std::cout << width << "x" << height
                << "\n";
      return -1;
    }
  std::cout << "loading to slice: " << d << "\n";
  memcpy( data+(d*width*height), im.getimageptr(),
          sizeof(double)*height*width);
  /*  double v;
  for( int i = 0; i < height; i++ )
    {
      for( int j = 0; j < width; j++ )
        {
          v = im.getpixel( i, j );
          avg = avg + v;
          setpixel( i, j, d, v );
        }
    }
  */
  return 0;
}

int image3d::setpixel( int h, int w, int d, double v )
{
  int o;
  o = calcoffset(h, w, d);
  if( o >= 0 )
    data[o] = v;
  else
    std::cout << "ERROR getpixel\n";
  return 0;
}

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

int image3d::normalizeminmax()
{
  double min = data[0];
  double max = data[0];

  const int histsz = 4096;
  double hist[4096];

  int e;
  e = height*width*depth;
  for( int i = 1; i < e; i++ )
    {
      if( data[i] < min )
        min = data[i];
      if( data[i] > max )
        max = data[i];
    }

  double scale = 1/(max-min);
  std::cout << "[" << min << ", " << max << "]\n";
  for(int i = 0; i < e; i++)
    {
      double t;
      t = data[i];
      t = t -min;
      t = t*scale;
      data[i] = t;
    }

  calcpmf( hist, histsz );
  double cum;
  cum = min = 0;
  while( cum < 0.001 )
    {
      cum = cum + hist[(int)min];
      min++;
      std::cout << min << " ";
    }
  std::cout << "\n";
  cum = 0;
  max = histsz-1;
  while( cum < 0.001 )
    {
      cum = cum + hist[(int)max];
      std::cout << max << "(" << hist[(int)max] << ") ";
      max = max -1;
    }
  std::cout << "\n";
  min = min / ( (double)histsz );
  max = max / ( (double)histsz );
  scale = 1/(max-min);
  std::cout << "[" << min << ", " << max << "]\n";
  for(int i = 0; i < e; i++)
    {
      double t;
      t = data[i];
      t = t-min;
      t = t*scale;
      if( t > 1 )
        t = 1;
      else if( t < 0 )
        t = 0;
      data[i] = t;
    }

  return 0;
}

#ifndef _NOMAGICK_
int image3d::write( std::string base )
{
  if( data == NULL )
    return 0;
  Magick::Geometry geo;
  geo.width(width);
  geo.height(height);
  Magick::Image im(geo, Magick::Color("black"));
  im.type( Magick::GrayscaleType );
  im.depth(8);
  for( int i = 0; i < depth; i++ )
    {
      char buffer[512];
      sprintf( buffer, "%s_%i.tiff", base.c_str(), i );
      getslice( im, i );
      im.write( buffer );
    }
  return 0;
}
#endif

int image3d::calcpmf( double *hist, int bins )
{
  int e;
  e = width*height*depth;
  for( int i = 0; i < bins; i++ )
    hist[i] = 0;
  for( int i = 0; i < e; i++ )
    {
      int b;
      b = (int)floor( data[i] * bins );
      if( b >= bins )
        b = bins-1;
      hist[b]++;
    }
  double scale = 1.0/( (double)(e) );
  for( int i = 0; i < bins; i++ )
    hist[i] = hist[i] * scale;
  return 0;
}

#ifndef _NOMAGICK_
int image3d::getslice( Magick::Image &im,  int s )
{
  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.shade( getpixel(i, j, s) );
          pixel_cache[i*width+j] = gray;
        }
    }
  im.syncPixels();
  return 0;
}
#endif

int image3d::medianfilter( int s )
{
  double *ndata;
  ndata = new double[height*width*depth];
  memset( ndata, 0, sizeof(double)*height*width*depth );
  int d2;
  d2 = depth/2;
  pthread_t tid;
  filterjob job, job0;
  job.data = data;
  job.dst = ndata;
  job.h = height;
  job.w = width;
  job.d = depth;
  job.s = s;
  job.sd = d2;
  job.ed = depth;
  job0 = job;
  job0.sd = 0;
  job0.ed = d2;
  pthread_create( &tid, NULL,
                  pthread_median,
                  &job );
  void *rt;
  rt = &job0;
  pthread_median( rt );

  pthread_join( tid, &rt );

  delete [] data;
  data = ndata;

  return 0;
}

