/** 
* @file HOGFeature.cpp
* @brief histogram of oriented gradients ( dalal and triggs )
* @author Erik Rodner
* @date 05/07/2008

*/
#include <iostream>

#include "HOGFeature.h"
#include "vislearning/cbaselib/FeaturePool.h"

using namespace OBJREC;

using namespace std;
using namespace NICE;


const double epsilon = 10e-8;

/** simple constructor */
HOGFeature::HOGFeature( const Config *conf )
{
    window_size_x = conf->gI("HOGFeature", "window_size_x", 21 );
    window_size_y = conf->gI("HOGFeature", "window_size_y", 21 );
    scaleStep = conf->gD("HOGFeature", "scale_step", sqrt(2) );
    numScales = conf->gI("HOGFeature", "num_scales", 5 );
    flexibleGrid = conf->gB("HOGFeature", "flexible_grid", false );

    numBins = conf->gI("HOGFeature", "num_bins", 9 );
    cellcountx = conf->gI("HOGFeature", "cellcountx", 10 );
    cellcounty = conf->gI("HOGFeature", "cellcounty", 10 );
}

/** simple destructor */
HOGFeature::~HOGFeature()
{
}

double HOGFeature::val( const Example *example ) const
{
    const NICE::MultiChannelImageT<double> & img = 
	example->ce->getDChannel ( CachedExample::D_INTEGRALEOH );
    int tm_xsize = img.xsize;
    int tm_ysize = img.ysize;

    int xsize;
    int ysize;
    example->ce->getImageSize ( xsize, ysize );

    /** without overlap: normalized cell and bin **/

    int wsx2, wsy2;
    int exwidth = example->width;
    if ( exwidth == 0 ) {
	wsx2 = window_size_x * tm_xsize / (2*xsize);
	wsy2 = window_size_y * tm_ysize / (2*ysize);
    } else {
	int exheight = example->height;
	wsx2 = exwidth * tm_xsize / (2*xsize);
	wsy2 = exheight * tm_ysize / (2*ysize);
    }
	
    int xx, yy;
    xx = ( example->x ) * tm_xsize / xsize;
    yy = ( example->y ) * tm_ysize / ysize;

    assert ( (wsx2 > 0) && (wsy2 > 0) );

    int xtl = xx - wsx2;
    int ytl = yy - wsy2;
    int xrb = xx + wsx2;
    int yrb = yy + wsy2;

#define BOUND(x,min,max) (((x)<(min))?(min):((x)>(max)?(max):(x)))
    xtl = BOUND ( xtl, 0, tm_xsize - 1 );
    ytl = BOUND ( ytl, 0, tm_ysize - 1 );
    xrb = BOUND ( xrb, 0, tm_xsize - 1 );
    yrb = BOUND ( yrb, 0, tm_ysize - 1 );
#undef BOUND

    double stepx = (xrb - xtl) / (double)( cellcountx );
    double stepy = (yrb - ytl) / (double)( cellcounty );
    int cxtl = (int)(xtl + stepx*cellx1);
    int cytl = (int)(ytl + stepy*celly1);
    int cxrb = (int)(xtl + stepx*cellx2);
    int cyrb = (int)(ytl + stepy*celly2);

    if ( cxrb <= cxtl ) cxrb = cxtl+1;
    if ( cyrb <= cytl ) cyrb = cytl+1;

    double A,B,C,D;

    assert ( bin < (int)img.numChannels );
    assert ( img.data[bin] != NULL );

    if ( (cxtl < 0) || (cxtl >= tm_xsize) )
    {
	fprintf (stderr, "cellcountx %d cellcounty %d\n", cellcountx, cellcounty );
	fprintf (stderr, "cxtl %d tm_xsize %d xsize %d\n", cxtl, tm_xsize, xsize );
	fprintf (stderr, "cellx1 %d stepx %f xtl %d xrb %d\n", cellx1, stepx, xtl, xrb );
    }
    if ( (cxrb < 0) || (cxrb >= tm_xsize) )
    {
	fprintf (stderr, "cellcountx %d cellcounty %d\n", cellcountx, cellcounty );
	fprintf (stderr, "cxrb %d tm_xsize %d xsize %d\n", cxrb, tm_xsize, xsize );
	fprintf (stderr, "cellx1 %d stepx %f xtl %d xrb %d\n", cellx1, stepx, xtl, xrb );
    }
    if ( (cytl < 0) || (cytl >= tm_ysize) )
    {
	fprintf (stderr, "cellcountx %d cellcounty %d\n", cellcountx, cellcounty );
	fprintf (stderr, "cytl %d tm_ysize %d ysize %d\n", cytl, tm_ysize, ysize );
	fprintf (stderr, "celly1 %d stepy %f ytl %d yrb %d\n", celly1, stepy, ytl, yrb );
    }
    if ( (cyrb < 0) || (cyrb >= tm_ysize) )
    {
	fprintf (stderr, "cellcountx %d cellcounty %d\n", cellcountx, cellcounty );
	fprintf (stderr, "cyrb %d tm_ysize %d ysize %d\n", cyrb, tm_ysize, ysize );
	fprintf (stderr, "celly1 %d stepy %f ytl %d yrb %d\n", celly1, stepy, ytl, yrb );
    }

    long kA = cxtl + cytl * tm_xsize;
    long kB = cxrb + cytl * tm_xsize;
    long kC = cxtl + cyrb * tm_xsize;
    long kD = cxrb + cyrb * tm_xsize;
    A = img.data[bin][ kA ];
    B = img.data[bin][ kB ];
    C = img.data[bin][ kC ];
    D = img.data[bin][ kD ];

    double val1 =  (D - B - C + A);
    double sum = val1*val1;
	for ( int b = 0 ; b < (int)img.numChannels ; b++)
	{
		if ( b == bin ) continue;
		A = img.data[b][ kA ];
		B = img.data[b][ kB ];
		C = img.data[b][ kC ];
		D = img.data[b][ kD ];
		double val = ( D - B - C + A );
		sum += val*val;
	}
    // FIXME: maybe L_1 normalization is sufficient
    sum = sqrt(sum);
    return ( val1 + epsilon ) / ( sum + epsilon );
}

void HOGFeature::explode ( FeaturePool & featurePool, bool variableWindow ) const
{
    int nScales = (variableWindow ? numScales : 1 );

    double weight = 1.0 / ( numBins * nScales );

    if ( flexibleGrid ) 
	weight *= 4.0 / ( cellcountx * (cellcountx - 1) * (cellcounty - 1) * cellcounty );
    else
	weight *= 1.0 / (cellcountx * cellcounty);

    for ( int i = 0 ; i < nScales ; i++ )
    {
	int wsy = window_size_y;
	int wsx = window_size_x;
	for ( int _cellx1 = 0 ; _cellx1 < cellcountx ; _cellx1++ )
	    for ( int _celly1 = 0 ; _celly1 < cellcounty ; _celly1++ )
		for ( int _cellx2 = _cellx1+1 ; 
			  _cellx2 < (flexibleGrid ? cellcountx : _cellx1+2) ; 
			  _cellx2++ )
		    for ( int _celly2 = _celly1+1 ; 
			      _celly2 < (flexibleGrid ? cellcounty : 
			      _celly1+2) ; _celly2++ )
			for ( int _bin = 0 ; _bin < numBins ; _bin++ )
			{
			    HOGFeature *f = new HOGFeature();
			    f->window_size_x = wsx;
			    f->window_size_y = wsy;
			    f->bin = _bin;
			    f->cellx1 = _cellx1;
			    f->celly1 = _celly1;
			    f->cellx2 = _cellx2;
			    f->celly2 = _celly2;
			    f->cellcountx = cellcountx;
			    f->cellcounty = cellcounty;
			    featurePool.addFeature ( f, weight ); 
			}
	wsx = (int) (scaleStep * wsx);
	wsy = (int) (scaleStep * wsy);
    }
}

Feature *HOGFeature::clone() const
{
    HOGFeature *f = new HOGFeature();
    f->window_size_x = window_size_x;
    f->window_size_y = window_size_y;
    f->bin = bin;
    f->cellx1 = cellx1;
    f->celly1 = celly1;
    f->cellx2 = cellx2;
    f->celly2 = celly2;
    f->cellcountx = cellcountx;
    f->cellcounty = cellcounty;
    f->flexibleGrid = flexibleGrid;

    return f;
}

Feature *HOGFeature::generateFirstParameter () const
{
    return clone();
}

void HOGFeature::restore (istream & is, int format)
{
    is >> window_size_x;
    is >> window_size_y;
    is >> bin;
    is >> cellx1;
    is >> celly1;

    is >> cellx2;
    is >> celly2;

    is >> cellcountx;
    is >> cellcounty;
}

void HOGFeature::store (ostream & os, int format) const
{
    os << "HOGFEATURE "
       << window_size_x << " "
       << window_size_y << " "
       << bin << " "
       << cellx1 << " "
       << celly1 << " ";

   os << cellx2 << " "
      << celly2 << " ";

    os << cellcountx << " "
       << cellcounty;
}

void HOGFeature::clear ()
{
}