#include <vector>
#include <string.h>
#include <fstream>

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

std::string pathBrand, pathFullFrame, pathTape, pathTape224, pathEndTape;

/* ------------------------------------------------------------------------------
FILE SYSTEM FUNCTIONS
------------------------------------------------------------------------------ */

void makeDirectories(std::string fileName, std::string outputPath, bool brands) {

	// Get now time
	std::time_t t = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now());
	struct tm *parts = std::localtime(&t);
	int day = parts->tm_mday, month = parts->tm_mon + 1, year = parts->tm_year + 1900, hours = parts->tm_hour, minutes = parts->tm_min, seconds = parts->tm_sec;
	std::string dayStr = std::to_string(day), monthStr = std::to_string(month), yearStr = std::to_string(year), hoursStr = std::to_string(hours), minutesStr = std::to_string(minutes), secondsStr = std::to_string(seconds);
	if (day < 10)
		dayStr = "0" + dayStr;
	if (month < 10)
		monthStr = "0" + monthStr;
	if (hours < 10)
		hoursStr = "0" + hoursStr;
	if (minutes < 10)
		minutesStr = "0" + minutesStr;
	if (seconds < 10)
		secondsStr = "0" + secondsStr;
	// Make directory with fileName name
	int outputFileNameDirectory = fs::create_directory(outputPath + fileName);
	// Update output path
	outputPath += fileName + "/" + yearStr + "-" + monthStr + "-" + dayStr + "_" + hoursStr + "-" + minutesStr + "-" + secondsStr + "/";
	// Create a new directory named as the current time for multiple runs
	int outputNowDirectory = fs::create_directory(outputPath);

	// Make output directories
	pathBrand = outputPath + "/brand";
	pathFullFrame = outputPath + "/fullFrame";
	pathTape = outputPath + "/tape";
	pathTape224 = outputPath + "/tape224";
	pathEndTape = outputPath + "/endTape";
	if (brands)
		int brandDirectory = fs::create_directory(pathBrand);
	int fullFrameDirectory = fs::create_directory(pathFullFrame);
	int nastroDirectory = fs::create_directory(pathTape);
	int nastro224Directory = fs::create_directory(pathTape224);
	int capsDirectory = fs::create_directory(pathEndTape);
}

void saveIrregularityImage(std::string safeTimeLabel, std::string fileName, cv::Mat frame, cv::Mat subFrame) {

	// if (savingPinchRoller) {
	// 	cv::imwrite(pathEndTape + "/" + fileName + "_" + safeTimeLabel + ".jpg", frame);
	// } else if (savingBrand) {
	// 	cv::imwrite(pathBrand + "/" + fileName + "_" + safeTimeLabel + ".jpg", frame);
	// 	savingBrand = false;
	// } else {
		// Writing image
		cv::imwrite(pathFullFrame + "/" + fileName + "_" + safeTimeLabel + ".jpg", frame);
		cv::imwrite(pathTape + "/" + fileName + "_" + safeTimeLabel + ".jpg", subFrame);

		cv::Mat resized224Nas;
		cv::resize(subFrame, resized224Nas, cv::Size(224, 224));
		cv::imwrite(pathTape224 + "/"+ fileName + "_" + safeTimeLabel + ".jpg", resized224Nas);
	// }

}


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

std::string getTimeLabel(int ms) {
				
	int mil = ms % 1000;
	int sec = ms / 1000;
	int min = (sec / 60) % 60;
	int hours = sec / 3600;
	sec = sec % 60;

	std::string hoursStr = std::to_string(hours), minStr = std::to_string(min), secStr = std::to_string(sec), milStr = std::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;
		}
	}
	
	std::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
std::string getSafeTimeLabel(int ms) {
				
	int mil = ms % 1000;
	int sec = ms / 1000;
	int min = (sec / 60) % 60;
	int hours = sec / 3600;
	sec = sec % 60;

	std::string hoursStr = std::to_string(hours), minStr = std::to_string(min), secStr = std::to_string(sec), milStr = std::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;
		}
	}
	
	std::string timeLabel = hoursStr + "-" + minStr + "-" + secStr + "-" + milStr;

	return timeLabel;

}

int getMilliCount() {
    timeb tb;
    ftime(&tb);
    int nCount = tb.millitm + (tb.time & 0xfffff) * 1000;
    return nCount;
}


int getMilliSpan(int nTimeStart) {
    int nSpan = getMilliCount() - nTimeStart;
    if (nSpan < 0)
        nSpan += 0x100000 * 1000;
    return nSpan;
}


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

void findFileNameFromPath(std::string* path, std::string* fileName, std::string* extension) {
  *path = path->substr(path->find_last_of("'") + 1, path->size());
  std::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(std::string videoPath, std::string &fileName, std::string &extension) {

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

    if (extension.compare("avi") != 0 && extension.compare("mp4") != 0 && extension.compare("mov") != 0) {
        std::cerr << "Input file extension must be \"avi\", \"mp4\" or \"mov\"." << std::endl;
        return -1;
    } else {
        std::cout << "\nVideo to be analysed: " << std::endl;  
        std::cout << "  File name:  " << fileName << std::endl;
        std::cout << "  Extension:  " << extension << std::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(5);
	std::cout << "Processing positive angles..." << endl;
	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 found with these parameters
			std::cout << "\033[0;31mNot found for positive angles.\033[0m" << endl;
			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(355);
	alg -> setMaxAngle(360);
	std::cout << "Processing negative angles..." << endl;
	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
			std::cout << "\033[0;31mNot found for negative angles.\033[0m" << endl;
			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, vector<Vec4f> &positions, Scalar color, int width, int height, int offsetX, int offsetY, float processingScale) {

	RotatedRect rr;
	Point2f rrpts[4];

	for (int i = 0; i < positions.size(); i++) {
		Point2f pos(positions[i][0]+offsetX, positions[i][1]+offsetY);
		float scale = positions[i][2];
		float angle = positions[i][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;

}

// This function finds the straight lines delimiting the rotated rectangle.
void findRectBound(cv::Point p1, cv::Point p2, cv::Point p3, cv::Point p4, double &m1, double &m2, double &m3, double &m4, double &q1, double &q2, double &q3, double &q4) {
	int y1 = p1.y;
	int y2 = p2.y;
	int y3 = p3.y;
	int y4 = p4.y;

	int x1 = p1.x;
	int x2 = p2.x;
	int x3 = p3.x;
	int x4 = p4.x;

	// Finding the straight lines intersecting in y1 [p0.y]
	m1 = (double)(y2 - y1)/(x2 - x1);
	q1 = (y1 - m1*x1);
		
	m4 = (double)(y4 - y1)/(x4 - x1);
	q4 = (y1 - m4*x1);

	// Finding the straight lines intersecting in y3 [p2.y]
	m2 = (double)(y3 - y2)/(x3 - x2);
	q2 = (y3 - m2*x3);
	
	m3= (double)(y4 - y3)/(x4 - x3);
	q3 = (y3 - m3*x3);
}

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

