/*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                          License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other materials provided with the distribution.
//
//   * The name of the copyright holders may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/

#include <iostream>
#include <fstream>
#include "opencv2/imgcodecs.hpp"
#include "opencv2/highgui.hpp"
#include "opencv2/stitching.hpp"
#include "opencv2/calib3d.hpp"
#include "opencv2/imgproc.hpp"
#include <opencv2/core/utility.hpp>
#include "opencv2/core.hpp"
#include <math.h>

using namespace std;
using namespace cv;

vector<Mat> imgs;

int showimgs;
int showimgs2 = 0;

int parseCmdArgs(int argc, char** argv);

int main(int argc, char* argv[])
{
	cout << "\nShow images? : ";
	cin >> showimgs;

	int retval = parseCmdArgs(argc, argv);
	if (retval) return -1;

	int num_imgs = 4;

	char * buffer = new char[2592*1944*3];
	char * bigbuffer = new char[3744 * 2 * 3744 * 3];
	char * bigbuffer2 = new char[3744 * 2 * 3744 * 3];
	char * full_photo_buffer = new char[3744 * 4 * 3744 * 3];
	int * cut = new int[1944];
	Vec3b color;
	Vec3b color2;

	//f = 1.8/3.76 * 2595 = 1241
	//x' = s*atan(x/f)
	//y' = s*atan(y/(sqrt(x^2 + f^2)))

	float f = 1241.0;
	float s = 1600.0;
	float x2, y2;
	Mat source_image;
	int warp = 0;
	int slideposition;
	int yslide;
	int keyval;

	Rect lastimg;
	Rect thisimg;
	Rect lastimg_overlap;
	Rect thisimg_overlap;
	Mat thisoverlap_mat;
	Mat lastoverlap_mat;
	Mat lastimageforoverlap_mat;
	int overlap_y;

	Mat target_image = Mat(1944, 2592, CV_8UC3, buffer);
	Mat large_image = Mat(3744, 3744 * 2, CV_8UC3, bigbuffer);
	Mat large_image_draft = Mat(3744, 3744 * 2, CV_8UC3, bigbuffer2);
	Mat full_3d_photo = Mat(3744 * 2, 3744 * 2, CV_8UC3, full_photo_buffer);

	Rect eyerect;

	for (int eye = 0; eye < 2; eye++) {
		if (eye == 0) {
			eyerect = cv::Rect(1095, 0, 1095, 1784);
		}
		else {
			eyerect = cv::Rect(0, 0, 1095, 1784);
		}


		for (int i2 = 0; i2 < num_imgs; i2++) {
			target_image = imgs[i2](eyerect);

			if (i2 > 0) {
				slideposition = 0;
				yslide = 0;
				keyval = 0;

				namedWindow("ree", WINDOW_NORMAL);
				resizeWindow("ree", 1872, 936);

				int movimg;
				cout << "\nMove image? : ";
				cin >> movimg;

				int finecut;
				cout << "\nFine cut? : ";
				cin >> finecut;

				int showold = 0;

				while (keyval != 113) {
					if (movimg == 0) {
						cout << "\nX position? : ";
						//slideposition = 972;
						cin >> slideposition;
						cout << "\nY position? : ";
						//yslide = -83;
						cin >> yslide;
						keyval = 113;
					}
					else {
						keyval = waitKey(0);
						if (keyval == 97) slideposition += 1; //a
						if (keyval == 115) slideposition += 15; //s
						if (keyval == 100) slideposition -= 1; //d
						if (keyval == 119) yslide += 1; //w
						if (keyval == 120) yslide -= 1; //x
						if (keyval == 121) showold = 1 - showold; //y
					}

					thisimg.x = slideposition; //(1255 * (num_imgs - i2 - 1)) + 
					thisimg.y = 980 + yslide;

					large_image.copyTo(large_image_draft);
					target_image.copyTo(large_image_draft(cv::Rect(thisimg.x, thisimg.y, target_image.cols, target_image.rows)));
					if (showold == 1) {
						imgs[i2 - 1].copyTo(large_image_draft(cv::Rect(lastimg.x, lastimg.y, target_image.cols, target_image.rows)));
					}
					imshow("ree", large_image_draft);
				}

				cout << "\nXpos\n";
				cout << slideposition;
				cout << "\nYpos\n";
				cout << yslide;

				thisimg.width = target_image.cols;
				thisimg.height = target_image.rows;

				//figure horizontal overlap, assume this image is left of last
				lastimg_overlap.x = 0;
				lastimg_overlap.width = thisimg.x + thisimg.width - lastimg.x;
				thisimg_overlap.x = thisimg.width - lastimg_overlap.width;
				thisimg_overlap.width = lastimg_overlap.width;

				//vertical overlap
				if (lastimg.y > thisimg.y) {
					lastimg_overlap.y = 0;
					lastimg_overlap.height = thisimg.y + thisimg.height - lastimg.y;
					thisimg_overlap.y = thisimg.height - lastimg_overlap.height;
					thisimg_overlap.height = lastimg_overlap.height;
					overlap_y = lastimg.y;
				}
				else if (lastimg.y < thisimg.y) {
					thisimg_overlap.y = 0;
					thisimg_overlap.height = lastimg.y + lastimg.height - thisimg.y;
					lastimg_overlap.y = lastimg.height - thisimg_overlap.height;
					lastimg_overlap.height = thisimg_overlap.height;
					overlap_y = thisimg.y;
				}
				else {
					thisimg_overlap.y = 0;
					lastimg_overlap.y = 0;
					lastimg_overlap.height = thisimg.height;
					thisimg_overlap.height = thisimg.height;
					overlap_y = lastimg.y;
				}

				thisoverlap_mat = target_image(thisimg_overlap);
				lastimageforoverlap_mat = imgs[i2 - 1](eyerect);
				lastoverlap_mat = lastimageforoverlap_mat(lastimg_overlap);

				if (showimgs) {
					imshow("overlap1", thisoverlap_mat);
					waitKey(0);

					imshow("overlap2", lastoverlap_mat);
					waitKey(0);
					destroyWindow("overlap1");
					destroyWindow("overlap2");
				}

				Mat overlap_diff;
				absdiff(thisoverlap_mat, lastoverlap_mat, overlap_diff);

				if (showimgs) {
					imshow("overlapdiff", overlap_diff);
					waitKey(0);
					destroyWindow("overlapdiff");
				}

				Mat overlap_blurred;

				Size blursize;
				blursize.width = 60;
				blursize.height = 60;

				blur(overlap_diff, overlap_blurred, blursize);

				if (showimgs) {
					imshow("overlapblur", overlap_blurred);
					waitKey(0);
					destroyWindow("overlapblur");
				}

				//find least different pixel in blurred image

				int min_intensity;
				int intensity;
				int min_index = 0;
				int oldMin_index = 0;

				for (int y = 0; y < overlap_blurred.rows; y++) {
					min_intensity = 10000;

					for (int x = 0; x < overlap_blurred.cols; x++) {
						color = overlap_blurred.at<Vec3b>(Point(x, y));
						intensity = (color[0] + color[1] + color[2]) * 1;

						if (y > 0) intensity += (abs(x - oldMin_index) * 1);
						intensity += (abs(x - (overlap_blurred.cols / 2)) * 1);

						if (intensity < min_intensity) {
							min_index = x;
							min_intensity = intensity;
						}
					}

					overlap_blurred.at<Vec3b>(Point(min_index, y))[0] = 255;
					overlap_blurred.at<Vec3b>(Point(min_index, y))[1] = 255;
					overlap_blurred.at<Vec3b>(Point(min_index, y))[2] = 255;
					oldMin_index = min_index;

					cut[y] = min_index;
				}

				if (showimgs) {
					imshow("overlapblur", overlap_blurred);
					waitKey(0);
					destroyWindow("overlapblur");
				}

				if (finecut == 1) {
					//do fine cut
					min_index = 0;

					for (int y = 0; y < overlap_blurred.rows; y++) {
						min_intensity = 10000;

						for (int x = 0; x < overlap_blurred.cols; x++) {
							color = overlap_diff.at<Vec3b>(Point(x, y));
							intensity = color[0] + color[1] + color[2];

							if (y > 0) intensity += (abs(x - cut[y]) * 3);

							if (intensity < min_intensity) {
								min_index = x;
								min_intensity = intensity;
							}
						}

						overlap_diff.at<Vec3b>(Point(min_index, y))[0] = 255;
						overlap_diff.at<Vec3b>(Point(min_index, y))[1] = 255;
						overlap_diff.at<Vec3b>(Point(min_index, y))[2] = 255;
						oldMin_index = min_index;

						cut[y] = min_index;
					}

					if (showimgs) {
						imshow("overlapblur", overlap_diff);
						waitKey(0);
						destroyWindow("overlapblur");
					}
				}

				//FEATHER

				int color_wt;
				int c0, c1, c2;

				Mat resultcut = thisoverlap_mat.clone();
				for (int y = 0; y < thisoverlap_mat.rows; y++) {
					for (int x = 0; x < thisoverlap_mat.cols; x++) {
						if (x > cut[y]) {
							color = lastoverlap_mat.at<Vec3b>(Point(x, y));
							color2 = thisoverlap_mat.at<Vec3b>(Point(x, y));
							if (x < (cut[y] + 64)) {
								color_wt = x - cut[y];
								c0 = ((int)color[0] * color_wt) >> 6;
								c0 += ((int)color2[0] * (64 - color_wt)) >> 6;
								c1 = ((int)color[1] * color_wt) >> 6;
								c1 += ((int)color2[1] * (64 - color_wt)) >> 6;
								c2 = ((int)color[2] * color_wt) >> 6;
								c2 += ((int)color2[2] * (64 - color_wt)) >> 6;
								thisoverlap_mat.at<Vec3b>(Point(x, y))[0] = (uchar)c0;
								thisoverlap_mat.at<Vec3b>(Point(x, y))[1] = (uchar)c1;
								thisoverlap_mat.at<Vec3b>(Point(x, y))[2] = (uchar)c2;
							}
							else {
								thisoverlap_mat.at<Vec3b>(Point(x, y)) = color;
							}
						}
					}
				}

				if (showimgs) {
					imshow("overlap1", thisoverlap_mat);
					waitKey(0);
				}

				thisoverlap_mat.copyTo(large_image_draft(cv::Rect(thisimg_overlap.x + thisimg.x, overlap_y, thisoverlap_mat.cols, thisoverlap_mat.rows)));

				large_image_draft.copyTo(large_image);

				lastimg = thisimg;
			}
			else {
				lastimg.x = (3744 * 2) - 2191;
				cout << "\nFirstX\n";
				cout << lastimg.x;
				cout << "\n";
				cout << large_image.cols;
				lastimg.y = 980;
				target_image.copyTo(large_image(cv::Rect(lastimg.x, lastimg.y, target_image.cols, target_image.rows)));
				lastimg.width = target_image.cols;
				lastimg.height = target_image.rows;
			}
		}

		if (eye == 0) {
			large_image.copyTo(full_3d_photo(cv::Rect(0, 0, 3744 * 2, 3744)));
		}
		else {
			large_image.copyTo(full_3d_photo(cv::Rect(0, 3744, 3744 * 2, 3744)));
		}
	}

	namedWindow("ree", WINDOW_NORMAL);
	resizeWindow("ree", 1872, 936);
	imshow("ree", large_image);
	waitKey(0);

	imwrite("theresult.jpg", large_image);
	imwrite("3dphoto.jpg", full_3d_photo);

	delete[] buffer;
	delete[] bigbuffer;
	delete[] bigbuffer2;

	return 0;
}


int parseCmdArgs(int argc, char** argv)
{
	char filename[50];

	//Load the camera distortion matrix
	//sprintf(filename, "cam%d_data.yml", i + 1);
	FileStorage fs("out_camera_data.yml", FileStorage::READ);
	Mat cameraMatrix;
	Mat distCoeffs;

	//cout << filename;

	fs["camera_matrix"] >> cameraMatrix;
	fs["distortion_coefficients"] >> distCoeffs;

	fs.release();

	for (int i = 1; i < argc; ++i)
	{
		Mat img = imread(argv[i]);
		if (img.empty())
		{
			cout << "Can't read image '" << argv[i] << "'\n";
			return -1;
		}

		//Undistort
		Mat rview;
		Mat croppedImage;
		Mat intermediary;
		Vec3b color;
		Size imageSize;

		if (1 == 0) {
			Size newSize;
			Mat temp = img.clone();
			undistort(temp, img, cameraMatrix, distCoeffs);
			intermediary = img;
		}
		else {
			imageSize = img.size();
			Size newSize;
			newSize.width = 2191;//2916;//3110;// 3240; //3888
			newSize.height = 1784;// 2188;// 2332;//2430; //2916

			Mat view, map1, map2;
			//fisheye::initUndistortRectifyMap(cameraMatrix, distCoeffs, Mat(),
			//	getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, 1, newSize, 0, true),
			//	newSize, CV_16SC2, map1, map2);

			fisheye::initUndistortRectifyMap(cameraMatrix, distCoeffs, Mat(),
				getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, 1.5, imageSize, 0, true),
				imageSize, CV_16SC2, map1, map2);

			//undistort( view, rview, cameraMatrix, distCoeffs, cameraMatrix );
			remap(img, rview, map1, map2, INTER_LINEAR);

			newSize = rview.size();

			// Setup a rectangle to define your region of interest
			/*Rect myROI((newSize.width - 2592)/2, (newSize.height - 1944)/2, 2592, 1944);//1584
			newSize = myROI.size();
			cout << "Width\n";
			cout << newSize.width;
			cout << "\nHeight\n";
			cout << newSize.height;
			cout << "\n";*/

			// Crop the full image to that image contained by the rectangle myROI
			// Note that this doesn't copy the data
			croppedImage = rview;// myROI);
			

			if (showimgs2) {
				namedWindow("woo1", WINDOW_NORMAL);
				resizeWindow("woo1", 648, 486);

				imshow("woo1", rview);
				waitKey(0);
				destroyWindow("woo1");
				//imshow("woo1", croppedImage);
				//waitKey(0);
			}

			intermediary = croppedImage;
		}

		//Do Spherical

		/*float f;
		float s;
		cout << "\ngive f : ";
		cin >> f;
		cout << "\ngive s : ";
		cin >> s;*/

		float f = 800;//1241.0;
		float s = 1170;//2300// 1600.0;
		float x2, y2;

		if (1 == 1) {
			for (int x = 0; x < imageSize.width; x++) {
				for (int y = 0; y < imageSize.height; y++) {
					x2 = f * tan((x - (imageSize.width/2)) / s);
					y2 = tan((y - (imageSize.height/2)) / s) * sqrt(pow(x2, 2) + pow(f, 2)) + (imageSize.height / 2);
					//y2 = (y - 972) / s * sqrt(pow(x2, 2) + pow(f, 2)) + 972;
					x2 += (imageSize.width / 2);

					if ((x2 < imageSize.width) && (x2 > -1) && (y2 < imageSize.height) && (y2 > -1)) {
						color = intermediary.at<Vec3b>(Point(int(x2), int(y2)));
						img.at<Vec3b>(Point(x, y)) = color;
					}
					else {
						img.at<Vec3b>(Point(x, y)) = 0;
					}
				}
			}
		}
		else {
			img = intermediary;
		}

		if (showimgs2) {
			imshow("redone", img);
			waitKey(0);
			destroyWindow("redone");

			sprintf(filename, "cam%d_dewarped.jpg", i-1);
			imwrite(filename, img);
		}

		imgs.push_back(img);
	}
	return 0;
}