/** 
* @file HOGVectorFeatures.cpp
* @brief extract hog features
* @author Alexander Arest
* @date 06/08/2009

*/

#include <iostream>


#include "vislearning/features/simplefeatures/HOGVectorFeatures.h"
#include "core/image/Convert.h"

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

HOGVectorFeatures::HOGVectorFeatures ( const Config * conf, int _xsize, int _ysize): FeatureFactory ( conf )
{  
    numBins = conf->gI( "HOGVectorFeatures", "numbins", 9 );
    usesigned = conf->gB( "HOGVectorFeatures", "usesigned", false );
    
    amount_cell_x = conf->gI("HOGVectorFeatures", "cell_x", 4 );
    amount_cell_y = conf->gI("HOGVectorFeatures", "cell_y", 4 );
    block_x = conf->gI("HOGVectorFeatures", "block_x", 2 );
    block_y = conf->gI("HOGVectorFeatures", "block_y", 2 );
}

HOGVectorFeatures::~HOGVectorFeatures()
{
}

int HOGVectorFeatures::convert ( const NICE::Image & img, NICE::Vector & vec )
{
	NICE::FloatImage imgd( img.width(), img.height());
	
	grayToFloat(img, &imgd);
	
	float *imgdraw = imgd.getPixelPointerXY(0,0);

	long int k = 0;
	
	float *gradient = new float[ imgd.width() * imgd.height() ];

	unsigned char *graddir = new unsigned char[ imgd.width() * imgd.height() ];
	
	FastFilter::calcGradient<float,float,unsigned char> ( imgdraw, imgd.width(), imgd.height(), gradient, graddir, numBins, usesigned );

	NICE::FloatImage imgd_gradient(gradient, imgd.width(), imgd.height(), GrayColorImageCommonImplementation::noAlignment);
	NICE::Image imgd_graddir(graddir, imgd.width(), imgd.height(), GrayColorImageCommonImplementation::noAlignment);

	Image tmp;
	floatToGray(imgd_gradient, &tmp);
				
	showImage(tmp, "Grad");

	cout << 6 << endl;
	k = 0;
	
// ***** 6.3 Spatial/Orientation binning *****
	double x_size = imgd_gradient.width();
	double y_size = imgd_gradient.height();
	int cell_x = int ( ceil( x_size / amount_cell_x ) );
	int cell_y = int ( ceil( y_size / amount_cell_y ) );

	vector<vector<double> > array_cell_hist_vec;

	for (int i = 0; i < amount_cell_x; i++){
		for (int j = 0; j < amount_cell_y; j++){
			vector<double> cell_hist_vec(numBins,0);
			for (int k = 0; k < cell_x; k++){
				int x_coord, y_coord;
				for (int l = 0; l < cell_y; l++){
					x_coord = i*cell_x + k;
					y_coord = j*cell_y + l;
					if ((x_coord < x_size) && (y_coord < y_size)){
						double gradient = imgd_gradient.getPixel(x_coord,y_coord);
						int graddir = imgd_graddir.getPixel(x_coord,y_coord);
						cell_hist_vec[graddir] += gradient;
					}
					else break;
				}
			}
			array_cell_hist_vec.push_back(cell_hist_vec);
		}
	}

// **************************************************

// ***** 6.4 Normalization and Descriptor Block *****

	int block_part_x = amount_cell_x - block_x + 1;
	int block_part_y = amount_cell_y - block_y + 1;

	vector<double> fd;	// final descriptor

	for (int i = 0; i < block_part_x; i++)
	{
		for (int j = 0; j < block_part_y; j++)
		{
			vector<double> fdi;	// final descriptor intermediate
			for (int k = 0; k < block_x; k++)
			{
				for (int l = 0; l < block_y; l++)
				{
					int number = (i+k)*amount_cell_y + (j+l);
					fdi.insert(fdi.begin(),array_cell_hist_vec.at(number).begin(),array_cell_hist_vec.at(number).end());
				}
			}
			vector<double>::iterator it;
			it = fdi.begin();
			double norm_factor = 0.00000000001;	//epsilon^2
			while(it!=fdi.end())	{
				norm_factor += (*it)*(*it);
				it++;
			}
			double sqrt_norm_factor = sqrt(norm_factor);
			it = fdi.begin();
			while(it!=fdi.end())	{
				*it /= sqrt_norm_factor;
				it++;
			}
			fd.insert(fd.begin(),fdi.begin(),fdi.end());
		}		
	}

//**************************************************
	
	int fdSize = ( int ) fd.size();
	vec.resize( fdSize );
	for ( int i = 0; i < fdSize; i++ )
		vec[ i ] = fd[ i ];

	//delete [] imgdraw;
	delete [] gradient;
	delete [] graddir;
	
	return 0;
}