/**
* @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.0f ) );
  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::MultiChannelImage3DT<double> & img =
    example->ce->getDChannel ( CachedExample::D_INTEGRALEOH );
  int tm_xsize = img.width();
  int tm_ysize = img.height();

  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.channels() );

  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.get ( cxtl, cytl, bin );
  B = img.get ( cxrb, cytl, bin );
  C = img.get ( cxtl, cyrb, bin );
  D = img.get ( cxrb, cyrb, bin );

  double val1 = ( D - B - C + A );
  double sum = val1 * val1;
  for ( int b = 0 ; b < ( int ) img.channels() ; b++ )
  {
    if ( b == bin ) 
      continue;
    A = img.get ( cxtl, cytl, b );
    B = img.get ( cxrb, cytl, b );
    C = img.get ( cxtl, cyrb, b );
    D = img.get ( cxrb, cyrb, b );
    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 ()
{
}