#ifdef NICE_USELIB_OPENMP
#include <omp.h>
#endif

#include "vislearning/features/regionfeatures/RFHoG.h"

#include <iostream>
#include "vislearning/baselib/FastFilter.h"

using namespace OBJREC;

using namespace std;
using namespace NICE;


RFHoG::RFHoG( const Config *_conf):RFStruct(_conf)
{
	numBins = conf->gI( "RFHoG", "numbins", 8 );
	usesigned = conf->gB( "RFHoG", "usesigned", false );
    
	amount_cell_x = conf->gI("RFHoG", "cell_x", 4 );
	amount_cell_y = conf->gI("RFHoG", "cell_y", 4 );
	block_x = conf->gI("RFHoG", "block_x", 2 );
	block_y = conf->gI("RFHoG", "block_y", 2 );
}

void RFHoG::extract ( const NICE::FloatImage &imgd_gradient, const NICE::Image &imgd_graddir, const RegionGraph &rg, const NICE::Matrix &mask, VVector & feats )
{
	int rgsize = rg.size();
	
	int block_part_x = amount_cell_x - block_x + 1;
	int block_part_y = amount_cell_y - block_y + 1;
	
	for(int r = 0; r < rgsize; r++)
	{
		Vector v(block_part_x * block_part_y * block_x * block_y * numBins);
		feats.push_back(v);
	}
	
#pragma omp parallel for
	for(int r = 0; r < rgsize; r++)
	{
		// ***** 6.3 Spatial/Orientation binning *****
		int x0,y0,x1,y1;
		rg[r]->getRect(x0,y0,x1,y1);
		double x_size = x1-x0;
		double y_size = y1-y0;
		double width = imgd_gradient.width();
		double height = 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;
		int pos = 0;
		for (int i = 0; i < amount_cell_x; i++){
			for (int j = 0; j < amount_cell_y; j++, pos++){
				vector<double> cell_hist_vec(numBins,0);
				array_cell_hist_vec.push_back(cell_hist_vec);
				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 + x0;
						y_coord = j*cell_y + l + y0;
						if ((x_coord <= x1) && (y_coord <= y_size+y1) && x_coord < width && y_coord < height){
							int reg = mask(x_coord, y_coord);
							//!TODO: eventuell auch mal ausprobieren mit Pixeln in Bounding Box, die nicht zur Region gehören
							if(reg != r)
								continue;
							
							double gradient = imgd_gradient.getPixel(x_coord,y_coord);
							int graddir = imgd_graddir.getPixel(x_coord,y_coord);
							array_cell_hist_vec[pos][graddir] += gradient;
						}
						else break;
					}
				}
			}
		}
	// **************************************************
	
	// ***** 6.4 Normalization and Descriptor Block *****	
		int co = 0;
		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 = 1e-10;	//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())
				{
					feats[r][co] = (*it) / sqrt_norm_factor;
					it++;
					co++;
				}
			}		
		}
	}
}

RFHoG::~RFHoG()
{
	
}