/**
* @file HaarFeature.cpp
* @brief simple haar like feature
* @author Erik Rodner
* @date 04/21/2008

*/

#include <iostream>

#include "vislearning/features/fpfeatures/HaarFeature.h"
#include "vislearning/cbaselib/FeaturePool.h"

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


void HaarFeature::explode ( FeaturePool & featurePool, bool variableWindow ) const
{
  HaarFeature *hf = new HaarFeature ( *this );

  hf->pos1 = 0;
  hf->pos2 = 0;
  hf->type = HAARTYPE_HORIZONTAL;
  for ( hf->pos1 = 0 ; hf->pos1 < hf->window_size_y - 1 ; hf->pos1 += hf->step_y )
    featurePool.addFeature ( hf->clone(), hf->step_y / ( double ) ( HAARTYPE_NUMTYPES * hf->window_size_y ) );

  hf->pos1 = 0;
  hf->pos2 = 0;
  hf->type = HAARTYPE_VERTICAL;
  for ( hf->pos1 = 0 ; hf->pos1 < hf->window_size_x - 1; hf->pos1 += hf->step_x )
    featurePool.addFeature ( hf->clone(), hf->step_x / ( double ) ( HAARTYPE_NUMTYPES * hf->window_size_x ) );

  hf->type = HAARTYPE_DIAGONAL;
  hf->pos1 = 0;
  hf->pos2 = 0;
  for ( hf->pos1 = 0 ; hf->pos1 < hf->window_size_x - 1; hf->pos1 += hf->step_x )
    for ( hf->pos2 = 0 ; hf->pos2 < hf->window_size_y - 1 ; hf->pos2 += hf->step_y )
      featurePool.addFeature ( hf->clone(),
                               hf->step_x * hf->step_y / ( double ) ( HAARTYPE_NUMTYPES * hf->window_size_x * hf->window_size_y ) );

  hf->pos1 = 0;
  hf->pos2 = 0;
  hf->type = HAARTYPE_3BLOCKS;
  for ( hf->pos1 = 0 ; hf->pos1 < hf->window_size_x - 2*hf->step_x ; hf->pos1 += hf->step_x )
    for ( hf->pos2 = hf->pos1 + hf->step_x; hf->pos2 < hf->window_size_x - hf->step_x ; hf->pos2 += hf->step_x )
      featurePool.addFeature ( hf->clone(),
                               ( 2.0 * hf->step_x ) / ( HAARTYPE_NUMTYPES * ( hf->window_size_x - hf->step_x ) ) );

  delete hf;
}

Feature *HaarFeature::clone() const
{
  HaarFeature *fp =  new HaarFeature ( *this );
  return fp;
}


/************* HaarFeature **************/
HaarFeature::HaarFeature ( const Config *conf )
{
  window_size_x = conf->gI ( "HaarFeature", "window_size_x", 24 );
  window_size_y = conf->gI ( "HaarFeature", "window_size_y", 24 );
  step_x = conf->gI ( "HaarFeature", "step_x", 1 );
  step_y = conf->gI ( "HaarFeature", "step_y", 1 );
}

HaarFeature::HaarFeature ( int _window_size_x,
                           int _window_size_y,
                           int _step_x,
                           int _step_y )
{
  window_size_x = _window_size_x;
  window_size_y = _window_size_y;
  pos1 = 1;
  pos2 = 0;
  step_x = _step_x;
  step_y = _step_y;
  type = HaarFeature::HAARTYPE_HORIZONTAL;
}

HaarFeature::~HaarFeature()
{
}

double HaarFeature::val ( const Example *example ) const
{
  NICE::MultiChannelImageT<long> & img = example->ce->getLChannel ( CachedExample::L_INTEGRALIMAGE );

  long **data = img.getDataPointer();
  const long *integralImage = data[0];
  int xsize = img.width();
  int ysize = img.height();

  int x = example->x;
  int y = example->y;
  long tl, tr, bl, br;

  int exwidth = example->width;
  if ( exwidth != 0 )
  {
    int exheight = example->height;
    tl = ( x - exwidth / 2 ) + ( y - exheight / 2 ) * xsize;
    tr = tl + exwidth - 1;
    bl = tl + ( exheight - 1 ) * xsize;
    br = bl + exwidth - 1;
  } else {
    tl = ( x - window_size_x / 2 ) + ( y - window_size_y / 2 ) * xsize;
    tr = tl + window_size_x - 1;
    bl = tl + ( window_size_y - 1 ) * xsize;
    br = bl + ( window_size_x - 1 );
  }

  assert ( tl < xsize*ysize );
  assert ( tr < xsize*ysize );
  if ( br >= xsize*ysize )
  {
    fprintf ( stderr, "xsize=%d, ysize=%d, x=%d, y=%d, wsy=%d, wsx=%d\n",
              xsize, ysize, x, y, window_size_x, window_size_y );
    fprintf ( stderr, "example: %d x %d\n",
              example->width, example->height );

  }
  assert ( bl < xsize*ysize );
  assert ( br < xsize*ysize );
  assert ( pos1 >= 0 );
  assert ( pos2 >= 0 );

//#if 0
  double value;
  if ( type == HaarFeature::HAARTYPE_HORIZONTAL )
  {
    long ml = tl + xsize * pos1;
    long mr = tr + xsize * pos1;
    assert ( ( ml >= 0 ) && ( ml < xsize*ysize ) );
    assert ( ( mr >= 0 ) && ( mr < xsize*ysize ) );
    value = 2 * integralImage[mr];
    value -= 2 * integralImage[ml];
    value +=   integralImage[tl];
    value +=   integralImage[tr];
    value -=   integralImage[bl];
    value -=   integralImage[br];
  } else if ( type == HaarFeature::HAARTYPE_VERTICAL ) {
    long mt = tl + pos1;
    long mb = bl + pos1;
    assert ( ( mt >= 0 ) && ( mt < xsize*ysize ) );
    assert ( ( mb >= 0 ) && ( mb < xsize*ysize ) );

    value = 2 * integralImage[mb];
    value -= 2 * integralImage[mt];
    value +=   integralImage[tl];
    value +=   integralImage[tr];
    value -=   integralImage[bl];
    value -=   integralImage[br];
  } else if ( type == HaarFeature::HAARTYPE_DIAGONAL ) {
    int p2o = pos2 * xsize;
    assert ( ( p2o >= 0 ) && ( p2o < xsize*ysize ) );
    assert ( ( tl + pos1 >= 0 ) && ( tl + pos1 < xsize*ysize ) );
    assert ( ( tl + p2o >= 0 ) && ( tl + p2o < xsize*ysize ) );
    assert ( ( bl + pos1 >= 0 ) && ( bl + pos1 < xsize*ysize ) );
    assert ( ( tr + p2o >= 0 ) && ( tr + p2o < xsize*ysize ) );
    assert ( ( tl + pos1 + p2o >= 0 ) && ( tl + pos1 + p2o < xsize*ysize ) );

    value = integralImage[tl];
    value += integralImage[bl];
    value += integralImage[br];
    value += integralImage[tr];
    value -= 2 * (
               integralImage[tl+pos1]
               + integralImage[tl+p2o]
               + integralImage[bl+pos1]
               + integralImage[tr+p2o]
             );
    value += 4 * integralImage[tl + pos1 + p2o];

  } else if ( type == HaarFeature::HAARTYPE_3BLOCKS ) {
    assert ( ( tl + pos1 >= 0 ) && ( tl + pos1 < xsize*ysize ) );
    assert ( ( bl + pos2 >= 0 ) && ( bl + pos2 < xsize*ysize ) );
    assert ( ( tl + pos2 >= 0 ) && ( tl + pos2 < xsize*ysize ) );
    assert ( ( bl + pos1 >= 0 ) && ( bl + pos1 < xsize*ysize ) );
    value = integralImage[tl];
    value +=  integralImage[br];
    value -=  integralImage[bl];
    value -=  integralImage[tr];
    value += 2 * (
               - integralImage[tl+pos1]
               - integralImage[bl+pos2]
               + integralImage[tl+pos2]
               + integralImage[bl+pos1]
             );
  } else {
    return -1;
  }

  assert ( NICE::isFinite( value ) );
//#else
//  throw("not yet adapted for new MultiChannelImageT!");
//  double value = 0.0;
//#endif
  return value;
}

void HaarFeature::restore ( istream & is, int format )
{
  is >> type;
  is >> window_size_x;
  is >> window_size_y;
  is >> pos1;
  is >> pos2;
}

void HaarFeature::store ( ostream & os, int format ) const
{
  os << "HAARFEATURE" << " " << type << " "
  << window_size_x << " " << window_size_y
  << " " << pos1 << " " << pos2;
}

void HaarFeature::clear()
{
  // nothing to do in my opinion
}

