/** Author: gilles

    Differences of blockwise averages at different scales on the ROI
    (a la Viola and Jones)
*/

#include <mash/heuristic.h>

using namespace Mash;


//------------------------------------------------------------------------------
// Declaration of the heuristic class
//------------------------------------------------------------------------------
class blockwise: public Heuristic
{
    //_____ Construction / Destruction __________
public:
    blockwise();
    virtual ~blockwise();


    //_____ Implementation of Heuristic __________
public:
    //--------------------------------------------------------------------------
    // Returns the number of features this heuristic computes
    //
    // When this method is called, the 'roi_extent' attribute is initialized
    //--------------------------------------------------------------------------
    virtual unsigned int dim();

    //--------------------------------------------------------------------------
    // Called once per image, before any computation 
    //
    // Pre-computes from a full image the data the heuristic will need to compute
    // features at any coordinates in the image
    //
    // When this method is called, the following attributes are initialized:
    //     - roi_extent
    //     - image
    //--------------------------------------------------------------------------
    virtual void prepareForImage();

    //--------------------------------------------------------------------------
    // Called once per image, after any computation 
    //
    // Frees the memory allocated by the prepareForImage() method
    //--------------------------------------------------------------------------
    virtual void finishForImage();

    //--------------------------------------------------------------------------
    // Called once per coordinates, before any computation
    //
    // Pre-computes the data the heuristic will need to compute features at the
    // given coordinates
    //
    // When this method is called, the following attributes are initialized:
    //     - roi_extent
    //     - image
    //     - coordinates
    //--------------------------------------------------------------------------
    virtual void prepareForCoordinates();
    
    //--------------------------------------------------------------------------
    // Called once per coordinates, after any computation 
    //
    // Frees the memory allocated by the prepareForCoordinates() method
    //--------------------------------------------------------------------------
    virtual void finishForCoordinates();

    //--------------------------------------------------------------------------
    // Computes the specified feature
    //
    // When this method is called, the following attributes are initialized:
    //     - roi_extent
    //     - image
    //     - coordinates
    //--------------------------------------------------------------------------
    virtual scalar_t computeFeature(unsigned int feature_index);


    //_____ Attributes __________
protected:
  int **_blocksum; // will contain cumulative sums in (x,y) over the ROI
    // TODO: Declare all the attributes you'll need here
};


//------------------------------------------------------------------------------
// Creation function of the heuristic
//------------------------------------------------------------------------------
extern "C" Heuristic* new_heuristic()
{
    return new blockwise();
}



/************************* CONSTRUCTION / DESTRUCTION *************************/

blockwise::blockwise()
{
    // TODO: Initialization of the attributes that doesn't depend of anything
}


blockwise::~blockwise()
{
    // TODO: Cleanup of the allocated memory still remaining
}


/************************* IMPLEMENTATION OF Heuristic ************************/

unsigned int blockwise::dim()
{
  return 16*16*5*5*4;
}


void blockwise::prepareForImage()
{
  // nothing
}


void blockwise::finishForImage()
{
  // nothing
}


void blockwise::prepareForCoordinates()
{
// Compute the coordinates of the top-left pixel of the region of interest
  unsigned int x0 = coordinates.x - roi_extent;
  unsigned int y0 = coordinates.y - roi_extent;

  unsigned int roi_size = roi_extent * 2; // here we put the center of the ROI inbetween pixels
 
  byte_t** pLines = image->grayLines();


  // Compute cumulative pixel sums in (x,y) over the ROI

  _blocksum = new int * [roi_size + 1];
  
  for (unsigned int x = 0; x <= roi_size; ++x)
    {
      int col_sum = 0;

      _blocksum[x] = new int [roi_size + 1];

        for (unsigned int y = 0; y <= roi_size; ++y)
      {
        if ( y==0 || x==0 )
          _blocksum[x][y] = 0;
        else
          {
        col_sum += pLines[y0 + y - 1][x0 + x - 1];     
        _blocksum[x][y] = _blocksum[x-1][y] + col_sum;
          }
      }
    }
}


void blockwise::finishForCoordinates()
{
  for (unsigned int x = 0; x <= 2 * roi_extent ; ++x)
    delete[] _blocksum[x];

  delete[] _blocksum;
}


scalar_t blockwise::computeFeature(unsigned int feature_index)
{
  unsigned int roi_size = roi_extent * 2;
  float stepsize = roi_extent * 2 / 16 ; 

  int feature_type = feature_index % 4;
  int x_size =  roi_extent / ( 1 << ( (feature_index / 4) % 5 ) ); // the size of the subregion:
  int y_size =  roi_extent / ( 1 << ( (feature_index / 4) / 5 ) % 5); // roi_extent / 2^k , k=0 to 4


  int x0 = stepsize * ((((feature_index / 4) / 5 ) / 5) % 16);
  int y0 = stepsize * (((((feature_index / 4) / 5 ) / 5) / 16 ) % 16);

  if ( x_size == 0 )
    x_size++;

  if ( y_size == 0 )
    y_size++;
  
  int xM = (x0 + x_size > roi_extent*2) ? (roi_extent * 2) : (x0 + x_size);
  int xm = (x0 - x_size > 0) ? (x0 - x_size) : 0;
  int yM = (y0 + y_size >  roi_extent*2) ? (roi_extent * 2) : (y0 + y_size);
  int ym =(y0 - y_size > 0) ? (y0 - y_size) : 0;
  

  if (feature_type == 0)   // mean value
    return (scalar_t) ( ( _blocksum[xM][yM]
              - _blocksum[xm][yM]
              - _blocksum[xM][ym]
              + _blocksum[xm][ym] ) / (float) (x_size * y_size) );

  if (feature_type == 1)   // horizontal difference
    return (scalar_t) ( ( _blocksum[xM][yM]
              - _blocksum[xm][yM]
              - 2*_blocksum[xM][y0]
              + 2*_blocksum[xm][y0]
              + _blocksum[xM][ym]
              - _blocksum[xm][ym] )/ (float) (x_size * y_size) );

  if (feature_type == 2)   // vertical difference
    return (scalar_t) ( ( _blocksum[xM][yM]
              - _blocksum[xM][ym]
              - 2*_blocksum[x0][yM]
              + 2*_blocksum[x0][ym]
              + _blocksum[xm][yM]
              - _blocksum[xm][ym] )/ (float) (x_size * y_size) );

  if (feature_type == 3)   // crossed (checkerboard) difference
    return (scalar_t) ( ( _blocksum[xM][yM]
              + _blocksum[xM][ym]
              + _blocksum[xm][yM]
              + _blocksum[xm][ym]
              - 2*_blocksum[xM][y0]
              - 2*_blocksum[xm][y0]
              - 2*_blocksum[x0][yM]
              - 2*_blocksum[x0][ym]
              + 4*_blocksum[x0][y0])/ (float) (x_size * y_size) );

}