#include "detector.hpp"

namespace ppd
{
	Response::Response():
			position(cv::Rect_<int>(0,0,0,0)),
			response(0.0)
	{}

	Response::Response(const Response& response)
	{
		this->response = response.getResponse();
		response.getPosition(this->position);
	}

	Response::Response(cv::Rect_<int>& position, float response):
			position(position),
			response(response)
	{}

	float Response::getResponse() const
	{
		return this->response;
	}

	void Response::setResponse(const float response)
	{
		this->response = response;
	}

	void Response::getPosition(cv::Rect_<int>& position) const
	{
		position = this->position;
	}

	void Response::setPosition(const cv::Rect_<int>& position)
	{
		this->position = position;
	}

	Detector::Detector():
			scan_row_step(1),
			scan_col_step(1),
			voc_size(1000),
			scale_factor(1.2f),
			_threshold(0.5f)
	{}

	Detector::Detector(unsigned int scan_row_step, unsigned int scan_col_step):
			scan_row_step(scan_row_step),
			scan_col_step(scan_col_step),
			voc_size(1000),
			scale_factor(1.2f),
			_threshold(0.5f)
	{};

	void Detector::extract_window(const cv::Rect_<int>& sc_window, const std::vector<std::vector<std::vector<int> > > iArray, cv::Mat& bow_feature)
	{
		bow_feature = 0.f;
		int tmp_normalization = 0;
//		cv::Mat tmpMat;
//		descriptors.copyTo(tmpMat);

		for(int i = 0; i < sc_window.height; i++)
		{
			for(int j = 0; j < sc_window.width; j++)
			{
				for(int k = 0; k < static_cast<int>(iArray[i+sc_window.y][j+sc_window.x].size()); k++)
				{
					if(iArray[i+sc_window.y][j+sc_window.x][k] > -1)
					{
						bow_feature.at<float>(0,iArray[i+sc_window.y][j+sc_window.x][k]) += 1.f;
						tmp_normalization++;
					}
				}
			}
		}

		if(tmp_normalization == 0)
			return;

		bow_feature /= tmp_normalization;

//		std::cout << bow_feature << std::endl;

//		std::cout << "In the method extract window, was processed: " << tmp_normalization << " keypoints" << std::endl;
	}

	void Detector::scan_image(const cv::Rect_<int>& sc_window, const std::vector<std::vector<std::vector<int> > > iArray, const ppd::SVM& classifier, const ppd::Scaler& scaler, std::vector<Response>& responses)
	{
		if(sc_window.height > static_cast<int>(iArray.size()) || sc_window.width > static_cast<int>(iArray[0].size()))
			return;
		int voc_size = this->voc_size;
		cv::Rect_<int> window(0,0,sc_window.width, sc_window.height) ;

		const int posThreadsMax = omp_get_max_threads();
		cv::Rect_<int>* windows = new cv::Rect_<int>[posThreadsMax];
		std::fill_n(windows, posThreadsMax, window);

		//Bad size of scanning window
		if(sc_window.width <= 0 || sc_window.height <= 0)
			return;

		int max_height = static_cast<int>(iArray.size()) - sc_window.height + 1;
		int max_width = static_cast<int>(iArray[0].size()) - sc_window.width + 1;

//		std::vector<cv::Rect_<int> > _positions;

		//V komentari je paralelizace nastavena tak, aby kazde vlakno dostalo celistvou cast obrazu a aby se nestridala po radcich.
		//Ukazalo se ale, ze vyhodnoceni nekterych casti obrazu trva dele, nez ostatni, pak se stane, ze
		//napriklad ze ctyr vlaken se tri uz flakaji, zatimco 4tvrte se jeste maka... proto je asi praktictejsi
		//volit dynamic planovani...
//		int data_part = (max_height/this->scan_row_step)/posThreadsMax;
		#pragma omp parallel for schedule(dynamic)//static, data_part) shared(windows) //firstprivate(window, bow)
		for(int row = 0; row < max_height; row+=this->scan_row_step)
		{
			int threadID = omp_get_thread_num();
			cv::Mat bow = cv::Mat::zeros(1, voc_size, CV_32FC1);
			for(int col = 0; col < max_width; col+=this->scan_col_step)
			{
				std::vector<float> probabilities;
				cv::Mat scaled_bow;
				windows[threadID].x = col;
				windows[threadID].y = row;
				this->extract_window(windows[threadID], iArray, bow);
				scaler.scale(bow, scaled_bow);
//				float result =
				classifier.predict_probability(scaled_bow, probabilities);
//				if( result > 0.f)
//				{
					#pragma omp critical(window_add)
					{
						responses.push_back(Response(windows[threadID], probabilities[0]));
					}
//				}
				//ToDo: Delete Only for evaluating the detector
//				this->img.at<float>(static_cast<int>(row+sc_window.height/2.f), static_cast<int>(col+sc_window.width/2.f)) = result;
			}
		}

		delete[] windows;
	}

	void Detector::detect(const cv::Mat& image, const ppd::Extractor& descriptor_extractor, const ppd::BOW& bow_extractor, const ppd::SVM& classifier, const ppd::Scaler& scaler, std::vector<Response>& positions, std::vector<Response>& responses, unsigned int min_neigbours)
	{
		//Initializations of internal vector of responses
		std::vector<Response> _responses;

		//Image size
		int height = image.rows;
		int width = image.cols;

		this->voc_size = bow_extractor.get_voc_size();

		//Represents image, where pixels are swaped by words from BoW
		std::vector<std::vector<std::vector<int> > > iArray_tmp;
		iArray_tmp.resize(height);
		for(int i = 0; i < height; i++)
		{
			iArray_tmp[i].resize(width, std::vector<int>(0,-1));
		}

		//Initialization of scanning windows
		if(this->sc_windows.empty())
			this->set_scan_windows(cv::Size_<int>(100,100), this->scale_factor, cv::Size_<int>(image.cols, image.rows));
		if(this->scale_factor > 1.f && this->sc_windows.size() == 1)
					this->set_scan_windows(cv::Size_<int>(this->sc_windows[0].width ,this->sc_windows[0].height), this->scale_factor, cv::Size_<int>(width, height));

		//BoW_descriptor for whole image
		cv::Mat bow_descriptor;

		//Keypoints for whole image
		std::vector<cv::KeyPoint> keypoints;

		//Descriptors of its keypoints
		cv::Mat descriptors;

		//Each descriptor is represented by word: indexes[index of descriptor] = index of word
		// If indexes[index of descriptor] has no word then = -1
		std::vector<int>* indexes = new std::vector<int>;

		//Detecting keypoints
		descriptor_extractor.detect(image, keypoints);

		//Compute descriptors. Keypoints for which a descriptor cannot be computed are removed.
		//keypoints[i] has its descriptor in descriptors[i] (according to OpenCv 2.3.1)
		descriptor_extractor.compute(image, keypoints, descriptors);

		//Extracting Words
		bow_extractor.extract(descriptors, bow_descriptor, indexes);

		//Fetching the iArray - representation of image, pixels = indexes of words
		for(unsigned int i = 0; i < keypoints.size(); i++)
		{
			iArray_tmp[cvRound(keypoints[i].pt.y)][cvRound(keypoints[i].pt.x)].push_back((*indexes)[i]);	// = i;
		}

//		if(this->sc_windows.size()  == 1)
//		{
//			this->set_scan_windows(cv::Size_<int>(this->sc_windows[0].width, this->sc_windows[0].height), this->scale_factor, cv::Size_<int>(width,height) );
//		}

		for(int i = 0; i < static_cast<int>(this->sc_windows.size()); i++)
		{
//			this->img = cv::Mat::zeros(iArray_tmp.size(), iArray_tmp[0].size(), CV_32FC1);

			if(this->sc_windows[i].width > image.cols || this->sc_windows[i].height > image.rows)
				break;
			std::cerr << "Scanning window: x = " << this->sc_windows[i].width << " | y = " << this->sc_windows[i].height << std::endl;
			this->scan_image(this->sc_windows[i], iArray_tmp, classifier, scaler, _responses);

//			//ToDo delete!!!
//			std::stringstream ss;
//			ss << i << ".jpg";
//			cv::Mat ucharMatScaled;
//			cv::normalize(img, img, 0, 1, cv::NORM_MINMAX, -1, cv::Mat());
//			img.convertTo(ucharMatScaled, CV_8UC1, 255, 0);
//
//			cv::imwrite(ss.str(), ucharMatScaled);
		}

		std::vector<Response> detections;
		this->merge(_responses, detections, min_neigbours);

		positions = detections;

		responses = _responses;

		delete indexes;
	};

	void Detector::set_scan_steps(const unsigned int scan_row_step, const unsigned int scan_col_step)
	{
		this->scan_row_step = scan_row_step;
		this->scan_col_step = scan_col_step;
	}

	void Detector::set_scan_windows(const cv::Size_<int> window_min, const float scale_factor, const cv::Size_<int> window_max)
	{
		this->sc_windows.clear();
		cv::Rect_<int> window = cv::Rect_<int>(0,0,window_min.width, window_min.height);
		this->sc_windows.push_back(window);
		this->scale_factor = scale_factor;

		if(scale_factor >= 1.f && (window_max.width > window_min.width && window_max.height > window_min.height))
		{
			while(window.width*scale_factor < window_max.width && window.y*scale_factor < window_max.height)
			{
				window.width *= scale_factor;
				window.height *= scale_factor;
				this->sc_windows.push_back(window);
			}
		}
	}

	void Detector::merge(const std::vector<ppd::Response>& responses, std::vector<ppd::Response>& positions, const int min_neighbours)
		{
			positions.clear();
			int* mask = new int[static_cast<int>(responses.size())];
			memset(mask, 1, static_cast<int>(responses.size()));

			std::vector<ppd::Response> detections;
			cv::Point_<float> center;
			int detected = 0;
			double weight = 0.;

			Response tmp_detection;

//			for(unsigned int i = 0; i < static_cast<unsigned int>(responses.size()); i++)
//			{
//				if(mask[i] == -1)
//					continue;
//
//				weight = fabs(responses[i].response);
//				tmp_detection.position.x = responses[i].position.x * weight;
//				tmp_detection.position.y = responses[i].position.y * weight;
//				tmp_detection.position.width = responses[i].position.width * weight;
//				tmp_detection.position.height = responses[i].position.height * weight;
//				tmp_detection.response = responses[i].response;
//
//				for(unsigned int j = 0; j < static_cast<unsigned int>(responses.size()); j++)
//				{
//					if(mask[j] == -1)
//						continue;
//
//					center = cv::Point_<float>(responses[j].position.x + (responses[j].position.width/2.f), responses[j].position.y + (responses[j].position.height/2.f));
//
//					if(responses[i].position.contains(center))
//					{
//						mask[j] = -1;
//						float response_new = fabs(responses[j].response);
//
//						if( response_new > fabs(tmp_detection.response))
//							tmp_detection.response = responses[j].response;
//
//						tmp_detection.position.x += responses[j].position.x * response_new;
//						tmp_detection.position.y += responses[j].position.y * response_new;
//						tmp_detection.position.width += responses[j].position.width * response_new;
//						tmp_detection.position.height += responses[j].position.height * response_new;
//						weight += response_new;
//						detected++;
//					}
//				}

			for(unsigned int i = 0; i < static_cast<unsigned int>(responses.size()); i++)
			{
				if(mask[i] == -1 || responses[i].response < _threshold)
					continue;

				mask[i] = -1;
				weight = fabs(responses[i].response);
				center = cv::Point_<float>(responses[i].position.x + (responses[i].position.width/2.f), responses[i].position.y + (responses[i].position.height/2.f));

				tmp_detection.position.x = responses[i].position.x * weight;
				tmp_detection.position.y = responses[i].position.y * weight;
				tmp_detection.position.width = responses[i].position.width * weight;
				tmp_detection.position.height = responses[i].position.height * weight;
				tmp_detection.response = responses[i].response;

				detected++;
				for(unsigned int j = 0; j < static_cast<unsigned int>(responses.size()); j++)
				{
					if(mask[j] == -1 || responses[j].response < _threshold)
						continue;

					if(responses[j].position.contains(center))
					{
						mask[j] = -1;
						float response_new = fabs(responses[j].response);

						if( response_new > fabs(tmp_detection.response))
							tmp_detection.response = responses[j].response;

						tmp_detection.position.x += responses[j].position.x * response_new;
						tmp_detection.position.y += responses[j].position.y * response_new;
						tmp_detection.position.width += responses[j].position.width * response_new;
						tmp_detection.position.height += responses[j].position.height * response_new;
						weight += response_new;
						detected++;
					}
				}

				if(min_neighbours <= detected)
				{
					tmp_detection.position.x /= weight;
					tmp_detection.position.y /= weight;
					tmp_detection.position.width /= weight;
					tmp_detection.position.height /= weight;

					detections.push_back(tmp_detection);
				}
				weight = 0.;
				detected = 0;
			}

			positions = detections;
			delete[] mask;
		}

	Scanner::Scanner():
				scan_row_step(1),
				scan_col_step(1),
				voc_size(1000),
				scale_factor(1.2f)
		{}

	Scanner::Scanner(unsigned int scan_row_step, unsigned int scan_col_step):
			scan_row_step(scan_row_step),
			scan_col_step(scan_col_step),
			voc_size(1000),
			scale_factor(1.2f)
	{}

	void Scanner::extract_window(const std::vector<std::vector<std::vector<int> > > iArray, const cv::Rect_<int>& sc_window, cv::Mat& feature, bool& extracted)
	{
		feature = 0.f;
		int tmp_normalization = 0;

		for(int i = 0; i < sc_window.height; i++)
		{
			for(int j = 0; j < sc_window.width; j++)
			{
				for(int k = 0; k < static_cast<int>(iArray[i+sc_window.y][j+sc_window.x].size()); k++)
				{
					if(iArray[i+sc_window.y][j+sc_window.x][k] > -1)
					{
						feature.at<float>(0,iArray[i+sc_window.y][j+sc_window.x][k]) += 1.f;
						tmp_normalization++;
					}
				}
			}
		}

		if(tmp_normalization == 0)
		{
			extracted = false;
			return;
		}

		feature /= tmp_normalization;
		extracted = true;

//		std::cout << bow_feature << std::endl;

//		std::cout << "In the method extract window, was processed: " << tmp_normalization << " keypoints" << std::endl;
	}

	void Scanner::scan_image(const std::vector<std::vector<std::vector<int> > > iArray, const cv::Rect_<int>& sc_window, cv::Mat& features)
	{
		if(sc_window.height > static_cast<int>(iArray.size()) || sc_window.width > static_cast<int>(iArray[0].size()))
			return;
		int voc_size = this->voc_size;
		cv::Rect_<int> window(0,0,sc_window.width, sc_window.height) ;

		const int posThreadsMax = omp_get_max_threads();
		cv::Rect_<int>* windows = new cv::Rect_<int>[posThreadsMax];
		std::fill_n(windows, posThreadsMax, window);

		//Bad size of scanning window
		if(sc_window.width <= 0 || sc_window.height <= 0)
			return;

		int max_height = static_cast<int>(iArray.size()) - sc_window.height + 1;
		int max_width = static_cast<int>(iArray[0].size()) - sc_window.width + 1;

//		std::vector<cv::Rect_<int> > _positions;

		//V komentari je paralelizace nastavena tak, aby kazde vlakno dostalo celistvou cast obrazu a aby se nestridala po radcich.
		//Ukazalo se ale, ze vyhodnoceni nekterych casti obrazu trva dele, nez ostatni, pak se stane, ze
		//napriklad ze ctyr vlaken se tri uz flakaji, zatimco 4tvrte se jeste maka... proto je asi praktictejsi
		//volit dynamic planovani...
//		int data_part = (max_height/this->scan_row_step)/posThreadsMax;
		#pragma omp parallel for schedule(dynamic)//static, data_part) shared(windows) //firstprivate(window, bow)
		for(int row = 0; row < max_height; row+=this->scan_row_step)
		{
			bool extracted = false;
			int threadID = omp_get_thread_num();
			cv::Mat bow = cv::Mat::zeros(1, voc_size, CV_32FC1);
			for(int col = 0; col < max_width; col+=this->scan_col_step)
			{
				windows[threadID].x = col;
				windows[threadID].y = row;
				this->extract_window(iArray, windows[threadID], bow, extracted);
				if(extracted)
				{
					#pragma omp critical(window_add)
					{
						features.push_back(bow);
					}
				}
			}
		}

		delete[] windows;
	}

	void Scanner::scan(const cv::Mat& image, const ppd::Extractor& descriptor_extractor, const ppd::BOW& bow_extractor, cv::Mat& features)
	{
		features = cv::Mat();
		//Image size
		int height = image.rows;
		int width = image.cols;

		this->voc_size = bow_extractor.get_voc_size();

		//Represents image, where pixels are swaped by words from BoW
		std::vector<std::vector<std::vector<int> > > iArray_tmp;
		iArray_tmp.resize(height);
		for(int i = 0; i < height; i++)
		{
			iArray_tmp[i].resize(width, std::vector<int>(0,-1));
		}

		//Initialization of scanning windows
		if(this->sc_windows.empty())
			this->set_scan_windows(cv::Size_<int>(100,100), 1.5);

		//BoW_descriptor for whole image
		cv::Mat bow_descriptor;

		//Keypoints for whole image
		std::vector<cv::KeyPoint> keypoints;

		//Descriptors of its keypoints
		cv::Mat descriptors;

		//Each descriptor is represented by word: indexes[index of descriptor] = index of word
		// If indexes[index of descriptor] has no word then = -1
		std::vector<int>* indexes = new std::vector<int>;

		//Detecting keypoints
		descriptor_extractor.detect(image, keypoints);

		//Compute descriptors. Keypoints for which a descriptor cannot be computed are removed.
		//keypoints[i] has its descriptor in descriptors[i] (according to OpenCv 2.3.1)
		descriptor_extractor.compute(image, keypoints, descriptors);

		//Extracting Words
		bow_extractor.extract(descriptors, bow_descriptor, indexes);

		//Fetching the iArray - representation of image, pixels = indexes of words
		for(unsigned int i = 0; i < keypoints.size(); i++)
		{
			iArray_tmp[cvRound(keypoints[i].pt.y)][cvRound(keypoints[i].pt.x)].push_back((*indexes)[i]);	// = i;
		}

//		if(this->sc_windows.size()  == 1)
//		{
//			this->set_scan_windows(cv::Size_<int>(this->sc_windows[0].width, this->sc_windows[0].height), this->scale_factor, cv::Size_<int>(width,height) );
//		}

		for(int i = 0; i < static_cast<int>(this->sc_windows.size()); i++)
		{
//			this->img = cv::Mat::zeros(iArray_tmp.size(), iArray_tmp[0].size(), CV_32FC1);

			if(this->sc_windows[i].width > image.cols || this->sc_windows[i].height > image.rows)
				break;
//				std::cerr << "Scanning window: x = " << this->sc_windows[i].width << " | y = " << this->sc_windows[i].height << std::endl;
			this->scan_image(iArray_tmp, this->sc_windows[i], features);

		}

		delete indexes;
	};

	void Scanner::set_scan_steps(const unsigned int scan_row_step, const unsigned int scan_col_step)
	{
		this->scan_row_step = scan_row_step;
		this->scan_col_step = scan_col_step;
	}

	void Scanner::set_scan_windows(const cv::Size_<int> window_min, const float scale_factor, const cv::Size_<int> window_max)
	{
		this->sc_windows.clear();
		cv::Rect_<int> window = cv::Rect_<int>(0,0,window_min.width, window_min.height);
		this->sc_windows.push_back(window);
		this->scale_factor = scale_factor;

		if(scale_factor <= 1.f || (window_max.width > window_min.width && window_max.height > window_min.height))
		{
			while(window.width*scale_factor < window_max.width && window.y*scale_factor < window_max.height)
			{
				window.width *= scale_factor;
				window.height *= scale_factor;
				this->sc_windows.push_back(window);
			}
		}
	}

}
