#include "FIGradients.h"

#include "vislearning/baselib/FastFilter.h"
#include "vislearning/image/GenericImageTools.h"

using namespace OBJREC;
using namespace NICE;
using namespace std;

void FIGradients::buildEOHMap ( CachedExample *ce,
                                int subsamplex, int subsampley,
                                int numBins, bool usesigned )
{
  int xsize;
  int ysize;
  ce->getImageSize ( xsize, ysize );
  int xsize_s = xsize / subsamplex;
  int ysize_s = ysize / subsampley;

  NICE::MultiChannelImage3DT<double> & eohimg = ce->getDChannel ( CachedExample::D_EOH );
  eohimg.reInit ( xsize_s, ysize_s, 1, numBins);

  double *gradient = new double[xsize*ysize];
  int *dir = new int[xsize*ysize];

  if ( ce->colorInformationAvailable() ) {
    NICE::MultiChannelImage3DT<int> & colorimg = ce->getIChannel ( CachedExample::I_COLOR );
    std::vector<int*> data = colorimg.getDataPointer();
    const int *r = data[0];
    const int *g = data[1];
    const int *b = data[2];
    FastFilter::calcColorGradient ( r, g, b, xsize, ysize,
                                    gradient, dir, numBins, usesigned );
  } else {
    NICE::MultiChannelImage3DT<int> & grayvalues = ce->getIChannel ( CachedExample::I_GRAYVALUES );
    std::vector<int*> data = grayvalues.getDataPointer();
    const int *gr = data[0];
    FastFilter::calcGradient ( gr, xsize, ysize, gradient, dir, numBins, usesigned );
  }

  eohimg.setAll ( 0 );
  std::vector<double*> data = eohimg.getDataPointer();
  long korig = 0;
  for ( int y = 0 ; y < ysize ; y++ )
    for ( int x = 0 ; x < xsize ; x++, korig++ )
    {
      int xs = x / subsamplex;
      int ys = y / subsampley;

      if ( xs >= xsize_s ) xs = xsize_s - 1;
      if ( xs < 0 ) xs = 0;
      if ( ys >= ysize_s ) ys = ysize_s - 1;
      if ( ys < 0 ) ys = 0;
      int k = xs + ys * xsize_s;
      int val = dir[korig];
      double strength = gradient[korig];

      assert ( val < eohimg.channels() );

      data[val][k] += strength;

      if ( !NICE::isFinite( data[val][k] ) ) {
        fprintf ( stderr, "EOH Image failed: %f\n", data[val][k] );
        exit ( -1 );
      }
    }

  delete [] gradient;
  delete [] dir;

  NICE::MultiChannelImage3DT<double> & eohintimg = ce->getDChannel ( CachedExample::D_INTEGRALEOH );
  eohintimg.reInit ( xsize_s, ysize_s, 1, numBins );
  for ( uint i = 0 ; i < ( uint ) numBins ; i++ )
  {
    ImageT<double> tmpEohImg         = eohimg.getChannelT(i);
    ImageT<double> tmpEohIntegralImg = eohintimg.getChannelT(i);
    GenericImageTools::calcIntegralImage ( tmpEohIntegralImg, tmpEohImg, xsize_s, ysize_s );
  }

}