#include "EigenFaceDetector.h"

cv::UMat _gWeight;
cv::UMat _gPhi;
cv::UMat _gPhi_reconstruct;

void thermoCamera::EigenFaceDetector::Init()
{
	_LoadMatrix(_locationOfMean, _mean, "mean");
	_LoadMatrix(_locationOfEigenVectors, _eigenvectors, "eigenvectors");
	_eigenvectors.convertTo(_eigenvectors, 6);
	//_LoadMatrix(_locationOfWeights, _weights, "weight");

	//std::ifstream input_file(_locationOfMaxWeight);
	//if (!input_file.is_open()) throw thermoCamera::MyError::cannotOpenFile;
	//input_file >> _maxWeight;
	//input_file.close();

	_isInit = true;
	
}

bool thermoCamera::EigenFaceDetector::FindFace(cv::UMat& image, cv::Rect* rect, const int numOfRect, int& numOfFace)
{
	if (image.empty()) throw thermoCamera::MyError::badInput;
	if (!rect) throw thermoCamera::MyError::badInput;
	if(numOfRect < 0 || numOfRect > 4) throw thermoCamera::MyError::badInput;

	if (_isInit) _flag = Flag::enabled;

	_bbox.clear();

	int numOfRectangles = 0;
	int width = image.cols;
	int height = image.rows;
	cv::UMat InputImage = image.clone();
	cv::resize(image, image, cv::Size(width / 2, height / 2),cv::INTER_AREA);

	bool pp = true;
	float fWidth;
	float fHeight;
	while (pp)
	{
		fWidth = image.cols;
		fHeight = image.rows;
		//Calculate 95% of the image width and height
		width = fWidth - ((fWidth / 100) * 20);
		height = fHeight - ((fHeight / 100) * 20);

		//if (width == image.cols || height == image.rows)
			//break;
		if (width <= 20 || height <= 20) break;

		const clock_t begin_time = clock();

		_SlideWindow(image, numOfRectangles, InputImage.cols, InputImage.rows,false);

		float tim = float(clock() - begin_time) / CLOCKS_PER_SEC;
		wchar_t txt[20];
		swprintf_s(txt, 20, L"%.1f", tim);
		//MessageBoxW(NULL, LPCWSTR(txt), L"Error", MB_OK);

		resize(image, image, cv::Size(width, height), cv::INTER_AREA);
		
		if (_flag == Flag::disabled) break;
		//pp = false;
	}
	
	//bool pp = true;
	//while (true)
	//{
	//	//Calculate 95% of the image width and height
	//	width = image.cols - ((image.cols / 100) * 20);
	//	height = image.rows - ((image.rows / 100) * 20);

	//	if (width == image.cols || height == image.rows)
	//		break;

	//	_SlideWindow(image, numOfRectangles, InputImage.cols, InputImage.rows, true);

	//	resize(image, image, cv::Size(width, height), cv::INTER_AREA);

	//	if (_flag == Flag::disabled) break;
	//	pp = false;
	//}
	if (_flag == Flag::computing) _flag = Flag::enabled;

	// Show
	cv::UMat inputClear = InputImage.clone();

	cv::Rect rectFromBbox;
	//for (size_t i = 0; i < numOfRectangles; i++)
	//{
	//	_GetRectFromBbox(rectFromBbox, i);
	//	rectangle(InputImage, rectFromBbox, cv::Scalar(255), 1, 8, 0);
	//}
	//cv::imshow("Hey", InputImage);
	//cv::waitKey(100);

	Bbox* bBox = _bbox.data();
	int numOfIndex = fast_nms(bBox, numOfRectangles, 0.2f);

	for (size_t i = 0; i < numOfIndex; i++)
	{
		_GetRectFromBbox(rectFromBbox, _index[i]);
		//rectangle(InputImage, rectFromBbox, cv::Scalar(255), 1, 8, 0);
		rect[i] = rectFromBbox;
	}
	//cv::imshow("Hey", InputImage);
	//cv::waitKey(100);

	numOfFace = numOfIndex;
}

void thermoCamera::EigenFaceDetector::_LoadMatrix(const char* filePath, cv::UMat& mat, const char* name)
{
	if (!filePath) throw thermoCamera::MyError::badInput;
	if (!name) throw thermoCamera::MyError::badInput;

	cv::Mat in;

	cv::FileStorage fs(filePath, cv::FileStorage::READ);
	fs[name] >> in;
	fs.release();

	in.copyTo(mat);
}

void thermoCamera::EigenFaceDetector::_MatrixToVector(cv::UMat& vector, cv::UMat& matrix)
{
	if (matrix.empty()) throw thermoCamera::MyError::badInput;

	uint N = matrix.cols * matrix.rows;

	vector = matrix.reshape(0, N);
}

void thermoCamera::EigenFaceDetector::_VectorToMatrix(cv::UMat& vector, cv::UMat& matrix, unsigned int N)
{
	if (vector.empty()) throw thermoCamera::MyError::badInput;
	if (N == 0 ) throw thermoCamera::MyError::badInput;

	matrix = vector.reshape(0, N);
}

int thermoCamera::EigenFaceDetector::_SlideWindow(cv::UMat& img, int &numOfRect, int width, int height, bool show)
{
	if (img.empty()) throw thermoCamera::MyError::badInput;

	cv::UMat DrawResultGrid = img.clone();
	cv::UMat DrawResultHere;
	cv::Rect windows;
	cv::UMat Roi;

	int widthOrigin = 640;
	int heightOrigin = 480;
	float score = 0;
	if (_flag == Flag::enabled) _flag = Flag::computing;

	// Cycle row step
	for (int row = 0; row <= img.rows - _windows_n_rows; row += _StepSlide)
	{
		// Cycle col step
		for (int col = 0; col <= img.cols - _windows_n_cols; col += _StepSlide)
		{
			// There could be feature evaluator  over Windows
			
			// resulting window   
			windows = { col, row, _windows_n_rows, _windows_n_cols };

			DrawResultHere = img.clone();

			// Draw only rectangle
			rectangle(DrawResultHere, windows, cv::Scalar(255), 1, 8, 0);

			// Select windows roi
			Roi = img(windows);
			//cv::imshow("Hey", Roi);
			//cv::waitKey(50);
			Roi = Roi.clone();
			cv::resize(Roi, Roi, cv::Size(_eigenFaceSize, _eigenFaceSize),cv::INTER_LINEAR);

			cv::rectangle(DrawResultGrid, windows, cv::Scalar(255), 1, 8, 0);
			
			if (_ComputeError(Roi, _mean, _eigenvectors, _K, _minErr,score))
			{
				_StretchWindow(windows, img, width, height);
				_AddNewRectangle(windows, numOfRect, score);
			}


			// Show grid 
			if (show) {
				cv::namedWindow("Show Grid", cv::WINDOW_AUTOSIZE);
				cv::imshow("Show Grid", DrawResultHere);
				cv::waitKey(1000);
			}
			if (_flag == Flag::disabled) break;
		}
		if (_flag == Flag::disabled) break;
	}
}

void thermoCamera::EigenFaceDetector::_ComputeWeights(cv::UMat& phi, cv::UMat& eigenvectors, cv::UMat& weights, int K)
{
	if (phi.empty()) throw thermoCamera::MyError::badInput;
	if (eigenvectors.empty()) throw thermoCamera::MyError::badInput;

	//cv::UMat eigenvectorsTT = eigenvectors.t();

	//solve(eigenvectorsTT, phi, weights, cv::DECOMP_NORMAL);

	cv::UMat phi2 = phi.clone();
	cv::normalize(phi2, phi2, 1, cv::NORM_L1);

	MyType var = 0;
	cv::UMat result;
	cv::UMat ps;
	weights = cv::UMat(K,1, CV_64F);
	for (int i = 0; i < K; i++)
	{

		var = 0;
		//for (size_t j = 1; j <= phi2.rows; j++)
		//{
			////var += eigenvectors.getMat(cv::ACCESS_READ).at<MyType>(cv::Point(j, i)) * phi2.getMat(cv::ACCESS_READ).at<MyType>(cv::Point(0, j));
			//cv::Mat s1 = (cv::Mat_<double>(1, 3) << 0, -1, 2);
			//cv::Mat s2 = (cv::Mat_<double>(3, 1) << 0, 1, 3);
			//cv::UMat src1;
			//s1.copyTo(src1);
			//cv::UMat src2;
			//s2.copyTo(src2);
			//cv::UMat dst;
			//cv::gemm(src1, src2, 1, cv::noArray(), 0, dst);
			////cv::multiply(src1, src2,dst);
		//}

		cv::gemm(eigenvectors.row(i), phi2.col(0), 1, cv::noArray(), 0, result);
		cv::add(weights.row(i), result, weights.row(i));
		//ps.push_back(result);
		//weights.getMat(cv::ACCESS_WRITE).push_back(var);
	}
}

void thermoCamera::EigenFaceDetector::_ComputePhiMatrix(cv::UMat& face, cv::UMat& mean, cv::UMat& phi, int type, const cv::Size size, const uint MaxNumOfPics)
{
	if (face.empty()) throw thermoCamera::MyError::badInput;
	if (mean.empty()) throw thermoCamera::MyError::badInput;
	if (size.width <= 0 || size.height <= 0) throw thermoCamera::MyError::badInput;
	if (MaxNumOfPics == 0) throw thermoCamera::MyError::badInput;

	uint buff = size.width * size.height;
	MyType var;
	cv::UMat vec;
	_MatrixToVector(vec, mean);
	if (MaxNumOfPics == 1)
	{
		phi = cv::UMat(buff, 1, CV_64F);
		cv::subtract(face.col(0), vec.col(0), phi.col(0));
	}
	else
	{
		phi = cv::UMat(buff, MaxNumOfPics, CV_64F);
		for (int M = 0; M < MaxNumOfPics; M++)
		{
			cv::subtract(face.col(M), vec.col(0), phi.col(M));
		}
	}
}

void thermoCamera::EigenFaceDetector::_ReconstructFromPhi(cv::UMat& phi, cv::UMat& mean)
{
	if (phi.empty()) throw thermoCamera::MyError::badInput;
	if (mean.empty()) throw thermoCamera::MyError::badInput;

	cv::UMat eigen = phi.clone();
	_MatrixToVector(mean, mean);
	normalize(eigen, eigen, 0, 255, cv::NORM_MINMAX);
	eigen.convertTo(eigen, 0);
	_VectorToMatrix(eigen, eigen, _eigenFaceSize);
	//resize(eigen, eigen, cv::Size(200, 200),cv::INTER_AREA);
	//cv::imshow("Mean", eigen);
	//cv::waitKey(100);
}

bool thermoCamera::EigenFaceDetector::_ComputeError(cv::UMat & input, cv::UMat & mean, cv::UMat & eigenvectors, int K, float minTh, float& score)
{
	if (input.empty()) throw thermoCamera::MyError::badInput;
	if (mean.empty()) throw thermoCamera::MyError::badInput;
	if (eigenvectors.empty()) throw thermoCamera::MyError::badInput;

	int width = input.cols;
	int height = input.rows;
	_gPhi.release();
	_gPhi_reconstruct = cv::UMat::zeros(cv::Size(1, width * height), 6);
	_MatrixToVector(input, input);
	_ComputePhiMatrix(input, mean, _gPhi, 6, cv::Size(width, height), 1);
	_ComputeWeights(_gPhi, eigenvectors, _gWeight, K);
	_ReconstructFromWeights(_gPhi_reconstruct, mean, _gWeight, eigenvectors, K);
	_ReconstructFromPhi(_gPhi, mean);
	_ReconstructFromPhi(_gPhi_reconstruct, mean);
	/*_MatrixToVector(mean, mean);
	phi += mean;
	phi_reconstruct += mean;
	phi.convertTo(phi, 0);
	normalize(phi_reconstruct, phi_reconstruct, 0, 255, cv::NORM_MINMAX);
	phi_reconstruct.convertTo(phi_reconstruct, 0);*/

	//cv::UMat err;
	//MyType err = cv::norm(phi, phi_reconstruct, cv::NORM_L2);
	//cv::matchTemplate(phi, phi_reconstruct, err, cv::TM_CCOEFF_NORMED);
	
	MyType err = cv::norm(_gPhi, _gPhi_reconstruct, cv::NORM_L2);
	err /= 1000000;

	if (err == 0)
	{
		score = -1;
		return true;
	}
	//std::cout << err << std::endl;
	score = 1/err;
	return (err < minTh) ? true : false;
}

//bool thermoCamera::EigenFaceDetector::_ComputeError(cv::UMat& input, cv::UMat& mean, cv::UMat& eigenvectors, int K, float minTh, float& score)
//{
//	if (input.empty()) throw thermoCamera::MyError::badInput;
//	if (mean.empty()) throw thermoCamera::MyError::badInput;
//	if (eigenvectors.empty()) throw thermoCamera::MyError::badInput;
//
//	cv::UMat weights;
//	int width = input.cols;
//	int height = input.rows;
//	cv::UMat phi;
//	cv::UMat phi_reconstruct = cv::UMat::zeros(cv::Size(1, width * height), 6);
//	_MatrixToVector(input, input);
//	_ComputePhiMatrix(input, mean, phi, 6, cv::Size(width, height), 1);
//	_ComputeWeights(phi, eigenvectors, weights, K);
//	_ReconstructFromWeights(phi_reconstruct, mean, weights, eigenvectors, K);
//	_ReconstructFromPhi(phi, mean);
//	_ReconstructFromPhi(phi_reconstruct, mean);
//	/*_MatrixToVector(mean, mean);
//	phi += mean;
//	phi_reconstruct += mean;
//	phi.convertTo(phi, 0);
//	normalize(phi_reconstruct, phi_reconstruct, 0, 255, cv::NORM_MINMAX);
//	phi_reconstruct.convertTo(phi_reconstruct, 0);*/
//
//	//cv::UMat err;
//	//MyType err = cv::norm(phi, phi_reconstruct, cv::NORM_L2);
//	//cv::matchTemplate(phi, phi_reconstruct, err, cv::TM_CCOEFF_NORMED);
//	
//	MyType err = cv::norm(phi, phi_reconstruct, cv::NORM_L2);
//	err /= 1000;
//
//	if (err == 0)
//	{
//		score = -1;
//		return true;
//	}
//	//std::cout << err << std::endl;
//	score = 1/err;
//	return (err < minTh) ? true : false;
//}

void thermoCamera::EigenFaceDetector::_ReconstructFromWeights(cv::UMat& phi, cv::UMat& mean, cv::UMat& weights, cv::UMat& eigenvectors, int K)
{
	if (phi.empty()) throw thermoCamera::MyError::badInput;
	if (mean.empty()) throw thermoCamera::MyError::badInput;
	if (eigenvectors.empty()) throw thermoCamera::MyError::badInput;

	cv::UMat eigenvectorsTT = eigenvectors.t();

	for (size_t i = 0; i < K; i++)
	{
		cv::add(phi, eigenvectorsTT.col(i).mul(weights.row(i)),phi);
	}
}

void thermoCamera::EigenFaceDetector::_AddNewRectangle(cv::Rect& windows,int& numActualRect, float score)
{
	int x0 = windows.x;
	int y0 = windows.y;
	int x1 = x0 + windows.width;
	int y1 = y0 + windows.height;
	Bbox tempBbox = { x0,y0,x1,y1,score };
	_bbox.push_back(tempBbox);
	numActualRect++;

	//// Save new rectangle
	//int oldMem = numActualRect;
	//Bbox* tempBbox = new Bbox[++numActualRect];

	//if (_bbox)
	//{
	//	memcpy_s(tempBbox, sizeof(*tempBbox) * numActualRect, _bbox, sizeof(*_bbox) * oldMem);
	//	if (_bbox)
	//	{
	//		delete[]   _bbox;
	//		_bbox = NULL;
	//	}
	//}

	//int i = numActualRect - 1;
	//int x0 = windows.x;
	//int y0 = windows.y;
	//int x1 = x0 + windows.width;
	//int y1 = y0 + windows.height;
	//tempBbox[i] = { x0,y0,x1,y1,score };

	//if (_bbox == NULL)
	//{
	//	_bbox = new Bbox[numActualRect];
	//	memcpy_s(_bbox, sizeof(*_bbox) * numActualRect, tempBbox, sizeof(*tempBbox) * numActualRect);
	//}
	//if (tempBbox) delete[] tempBbox;
}

void  thermoCamera::EigenFaceDetector::_transformLinear2D(int& x, int& y, int& xReal, int& yReal, float inlox, float inupx, float outlox, float outupx, float inloy, float inupy, float outloy, float outupy)
{
	// for x
	float spanx = ((outupx - outlox) / (inupx - inlox));
	xReal = (x - inlox) * spanx + outlox;
	//for y
	float spany = ((outupy - outloy) / (inupy - inloy));
	yReal = (y - inloy) * spany + outloy;
}

void thermoCamera::EigenFaceDetector::_GetRectFromBbox(cv::Rect& rect,const int whichOne) 
{
	//rect = { _bbox[whichOne].x0,_bbox[whichOne].y0,_bbox[whichOne].x1,_bbox[whichOne].y1 };
	rect.x = _bbox[whichOne].x0;
	rect.y = _bbox[whichOne].y0;
	rect.width = _bbox[whichOne].x1 - rect.x;
	rect.height = _bbox[whichOne].y1 - rect.y;
}

void thermoCamera::EigenFaceDetector::_StretchWindow(cv::Rect& input, cv::UMat& img, int newWidth, int newHeight)
{
	if (input.empty()) throw thermoCamera::MyError::badInput;
	if (img.empty()) throw thermoCamera::MyError::badInput;
	if (newWidth <= 0) throw thermoCamera::MyError::badInput;
	if (newHeight <= 0) throw thermoCamera::MyError::badInput;

	// Compute new coordinates
	cv::Point newCoor;
	_transformLinear2D(input.x, input.y, newCoor.x, newCoor.y, 0, img.cols, 0, newWidth, 0, img.rows, 0, newHeight);

	cv::Point tempRight;
	cv::Point tempBottom;
	tempRight.x = input.x + input.width;
	tempBottom.x = tempRight.x;
	tempBottom.y = input.y + input.height;

	_transformLinear2D(tempRight.x, tempRight.y, tempRight.x, tempRight.y, 0, img.cols, 0, newWidth, 0, img.rows, 0, newHeight);
	_transformLinear2D(tempBottom.x, tempBottom.y, tempBottom.x, tempBottom.y, 0, img.cols, 0, newWidth, 0, img.rows, 0, newHeight);

	input.x = newCoor.x;
	input.y = newCoor.y;
	input.width = tempRight.x - newCoor.x;
	input.height = tempBottom.y - newCoor.y;

}

/////////////////////////////////// Fast non-maximum suppression //////////////////

void thermoCamera::EigenFaceDetector::sort_idx(const Bbox* rects, int* idxes, int n) {
	/* sort indexes descending by scores */
	int i, j;
	for (i = 0; i < n; ++i) {
		for (j = i + 1; j < n; ++j) {
			int ti = idxes[i], tj = idxes[j];
			if (rects[tj].score < rects[ti].score) {
				idxes[i] = tj;
				idxes[j] = ti;
			}
		}
	}
}

int thermoCamera::EigenFaceDetector::sort_stable(int* arr, int n) {
	/* stable move all -1 to the end */
	int i = 0, j = 0;

	while (i < n) {
		if (arr[i] == -1) {
			if (j < i + 1)
				j = i + 1;
			while (j < n) {
				if (arr[j] == -1) ++j;
				else {
					arr[i] = arr[j];
					arr[j] = -1;
					j++;
					break;
				}
			}
			if (j == n) return i;
		}
		++i;
	}
	return i;
}

int thermoCamera::EigenFaceDetector::fast_nms(Bbox* rects, int num, float overlap_th) {

	void* pmem = malloc(sizeof(int) * (num + num) + sizeof(float) * num);
	int* idxes = (int*)pmem;
	int* pick = idxes + num;
	float* invareas = (float*)(pick + num);
	int idx_count = num;
	int counter = 0, last_idx;
	int x0, y0, x1, y1;
	int tx0, ty0, tx1, ty1;

	for (int i = 0; i < num; ++i) idxes[i] = i;
	sort_idx(rects, idxes, num);

	for (int i = 0; i < num; ++i) {
		int ti = idxes[i];
		Bbox r = rects[idxes[i]];
		invareas[ti] = 1.0f / ((r.x1 - r.x0 + 1) * (r.y1 - r.y0 + 1));
	}

	while (idx_count > 0) {
		int tmp_idx = idx_count - 1;
		last_idx = idxes[tmp_idx];
		pick[counter++] = last_idx;

		x0 = rects[last_idx].x0;
		y0 = rects[last_idx].y0;
		x1 = rects[last_idx].x1;
		y1 = rects[last_idx].y1;

		idxes[tmp_idx] = -1;

		for (int i = tmp_idx - 1; i != -1; i--) {

			Bbox r = rects[idxes[i]];
			tx0 = fast_max(x0, r.x0);
			ty0 = fast_max(y0, r.y0);
			tx1 = fast_min(x1, r.x1);
			ty1 = fast_min(y1, r.y1);

			tx0 = tx1 - tx0 + 1;
			ty0 = ty1 - ty0 + 1;
			if (tx0 > 0 && ty0 > 0) {
				if (tx0 * ty0 * invareas[idxes[i]] > overlap_th) {
					idxes[i] = -1;
				}
			}
		}
		idx_count = sort_stable(idxes, idx_count);
	}

	_index = new unsigned int[counter];
	/* just give the selected rects' indexes, modification needed for real use */
	for (int i = 0; i < counter; ++i) {
		_index[i] = pick[i];
	}
	//for (int i = 0; i < counter; ++i) {
	//	std::cout << pick[i] << std::endl;
	//}
	return counter;

	free(pmem);
}