#include <FaceClassifier/miscellaneous.h>
#include <FaceClassifier/LBPSVMFaceClassifier.h>

#include <opencv/cvaux.h>

#define PI 3.1416

namespace face {

void LBPSVMFaceClassifier::init(const std::list<SPLIT> split, const int p, const double r)
{
	int i, j, k;
	this->p = p;
	this->r = r;
	this->split = split;
	this->ceilingR = (int)ceil(r);
	this->widthOfPreprocessedImg = this->width - (int)(2 * ceilingR);
	this->heightOfPreprocessedImg = this->height - (int)(2 * ceilingR);

	// calculating the vectors pointing from a central point
	this->vector = new double*[p];
	for (i = 0; i < p; i++) {
		vector[i] = new double[2];
	}
	for (i = 0; i < p; i++) { // counter-clockwise on the image
		vector[i][0] = misc::roundPrec(r * cos(2.0 * PI / p * i), 4);
		vector[i][1] = -misc::roundPrec(r * sin(2.0 * PI / p * i), 4);
	}

	std::list<SPLIT>::const_iterator itr;
	for (itr = split.begin(); itr != split.end(); itr++) {

		int nx = (int)(this->widthOfPreprocessedImg / (*itr).w), ny = (int)(this->heightOfPreprocessedImg / (*itr).h);
		int startX = (int)((this->widthOfPreprocessedImg - nx * (*itr).w) / 2);
		int startY = (int)((this->widthOfPreprocessedImg - ny * (*itr).h) / 2);
		k = 0;
		for (i = 0; i < ny; i++) {
			for (j = 0; j < nx; j++) {
				rect.push_back(cvRect(startX + j * (*itr).w, startY + i * (*itr).h, (*itr).w, (*itr).h));
				k++;
			}
		}
		rectOfSplit.push_back(k);
	}
	this->nOfFeatures = rect.size() * (int)pow(2.0, p);
}

LBPSVMFaceClassifier::LBPSVMFaceClassifier()
{
}

LBPSVMFaceClassifier::LBPSVMFaceClassifier(const std::string name, const std::map<float, std::string> classLabel, const int width, const int height, const std::list<SPLIT> split, const int p, const double r) : SVMFaceClassifier(name, classLabel, width, height)
{
	init(split, p, r);
	std::cout << ToString() << " constructed.\n";
}

LBPSVMFaceClassifier::LBPSVMFaceClassifier(const std::string name, const std::map<float, std::string> classLabel, const int width, const int height, const std::list<SPLIT> split, const int p, const double r, const std::string modelFile) : SVMFaceClassifier(name, classLabel, width, height, modelFile)
{
	init(split, p, r);
	std::cout << ToString() << " constructed.\n";
}

LBPSVMFaceClassifier::~LBPSVMFaceClassifier()
{
}

std::string LBPSVMFaceClassifier::ToString()
{
	std::stringstream ss;
	ss << "LBPSVMClassifier [\n" <<
		"\tname = \"" + name + "\"\n";
	if (modelFile != "") ss << "\tmodelFile = \"" + modelFile + "\"\n";
	ss << "\tclassLabels [\n";
	if (!classLabel.empty()) {
		std::map<float, std::string>::iterator itr;
		for (itr = classLabel.begin(); itr != classLabel.end(); itr++) {
			ss << "\t\t" << (*itr).first << " refers to \"" + (*itr).second + "\"\n";
		}
	}
	ss << "\t]\n\twidth = " << width << "\n\theight = " << height
		<< "\n\twidthOfPreprocessedImg = " << widthOfPreprocessedImg << "\n\theightOfPreprocessedImg = " << heightOfPreprocessedImg;
	ss << "\n\tsplits [\n";
	if (!split.empty()) {
		std::list<SPLIT>::iterator itr;
		for (itr = split.begin(); itr != split.end(); itr++) {
			ss << "\t\t(" << (*itr).w << ", " << (*itr).h << ")\n";
		}
	}
	ss << "\t]\n\tp = " << p << "\n\tr = " << r << "\n\tnOfFeatures = " << nOfFeatures << "\n]";
	return ss.str();
}

double LBPSVMFaceClassifier::bilinearInterpolation(const double f00, const double f01, const double f10, const double f11, const double x, const double y) {
	return f00 * (1 - x) * (1 - y) + f10 * x * (1 - y) + f01 * (1 - x) * y + f11 * x * y;
}

double LBPSVMFaceClassifier::interpolationAt(const IplImage * const img, const double x, const double y) {
	int floorX, floorY;
	double f00, f01, f10, f11, fractionX, fractionY;
	floorX = (int)x; floorY = (int)y;
	fractionX = x - floorX; fractionY = y - floorY;
	if (fractionX == 0 && fractionY == 0) {
		return cvGet2D(img, floorY, floorX).val[0]; // if we are on a rasterpoint
	}
	if (fractionX == 0) { // if we are on a rasterline in the y direction
		f00 = f10 = cvGet2D(img, floorY, floorX).val[0];
		f01 = f11 = cvGet2D(img, floorY + 1, floorX).val[0];
	}
	if (fractionY == 0) { // if we are on a rasterline in the x direction
		f00 = f01 = cvGet2D(img, floorY, floorX).val[0];
		f10 = f11 = cvGet2D(img, floorY, floorX + 1).val[0];
	}
	if (fractionX != 0 && fractionY != 0) { // general case
		f00 = cvGet2D(img, floorY, floorX).val[0];
		f01 = cvGet2D(img, floorY + 1, floorX).val[0];
		f10 = cvGet2D(img, floorY, floorX + 1).val[0];
		f11 = cvGet2D(img, floorY + 1, floorX + 1).val[0];
	}
	return bilinearInterpolation(f00, f01, f10, f11, fractionX, fractionY);
}

int LBPSVMFaceClassifier::LBPCodeAt(const IplImage * const img, const int x, const int y) // x and y are in the resized image's coordinate system
{
	int i, LBPCode = 0;
	double threshold = (double)(cvGet2D(img, y, x).val[0]);
	double temp;
	for (i = 0; i < p; i++) {
		temp = interpolationAt(img, (double)x + vector[i][0], (double)y + vector[i][1]);
		if (temp >= threshold) LBPCode += (int)pow(2.0, i);
	}
	return LBPCode;
}

IplImage *LBPSVMFaceClassifier::Preprocess(const IplImage * const img)
{
	int i, j;
	CvScalar px;
	IplImage *grayImg = cvCreateImage(cvSize(img->width, img->height), img->depth, 1);
	IplImage *resizedImg = cvCreateImage(cvSize(this->width, this->height), img->depth, 1);
//	IplImage *newImg = cvCreateImage(cvSize(widthOfPreprocessedImg, heightOfPreprocessedImg), IPL_DEPTH_16U, 1);
	IplImage *newImg = cvCreateImage(cvSize(widthOfPreprocessedImg, heightOfPreprocessedImg), IPL_DEPTH_8U, 1);
	if (img->nChannels == 1) {
		cvCopyImage(img, grayImg);
	}
	else {
		cvCvtColor(img, grayImg, CV_RGB2GRAY);
	}
	cvEqualizeHist(grayImg, grayImg);
	cvResize(grayImg, resizedImg, CV_INTER_LINEAR);

	for (i = 0; i < heightOfPreprocessedImg; i++) {
		for (j = 0; j < widthOfPreprocessedImg; j++) {
			px.val[0] = LBPCodeAt(resizedImg, j + ceilingR, i + ceilingR);
			cvSet2D(newImg, i, j, px);
		}
	}
	cvReleaseImage(&grayImg);
	cvReleaseImage(&resizedImg);
	return newImg;
}

void LBPSVMFaceClassifier::CreateFeatureVector(const IplImage * const img, CvMat * const fv)
{
	std::list<CvRect>::iterator itr;
/*	for (itr = rect.begin(); itr != rect.end(); itr++) {
		printf("[%i,%i]\n", (*itr).x, (*itr).y);
	}*/
	int arraySize = (int)pow(2.0, p);
	int *histogram;
	int i, j;
	float *ptr = (float*)fv->data.ptr;
	IplImage *preprocessedImg = Preprocess(img);
	for (itr = rect.begin(); itr != rect.end(); itr++) {
		histogram = new int[arraySize];
		for (i = 0; i < arraySize; i++) {
			histogram[i] = 0;
		}
		int startX, startY, endX, endY;
		startX = (*itr).x; endX = (*itr).x + (*itr).width;
		startY = (*itr).y; endY = (*itr).y + (*itr).height;
		for (i = startY;  i < endY; i++) {
			for (j = startX; j < endX; j++) {
				histogram[(int)cvGet2D(preprocessedImg, i, j).val[0]]++;
			}
		}
		for (i = 0; i < arraySize; i++) {
			*ptr++ = (float)histogram[i];
		}
		delete [] histogram;
	}
	cvReleaseImage(&preprocessedImg);
}

}
