#include "opencv2/opencv.hpp"
#include <iostream>
#include <chrono>

using namespace cv;
using namespace std;
using namespace std::chrono;

int check_read(Mat image);
Mat mask_create(Mat input, Mat ref, char threshold);
Mat registrate(Mat im1, Mat im2, Mat im1_gray, Mat im2_gray, int warp_mode, int number_of_iterations, double termination_eps, int i);
Mat decrop(Mat im, Mat premask, int dx1, int dx2, int dy1, int dy2);
Mat output_create(Mat output, Mat mask);
Mat demask(Mat mask);
Mat demask2(Mat mask);
Mat demask3(Mat mask);
Mat maskmerge(Mat mask1, Mat mask2);

int main(void)
{
	int threshold = 30;
	//clock-start
	high_resolution_clock::time_point t1 = high_resolution_clock::now();

	// Read the images to be aligned
	cout << "Reading image1 - ";
	Mat image1 = imread("./image1.jpg");
	check_read(image1);
	cout << "Reading image2 - ";
	Mat image2 = imread("./image2.jpg");
	check_read(image2);
	cout << std::endl;

	high_resolution_clock::time_point t2 = high_resolution_clock::now();

	int height = image1.rows;
	int width = image1.cols;

	Mat im1, im2;

	cout << "Resizing images - ";
	resize(image1, im1, Size(320, 240), 2.28, 2.28, cv::INTER_LANCZOS4);
	resize(image2, im2, Size(320, 240), 2.28, 2.28, cv::INTER_LANCZOS4);
	cout << "DONE" << std::endl;
	high_resolution_clock::time_point t3 = high_resolution_clock::now();
	// Convert images to gray scale;
	cout << "Converting images to grayscale - ";
	Mat im1_gray, im2_gray;
	cvtColor(im1, im1_gray, CV_BGR2GRAY);
	cvtColor(im2, im2_gray, CV_BGR2GRAY);
	cout << "DONE" << std::endl << std::endl;
	high_resolution_clock::time_point t4 = high_resolution_clock::now();
	imwrite("./output/STEP01_im1_gray1.jpg", im1_gray);
	imwrite("./output/STEP01_im2_gray1.jpg", im2_gray);

	Mat premask1 = mask_create(im1_gray, im2_gray, threshold);


	// Define the motion model
	const int warp_mode = MOTION_HOMOGRAPHY;

	// Specify the number of iterations.
	int number_of_iterations = 1000000;

	// Specify the threshold of the increment in the correlation coefficient between two iterations
	double termination_eps = 1e-4;

	Mat im2_aligned = registrate(im1, im2, im1_gray, im2_gray, warp_mode, number_of_iterations, termination_eps, 1);

	high_resolution_clock::time_point t5 = high_resolution_clock::now();

	Mat im2_aligned_gray;
	cvtColor(im2_aligned, im2_aligned_gray, CV_BGR2GRAY);
	imwrite("./output/STEP03_im2_aligned_gray1.jpg", im2_aligned_gray);  //

	Mat premask2 = mask_create(im1_gray, im2_aligned_gray, threshold);

	Mat im12 = im1(Rect(32, 24, 256, 192)).clone();
	Mat im22 = im2_aligned(Rect(32, 24, 256, 192)).clone();
	im1 = im12;
	im2 = im22;
	cvtColor(im1, im1_gray, CV_BGR2GRAY);
	cvtColor(im2, im2_gray, CV_BGR2GRAY);
	Mat premask4 = mask_create(im1_gray, im2_gray, threshold);
	high_resolution_clock::time_point t6 = high_resolution_clock::now();

	im2_aligned = registrate(im1, im2, im1_gray, im2_gray, MOTION_AFFINE, number_of_iterations, 1e-7, 2);

	cvtColor(im2_aligned, im2_aligned_gray, CV_BGR2GRAY);

	high_resolution_clock::time_point t7 = high_resolution_clock::now();
	imwrite("./output/STEP05_im2_aligned_gray2.jpg", im2_aligned_gray); //

	//IMAGE PROCESSING ==============================================================================
	cout << "Processing images" << std::endl;
	//create mask

	Mat mask = mask_create(im2_aligned_gray, im1_gray, threshold);

	//decroping
	cout << "Decroping" << std::endl;
	Mat DCmask = decrop(mask, premask1, 32, 256, 24, 192);
	cout << "Decropped" << std::endl;
	//removing lines
	Mat DCmaskNL = demask(DCmask);
	
	Mat DCmaskNL3 = demask3(DCmask);
	//Merging masks
	Mat DCmaskM = maskmerge(DCmaskNL, DCmaskNL3);
	//enlarging mask
	Mat Bigmask;
	resize(DCmaskM, Bigmask, Size(width, height), 0, 0, cv::INTER_LANCZOS4);
	//created
	
	cout << "Showing mask" << std::endl;
	imshow("PreMask1", premask1);
	imwrite("./output/STEP02_premask1.jpg", premask1); //
	imshow("PreMask2", premask2);
	imwrite("./output/STEP03_premask2.jpg", premask2); //
	imshow("PreMask4", premask4);
	imwrite("./output/STEP06_premask4.jpg", premask4);
	imshow("Mask", mask);
	imwrite("./output/STEP05_mask.jpg", mask); //
	imshow("DeCropMask", DCmask);
	imwrite("./output/STEP07_DCmask.jpg", DCmask);
	imshow("DeCrMask_NoLines", DCmaskNL);
	imwrite("./output/STEP08_DCmaskNL.jpg", DCmaskNL);
	imshow("DeCrMask_NoLines3", DCmaskNL3);
	imwrite("./output/STEP09_DCmaskNL3.jpg", DCmaskNL3);
	imshow("DeCrMask_Merged", DCmaskM);
	imwrite("./output/STEP10_DCmaskM.jpg", DCmaskM);
	imshow("BigMask", Bigmask);
	imwrite("./output/STEP11_Bigmask.jpg", Bigmask);
	
	high_resolution_clock::time_point t8 = high_resolution_clock::now();
	Mat output = output_create(image1, Bigmask);
	high_resolution_clock::time_point t9 = high_resolution_clock::now();
	imshow("Output", output);

	auto time12 = duration_cast<milliseconds>(t2 - t1).count();
	auto time23 = duration_cast<milliseconds>(t3 - t2).count();
	auto time34 = duration_cast<microseconds>(t4 - t3).count();
	auto time45 = duration_cast<milliseconds>(t5 - t4).count();
	auto time56 = duration_cast<milliseconds>(t6 - t5).count();
	auto time67 = duration_cast<milliseconds>(t7 - t6).count();
	auto time78 = duration_cast<milliseconds>(t8 - t7).count();
	auto time89 = duration_cast<milliseconds>(t9 - t8).count();
	auto time19 = duration_cast<milliseconds>(t9 - t1).count();
	cout << "Calculation times in milliseconds:" << std::endl;
	cout << "Loading images:      " << time12 << std::endl;
	cout << "Downsizing images:   " << time23 << std::endl;
	cout << "Graying images [us]: " << time34 << std::endl;
	cout << "TransHOMO:           " << time45 << std::endl;
	cout << "Cropping:            " << time56 << std::endl;
	cout << "TransAFFINE:         " << time67 << std::endl;
	cout << "Mask creation:       " << time78 << std::endl;
	cout << "Output creation:     " << time89 << std::endl;
	cout << "Alltogether:         " << time19 << std::endl << std::endl;

	imwrite("./output/output.jpg", output);

	imwrite("./output/STEP04_im1_gray2.jpg", im1_gray); //
	imwrite("./output/STEP04_im2_gray2.jpg", im2_gray); //

	waitKey(0);
	system("PAUSE");
}


//===========================================================================================================
//===========================================================================================================
//===========================================================================================================
//===========================================================================================================

int check_read(Mat image)
{
	if (!image.data) // Check for invalid input
	{
		cout << "FAILED" << std::endl;
		return -1;
	}
	else
	{
		cout << "DONE" << std::endl;
	}
	return(0);
}

Mat mask_create(Mat input, Mat ref, char threshold)
{
	//create mask
	cout << "Creating mask - " << std::endl;
	Mat mask = ref.clone();
	Mat mask1 = ref.clone();
	Mat mask2 = ref.clone();
	mask1 = mask1(Range::all(), Range::all()) - input(Range::all(), Range::all());
	mask2 = -mask2(Range::all(), Range::all()) + input(Range::all(), Range::all());
	mask = mask1(Range::all(), Range::all()) + mask2(Range::all(), Range::all());
	cout << "DONE" << std::endl;
	//preview mask
	
	cout << "Showing preview of the mask" << std::endl;
	Mat C(mask, Rect(0, 0, 5, 5));
	cout << C << std::endl;
	
	//modify mask
	cout << "Counting dimensions" << std::endl;
	int rows = mask.rows;
	int cols = mask.cols;
	cout << "Modifying mask into I/O" << std::endl;
	//setting mask into I/O form (0…match, 255…difference)
	for (int i = 0; i < rows; i++)
	{
		for (int j = 0; j < cols; j++)
		{

			if (mask.at<uchar>(i, j) > threshold)
			{
				mask.at<uchar>(i, j) = 0;
			}
			else mask.at<uchar>(i, j) = 255;


		}
	}
	return mask;
}

Mat registrate(Mat im1, Mat im2, Mat im1_gray, Mat im2_gray, int warp_mode, int number_of_iterations, double termination_eps, int i)
{
	// Set a 2x3 or 3x3 warp matrix depending on the motion model.
	Mat warp_matrix;

	// Initialize the matrix to identity
	if (warp_mode == MOTION_HOMOGRAPHY)
		warp_matrix = Mat::eye(3, 3, CV_32F);
	else
		warp_matrix = Mat::eye(2, 3, CV_32F);
	// Define termination criteria
	TermCriteria criteria(TermCriteria::COUNT + TermCriteria::EPS, number_of_iterations, termination_eps);

	// Run the ECC algorithm. The results are stored in warp_matrix.
	cout << "Finding Transform" << i << " - ";
	findTransformECC(
		im1_gray,
		im2_gray,
		warp_matrix,
		warp_mode,
		criteria
		);
	cout << "DONE" << std::endl;

	// Storage for warped image.
	Mat im2_aligned;

	if (warp_mode != MOTION_HOMOGRAPHY)
		// Use warpAffine for Translation, Euclidean and Affine
		warpAffine(im2, im2_aligned, warp_matrix, im1.size(), INTER_LINEAR + WARP_INVERSE_MAP);
	else
		// Use warpPerspective for Homography
		warpPerspective(im2, im2_aligned, warp_matrix, im1.size(), INTER_LINEAR + WARP_INVERSE_MAP);
	return (im2_aligned);
}

Mat decrop(Mat mask, Mat premask, int dx1, int dx2, int dy1, int dy2)
{
	Mat image = premask.clone();
	image = image + 255;
	for (int i = (dy1 + 5); i < (dy2 + dy1 - 5); i++)
	{
		for (int j = (dx1 + 5); j < (dx2 + dx1 - 5); j++)
		{
			image.at<uchar>(i, j) = mask.at<uchar>((i - dy1), (j - dx1));
		}
	}
	return (image);
}

Mat output_create(Mat output, Mat mask)
{
	//modify mask
	cout << "Counting dimensions" << std::endl;
	int rows = mask.rows;
	int cols = mask.cols;
	cout << "Modifying mask into I/O" << std::endl;
	//setting mask into I/O form (0…match, 255…difference)
	for (int i = 0; i < rows; i++)
	{
		for (int j = 0; j < cols; j++)
		{
			if (mask.at<uchar>(i, j) < 100)
			{
				output.at<uchar>(i, (3 * j) + 2) = 255;
			}
		}
	}
	return output;
}

Mat demask(Mat mask)
{
	Mat output = mask.clone();
	Mat output2 = mask.clone();
	Canny(mask, output, 50, 200, 3);
	vector<Vec4i> lines;
	vector<Vec4i> liness;
	HoughLinesP(output, lines, 1, CV_PI / 180, 25, 20, 5);
	HoughLinesP(output, liness, 1, CV_PI / 180, 40, 0, 5);

	for (size_t i = 0; i < lines.size(); i++)
	{
		line(output2, Point(lines[i][0], lines[i][1]), Point(lines[i][2], lines[i][3]), Scalar(255), 3, 8);
	}
	for (size_t i = 0; i < liness.size(); i++)
	{
		line(output2, Point(liness[i][0], liness[i][1]), Point(liness[i][2], liness[i][3]), Scalar(255), 3, 8);
	}
	return (output2);
}
//USED - erase lines
Mat demask3(Mat mask)
{
	Mat output = mask.clone();
	Mat canny_output;
	vector<vector<Point> > contours;
	vector<Vec4i> hierarchy;

	// Detect edges using canny
	int thr = 50;
	Canny(mask, canny_output, thr, thr * 2, 3);
	// Find contours
	findContours(canny_output, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, Point(0, 0));

	for (int i = 0; i< contours.size(); i++)
	{
		drawContours(output, contours, i, Scalar(255), 2, 8, hierarchy, 0, Point());
	}

	return(output);
}
//USED - erase edges
Mat maskmerge(Mat mask1, Mat mask2)
{
	Mat inv1, inv2;
	bitwise_not(mask1, inv1);
	bitwise_not(mask2, inv2);
	Mat output = inv1.mul(inv2);
	bitwise_not(output, output);

	return(output);
}
//USED