using namespace cv;
using namespace std;
namespace fs = std::filesystem;

/*************************************************************************************************/
/*************************************** OpenCV FUNCTIONS ****************************************/
/*************************************************************************************************/

/**
 * @brief Detects a given shape in an image, using a the OpenCV algorithm GeneralizedHoughGuil.
 *
 * @param[in] alg the algorithm instance;
 * @param[in] templateShape the shape to detect;
 * @param[in] posThresh the position votes threshold;
 * @param[out] positivePositions vector representing the position assigned to each found rectangle for positive angles;
 * @param[out] positiveVotes vector representing the vote assigned to each found rectangle for positive angles;
 * @param[out] negativePositions vector representing the position assigned to each found rectangle for negative angles;
 * @param[out] negativeVotes vector representing the vote assigned to each found rectangle for negative angles;
 * @param[in] processingArea the image to be processed.
 */
void detectShape(Ptr<GeneralizedHoughGuil> alg, Mat templateShape, int posThresh, vector<Vec4f> &positivePositions, Mat &positiveVotes, vector<Vec4f> &negativePositions, Mat &negativeVotes, Mat processingArea) {

	alg -> setPosThresh(posThresh);
	alg -> setTemplate(templateShape);

	int oldSizePositive = 0;
	int i = 0;
	int maxVote = 0;

	// Process shapes with positive angles
	alg -> setMinAngle(0);
	alg -> setMaxAngle(3);
	while (true) {
		alg -> detect(processingArea, positivePositions, positiveVotes);
		int currentSize = positivePositions.size();
		if (currentSize == 1) {
			// We detected the most interesting shape
			break;
		} else if (currentSize == 0 && oldSizePositive > 0) {
			// It is not possible to detect only one shape with the current parameters
			alg -> setPosThresh(posThresh+i-1); // Decrease position value
			alg -> detect(processingArea, positivePositions, positiveVotes); // Detect all available shapes
			break;
		} else if (currentSize == 0 && oldSizePositive == 0) {
			// Impossible to find with these parameters
			break;
		}
		oldSizePositive = currentSize;
		// Find maximum vote
		for (int j = 0; j < positiveVotes.cols / 3; j++) {
			if (positiveVotes.at<int>(3*j) > maxVote)
				maxVote = positiveVotes.at<int>(3*j);
		}

		if (currentSize > 10) {
			i += 5; // To speed up computation when there are too many matches
		} else if (maxVote - (posThresh + i) > 100) {
			i += 100; // To speed up computation when there are few super high matches
		} else {
			i++;
		}
		alg -> setPosThresh(posThresh+i);
	}

	int oldSizeNegative = 0;
	// Reset incremental position value
	i = 0;
	maxVote = 0;
	// Process shapes with negative angles
	alg -> setMinAngle(357);
	alg -> setMaxAngle(360);
	while (true) {
		alg -> detect(processingArea, negativePositions, negativeVotes);
		int currentSize = negativePositions.size();
		if (currentSize == 1) {
			// We detected the most interesting shape
			break;
		} else if (currentSize == 0 && oldSizeNegative > 0) {
			// It is not possible to detect only one shape with the current parameters
			alg -> setPosThresh(posThresh+i-1); // Decrease position value
			alg -> detect(processingArea, negativePositions, negativeVotes); // Detect all available shapes
			break;
		} else if (currentSize == 0 && oldSizeNegative == 0) {
			// Impossible to found with these parameters
			break;
		}
		oldSizeNegative = currentSize;

		// Find maximum vote
		for (int j = 0; j < positiveVotes.cols / 3; j++) {
			if (positiveVotes.at<int>(3*j) > maxVote)
				maxVote = positiveVotes.at<int>(3*j);
		}

		if (currentSize > 10) {
			i += 5; // To speed up computation when there are too many matches
		} else if (maxVote - (posThresh + i) > 100) {
			i += 100; // To speed up computation when there are few super high matches
		} else {
			i++;
		}
		alg -> setPosThresh(posThresh+i);
	}
}


/**
 * @brief Draw rectangles on an image.
 *
 * @param frame Frame on which the rectangles will be drawn;
 * @param positions The position of the rectangle;
 * @param color The color of the rectangle;
 * @param width The width of the rectangle;
 * @param height The height of the rectangle;
 * @param offsetX X offset on the position of the rectangle;
 * @param offsetY Y offset on the position of the rectangle;
 * @param processingScale Scaling factor, useful for downsizing.
 * @return RotatedRect Object representing the drawn rectangle.
 */
RotatedRect drawShapes(Mat frame, Vec4f &positions, Scalar color, int width, int height, int offsetX, int offsetY, float processingScale) {

	RotatedRect rr;
	Point2f rrpts[4];

	Point2f pos(positions[0]+offsetX, positions[1]+offsetY);
	float scale = positions[2];
	float angle = positions[3];

	rr.center = pos * processingScale;
	rr.size = Size2f(width * scale * processingScale, height * scale * processingScale);
	rr.angle = angle;

	rr.points(rrpts);

	line(frame, rrpts[0], rrpts[1], color, 2);
	line(frame, rrpts[1], rrpts[2], color, 2);
	line(frame, rrpts[2], rrpts[3], color, 2);
	line(frame, rrpts[3], rrpts[0], color, 2);

	return rr;
}


/**
 * @brief Function to deinterlace the current image.
 *
 * @param[in] frame image to be processed;
 * @param[out] odd_frame odd plane;
 * @param[out] even_frame even plane.
 */
void separateFrame(cv::Mat frame, cv::Mat &odd_frame, cv::Mat &even_frame) {

	int i_odd_frame = 0;
	int i_even_frame = 0;

	for (int i = 0; i < frame.rows; i++) {
		for (int j = 0; j < frame.cols; j++) {
			if (i % 2 == 0) {
				even_frame.at<cv::Vec3b>( i_even_frame, j )[0] = frame.at<cv::Vec3b>(i, j)[0];
				even_frame.at<cv::Vec3b>( i_even_frame, j )[1] = frame.at<cv::Vec3b>(i, j)[1];
				even_frame.at<cv::Vec3b>( i_even_frame, j )[2] = frame.at<cv::Vec3b>(i, j)[2];
			} else {
				odd_frame.at<cv::Vec3b>( i_odd_frame, j )[0] = frame.at<cv::Vec3b>(i, j)[0];
				odd_frame.at<cv::Vec3b>( i_odd_frame, j )[1] = frame.at<cv::Vec3b>(i, j)[1];
				odd_frame.at<cv::Vec3b>( i_odd_frame, j )[2] = frame.at<cv::Vec3b>(i, j)[2];
			}
		}

		if (i % 2 == 0) {
			i_even_frame++;
		} else {
			i_odd_frame++;
		}
	}

	return;

}


/**
 * @brief Compute the number of different pixels between two frames.
 *
 * @param prevFrame the first frame;
 * @param currentFrame the second frame.
 * @return cv::Mat A black and white frame, where black pixels represent a difference, while white pixels represent an equality.
 */
cv::Mat difference(cv::Mat &prevFrame, cv::Mat &currentFrame) {
	cv::Mat diff = currentFrame.clone();
	for (int i = 0; i < currentFrame.rows; i++) {
		for (int j = 0; j < currentFrame.cols; j++) {
			if (prevFrame.at<cv::Vec3b>(i, j)[0] != currentFrame.at<cv::Vec3b>(i, j)[0] || prevFrame.at<cv::Vec3b>(i, j)[1] != currentFrame.at<cv::Vec3b>(i, j)[1] || prevFrame.at<cv::Vec3b>(i, j)[2] != currentFrame.at<cv::Vec3b>(i, j)[2]) {
				// Different pixels
				diff.at<cv::Vec3b>(i, j)[0] = 0;
			} else {
				// Identical pixels
				diff.at<cv::Vec3b>(i, j)[0] = 255;
			}
		}
	}
	return diff;
}