/*
 * ObservationGenerator.cpp
 *
 * This class generates an observation based on blob extraction, whereas the centroid
 * of the biggest blob of a frame is defined as an observation. The underlying foreground
 * detection is realised with a Multi-Modal Gaussian model.
 *
 *  Created on: Apr 2, 2020
 *      Author: lukas
 */

#include "opencv2/opencv.hpp"
#include "ObservationGenerator.hpp"

using namespace cv;
using namespace std;

/**
 * Default constructor of an ObservationGenerator.
 * Uses a set of overall good parameters.
 */
ObservationGenerator::ObservationGenerator() {
	// Background substraction params
	_bks_lr = -1;
	_var_threshold = 18;
	_history = 80;
	_pMOG2 = cv::createBackgroundSubtractorMOG2(_history,
												_var_threshold,
												true);

	// Blob params
	_connectivity = 8;
	_min_width = 15;
	_min_height = 15;

	// Morph params
	_element_size = 5;
	_erosion_type = MORPH_RECT;
	_struct_element = getStructuringElement(_erosion_type,
										    Size(_element_size, _element_size));
}

ObservationGenerator::~ObservationGenerator() {
}

/**
 * Applies a background subtraction with the current model, given a frame.
 *
 * @param frame Mat a 1-channeled 2D OpenCV matrix containing the current frame.
 */
void ObservationGenerator::doBackGroundSubstraction(Mat frame) {
	_pMOG2->apply(frame, _fgmask, _bks_lr);
}

/**
 * Generates an observation.
 *
 * Extract the biggest blob of a given frame, if one exists, and populates the
 * given observation variable with it.
 *
 * @param frame Mat a 1-channeled 2D OpenCV matrix containing the current frame.
 * @param &observation Mat, a pointer to the observation variable which will be populated
 * with the centroid of the biggest blob.
 *
 * @returns int, -1 if no biggest blob was found (depending on the extraction parameters
 * defined in the constructor, or 1 if a biggest blob was found.
 */
int ObservationGenerator::generateObservation(Mat frame, Mat &observation) {
	Mat filled_mask;
	cvBlob biggest_blob;

	// get fgmask
	doBackGroundSubstraction(frame);

	// Morphological Opening (erosion & dilation)
	erode(_fgmask, _fgmask, _struct_element);
	dilate(_fgmask, _fgmask, _struct_element);

	// Do CCA and get the biggest blob
	_fgmask.convertTo(filled_mask, CV_32SC1);
	int blob_found = extractBiggestBlog(_fgmask, filled_mask, biggest_blob, _connectivity);
	// No blob was found, no observation can be made OR biggest blob is to small
	if (blob_found == -1)
		return -1;

	_blob = biggest_blob;

	// Get x and y of blob (our observation value) with moments calculation (see docs)
	Mat biggest_blob_mask = filled_mask == biggest_blob.ID;
	Moments m = moments(biggest_blob_mask, true);
	observation.at<float>(0,0) = m.m10/m.m00;
	observation.at<float>(1,0) = m.m01/m.m00;

	return 1;
}

/**
 * Extracts the biggest blob under some constraint from a foreground mask.
 *
 * This method performs connected component analysis on a given foreground mask to extract blobs in
 * the image. These blobs are filtered according to their size and then the biggest blob is
 * extracted and populated in the pointer.
 *
 * @param fgmask Mat, OpenCV 2D matrix containing the foreground mask
 * @param &filled_mask Mat, pointer to a Mat variable which will be used to save
 * the result of the connected component analysis (also called auxiliary image)
 * @param &biggest_blob cvBlob, pointer which will be populated with the biggest blob
 * @param connectivity, connectivity parameter of the connected component analysis
 *
 * @returns int, -1 if no biggest blob was found, 1 if a biggest blob was found
 */
int ObservationGenerator::extractBiggestBlog(cv::Mat fgmask, Mat &filled_mask, cvBlob &biggest_blob, int connectivity)
{
	vector<cvBlob> bloblist;

	// Temp variables to find the biggest blob
	double max_size = 0;
	int max_indx;

	// Connected component analysis
	int blob_label = 256; // start at 256 so no interference with shadow and foreground
	for (int x = 0; x < filled_mask.rows; x++)
	{
		for (int y = 0; y < filled_mask.cols; y++)
		{
			int current_val = filled_mask.at<int>(x,y);
			if (current_val == 255)
			{
				// Fill and get blob
				Point p = Point(y, x);
				Rect r;
				floodFill(filled_mask, p, blob_label, &r, 0, 0, connectivity); // upDiff, loDiff = 0

				if (r.width >= _min_width && r.height >= _min_height) {
					cvBlob b = initBlob(blob_label, r.x, r.y, r.width, r.height);
					bloblist.push_back(b);

					// Check whether biggest blob so far
					Mat blob_mask = filled_mask == b.ID;
					double current_size = sum(blob_mask)[0];

					if (current_size > max_size)
					{
						max_size = current_size;
						max_indx = bloblist.size() - 1;
					}
				}
				blob_label ++;
			}
		}
	}

	// If no blob found return -1, otherwise set blob and return 1
	if (bloblist.size() == 0)
		return -1;

	biggest_blob = bloblist[max_indx];
	return 1;

}


/* LEGACY CODE FROM HERE, MIGHT BE NEEDED LATER */

//void ObservationGenerator::extractBlobs(cv::Mat fgmask, std::vector<cvBlob> &bloblist, int connectivity)
//{
//	Mat aux; // image to be updated each time a blob is detected (blob cleared)
//	fgmask.convertTo(aux,CV_32SC1);
//
//	// Connected component analysis
//	int num_blobs = 255; // start at 255 so no interference with shadow and foreground
//	for (int x = 0; x < aux.rows; x++)
//	{
//		for (int y = 0; y < aux.cols; y++)
//		{
//			int current_val = aux.at<int>(x,y);
//			if (current_val == 255)
//			{
//				num_blobs ++;
//
//				Point p = Point(y, x);
//				Rect r;
//				// upDiff, loDiff = 0
//				floodFill(aux, p, num_blobs, &r, 0, 0, connectivity);
//
//				cvBlob b = initBlob(num_blobs, r.x, r.y, r.width, r.height);
//				bloblist.push_back(b);
//			}
//		}
//	}
//}
//
//void ObservationGenerator::removeSmallBlobs(std::vector<cvBlob> bloblist_in, std::vector<cvBlob> &bloblist_out, int min_width, int min_height)
//{
//	for(int i = 0; i < bloblist_in.size(); i++)
//	{
//		cvBlob blob_in = bloblist_in[i]; //get ith blob
//		if (blob_in.w >= min_width && blob_in.h >= min_height)
//		{
//			bloblist_out.push_back(blob_in); // void implementation (does not remove)
//		}
//
//	}
//}
//
//void ObservationGenerator::getBiggestBlob(std::vector<cvBlob> bloblist_in, cvBlob &biggest_blob) {
//	int max_val = 0;
//	int max_indx;
//
//	for(int i = 0; i < bloblist_in.size(); i++)
//	{
//		cvBlob blob_in = bloblist_in[i];
//		int current_max_val = blob_in.w * blob_in.h;
//
//		if (current_max_val > max_val)
//		{
//			max_val = current_max_val;
//			max_indx = i;
//		}
//
//	}
//
//	biggest_blob = bloblist_in[max_indx];
//}