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


/* ------------------------------------------------------------------------------
TIME FUNCTIONS
------------------------------------------------------------------------------ */

string getTimeLabel(int ms) {

	int mil = ms % 1000;
	int sec = ms / 1000;
	int min = (sec / 60) % 60;
	int hours = sec / 3600;
	sec = sec % 60;

	string hoursStr = to_string(hours), minStr = to_string(min), secStr = to_string(sec), milStr = to_string(mil);
	if (hours < 10)
		hoursStr = "0" + hoursStr;
	if (min < 10)
		minStr = "0" + minStr;
	if (sec < 10)
		secStr = "0" + secStr;
	if (mil < 100) {
		if (mil < 10) {
			milStr = "00" + milStr;
		} else {
			milStr = "0" + milStr;
		}
	}

	string timeLabel = hoursStr + ":" + minStr + ":" + secStr + "." + milStr;

	return timeLabel;

}

// This function is exactly like getTimeLabel, but without punctuation in the returned string
// due to file system necessities
string getSafeTimeLabel(int ms) {

	int mil = ms % 1000;
	int sec = ms / 1000;
	int min = (sec / 60) % 60;
	int hours = sec / 3600;
	sec = sec % 60;

	string hoursStr = to_string(hours), minStr = to_string(min), secStr = to_string(sec), milStr = to_string(mil);
	if (hours < 10)
		hoursStr = "0" + hoursStr;
	if (min < 10)
		minStr = "0" + minStr;
	if (sec < 10)
		secStr = "0" + secStr;
	if (mil < 100) {
		if (mil < 10) {
			milStr = "00" + milStr;
		} else {
			milStr = "0" + milStr;
		}
	}

	string timeLabel = hoursStr + "-" + minStr + "-" + secStr + "-" + milStr;

	return timeLabel;

}


/* ------------------------------------------------------------------------------
PARSING FUNCTIONS
------------------------------------------------------------------------------ */

void findFileNameFromPath(string* path, string* fileName, string* extension) {
	*path = path->substr(path->find_last_of("'") + 1, path->size());
	string path_without_extension = path->substr(0, path->find_last_of("."));
	*fileName = path_without_extension.substr(path_without_extension.find_last_of("/") + 1, path_without_extension.size());
	*extension = path->substr(path->find_last_of(".") + 1, path->size());
}

// Obtain video file name without path
int findFileName(string videoPath, string &fileName, string &extension) {

    // Divide file name from extension
    findFileNameFromPath(&videoPath, &fileName, &extension);

    if (extension.compare("avi") != 0 && extension.compare("mp4") != 0 && extension.compare("mov") != 0) {
        cerr << "Input file extension must be \"avi\", \"mp4\" or \"mov\"." << endl;
        return -1;
    } else {
        cout << "Video to be analysed: " << endl;
        cout << "    File name:  " << fileName << endl;
        cout << "    Extension:  " << extension << endl;
    }
    return 0;
}


/* ------------------------------------------------------------------------------
COMPUTER VISION FUNCTIONS
------------------------------------------------------------------------------ */

// Function to detect shape in frame
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);
	}
}

// Function to draw detected shapes in a frame
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;
}

// Function to separate even and odd frame half planes
void separateFrame(cv::Mat frame, cv::Mat &frame_dispari, cv::Mat &frame_pari) {

	int i_frame_dispari = 0;
	int i_frame_pari = 0;

	for (int i = 0; i < frame.rows; i++) {
		for (int j = 0; j < frame.cols; j++) {
			if (i % 2 == 0) {
				frame_pari.at<cv::Vec3b>( i_frame_pari, j )[0] = frame.at<cv::Vec3b>(i, j)[0];
				frame_pari.at<cv::Vec3b>( i_frame_pari, j )[1] = frame.at<cv::Vec3b>(i, j)[1];
				frame_pari.at<cv::Vec3b>( i_frame_pari, j )[2] = frame.at<cv::Vec3b>(i, j)[2];
			} else {
				frame_dispari.at<cv::Vec3b>( i_frame_dispari, j )[0] = frame.at<cv::Vec3b>(i, j)[0];
				frame_dispari.at<cv::Vec3b>( i_frame_dispari, j )[1] = frame.at<cv::Vec3b>(i, j)[1];
				frame_dispari.at<cv::Vec3b>( i_frame_dispari, j )[2] = frame.at<cv::Vec3b>(i, j)[2];
			}
		}

		if (i % 2 == 0) {
			i_frame_pari++;
		} else {
			i_frame_dispari++;
		}
	}

	return;

}

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;
}