/**
* @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
{
  const NICE::MultiChannelImageT<long> & img = example->ce->getLChannel( CachedExample::L_INTEGRALIMAGE );

  const long *integralImage = img.data[0];
  int xsize = img.xsize;
  int ysize = img.ysize;

  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 );

  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( finite( value ) );

  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
}