#include "histequalize.h"

histequalize::histequalize()
{
  hist = NULL;
  mapping = NULL;
  npixels = 0;
  nbins = 0;
}

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

int histequalize::del()
{
  if( hist != NULL )
    delete [] hist;
  if( mapping != NULL )
    delete [] mapping;
  mapping = NULL;
  hist = NULL;
  npixels = 0;
  nbins = 0;
  return 0;
}

int histequalize::init( int x )
{
  del();
  nbins = x;
  hist = new double [nbins];
  mapping = new double [nbins];
  for( int i = 0; i < nbins; i++ )
    hist[i] = 0;
  npixels = 0;
  return 0;
}

int histequalize::printR()
{
  if( hist != NULL )
    {
      int max;
      max = 0;
      int i;
      max = nbins-1;
      while( hist[max] < 1 )
        max--;
      std::cout << "max: " << max << "\n";
      for(i = 0; i <= max; i++ )
        {
          std::cout << std::setprecision(20) << hist[i] << " " 
                    << std::setprecision(10);
        }
    }
  std::cout << "\n";
  return 0;
}

int histequalize::addimage( image2d *im, image2d *mask )
{
  int h, w;
  h = im->getheight();
  w = im->getwidth();
  for( int i = 0; i < h; i++ )
    {
      for( int j = 0; j < w; j++ )
        {
          double v;
          if( mask->getpixel(i,j) > 0.5 )
            {
              v = im->getpixel(i,j);
              int vd;
              vd = (int)round( v*(nbins-1) );
              if( vd >= nbins )
                {
                  std::cout << i << " " << j << " : ";
                  std::cout << v << "\n";
                  std::cout << "hist calc error\n";
                  exit(0);
                }
              hist[vd] = hist[vd] + 1;
            }
        }
    }
  npixels = npixels + h*w;
  mappingdone = 0;
  return 0;
}

int histequalize::addimage( image2d *im )
{
  int h, w;
  h = im->getheight();
  w = im->getwidth();
  for( int i = 0; i < h; i++ )
    {
      for( int j = 0; j < w; j++ )
        {
          double v;
          v = im->getpixel(i,j);
          int vd;
          vd = (int)round( v*(nbins-1) );
          if( vd >= nbins )
            {
              std::cout << i << " " << j << " : ";
              std::cout << v << "\n";
              std::cout << "hist calc error\n";
              exit(0);
            }
          hist[vd] = hist[vd] + 1;
        }
    }
  npixels = npixels + h*w;
  mappingdone = 0;
  return 0;
}

int histequalize::equalize( image2d *im )
{
  if( mappingdone == 0 )
    {
      double *cdf;
      cdf = new double [nbins];
      cdf[0] = hist[0]/npixels;
      double entropy;
      double nnp;
      nnp = 0;
      for( int i = 0; i < nbins; i++ )
        nnp = nnp + hist[i];
      if( hist[0] > 0 )
        entropy = hist[0]/npixels*log(hist[0]/npixels);
      else
        entropy = 0;
      npixels = nnp;
      for( int i = 1; i < nbins; i++ )
        {
          cdf[i] = cdf[i-1]+(hist[i]/npixels);
          if( hist[i] > 0 )
            entropy = entropy + (hist[i]/npixels) * log(hist[i]/npixels);
          if( cdf[i] > 1.00001 )
            {
              std::cout << "ERROR -2\n";
              std::cout << "cdf " << i << "("
                        << nbins << "):" << cdf[i] << "\n";
              std::cout << "npixels: " << npixels << "\n";
              exit(0);
            }
        }
      
      if( fabs(cdf[nbins-1] - 1) > 0.001 )
        {
          std::cout << "ERROR cdf calc\n";
          std::cout << cdf[nbins-1] << "\n";
          exit(0);
        }
      for( int i = 0; i < nbins; i++ )
        mapping[i] = round(cdf[i]*( (double)(nbins-1) ));
      for( int i = 0; i < nbins; i++ )
        {
          mapping[i] = mapping[i]/( (double)(nbins) );
          if( mapping[i] > 1 )
            {
              std::cout << "ERROR -1\n";
              exit(0);
            }
        }
      delete [] cdf;
      mappingdone = 1;
    }
  int h, w;
  h = im->getheight();
  w = im->getwidth();
  for( int i = 0; i < h; i++ )
    {
      for( int j = 0; j < w; j++ )
        {
          double v;
          int vd;
          v = im->getpixel(i,j);
          if( v < 0 || v > 1 )
            {
              std::cout << "ERROR 0\n";
              exit(0);
            }
          vd = (int)round( v * (nbins-1));
          if( vd < 0 || vd >= nbins )
            {
              std::cout << "ERROR 2\n";
              exit(0);
            }
          v = mapping[vd];
          if( v < 0 || v > 1 )
            {
              std::cout << "ERROR 1\n";
              exit(0);
            }
          im->setpixel( i, j, v );
        }
    }
  return 0;
}
