/** 
* @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;
// refactor-nice.pl: check this substitution
// old: using namespace ice;
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
}