 #include "FaceDetection.h"

//////////////////////////////////////////////////////////////////////////
//////////////////// Constructors ////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////

thermoCamera::FaceDetector::FaceDetector()
{
	_active = false;
	_isSetRelativeTemp = false;
	_showOpenCVWin = false;
	_thresholdFrequency = 1;
	_thresholdTemperture = 0.3f;
	_left = 700;
	_top = 90;
	_width = 0;
	_height = 0;
	_widthOut = 0;
	_heightOut = 0;
	_tempFace = 0.f;
	_locFaceTemp = {0,0};
	_hbitmap = NULL;
	_tempMatch.setScaleInvariant();
	_tempMatch.setThreshold(0.15);
	_tempMatch.setResizeTemplate(4.f);
	_percentOveralpped = 10;
	_rectAreMissing = 0;
}

thermoCamera::FaceDetector::~FaceDetector()
{

}

//////////////////////////////////////////////////////////////////////////
//////////////////// Public methods //////////////////////////////////////
//////////////////////////////////////////////////////////////////////////

void thermoCamera::FaceDetector::Init(HWND hWnd)
{
	//Create a compatible backbuffer
	/*HDC current = GetDC(hWnd);
	RECT clientrect;

	GetClientRect(hWnd, &clientrect);
	HDC buff = CreateCompatibleDC(current);
	_backbuffer = buff;*/
}

int thermoCamera::FaceDetector::getLeft()
{
	return _left;
}

int thermoCamera::FaceDetector::getTop()
{
	return _top;
}

int thermoCamera::FaceDetector::getWidth()
{
	return _width;
}

int thermoCamera::FaceDetector::getNumOfRect()
{
	return _faceRectangle.size();
}

int thermoCamera::FaceDetector::getNumOfTrackers()
{
	int val = 0;
	for (size_t i = 0; i < _trackers.size(); i++)
	{
		if (_trackers[i].updated)
		{
			++val;
		}
	}
	return val;
}

double thermoCamera::FaceDetector::GetTimerOfTracker(int index)
{
	if (_trackers.size() > index)
	{
		_trackers[index].timer.stop();
		double time = _trackers[index].timer.getTimeSec();
		_trackers[index].timer.start();
		return time;
	}
	else return 0.0;

}

float thermoCamera::FaceDetector::GetTemperatureOfTracker(int index)
{
	if (_trackers.size() > index)
	{
		float temp = _trackers[index].tempPrevious;

		return temp;
	}
	else return 0.f;
}

int thermoCamera::FaceDetector::getHeight()
{
	return _height;
}


void thermoCamera::FaceDetector::setActive()
{
	_active = true;
}

void thermoCamera::FaceDetector::resetActive()
{
	_active = false;
	//_eigenDetector.Abort();
}


void thermoCamera::FaceDetector::SetPathClasiffier(const TCHAR* path)
{
	if (path == NULL) return;

	_pathToClassifier = path;
}

bool thermoCamera::FaceDetector::isActive()
{
	return _active;
}

cv::Rect thermoCamera::FaceDetector::getFaceRectangle(int whichRect)
{
	if(whichRect >=0 && whichRect <= 3)
	return _faceRectangle[whichRect];
}

cv::UMat thermoCamera::FaceDetector::K_means(cv::UMat Input, int K)
{
	if (Input.empty()) throw thermoCamera::MyError::badInput;
	if (K <= 0) throw thermoCamera::MyError::badInput;

	cv::UMat samples(Input.rows * Input.cols, Input.channels(), CV_32F);
	for (int y = 0; y < Input.rows; y++)
		for (int x = 0; x < Input.cols; x++)
			for (int z = 0; z < Input.channels(); z++)
				if (Input.channels() == 3) {
					samples.getMat(ACCESS_WRITE).at<float>(y + x * Input.rows, z) = Input.getMat(ACCESS_READ).at<Vec3b>(y, x)[z];
				}
				else {
					samples.getMat(ACCESS_WRITE).at<float>(y + x * Input.rows, z) = Input.getMat(ACCESS_READ).at<uchar>(y, x);
				}

	Mat labels;
	int attempts = 5;
	Mat centers;
	kmeans(samples, K, labels, TermCriteria(cv::TermCriteria::MAX_ITER | cv::TermCriteria::EPS, 10000, 0.0001), attempts, KMEANS_PP_CENTERS, centers);


	cv::UMat new_image(Input.size(), Input.type());
	for (int y = 0; y < Input.rows; y++)
		for (int x = 0; x < Input.cols; x++)
		{
			int cluster_idx = labels.at<int>(y + x * Input.rows, 0);
			if (Input.channels() == 3) {
				for (int i = 0; i < Input.channels(); i++) {
					new_image.getMat(ACCESS_WRITE).at<Vec3b>(y, x)[i] = centers.at<float>(cluster_idx, i);
				}
			}
			else {
				new_image.getMat(ACCESS_WRITE).at<uchar>(y, x) = centers.at<float>(cluster_idx, 0);
			}
		}
	//imshow("clustered image", new_image);
	return new_image;
}

void thermoCamera::FaceDetector::DrawFaceRectangle(HWND& hWnd, HDC& hdc) const
{
	if (hdc)
	{
		HPEN Pen = CreatePen(PS_SOLID, 3, 0x0000FF00);

		RECT _wndRect;
		GetClientRect(hWnd, &_wndRect);

		SelectObject(hdc, Pen);
		SelectObject(hdc, GetStockObject(NULL_BRUSH));
		
		////
		//for (size_t i = 0; i < _faceRectangle.size(); i++)
		//{

		//	if (_faceRectangle[i].width > 0 && _faceRectangle[i].height > 0)
		//	{
		//		int left = LEFTOUT + _faceRectangle[i].x;
		//		int top = TOPOUT + _faceRectangle[i].y;
		//		int width = _faceRectangle[i].width + left;
		//		int height = _faceRectangle[i].height + top;
		//		POINT pLeft = { left,top };
		//		POINT pRight = { width, height };

		//		ScreenToClient(hWnd, &pLeft);

		//		Rectangle(hdc, left, top, width, height);
		//	}
		//	
		//}

		////		
		//for (size_t i = 0; i < _trackers.size(); i++)
		//{
		//	if (_trackers[i].updated)
		//	{
		//		int left = LEFTOUT + _trackers[i].rectFace.x;
		//		int top = TOPOUT + _trackers[i].rectFace.y;
		//		int width = _trackers[i].rectFace.width + left;
		//		int height = _trackers[i].rectFace.height + top;
		//		POINT pLeft = { left,top };
		//		POINT pRight = { width, height };

		//		ScreenToClient(hWnd, &pLeft);

		//		Rectangle(hdc, left, top, width, height);
		//	}		
		//}

		if (Pen) DeleteObject(Pen);
	}
}


void thermoCamera::FaceDetector::DrawTempFace(HWND& hWnd, thermoCamera::VideoDisplay& video, HDC& hdc) const
{
	////
	//TCHAR chText[20];
	////_gcvt_s(chText, _trackers.size(), 5);

	//if (_trackers.size() > 0)
	//{
	//	if (_trackers[0].tracked)
	//	{
	//		_trackers[0].timer
	//		_trackers[0].timer.stop();
	//		double time = _trackers[0].timer.getTimeSec();
	//		_trackers[0].timer.start();
	//		snprintf(chText, sizeof(chText), "%.1lf", time);

	//		WCHAR outText[20];
	//		mbstowcs_s(NULL, outText, sizeof(outText) / 2, chText, sizeof(chText));
	//		SetWindowTextW(hEdit, outText);
	//	}			
	//}
		

	//float temp = 0;
	//for (int i = 0; i < _pointsOfTemperature.size(); i++)
	//{
	//	temp = video.GetTemperature(_pointsOfTemperature[i].x, _pointsOfTemperature[i].y,false);
	//	POINT p = { _pointsOfTemperature[i].x + LEFTOUT,_pointsOfTemperature[i].y + TOPOUT };

	//	Ellipse(hdc, p.x, p.y, p.x + 8, p.y + 8);
	//	_MouseTemp(hWnd, hdc, p,i, &video);
	//}
	////_pointsOfTemperature.clear();

	for (int i = 0; i < _trackers.size(); i++)
	{
		if (_pointsOfTemperature.size() > i)
		{
			if (_trackers[i].updated)
			{
				POINT p = { _trackers[i].pointTemp.x + LEFTOUT,_trackers[i].pointTemp.y + TOPOUT };

				Ellipse(hdc, p.x, p.y, p.x + 8, p.y + 8);
				_GetTempOfFace(hWnd, hdc, p, i, &video);
			}
		}
	}
}

//void thermoCamera::FaceDetector::DetectGlass(HBITMAP hBitMap, thermoCamera::VideoCapture* video, int width, int height)
//{
//	cv::Rect roi = { _widthOut,0,width,height };
//	cv::UMat img;
//	_loadImageFromBitmap(hBitMap, img, 1);
//	img = img(roi);
//	cv::cvtColor(img, img, cv::COLOR_RGB2GRAY);
//	normalize(img, img, 0, 255, cv::NORM_MINMAX);
//	cv::UMat croppedImage;
//
//
//	// Classifier
//
//	const std::string pathToClassifier = "C:\\Users\\matej\\source\\repos\\ThermalFaceDetection\\ThermalFaceDetection\\cascadeGlass11.xml";
//	std::vector<cv::Rect> rectOfFaces;
//
//	cv::CascadeClassifier myClassifier(pathToClassifier);
//
//
//	myClassifier.detectMultiScale(
//		img,
//		rectOfFaces
//	);
//
//	if (rectOfFaces.empty())
//	{
//		_numRectOfFace = 0;
//		_pointsOfTemperature.clear();
//		_tempOfFace.clear();
//		return;
//	}
//
//	_numRectOfFace = 0;
//	_pointsOfTemperature.clear();
//	_tempOfFace.clear();
//	int scale = 40;
//
//	for (size_t i = 0; i < rectOfFaces.size(); i++)
//	{
//		if (rectOfFaces.at(i).y > scale && rectOfFaces.at(i).height < (img.rows - scale))
//		{
//			rectOfFaces.at(i).y -= scale;
//			rectOfFaces.at(i).height += scale;
//		}
//
//
//		croppedImage = img(rectOfFaces.at(i));
//
//		normalize(croppedImage, croppedImage, 0, 255, cv::NORM_MINMAX);
//
//		croppedImage = K_means(croppedImage, 2);
//
//		normalize(croppedImage, croppedImage, 0, 255, cv::NORM_MINMAX);
//
//		if (_localizeGlass(croppedImage, rectOfFaces.at(i), &video, false))
//		{
//			_StretchWindow(rectOfFaces.at(i), img, _widthOut, _heightOut);
//			_faceRectangle[i] = { rectOfFaces.at(i).x,rectOfFaces.at(i).y,rectOfFaces.at(i).width,rectOfFaces.at(i).height };
//
//			_numRectOfFace++;
//			if (i == 3) break;
//		}
//	}
//}


void thermoCamera::FaceDetector::LBP(HBITMAP hBitMap, thermoCamera::VideoDisplay& video, int width, int height)
{
	cv::UMat img;

	if (_widthOut <= 0 || _heightOut <= 0) return;

	if (_pathToClassifier.empty())
	{
		throw thermoCamera::MyError::cannotOpenFile;
	}

	_loadImageFromBitmap(hBitMap, img,&video,1);

	if (img.empty() )
	{
		throw thermoCamera::MyError::cannotDrawOutput;
	}

	cv::cvtColor(img, img, cv::COLOR_RGB2GRAY);
	normalize(img, img, 0, 255, cv::NORM_MINMAX);
	cv::UMat croppedImage;

	if (_showOpenCVWin)
	{
		cv::imshow("Hey", img);
		cv::waitKey(100);
	}

	// Classifier
	std::vector<cv::Rect> rectOfFaces;
	std::vector<cv::Rect> rectOfGlasses;

	#if UNICODE
		std::string classifierPath = _ws2s(_pathToClassifier);
	#else
		std::string classifierPath = _pathToClassifier;
	#endif

	cv::CascadeClassifier myClassifier(classifierPath);


	myClassifier.detectMultiScale(
		img,
		rectOfFaces
		//1.05
	);

	////
	//#if UNICODE
	//	std::string classifierPath2 = _ws2s(PATH_TO_GLASS);
	//#else
	//	std::string classifierPath2 = PATH_TO_GLASS;
	//#endif
	//cv::CascadeClassifier myClassifier2(classifierPath2);

	//myClassifier2.detectMultiScale(
	//	img,
	//	rectOfGlasses
	//);

	//rectOfFaces.insert(rectOfFaces.end(), rectOfGlasses.begin(), rectOfGlasses.end());

	_faceRectangle.clear();
	_pointsOfTemperature.clear();
	_tempOfFace.clear();
	float scale = 40;

	for (size_t i = 0; i < rectOfFaces.size(); i++)
	{
		if (rectOfFaces.at(i).y > scale && rectOfFaces.at(i).height < (img.rows-scale))
		{
			rectOfFaces.at(i).y -= scale;
			rectOfFaces.at(i).height += scale;
		}
		else if (rectOfFaces.at(i).y > scale/2.f && rectOfFaces.at(i).height < (img.rows - (int)(scale/2.f)))
		{
			rectOfFaces.at(i).y -= (scale/2.f);
			rectOfFaces.at(i).height += (scale/2.f);
		}
		

		croppedImage = img(rectOfFaces.at(i)).clone();
		normalize(croppedImage, croppedImage, 0, 255, cv::NORM_MINMAX);
		croppedImage = K_means(croppedImage, 2);		
		normalize(croppedImage, croppedImage, 0, 255, cv::NORM_MINMAX);
				
		if (_localizeEye(croppedImage, rectOfFaces.at(i), &video, false))
		{
			_StretchWindow(rectOfFaces.at(i), img, _widthOut, _heightOut);
			_faceRectangle.push_back(cv::Rect(rectOfFaces.at(i).x, rectOfFaces.at(i).y, rectOfFaces.at(i).width, rectOfFaces.at(i).height));
		}
	}

	_ValidateTrackingObject();
	_DestroyRedundantRectangles();
	_TrackingFace(img,false);
}

void thermoCamera::FaceDetector::_ValidateTrackingObject()
{
	bool found = false;

	if (_faceRectangle.size() > 0)
	{
		for (size_t trackerNum = 0; trackerNum < _trackers.size(); trackerNum++)
		{
			found = false;
			if (_trackers[trackerNum].tracked)
			{
				for (size_t i = 0; i < _faceRectangle.size(); i++)
				{
					if (_isOverLapped(_trackers[trackerNum].rectFace, _faceRectangle[i], _percentOveralpped))
					{
						//_trackers[trackerNum].rectFace = _faceRectangle[i];
						if (std::abs(_trackers[trackerNum].tempPrevious - _trackers[trackerNum].temp) > _thresholdTemperture)
						{
							_trackers[trackerNum].tempPrevious = _trackers[trackerNum].temp;
						}					
						_trackers[trackerNum].temp = _tempOfFace[i];
						_trackers[trackerNum].pointTemp = _pointsOfTemperature[i];
						found = true;
						_trackers[trackerNum].frequency = 0;
						break;
					}
				}
				if (found == false)
				{
					_trackers[trackerNum].frequency++;
					if (_trackers[trackerNum].frequency > _thresholdFrequency)
						_trackers.erase(_trackers.begin() + trackerNum);
				}
			}
			else
			{
				for (size_t i = 0; i < _faceRectangle.size(); i++)
				{
					if (_isOverLapped(_trackers[trackerNum].rectFace, _faceRectangle[i], _percentOveralpped))
					{
						//_trackers[trackerNum].rectFace = _faceRectangle[i];
						if (std::abs(_trackers[trackerNum].tempPrevious - _trackers[trackerNum].temp) > _thresholdTemperture)
						{
							_trackers[trackerNum].tempPrevious = _trackers[trackerNum].temp;
						}
						_trackers[trackerNum].temp = _tempOfFace[i];
						_trackers[trackerNum].pointTemp = _pointsOfTemperature[i];
						found = true;
						break;
					}
				}
				if (found == false)
				{
					_trackers.erase(_trackers.begin() + trackerNum);
				}
				if (found)
				{
					_trackers[trackerNum].frequency++;
				}
			}
		}
		_rectAreMissing = 0;
	}
	else
	{
		_rectAreMissing++;
		if(_rectAreMissing > 5)
			_trackers.clear();
	}
}

void thermoCamera::FaceDetector::_DestroyRedundantRectangles()
{
	if (_trackers.size() > 1)
	{
		//// descending order
		//if (_trackers.size() > 1)
		//{
		//	sort(_trackers.begin(), _trackers.end(), [](const Tracker& lhs, const Tracker& rhs)
		//		{
		//			return lhs.frequency > rhs.frequency;
		//		});
		//}


		size_t i = _trackers.size()-1;
		while (i > 0)
		{
			for (size_t j = 0; j < i; j++)
			{
				if (_isOverLapped(_trackers[i].rectFace, _trackers[j].rectFace,0))
				{
					_trackers.erase(_trackers.begin()+ i);
					break;
				}
			}
			--i;
		}
	}
}

void  thermoCamera::FaceDetector::_TrackingFace(cv::UMat& img, bool show)
{
	cv::resize(img, img, cv::Size(_widthOut, _heightOut));
	cv::cvtColor(img, img, COLOR_GRAY2BGR);

	if (_faceRectangle.size() == 0) return;

	if (_trackers.size() == 0)
	{
		for (size_t i = 0; i < _faceRectangle.size(); i++)
		{
			thermoCamera::Tracker structTracker =
			{
				cv::TrackerCSRT::create(),
				false,
				_faceRectangle[i],
				0,
				0.f,
				0.f,
				false,
				{0,0}
			};
			_trackers.push_back(structTracker);
		}
	}
	else
	{
		bool found = false;
		for (size_t i = 0; i < _faceRectangle.size(); i++)
		{
			for (size_t trackerNum = 0; trackerNum < _trackers.size(); trackerNum++)
			{
				if (_isOverLapped(_faceRectangle[i], _trackers[trackerNum].rectFace, _percentOveralpped) == true)
				{
					found = true;
					break;
				}
			}
			if (found == false)
			{
				thermoCamera::Tracker structTracker =
				{
					cv::TrackerCSRT::create(),
					false,
					_faceRectangle[i],
					0,
					0.f,
					0.f,
					false,
					{0,0}
				};
				_trackers.push_back(structTracker);
			}
		}
	}

	bool ok = false;
	for (size_t i = 0; i < _trackers.size(); i++)
	{
		if (_trackers[i].tracked)
		{
			try
			{
				ok = _trackers[i].tracker->update(img, _trackers[i].rectFace);
			}
			catch (std::exception& e)
			{

			}
			
			if(ok) 
			{
				_trackers[i].updated = true;
			}
			else
			{
				_trackers.erase(_trackers.begin() + i);
			}
		}
		else if(_trackers[i].frequency > _thresholdFrequency)
		{
			_trackers[i].tracker->init(img, _trackers[i].rectFace);
			_trackers[i].tracked = true;
			_trackers[i].frequency = 0;
			_trackers[i].updated = false;
			_trackers[i].timer.start();
			//cv::rectangle(img, _trackers[i].rectFace, cv::Scalar(0, 255, 0));
		}
	}

	if (show)
	{
		cv::imshow("Hey", img);
		cv::waitKey(50);
	}
}

void thermoCamera::FaceDetector::_ValidateTrackingObjectFaces()
{
	bool found = false;

	if (_faceRectangle.size() > 0)
	{
		for (size_t i = 0; i < _faces.size(); i++)
		{
			found = false;
			if (_faces[i].tracked)
			{
				for (size_t i = 0; i < _faceRectangle.size(); i++)
				{
					if (_isOverLapped(_faces[i].rectangle, _faceRectangle[i], _percentOveralpped))
					{
						_faces[i].rectangle = _faceRectangle[i];
						found = true;
						_faces[i].frequency = 0;
						break;
					}
				}
				if (found == false) 
				{
					_faces[i].frequency++;
					if(_faces[i].frequency > 1)
						_faces.erase(_faces.begin() + i);
				}
			}
			else
			{
				for (size_t i = 0; i < _faces.size(); i++)
				{
					if (_isOverLapped(_faces[i].rectangle, _faceRectangle[i], _percentOveralpped))
					{
						_faces[i].rectangle = _faceRectangle[i];
						found = true;
						break;
					}
				}
				if (found == false)
				{
					_faces.erase(_faces.begin() + i);
				}
				if (found)
				{
					_faces[i].frequency++;
				}
			}
		}		
	}
	else
	{
		_faces.clear();
	}
}

void  thermoCamera::FaceDetector::Tracking(thermoCamera::VideoDisplay& video)
{
	if (_widthOut <= 0 || _heightOut <= 0) return;

	cv::UMat img;
	_loadImageFromBitmap(video.GetHBITMAP(INDEX_HBITMAP_OF_DETECTION), img, &video, 1);

	if (img.empty()) return;

	SIZE sz = video.GetSizeOfInputImage();
	if (img.cols != sz.cx && img.rows != sz.cy) return;

	cv::imshow("J", img);
	cv::waitKey(2);

	//cv::resize(img, img, cv::Size(_widthOut, _heightOut));
	//cv::cvtColor(img, img, COLOR_GRAY2BGR);
	
	if (_faceRectangle.size() == 0) return;

	bool ok = false;
	for (size_t i = 0; i < _trackers.size(); i++)
	{
		if (_trackers[i].tracked)
		{
			try
			{
				ok = _trackers[i].tracker->update(img, _trackers[i].rectFace);
			}
			catch (std::exception& e)
			{

			}

			if (ok)
			{
				//cv::rectangle(img, _trackers[i].rectFace, cv::Scalar(0, 255, 0));
			}
			else
			{
				_trackers.erase(_trackers.begin() + i);
			}
		}
	}

	////
	//for (size_t i = 0; i < _faces.size(); i++)
	//{
	//	if(_faces[i].frequency > 2)
	//	{
	//		_faces[i].tracked = true;
	//		_faces[i].frequency = 0;
	//		cv::rectangle(img, _faces[i].rectangle, cv::Scalar(0, 255, 0));
	//	}
	//}

}


void thermoCamera::FaceDetector::TemplateMatching(HBITMAP hBitMap, thermoCamera::VideoDisplay& video, int width, int height)
{
		//////////////////////////////////////////Temperature measurement ////////////////////////////////////////////
	    
		const char* tempName = "C:\\Images\\Temp.jpg";
		int err = -2;
		_tempMatch.setTemplate(tempName);

		_width =  width;
		_height = height;

		cv::UMat uImg;
		_loadImageFromBitmap(hBitMap, uImg, &video, 1);
		if (uImg.empty())
		{
			throw thermoCamera::MyError::cannotDrawOutput;
		}

		cv::Mat img = uImg.getMat(ACCESS_READ);
		cvtColor(img, img, cv::COLOR_BGRA2BGR);
		img = img.clone();
		cv::Mat redPart[3];
		cv::split(img, redPart);

		cv::Point left = { 0,0 };
		cv::Point right = { 0,0 };


		cv::threshold(redPart[2], redPart[2], 0, 255, cv::THRESH_OTSU);

		_faceRectangle.clear();

		if (!img.empty())
		{
			if (_tempMatch.getError() == 0)
			{
				_tempMatch.findTemplateInImage(redPart[2]);
				redPart[2] = _tempMatch.getImage();
				if (!redPart[2].empty())
				{
					//cv::imshow("Hey", redPart[2]);
					//cv::waitKey(100);
				}
			}
			if (!redPart[2].empty())
			{
				_tempMatch.getRectangle(left.x, left.y, right.x, right.y);

				cv::Rect myROI = cv::Rect(left.x,left.y,right.x,right.y);
				RECT siz = video.GetSizeOfVideo(INDEX_HBITMAP_OF_VIDEO);
			
				if (myROI.width > 0)
				{
					_StretchWindow(myROI, redPart[2], siz.right, siz.bottom);
					_faceRectangle.push_back(myROI);
				}
			}
		}
		
}

void thermoCamera::FaceDetector::FindFace(HBITMAP hBitMap, thermoCamera::VideoDisplay& video, int width, int height)
{
	if (hBitMap == NULL) return;

	if (_eigenDetector.isInit() == false) _eigenDetector.Init();

	RECT siz = video.GetSizeOfVideo(INDEX_HBITMAP_OF_VIDEO);
	int widthOut = siz.right;
	int heightOut = siz.bottom;


	cv::UMat uImg;
	_loadImageFromBitmap(hBitMap, uImg, &video, 1);
	if (uImg.empty())
	{
		throw thermoCamera::MyError::cannotDrawOutput;
	}

	//cv::Mat img = uImg.getMat(ACCESS_READ);
	//cvtColor(img, img, cv::COLOR_BGRA2BGR);
	//img = img.clone();

	cv::UMat inv;
	cv::cvtColor(uImg, inv, cv::COLOR_RGB2GRAY);
	inv = inv.clone();

	cv::normalize(inv, inv, 0,255,cv::NORM_MINMAX);
	siz = video.GetSizeOfVideo(INDEX_HBITMAP_OF_DETECTION);
	cv::resize(inv, inv, cv::Size(siz.right, siz.bottom), cv::INTER_LINEAR);
	//cv::imshow(_nameWin, inv);
	//cv::waitKey(50);
		
	cv::Rect faces[10];
	int numOfRect = 0;
	const clock_t begin_time = clock();
	_eigenDetector.FindFace(inv, faces, 4, numOfRect);
	float tim = float(clock() - begin_time) / CLOCKS_PER_SEC;

	std::string bla;
	const size_t hh = 50;
	wchar_t txt[hh];
	swprintf_s(txt, hh, L"%.2f", tim);
	//MessageBox(NULL, txt, TEXT("Warning"), MB_OK);

	_faceRectangle.clear();
	if (numOfRect > 4) numOfRect = 4;
	if (numOfRect > 0)
	{
		for (size_t i = 0; i < numOfRect; i++)
		{
			_StretchWindow(faces[i], uImg, widthOut, heightOut);
			faces[i].x -= 10;
			faces[i].y -= 10;
			faces[i].width += 10;
			faces[i].height += 10;
			_faceRectangle.push_back(cv::Rect(faces[i].x, faces[i].y, faces[i].width, faces[i].height));
		}
	}

	////
	//cv::Rect foreheadRect;
	//_foreheadRect.clear();
	//for (size_t i = 0; i < rectOfFace.size(); i++)
	//{
	//	//rectangle(originImg, rectOfFaces.at(i), cv::Scalar(255), 1, 8, 0);
	//	//_StretchWindow(rectOfFaces.at(i), originImg, _widthOut, _heightOut);
	//	//_faceRectangle[i] = { rectOfFaces.at(i).x,rectOfFaces.at(i).y,rectOfFaces.at(i).width,rectOfFaces.at(i).height };

	//	if (_FindForehead(inv, rectOfFace.at(i), foreheadRect, i, false))
	//	{
	//		_foreheadRect.push_back(foreheadRect);
	//	}
	//}

	//_MeasureForehead( &video, rectOfFace);
}


void  thermoCamera::FaceDetector::_GetTempOfFace(HWND hWnd, HDC hdc, POINT p,int num, thermoCamera::VideoDisplay* video) const
{
	HFONT hFont = CreateFont(40, 0, 0, 0, FW_BOLD, 0, 0, 0, 0, 0, 0, 2, 0, TEXT("SYSTEM_FIXED_FONT"));
	HFONT hTmp = (HFONT)SelectObject(hdc, hFont);
	SetBkMode(hdc, OPAQUE);

	int boundary[4];
	RECT rectOfDisplay = video->GetSizeOfVideo(INDEX_HBITMAP_OF_VIDEO);
	boundary[0] =  rectOfDisplay.left + rectOfDisplay.right;
	boundary[1] = rectOfDisplay.left;
	boundary[2] = rectOfDisplay.top + rectOfDisplay.bottom;
	boundary[3] = rectOfDisplay.top;
	if (p.x < boundary[0] && p.x >= boundary[1] && p.y < boundary[2] && p.y >= boundary[3])
	{
		TCHAR chText[100];
		int x = (p.x);
		int y = (p.y);// -video->getTop();
		float temperature = _trackers[num].tempPrevious;
		if (temperature <= 0.f) return;

		POINT point;
		//temperature += video->GetCalibrationValue();

		#if UNICODE
				swprintf(chText, sizeof(chText), TEXT("%.1f"), temperature);
		#else
				snprintf(chText, sizeof(chText), "%.1f", temperature);
		#endif
		
		if (temperature < video->GetAlarmThreshold())
			SetTextColor(hdc, RGBBLACK); 
		else
			SetTextColor(hdc, RGBRED); 

		#if UNICODE
				int scale = 40;
				if (p.y > (boundary[3] + 100) && p.x <= (boundary[0] - 100))
					TextOutW(hdc, p.x, p.y - scale, chText, lstrlen(chText));
				else if (p.y > (boundary[3] + 100) && p.x > (boundary[0] - 100))
					TextOutW(hdc, p.x - scale, p.y - scale, chText, lstrlen(chText));
				else if (p.y < (boundary[3] + 100) && p.x >(boundary[0] - 100))
					TextOutW(hdc, p.x - scale, p.y + scale, chText, lstrlen(chText));
				else
					TextOutW(hdc, p.x, p.y + scale, chText, lstrlen(chText));
		#else
				int scale = 40;
				if (p.y > (boundary[3] + 100) && p.x <= (boundary[0] - 100))
					TextOutA(hdc, p.x, p.y - scale, chText, lstrlen(chText));
				else if (p.y > (boundary[3] + 100) && p.x > (boundary[0] - 100))
					TextOutA(hdc, p.x - scale, p.y - scale, chText, lstrlen(chText));
				else if (p.y < (boundary[3] + 100) && p.x >(boundary[0] - 100))
					TextOutA(hdc, p.x - scale, p.y + scale, chText, lstrlen(chText));
				else
					TextOutA(hdc, p.x, p.y + scale, chText, lstrlen(chText));
		#endif
	}
	DeleteObject(SelectObject(hdc, hTmp));
}


void thermoCamera::FaceDetector::_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;
}

void thermoCamera::FaceDetector::_StretchWindow(cv::Rect& input, cv::Mat& 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;
}

//
void thermoCamera::FaceDetector::_loadImageFromBitmap(HBITMAP hBitmap, cv::UMat& inputImg, thermoCamera::VideoDisplay* video, int flag)
{
	if (hBitmap == NULL) return;

	//TCHAR loc[] = "\\Bitmap.bmp";
	//strcat_s(pathToBitmap, sizeof(pathToBitmap), loc);

	//CImage img;
	//img.Attach(hBitmap);

	SIZE rect = video->GetSizeOfInputImage();
	inputImg.create(rect.cy, rect.cx, CV_8UC4);

	_hwnd2mat(hBitmap, video->GetHDC(INDEX_HBITMAP_OF_DETECTION),inputImg);

	#if UNICODE
		////
		//std::string path = _ws2s(_pathToClassifier);
		//size_t last = path.find_last_of('\\');
		//path = path.erase(last);
		//path = path + "\\Bitmap.bmp";

		//std::wstring wpath = _pathToClassifier;
		//wpath = wpath.erase(last);
		//wpath = wpath + TEXT("\\Bitmap.bmp");

		//img.Save(wpath.c_str());

		//inputImg = cv::imread(path, flag);
	#else
		//std::string path = _pathToClassifier;
		//size_t last = path.find_last_of('\\');
		//path = path.erase(last);

		//path = path + "\\Bitmap.bmp";

		////img.Save(path.c_str());

		//inputImg = cv::imread(path, flag);
	#endif
}

//
bool thermoCamera::FaceDetector::_localizeEye(cv::UMat& img, cv::Rect& rect, thermoCamera::VideoDisplay* video, bool show)
{
	cv::UMat imgClone = img.clone();
	float max1 = 0; 
	float val = 0;
	cv::Point cen1 = { 0,0 };
	cv::Point p = { 0,0 };

	for (size_t col = 0; col < rect.width; col++)
	{
		for (size_t row = 0; row < rect.height; row++)
		{
			if (img.getMat(ACCESS_READ).at<uchar>(cv::Point(col, row)) == 0)
			{
				val = 0;
			}
			else
			{
				val = video->GetTemperature(col + rect.x, row + rect.y, false);
			}

			if (val > max1)
			{
				max1 = val;
				cen1 = cv::Point(col, row);
			}
			
		}
	}

	if (max1 >= 45.f || max1 <= 30.f) return false;

	cv::cvtColor(img, img, COLOR_GRAY2RGB);
	cv::circle(img, cen1, 3, cv::Scalar(255,0,0));

	val = 0.80f * (float)cen1.y;

	cv::Point p1 = { 0,(int)val };
	cv::Point p2 = { img.cols,(int)val};
	cv::line(img, p1, p2, cv::Scalar(0, 255, 0));

	if (show)
	{
		resize(img, img, Size(640, 480));
		cv::imshow("Hey", img);
		cv::waitKey(100);
	}

	int boundary = (int)val;
	val = 0;
	max1 = 0;
	cv::Rect newRect = rect;
	//_StretchWindow(newRect, imgOrigin, _widthOut, _heightOut);

	for (int col = 0; col < rect.width; col++)
	{
		for (int row = 0; row < boundary; row++)
		{
			if (imgClone.getMat(ACCESS_READ).at<uchar>(cv::Point(col, row)) == 0)
			{
				val = 0;
			}
			else
			{
				val = video->GetTemperature(col + rect.x, row + rect.y, false);
			}

			if (val > max1)
			{
				max1 = val;
				p = { rect.x + col,rect.y + row };
			}

		}
	}

	if (max1 >= 30.f)
	{
		RECT rec = video->GetSizeOfVideo(INDEX_HBITMAP_OF_DETECTION);
		if (rec.right == 0 || rec.bottom == 0) return false;

		_transformLinear2D(p.x, p.y, p.x, p.y, 0, rec.right, 0, _widthOut, 0, rec.bottom, 0, _heightOut);
		_pointsOfTemperature.push_back(p);

		float coef, slope;
		coef = 23.847;
		slope = 0.3646;

		//float emisivity_actual = 1;
		//float emisivity_body = 0.98;
		//max1 = max1 * pow((emisivity_actual / emisivity_body), 1.f / 4.f);
		max1 = slope * max1 + coef;

		max1 += video->GetCalibratedValue();

		_tempOfFace.push_back(max1);
		return true;
	}
	else return false;
}

////
//bool thermoCamera::FaceDetector::_localizeGlass(cv::UMat& img, cv::Rect& rect, thermoCamera::VideoCapture** video, bool show)
//{
//	cv::UMat imgClone = img.clone();
//	float max1 = 0;
//	float val = 0;
//	cv::Point cen1 = { 0,0 };
//	cv::Point p = { 0,0 };
//
//	for (size_t col = 0; col < rect.width; col++)
//	{
//		for (size_t row = 0; row < rect.height; row++)
//		{
//			if (img.at<uchar>(cv::Point(col, row)) == 0)
//			{
//				val = 0;
//			}
//			else
//			{
//				val = (*video)->GetTemperature(col + rect.x, row + rect.y, false);
//			}
//
//			if (val > max1)
//			{
//				max1 = val;
//				cen1 = cv::Point(col, row);
//			}
//
//		}
//	}
//
//	if (max1 >=  40.f || max1 <= 33.f) return false;
//
//	if (show)
//	{
//		resize(img, img, Size(640, 480));
//		cv::imshow("Hey", img);
//		cv::waitKey(100);
//	}
//
//	p = { rect.x + cen1.x,rect.y + cen1.y };
//	
//	_transformLinear2D(p.x, p.y, p.x, p.y, 0, (*video)->getWidth(), 0, _widthOut, 0, (*video)->getHeight(), 0, _heightOut);
//	_pointsOfTemperature.push_back(p);
//	_tempOfFace.push_back(max1);
//	return true;
//
//	cv::cvtColor(img, img, COLOR_GRAY2RGB);
//	cv::circle(img, cen1, 3, cv::Scalar(255, 0, 0));
//
//	val = 0.85f * (float)cen1.y;
//
//	cv::Point p1 = { 0,(int)val };
//	cv::Point p2 = { img.cols,(int)val };
//	cv::line(img, p1, p2, cv::Scalar(0, 255, 0));
//
//	int boundary = (int)val;
//	val = 0;
//	max1 = 0;
//	cv::Rect newRect = rect;
//	//_StretchWindow(newRect, imgOrigin, _widthOut, _heightOut);
//
//	for (int col = 0; col < rect.width; col++)
//	{
//		for (int row = 0; row < boundary; row++)
//		{
//			if (imgClone.at<uchar>(cv::Point(col, row)) == 0)
//			{
//				val = 0;
//			}
//			else
//			{
//				val = (*video)->GetTemperature(col + rect.x, row + rect.y, false);
//			}
//
//			if (val > max1)
//			{
//				max1 = val;
//				p = { rect.x + col,rect.y + row };
//			}
//
//		}
//	}
//
//	if (max1 >= 30.f)
//	{
//		_transformLinear2D(p.x, p.y, p.x, p.y, 0, (*video)->getWidth(), 0, _widthOut, 0, (*video)->getHeight(), 0, _heightOut);
//		_pointsOfTemperature.push_back(p);
//		_tempOfFace.push_back(max1);
//		return true;
//	}
//	else return false;
//}

void thermoCamera::FaceDetector::_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;
}

bool thermoCamera::FaceDetector::_isOverLapped(cv::Rect rect1, cv::Rect rect2,int percent)
{
	Point l1 = { rect1.x,rect1.y };
	Point r1 = { rect1.width + rect1.x,rect1.height + rect1.y };
	Point l2 = { rect2.x,rect2.y };
	Point r2 = { rect2.width+rect2.x,rect2.height+rect2.y };
	// To check if either rectangle is actually a line
	// For example :  l1 ={-1,0}  r1={1,1}  l2={0,-1}
	// r2={0,1}

	if (l1.x == r1.x || l1.y == r2.y || l2.x == r2.x
		|| l2.y == r2.y) {
		// the line cannot have positive overlap
		return false;
	}

	// If one rectangle is on left side of other
	if (l1.x >= r2.x || l2.x >= r1.x)
		return false;

	// If one rectangle is above other
	if (l1.y >= r2.y || l2.y >= r1.y)
		return false;

	return _intersection(rect1, rect2, percent) ? true : false;

}

bool thermoCamera::FaceDetector::_isOverLapped(cv::Rect rect1, RECT rect2)
{
	Point l1 = { rect1.x,rect1.y };
	Point r1 = { rect1.width - rect1.x,rect1.height - rect1.y };
	Point r2 = { rect2.left,rect2.top };
	Point l2 = { rect2.right,rect2.bottom};
	// To check if either rectangle is actually a line
	// For example :  l1 ={-1,0}  r1={1,1}  l2={0,-1}
	// r2={0,1}

	if (l1.x == r1.x || l1.y == r2.y || l2.x == r2.x
		|| l2.y == r2.y) {
		// the line cannot have positive overlap
		return false;
	}

	// If one rectangle is on left side of other
	if (l1.x >= r2.x || l2.x >= r1.x)
		return false;

	// If one rectangle is above other
	if (l1.y <= r2.y || l2.y <= r1.y)
		return false;

	return true;
}

bool thermoCamera::FaceDetector::_intersection(const cv::Rect& rect1, const cv::Rect& rect2, int percentage)
{
	Point l1 = { rect1.x,rect1.y };
	Point r1 = { rect1.width + rect1.x,rect1.height + rect1.y };
	Point l2 = { rect2.x,rect2.y };
	Point r2 = { rect2.width + rect2.x,rect2.height + rect2.y };

	float intersect_inside_of_rectangles = ( min(r1.y, r2.y)- max(l1.y, l2.y)) * (min(r1.x, r2.x) - max(l1.x, l2.x));

	float rect1Area = (r1.y - l1.y) * (r1.x - l1.x);
	float rect2Area = (r2.y - l2.y) * (r2.x - l2.x);

	float percent = (intersect_inside_of_rectangles / (rect1Area + rect2Area - intersect_inside_of_rectangles))*100;

	return (percent > percentage) ? true : false;
}

void thermoCamera::FaceDetector::_tempOutwithRect(HDC& hdc, POINT p, TCHAR* text, COLORREF color)
{
	RECT computeRect = { 0,0,0,0 };
	RECT rec = { 0,0,0,0 };

	DrawText(hdc, text, lstrlen(text), &computeRect, DT_CALCRECT | DT_SINGLELINE);

	rec = { p.x,p.y,p.x + computeRect.right,p.y + computeRect.bottom };
	FillRect(hdc, &rec, CreateSolidBrush(color));

	DrawText(hdc, text, lstrlen(text), &rec, DT_SINGLELINE | DT_CENTER);
}

void thermoCamera::FaceDetector::_CImageToMat(CImage& cimage, Mat& mat)
{
	if (true == cimage.IsNull())
	{
		return;
	}
	int nChannels = cimage.GetBPP() / 8;
	if ((1 != nChannels) && (3 != nChannels))
	{
		return;
	}
	int nWidth = cimage.GetWidth();
	int nHeight = cimage.GetHeight();
	//Rebuild mat
	if (1 == nChannels)
	{
		mat.create(nHeight, nWidth, CV_8UC1);
	}
	else if (3 == nChannels)
	{
		mat.create(nHeight, nWidth, CV_8UC3);
	}
	// Copy data
	uchar * pucRow; //point pointer to the data area
	uchar* pucImage = (uchar*)cimage.GetBits(); //pointer to the data area
	int nStep = cimage.GetPitch(); //The number of bytes per line, note that this return value has positive and negative
	for (int nRow = 0; nRow < nHeight; nRow++)
	{
		pucRow = (mat.ptr<uchar>(nRow));
		for (int nCol = 0; nCol < nWidth; nCol++)
		{
			if (1 == nChannels)
			{
				pucRow[nCol] = *(pucImage + nRow * nStep + nCol);
			}
			else if (3 == nChannels)
			{
				for (int nCha = 0; nCha < 3; nCha++)
				{
					pucRow[nCol * 3 + nCha] = *(pucImage + nRow * nStep + nCol * 3 + nCha);
				}
			}
		}
	}

}

void thermoCamera::FaceDetector::_hwnd2mat(HBITMAP hBitMap, HDC BackBuffer, cv::UMat& src)
{
	if (hBitMap == NULL || src.empty()) return;

	int height, width;
	BITMAPINFOHEADER  bi;

	width = src.cols;
	height = src.rows;

	// create a bitmap
	//hbwindow = CreateCompatibleBitmap(hwindowDC, width, height);
	bi.biSize = sizeof(BITMAPINFOHEADER);    //http://msdn.microsoft.com/en-us/library/windows/window/dd183402%28v=vs.85%29.aspx
	bi.biWidth = width;
	bi.biHeight = -height;  //this is the line that makes it draw upside down or not
	bi.biPlanes = 1;
	bi.biBitCount = 32;
	bi.biCompression = BI_RGB;
	bi.biSizeImage = 0;
	bi.biXPelsPerMeter = 0;
	bi.biYPelsPerMeter = 0;
	bi.biClrUsed = 0;
	bi.biClrImportant = 0;

	GetDIBits(BackBuffer, hBitMap, 0, height, src.getMat(ACCESS_READ).data, (BITMAPINFO*)&bi, DIB_RGB_COLORS);  //copy from hwindowCompatibleDC to hbwindow

	// avoid memory leak
	//DeleteObject(hbwindow); DeleteDC(hwindowCompatibleDC); ReleaseDC(hwnd, hwindowDC);
}

std::string thermoCamera::FaceDetector::_ws2s(const std::wstring& s)
{
	int slength = (int)s.length() + 1;
	int len = WideCharToMultiByte(CP_ACP, 0, s.c_str(), slength, 0, 0, 0, 0);
	std::string r(len, '\0');
	WideCharToMultiByte(CP_ACP, 0, s.c_str(), slength, &r[0], len, 0, 0);
	return r;
}

std::wstring thermoCamera::FaceDetector::_s2ws(const std::string& s)
{
	int len;
	int slength = (int)s.length() + 1;
	len = MultiByteToWideChar(CP_ACP, 0, s.c_str(), slength, 0, 0);
	std::wstring r(len, L'\0');
	MultiByteToWideChar(CP_ACP, 0, s.c_str(), slength, &r[0], len);
	return r;
}