/** 
* @file Image_tools.cpp
* @brief Contains tools for Image_Processing
* @author Alexander Lütz
* @date 18/11/2010
*/

#include "Image_tools.h"
// #include "vislearning/cbaselib/MultiDataset.h" //only for showImage()
#include <cmath> //floor
#include <algorithm> //min

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

/**
* @brief Simple constructor
* @author Alexander Lütz
* @date  18/11/2010
*/
Image_tools::Image_tools() {
}

/**
* @brief Simple destructor
* @author Alexander Lütz
* @date 18/11/2010
*/
Image_tools::~Image_tools() {

}

/**
* @brief Calculates Gradient-X-Image and Gradient-Y-Image for a Greyscale-Image
* @author Alexander Lütz
* @date 18/11/2010
*/
void Image_tools::calculateGradients(const NICE::Image & origImage, NICE::ImageT<float> & grad_x_Image, NICE::ImageT<float> & grad_y_Image )
{
	grad_x_Image.resize(origImage.width(), origImage.height());
	grad_y_Image.resize(origImage.width(), origImage.height());
	//init
	grad_x_Image.set(0.0);
	grad_y_Image.set(0.0);
	
	//image_border
	for (int j = 0; j < origImage.height(); j++)
	{
		grad_x_Image.setPixel(0,j, 0);
		grad_x_Image.setPixel(origImage.width()-1,j,0);
	}

	for (int i = 0; i < origImage.width(); i++)
	{
		grad_y_Image.setPixel(i,0,0);
		grad_y_Image.setPixel(i,origImage.height()-1, 0);
	}

	//init
	int left = 0;
	int actual = 0;
	int right = 0;

	//inside image
	for (int y = 0; y < origImage.height(); y++)
	{
		actual = origImage(0,y);
		right = origImage(1,y);
		for (int x = 1; x < (origImage.width()-1); x++)
		{
			left = actual;
			actual = right;
			right = origImage(x+1,y);
			grad_x_Image.setPixel(x,y,-left+right);
		}
	}

	//inside image
	for (int x = 0; x < origImage.width(); x++)
	{
		actual = origImage(x,0);
		right = origImage(x,1);
		for (int y = 1; y < (origImage.height()-1); y++)
		{
			left = actual;
			actual = right;
			right = origImage(x,y+1);
 			grad_y_Image.setPixel(x,y,-left+right);
		}
	}

}

/**
* @brief Calculates Gradient-X-Image and Gradient-Y-Image for a ColorImage (RGB)
* @author Alexander Lütz
* @date 18/11/2010
*/
void Image_tools::calculateGradients(NICE::ColorImage origColorImage, NICE::ImageT<float> & grad_x_Image, NICE::ImageT<float> & grad_y_Image)
{
	grad_x_Image = NICE::ImageT<float>(origColorImage.width(), origColorImage.height());
	grad_y_Image = NICE::ImageT<float>(origColorImage.width(), origColorImage.height());

	NICE::ImageT<float> grad_x_Image_R;
	NICE::ImageT<float> grad_x_Image_G;
	NICE::ImageT<float> grad_x_Image_B;
	
	NICE::ImageT<float> grad_y_Image_R;
	NICE::ImageT<float> grad_y_Image_G;
	NICE::ImageT<float> grad_y_Image_B;

	NICE::Image * channel_Image_R = origColorImage.getChannel(0);
	NICE::Image * channel_Image_G = origColorImage.getChannel(1);
	NICE::Image * channel_Image_B = origColorImage.getChannel(2);
	calculateGradients(*channel_Image_R, grad_x_Image_R,  grad_y_Image_R);
	calculateGradients(*channel_Image_G, grad_x_Image_G,  grad_y_Image_G);
	calculateGradients(*channel_Image_B, grad_x_Image_B,  grad_y_Image_B);

	//use maximum in each pixel
	for (int y = 0; y < origColorImage.height(); y++)
		for (int x = 0; x < origColorImage.width(); x++)
		{
			int max_x(grad_x_Image_R.getPixel(x,y));
			int max_y(grad_y_Image_R.getPixel(x,y));
			if (abs(grad_x_Image_G.getPixel(x,y)) > abs(max_x))
				max_x = grad_x_Image_G.getPixel(x,y);
			if (abs(grad_y_Image_G.getPixel(x,y)) > abs(max_y))
				max_y = grad_y_Image_G.getPixel(x,y);
			if (abs(grad_x_Image_B.getPixel(x,y)) > abs(max_x))
				max_x = grad_x_Image_B.getPixel(x,y);
			if (abs(grad_y_Image_B.getPixel(x,y)) > abs(max_y))
				max_y = grad_y_Image_B.getPixel(x,y);
			grad_x_Image.setPixel(x,y,max_x);
			grad_y_Image.setPixel(x,y,max_y);
		}
}

/**
* @brief Calculates Gradient-orientations, only possible, if number_Of_Bins smaller than 256
* @author Alexander Lütz
* @date 18/11/2010
*/
void Image_tools::calculateGradientOrientations(const NICE::ImageT<float> & grad_x_Image, const NICE::ImageT<float> & grad_y_Image , const int & number_Of_Bins, NICE::Image & gradient_orientations, const bool unsignedBins)
{
	gradient_orientations = NICE::Image(grad_x_Image.width(), grad_x_Image.height());

	double bin_width = 180.0/number_Of_Bins;
	if (!unsignedBins)
		bin_width *= 2.0;

	for (int y = 0; y < grad_x_Image.height(); y++) 
	{
		for (int x = 0; x < grad_x_Image.width(); x++) 
		{
			double angle = (atan2(grad_x_Image.getPixel(x,y),grad_y_Image.getPixel(x,y)) + M_PI)*180/M_PI;
//NOTE It would be better, if we would subtract 1/2 binsize, but Anna Bosch hasn't done it in her original paper, so we won't do it as well
// 			angle = abs (angle*180/M_PI -0.5*bin_width); //atan2 and - 1/2 bin

			while (angle >= 360.0)
				angle -= 360.0;
			while (angle < 0.0) //can not be reached, but doesn't matter
				angle += 360.0;

			if (unsignedBins)
			{
				while (angle>=180.0)
					angle -= 180.0;
			}


//NOTE Update 2011-02-10: ceil is ok, if the indicees are from 1 to number_Of_Bins. Of course we do NOT want this, but instaed we deal with 0 to number_Of_Bins-1. Therefor floor is our choice!
			int bin = (int) floor(angle/bin_width );

			gradient_orientations.setPixel(x,y,bin);
			//TODO some error message, throwing an exception, whatever
			if (bin > number_Of_Bins) 
				cerr << "Image_tools::calculateGradientOrientations bin " << bin << " > number_Of_Bins " << number_Of_Bins << "with angle " << angle << " and bin_width " << bin_width << endl;
		}	
	}
	
	NICE::Image gradient_orientations_visual (grad_x_Image.width(), grad_x_Image.height());
	for (int y = 0; y < grad_x_Image.height(); y++) 
	{	
		for (int x = 0; x < grad_x_Image.width(); x++) 
		{
			gradient_orientations_visual(x,y) = (int) floor(255 * ((double)gradient_orientations(x,y)/number_Of_Bins));
		}
	}
// 	gradient_orientations_visual.writePGM("/home/luetz/foo.pgm");
}

/**
* @brief Calculates Gradient-orientations
* @author Alexander Lütz
* @date 18/11/2010
*/
void Image_tools::calculateGradientOrientations(const NICE::GrayImage16s & grad_x_Image, const NICE::GrayImage16s & grad_y_Image , const int & number_Of_Bins, NICE::Image & gradient_orientations, const bool unsignedBins)
{
	gradient_orientations = NICE::Image(grad_x_Image.width(), grad_x_Image.height());

	double bin_width = 180.0/number_Of_Bins;
	if (!unsignedBins)
		bin_width *= 2.0;

	for (int y = 0; y < grad_x_Image.height(); y++) 
	{
		for (int x = 0; x < grad_x_Image.width(); x++) 
		{

			double angle = (atan2( (double)grad_x_Image.getPixel(x,y),(double)grad_y_Image.getPixel(x,y)) + M_PI)*180/M_PI;
//NOTE It would be better, if we would subtract 1/2 binsize, but Anna Bosch hasn't done it in her original paper, so we won't do it as well
// 			angle = abs (angle*180/M_PI -0.5*bin_width); //atan2 and - 1/2 bin

			while (angle >= 360.0)
				angle -= 360.0;

			if (unsignedBins)
			{
				while (angle>=180.0)
					angle -= 180.0;
			}

//NOTE Update 2011-02-10: ceil is ok, if the indicees are from 1 to number_Of_Bins. Of course we do NOT want this, but instaed we deal with 0 to number_Of_Bins-1. Therefor floor is our choice!
			int bin = (int) floor(angle/bin_width );

			gradient_orientations.setPixel(x,y,bin);
		}
	}
/*
	NICE::Image gradient_orientations_visual (grad_x_Image.width(), grad_x_Image.height());
	for (int y = 0; y < grad_x_Image.height(); y++) 
	{	
		for (int x = 0; x < grad_x_Image.width(); x++) 
		{
			gradient_orientations_visual(x,y) = (int) floor(255 * ((double)gradient_orientations(x,y)/number_Of_Bins));
		}
	}
	gradient_orientations_visual.writePGM("/home/luetz/foo.pgm");*/
// 	showImage(gradient_orientations_visual);
}

/**
* @brief Calculates Gradient-magnitudes
* @author Alexander Lütz
* @date 18/11/2010
*/
void Image_tools::calculateGradientMagnitudes(const NICE::ImageT<float>  & grad_x_Image, const NICE::ImageT<float> & grad_y_Image, NICE::ImageT<float> & gradient_magnitudes)
{
	//init
	gradient_magnitudes.resize(grad_x_Image.width(), grad_x_Image.height());
	gradient_magnitudes.set(0.0);

	for (int y = 0; y < grad_x_Image.height(); y++) 
		for (int x = 0; x < grad_x_Image.width(); x++) 
		{
			float magnitude = sqrt(pow(grad_x_Image.getPixel(x,y),2) + pow(grad_y_Image.getPixel(x,y),2));
			gradient_magnitudes.setPixel(x,y,magnitude);
		}

// 	NICE::Image gradient_magnitudes_visual = NICE::Image(grad_x_Image.width(), grad_x_Image.height());
// 	for (int y = 0; y < grad_x_Image.height(); y++) 
// 	{	
// 		for (int x = 0; x < grad_x_Image.width(); x++) 
// 		{
// 			gradient_magnitudes_visual(x,y) = (int) floor( ((double)gradient_magnitudes(x,y)/sqrt(2)));
// 		}
// 	}
// 	showImage(gradient_magnitudes_visual);
}

/**
* @brief Normalized a descriptor Block, using L2-Norm, not implemented completely. Just have a look in the original paper of Dalal and Triggs for further details.
* @author Alexander Lütz
* @date 22/11/2010
*/
std::vector<float> Image_tools::normalizeBlockDescriptor(const std::vector<float> & orig_Block_Descriptor, const float epsilon)
{
	double sum_of_squares = pow(epsilon,2);
	for (std::vector<float>::const_iterator it = orig_Block_Descriptor.begin(); it != orig_Block_Descriptor.end(); it++)
	{
		sum_of_squares += pow((*it),2);
	}

	std::vector<float> normalized_Block_Descriptor;

	for (std::vector<float>::const_iterator it = orig_Block_Descriptor.begin(); it != orig_Block_Descriptor.end(); it++)
	{
		normalized_Block_Descriptor.push_back((*it)/sum_of_squares);
	}

	return normalized_Block_Descriptor;
}

/**
* @brief calculates the resulting HoG-Features for an image by normalizing spatial blocks und storing the resulting normalized histograms in a vector - not implemented up to now
* @author Alexander Lütz
* @date 22/11/2010
*/
std::vector< std::vector<float> > Image_tools::calculateResultingHogFeatures(const NICE::Image & gradient_orientations, const NICE::ImageT<float> & gradient_magnitudes, const int & blocksize, const int & cellsize)
{
	std::vector< std::vector<float> > HoG_features;
	return HoG_features;
}