/*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>
#include <time.h>

using namespace std;
using namespace cv;

vector<Mat> imgs;
vector<Mat> masks;
int num_imgs = 4;
Mat mean_mask;

int showimgs;
int showimgs2 = 0;
int test_images = 0;
int incremental_flow = 1;

int parseCmdArgs(int argc, char** argv);
void CalcFinalFlow(Mat &BlurredFlow, Mat &OutputFlow, int max_search_distance);
void CalcColumnFlow(Mat &SmallFlow, Mat &ColumnFlow, int x_inc, int y_inc, int x_offset, int y_offset, int max_search_distance);
void SphericalWarp(Mat &Equirect, Mat &Spherical);
void CenterImg(Mat &RawImg, Mat &CenteredImg);
void ColorCorrectImgs();
void SmartCut(Mat &Rmat, Mat &Lmat, Mat &OutputMat);

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

	vector<int> x_positions;
	vector<int> y_positions;
	int file_read;
	cout << "\nRead from file? : ";
	cin >> file_read;
	FileStorage fs;
	if (file_read) {
		fs = FileStorage("stitchingsettings.ssf", FileStorage::READ);
	} else {
		fs = FileStorage("stitchingsettings.ssf", FileStorage::WRITE);
	}

	//Feature Search Parameters
	Size blursize = Size(5, 4);
	Size color_mat_size = Size(4, 4);
	Size first_blur_sz = Size(15, 9);
	Size second_blur_sz = Size(31, 31);
	int feature_width = 5;
	int feature_height = 4;
	int x_inc = 5; //5
	int y_inc = 10; //10
	int search_increment = 2; //4 works best if blur is 10
	int max_search_distance = 200;
	int color_diff_thresh = 1200; //was 120 //good at 600

	if (file_read) {
		fs["bw"] >> blursize.width;
		fs["bh"] >> blursize.height;
		fs["first_blur"] >> first_blur_sz;
		fs["second_blur"] >> second_blur_sz;
		fs["fw"] >> feature_width;
		fs["fh"] >> feature_height;
		fs["x_inc"] >> x_inc;
		fs["y_inc"] >> y_inc;
		fs["search_inc"] >> search_increment;
		fs["max_sr_dst"] >> max_search_distance;
		fs["cdt"] >> color_diff_thresh;
		fs["cmw"] >> color_mat_size.width;
		fs["cmh"] >> color_mat_size.height;

		fs["x_positions"] >> x_positions;
		fs["y_positions"] >> y_positions;
	}
	else {
		fs << "bw" << blursize.width;
		fs << "bh" << blursize.height;
		fs << "first_blur" << first_blur_sz;
		fs << "second_blur" << second_blur_sz;
		fs << "fw" << feature_width;
		fs << "fh" << feature_height;
		fs << "x_inc" << x_inc;
		fs << "y_inc" << y_inc;
		fs << "search_inc" << search_increment;
		fs << "max_sr_dst" << max_search_distance;
		fs << "cdt" << color_diff_thresh;
		fs << "cmw" << color_mat_size.width;
		fs << "cmh" << color_mat_size.height;
	}

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

	int * cut = new int[1944];
	Vec3b color;
	Vec3b color0;
	Vec3b color1;
	Vec3b color2;

	Mat source_image;
	int warp = 0;
	int slideposition;
	int yslide;
	int keyval;

	Rect lastimg;
	Rect thisimg;
	Rect lastimg_overlap;
	Rect thisimg_overlap;
	Mat Loverlap_mat;
	Mat Roverlap_mat;
	Mat lastimageforoverlap_mat;
	int overlap_y;
	
	Mat target_image;
	Mat large_image = Mat(Size(3392 * 2, 2200), CV_8UC4);
	Mat large_image_draft = Mat(large_image.size(), CV_8UC4);
	Mat full_3d_photo = Mat(Size(3392 * 2, 3392 * 2), CV_8UC4);

	Rect eyerect;

	for (int eye = 0; eye < 2; eye++) {
		if (eye == 0) { //Left Eye
			eyerect = cv::Rect(1076, 0, 1696, 2000);
		}
		else { //Right Eye
			eyerect = cv::Rect(228, 0, 1696, 2000);
		}

		/*lastimg.x = large_image.cols - target_image.cols / 2;
		cout << "\nFirstX\n";
		cout << lastimg.x;
		lastimg.y = (large_image.rows - target_image.rows) / 2;// 980;
		cout << "\nFirsty\n";
		cout << lastimg.y;
		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;*/

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

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

				namedWindow("PositionImage", WINDOW_NORMAL);
				resizeWindow("PositionImage", 3744, 1100);

				int movimg;
				cout << "\nMove image? : ";
				if (test_images || file_read) {
					movimg = 0;
				}
				else {
					cin >> movimg;
				}

				int showold = 0;

				//Manually Place Left Image
				while (keyval != 113) {
					if (movimg == 0) {
						cout << "\nX position? : ";
						if (test_images) {
							slideposition = 5297;
						}
						else if (file_read) {
							slideposition = x_positions[i2-1];
						} else {
							cin >> slideposition;
						}
						cout << "\nY position? : ";
						if (test_images) {
							yslide = (large_image.rows - target_image.rows) / 2;
						} else if (file_read){
							yslide = y_positions[i2-1];
						} else {
							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
					}

					if (file_read == 0) {
						x_positions.push_back(slideposition);
						y_positions.push_back(yslide);
					}

					thisimg.x = slideposition;
					thisimg.y = 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)), masks[i2](eyerect));
					if (showold == 1) {
						imgs[i2 - 1](eyerect).copyTo(large_image_draft(cv::Rect(lastimg.x, lastimg.y, eyerect.width, eyerect.height)), masks[i2 - 1](eyerect));
					}
					imshow("PositionImage", large_image_draft);
				}

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

				//Calculate Overlap

				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;

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

				Loverlap_mat = target_image(thisimg_overlap);
				lastimageforoverlap_mat = imgs[i2 - 1](eyerect);
				Roverlap_mat = lastimageforoverlap_mat(lastimg_overlap);

				if (showimgs) {
					namedWindow("overlap1", WINDOW_NORMAL);
					resizeWindow("overlap1", Loverlap_mat.cols / 2, Loverlap_mat.rows / 2);
					imshow("overlap1", Loverlap_mat);
					waitKey(0);

					namedWindow("overlap2", WINDOW_NORMAL);
					resizeWindow("overlap2", Roverlap_mat.cols / 2, Roverlap_mat.rows / 2);
					imshow("overlap2", Roverlap_mat);
					waitKey(0);
					destroyWindow("overlap1");
					destroyWindow("overlap2");
				}

				//New overlap stitching!
				//Loverlap_mat is left
				//Roverlap_mat is right

				Mat Loverlap_mean_mask = masks[i2](eyerect)(thisimg_overlap);
				Mat Roverlap_mean_mask = masks[i2 - 1](eyerect)(lastimg_overlap);

				if (showimgs2){
					namedWindow("Rmask", WINDOW_NORMAL);
					resizeWindow("Rmask", Roverlap_mat.cols / 2,Roverlap_mat.rows / 2);
					imshow("Rmask", Roverlap_mean_mask);
					waitKey(0);

					namedWindow("Lmask", WINDOW_NORMAL);
					resizeWindow("Lmask", Loverlap_mat.cols / 2, Loverlap_mat.rows / 2);
					imshow("Lmask", Loverlap_mean_mask);
					waitKey(0);

					destroyWindow("Rmask");
					destroyWindow("Lmask");
				}

				int x_offset = (feature_width * (color_mat_size.width - 1)) / 2;
				int y_offset = (feature_height * (color_mat_size.height - 1)) / 2;

				Mat Roverlap_blurred;
				Mat Loverlap_blurred;

				blur(Roverlap_mat, Roverlap_blurred, blursize);
				blur(Loverlap_mat, Loverlap_blurred, blursize);
				//GaussianBlur(Roverlap_mat, Roverlap_blurred, blursize, 0, 0); Gaussian doesn't work as well
				//GaussianBlur(Loverlap_mat, Loverlap_blurred, blursize, 0, 0);

				if (showimgs){
					namedWindow("Left", WINDOW_NORMAL);
					resizeWindow("Left", Loverlap_mat.cols / 2, Loverlap_mat.rows / 2);
					imshow("Left", Loverlap_blurred);
					imwrite("lblur.jpg", Loverlap_blurred);
					waitKey(0);
					destroyWindow("Left");

					namedWindow("Right", WINDOW_NORMAL);
					resizeWindow("Right", Loverlap_mat.cols / 2, Loverlap_mat.rows / 2);
					imshow("Right", Roverlap_blurred);
					imwrite("rblur.jpg", Roverlap_blurred);
					waitKey(0);
					destroyWindow("Right");
				}

				int match_count = 0;
				int current_match_pixel;
				int current_min_match_thresh;
				int total_diff;
				int progress_counter = 0;
				int progress_counter_old = 0;
				int last_min_match_thresh;

				Mat Lcolors = Mat(color_mat_size, CV_8UC4);
				Mat Rcolors = Mat(color_mat_size, CV_8UC4);
				Mat color_diffs = Mat(color_mat_size, CV_8UC4);
				Scalar diff_mean;
				Scalar l_mean;
				Scalar r_mean;

				Size flow_vis_size;
				flow_vis_size.width = Roverlap_blurred.cols / x_inc;
				flow_vis_size.height = Roverlap_blurred.rows / y_inc;
				Mat flow_visualization(flow_vis_size, CV_8UC1);

				Size Rflow_vis_size;
				Rflow_vis_size.width = Roverlap_blurred.cols / x_inc;
				Rflow_vis_size.height = Roverlap_blurred.rows / y_inc;

				Mat Rflow_visualization(Rflow_vis_size, CV_32FC1);
				Rflow_visualization.setTo(0.0f);

				Mat flow_mat(flow_vis_size, CV_32FC2);
				for (int x = 0; x < flow_mat.cols; x++) {
					for (int y = 0; y < flow_mat.rows; y++) {
						flow_mat.at<Vec2f>(Point(x, y))[0] = 0;
						flow_mat.at<Vec2f>(Point(x, y))[1] = 0;
					}
				}
				
				for (int y = 0; y < (Roverlap_blurred.rows - y_inc - (feature_height * (color_mat_size.height - 1))); y += y_inc) {
					for (int x = 0; x < (Roverlap_blurred.cols - (feature_width * (color_mat_size.width - 1))); x += x_inc) {
						current_min_match_thresh = color_diff_thresh;
						current_match_pixel = -1;

						for (int py = 0; py < Rcolors.rows; py++) {
							for (int px = 0; px < Rcolors.cols; px++) {
								Rcolors.at<Vec4b>(Point(px, py)) = Roverlap_blurred.at<Vec4b>(Point(x + (feature_width * px), y + (feature_height * py)));
							}
						}

						int search_max = min(x + max_search_distance, Roverlap_blurred.cols - (feature_width * 3));

						Mat Rsearcharea = Roverlap_blurred(Rect(x, y, search_max - x, feature_height * Rcolors.rows));
						r_mean = mean(Rsearcharea);
						Mat Lsearcharea = Loverlap_blurred(Rect(x, y, search_max - x, feature_height * Rcolors.rows));
						l_mean = mean(Lsearcharea);

						Mat R_mats[4];
						split(Rcolors, R_mats);
						R_mats[0].convertTo(R_mats[0], CV_8U, 1.0, l_mean.val[0] - r_mean.val[0]);
						R_mats[1].convertTo(R_mats[1], CV_8U, 1.0, l_mean.val[1] - r_mean.val[1]);
						R_mats[2].convertTo(R_mats[2], CV_8U, 1.0, l_mean.val[2] - r_mean.val[2]);
						merge(R_mats, 4, Rcolors);
						
						//Now, start at the same point in the LEFT image and increment X until a matching feature in the left image is found
						for (int x3 = x; x3 < search_max; x3 += search_increment) {
							for (int py = 0; py < Lcolors.rows; py++) {
								for (int px = 0; px < Lcolors.cols; px++) {
									Lcolors.at<Vec4b>(Point(px, py)) = Loverlap_blurred.at<Vec4b>(Point(x3 + (feature_width * px), y + (feature_height * py)));
								}
							}

							absdiff(Lcolors, Rcolors, color_diffs);
							diff_mean = mean(color_diffs);
							total_diff = pow(diff_mean.val[0], 2) + pow(diff_mean.val[1], 2) + pow(diff_mean.val[2], 2);

							if (total_diff < current_min_match_thresh) { //If this match is satisfying
								last_min_match_thresh = current_min_match_thresh;
								current_min_match_thresh = total_diff;
								current_match_pixel = x3;
							}
						}
						
						if (current_match_pixel > -1) { //if match found
							float confidence_factor = float(last_min_match_thresh) / float(current_min_match_thresh + 1);
							float constant_factor = confidence_factor;
							float distance_factor = float(current_match_pixel - x) * confidence_factor;

							if (Rflow_visualization.at<float>(Point(current_match_pixel / x_inc, y / y_inc)) < confidence_factor) {
								flow_mat.at<Vec2f>(Point(((x + x_offset)) / (x_inc), (y + y_offset) / y_inc))[0] = constant_factor;
								flow_mat.at<Vec2f>(Point(((x + x_offset)) / (x_inc), (y + y_offset) / y_inc))[1] = distance_factor;

								match_count++;
								flow_visualization.at<uchar>(Point(x / x_inc, y / y_inc)) = 50 + (current_match_pixel - x) * 2;
								Rflow_visualization.at<float>(Point(current_match_pixel / x_inc, y / y_inc)) = confidence_factor;
							}
						}
						else {
							flow_visualization.at<uchar>(Point(x / x_inc, y / y_inc)) = 0;

							flow_mat.at<Vec2f>(Point(((x + x_offset)) / (x_inc), (y + y_offset) / y_inc))[0] = 0;
							flow_mat.at<Vec2f>(Point(((x + x_offset)) / (x_inc), (y + y_offset) / y_inc))[1] = 0;
						}

						progress_counter = (100 * y) / Roverlap_blurred.rows;
						if (progress_counter > progress_counter_old) {
							cout << "\nProgress: ";
							cout << progress_counter;
							progress_counter_old = progress_counter;
						}
					}
				}

				imwrite("matchedthing.jpg", flow_visualization);

				if (showimgs) {
					namedWindow("Flow", WINDOW_NORMAL);
					resizeWindow("Flow", flow_mat.cols, flow_mat.rows);
					imshow("Flow", flow_mat);
					waitKey(0);
					destroyWindow("Flow");
				}
				
				cout << "\nMatches: ";
				cout << match_count;

				cout << "\nBlurring small flow mat";

				Mat blurred_flow;
				GaussianBlur(flow_mat, blurred_flow, first_blur_sz, 0, 0); //good at 15

				Mat column_flow;
				CalcColumnFlow(blurred_flow, column_flow, x_inc, y_inc, x_offset, y_offset, max_search_distance);

				Mat blurred_column_flow;
				GaussianBlur(column_flow, blurred_column_flow, second_blur_sz, 0, 0); //good at 31

				cout << "\nCreating correct big column mat";
				Mat final_column_flow;
				CalcFinalFlow(blurred_column_flow, final_column_flow, max_search_distance);

				if (showimgs) {
					namedWindow("Flow", WINDOW_NORMAL);
					resizeWindow("Flow", flow_mat.cols / 2, flow_mat.rows / 2);
					imshow("Flow", blurred_flow);
					waitKey(0);
					destroyWindow("Flow");
				}
				
				cout << "\nCreating shifted image";

				//Now, create the shifted image
				Mat Rflowed_image(Loverlap_mat.size(), CV_8UC4);
				Mat Lflowed_image(Loverlap_mat.size(), CV_8UC4);
				Vec4b Rpixel;
				Vec4b Lpixel;
				Vec4b resultpixel;
				int width_pix = final_column_flow.cols - max_search_distance;
				float fade_proportion;

				//Here we initialize the image with a background

				Roverlap_mat.copyTo(Rflowed_image);
				Loverlap_mat.copyTo(Lflowed_image);

				cout << "\nPart 2";

				//Now fill it with optical-flowed pixels

				Vec4b final_pixel;
				Vec4b final_pixel2;
				int final_flow_amount_r;
				int final_flow_amount_l;

				for (int x = 0; x < width_pix; x++) {
					for (int y = 0; y < final_column_flow.rows; y++) {
						final_flow_amount_r = x - final_column_flow.at<Vec3i>(Point(x, y))[0];
						final_flow_amount_l = x + final_column_flow.at<Vec3i>(Point(x, y))[1];
						fade_proportion = float(x) / float(width_pix);
						if ((final_flow_amount_r > 0) && (final_flow_amount_l < width_pix)){
							//addWeighted(Loverlap_mat.at<Vec4b>(Point(final_flow_amount_l, y)), 1 - fade_proportion, Roverlap_mat.at<Vec4b>(Point(final_flow_amount_r, y)), fade_proportion, 0, final_pixel);
							//flowed_image.at<Vec4b>(Point(x, y)) = final_pixel;
							Rflowed_image.at<Vec4b>(Point(x, y)) = Roverlap_mat.at<Vec4b>(Point(final_flow_amount_r, y));
							Lflowed_image.at<Vec4b>(Point(x, y)) = Loverlap_mat.at<Vec4b>(Point(final_flow_amount_l, y));
						}
					}
				}
				
				cout << "\nDoing smart cut";

				Mat flowed_image;
				SmartCut(Rflowed_image, Lflowed_image, flowed_image);

				Mat bgr_flowed_mat;
				cvtColor(flowed_image, bgr_flowed_mat, CV_BGRA2BGR);

				stringstream fi_name;
				fi_name << "FlowedImage" << i2 << ".jpg";
				imwrite(fi_name.str(), bgr_flowed_mat);

				flowed_image.copyTo(large_image_draft(Rect(thisimg_overlap.x + thisimg.x, overlap_y, flowed_image.cols, flowed_image.rows)));

				large_image_draft.copyTo(large_image);

				lastimg = thisimg;
			}
			else {
				lastimg.x = large_image.cols - target_image.cols;// (3744 * 2) - 2191;
				cout << "\nFirstX\n";
				cout << lastimg.x;
				lastimg.y = (large_image.rows - target_image.rows) / 2;// 980;
				cout << "\nFirsty\n";
				cout << lastimg.y;
				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 ((file_read == 0) && (eye == 0)) {
			fs << "x_positions" << x_positions;
			fs << "y_positions" << y_positions;
		}

		if (eye == 0){
			imwrite("bigimage_left.jpg", large_image);
		}
		else {
			imwrite("bigimage_right.jpg", large_image);
		}

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

	fs.release();

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

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

	delete[] cut;

	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();

	Mat map1, map2;

	Size newSize;
	newSize.width = 3000;//2916;//3110;// 3240; //3888
	newSize.height = 2000;// 2188;// 2332;//2430; //2916

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

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

		if (test_images == 0) { //1 for undistort
			

			Mat Img_RGBA(img.size(), CV_8UC4);
			cvtColor(img, Img_RGBA, CV_BGR2BGRA, 4);

			if (showimgs2) {
				namedWindow("redone", WINDOW_NORMAL);
				resizeWindow("redone", 1500, 1000);

				imshow("redone", Img_RGBA);
				waitKey(0);
				destroyWindow("redone");
			}

			//Center: Create large mat with alpha background, get binary image and set as alpha channel of image, find center, paste onto large mat, rotate around center
			Mat centered_img;
			CenterImg(Img_RGBA, centered_img);

			if (showimgs2) {
				namedWindow("redone", WINDOW_NORMAL);
				resizeWindow("redone", 1500, 1000);

				imshow("redone", centered_img);
				waitKey(0);
				destroyWindow("redone");
			}

			//Undistort: Use remap() with alpha background
			Mat rview = Mat(centered_img.size(), CV_8UC4);
			Vec4b default_val;
			default_val[0] = 127;
			default_val[1] = 127;
			default_val[2] = 127;
			default_val[3] = 0;
			rview.setTo(default_val);
			//imshow("rview", rview);
			//waitKey(0);
			remap(centered_img, rview, map1, map2, INTER_LINEAR, BORDER_TRANSPARENT);
			//imshow("rview", rview);
			//waitKey(0);

			if (showimgs2) {
				namedWindow("mask", WINDOW_NORMAL);
				resizeWindow("mask", 1500, 1000);

				Mat i_planes[4];
				split(rview, i_planes);
				imshow("mask", i_planes[3]);
				waitKey(0);
				destroyWindow("mask");
			}

			//After: Color correct: Take averages of each image with alpha mask, average averages, apply mx+b to get averages same
			//Spherical Warp: Spherical warp while preserving alpha mask
			Mat SphericalImage;
			SphericalWarp(rview, SphericalImage);

			if (showimgs2) {
				namedWindow("redone", WINDOW_NORMAL);
				resizeWindow("redone", 1500, 1000);

				imshow("redone", SphericalImage);
				waitKey(0);
				destroyWindow("redone");

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

			imgs.push_back(SphericalImage);
		}
		 else {
			 mean_mask = Mat(img.size(), CV_8U, 1);
			 Mat img_rgba(img.size(), CV_8UC4);
			 cvtColor(img, img_rgba, CV_RGB2RGBA);
			 imgs.push_back(img_rgba);
		}
	}

	ColorCorrectImgs();

	return 0;
}

void ColorCorrectImgs() {
	Mat i_planes[4];
	vector<Scalar>img_averages;
	int all_avg_0 = 0;
	int all_avg_1 = 0;
	int all_avg_2 = 0;

	for (int i=0; i<num_imgs; i++){
		split(imgs[i], i_planes);
		
		Scalar this_average = mean(imgs[i], i_planes[3]);
		img_averages.push_back(this_average);

		all_avg_0 += this_average.val[0];
		all_avg_1 += this_average.val[1];
		all_avg_2 += this_average.val[2];

		if (showimgs2) {
			namedWindow("predone", WINDOW_NORMAL);
			resizeWindow("predone", 1500, 1000);

			imshow("predone", i_planes[3]);
			waitKey(0);
			destroyWindow("predone");
		}
	}

	all_avg_0 /= num_imgs;
	all_avg_1 /= num_imgs;
	all_avg_2 /= num_imgs;

	cout << "\navg0all: " << all_avg_0;

	for (int i = 0; i<num_imgs; i++) {
		split(imgs[i], i_planes);

		float mean_diff_0 = all_avg_0 - img_averages[i].val[0];
		cout << "\nthis avg 0: " << img_averages[i].val[0];
		float mean_diff_1 = all_avg_1 - img_averages[i].val[1];
		float mean_diff_2 = all_avg_2 - img_averages[i].val[2];

		i_planes[0].convertTo(i_planes[0], CV_8UC1, 1, mean_diff_0);
		i_planes[1].convertTo(i_planes[1], CV_8UC1, 1, mean_diff_1);
		i_planes[2].convertTo(i_planes[2], CV_8UC1, 1, mean_diff_2);

		//merge em back
		merge(i_planes, 4, imgs[i]);
		masks.push_back(i_planes[3]);

		Scalar this_average = mean(imgs[i], i_planes[3]);
		cout << "\nNew average 0: " << this_average.val[0];

		if (showimgs2) {
			namedWindow("redone", WINDOW_NORMAL);
			resizeWindow("redone", 1500, 1000);

			imshow("redone", imgs[i]);
			waitKey(0);
			destroyWindow("redone");
		}
	}
}

void SphericalWarp(Mat &Equirect, Mat &Spherical) {
	//Do Spherical Warp
	//while (1){

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

	//Mat intermediary_rgba(intermediary.size(), CV_8UC4);
	//cvtColor(intermediary, intermediary_rgba, CV_BGR2RGBA, 4);

	Spherical = Mat(Equirect.size(), CV_8UC4);

	float f = 450; //was 450
	float s = 1125; //was 1125
	float x2, y2;
	Vec3b color;

	for (int x = 0; x < Spherical.cols; x++) {
		for (int y = 0; y < Spherical.rows; y++) {
			x2 = f * tan((x - (Spherical.cols / 2)) / s);
			y2 = tan((y - (Spherical.rows / 2)) / s) * sqrt(pow(x2, 2) + pow(f, 2)) + (Spherical.rows / 2);
			//y2 = (y - 972) / s * sqrt(pow(x2, 2) + pow(f, 2)) + 972;
			x2 += (Spherical.cols / 2);

			if ((x2 < Spherical.cols) && (x2 > -1) && (y2 < Spherical.rows) && (y2 > -1)) {
				//color = Equirect.at<Vec3b>(Point(int(x2), int(y2)));
				//Spherical.at<Vec4b>(Point(x, y))[0] = color[0];
				//Spherical.at<Vec4b>(Point(x, y))[1] = color[1];
				Spherical.at<Vec4b>(Point(x, y)) = Equirect.at<Vec4b>(Point(int(x2), int(y2)));

				//Spherical.at<Vec4b>(Point(x, y))[3] = 255;
			}
			else {
				Spherical.at<Vec4b>(Point(x, y))[0] = 127;
				Spherical.at<Vec4b>(Point(x, y))[1] = 127;
				Spherical.at<Vec4b>(Point(x, y))[2] = 127;
				Spherical.at<Vec4b>(Point(x, y))[3] = 0;
			}
		}
	}

	if (showimgs2){
		namedWindow("redone", WINDOW_NORMAL);
		resizeWindow("redone", 1500, 1000);

		imshow("redone", Spherical);
		waitKey(0);
		destroyWindow("redone");
	}
	//}
}

void CenterImg(Mat &RawImg, Mat &CenteredImg) {
	Mat Intermediate = Mat(Size(RawImg.cols * 2, RawImg.rows * 2), CV_8UC4);
	Vec4b default_val;
	default_val[0] = 127;
	default_val[1] = 127;
	default_val[2] = 127;
	default_val[3] = 0;
	Intermediate.setTo(default_val);

	CenteredImg = Mat(Size(3000, 2000), CV_8UC4);

	Mat grayscale;
	cvtColor(RawImg, grayscale, COLOR_BGR2GRAY);

	Mat mask;
	//cout << "\nThresh? ";
	int thresh = 25;
	//cin >> thresh;
	threshold(grayscale, mask, thresh, 255, CV_8UC1);

	// Find the largest contour in the mask
	// Then find the center of gravity of all the points
	// If this is accurate enough it will be very close near the center
	vector< vector<Point> > contours;
	findContours(mask, contours, RETR_EXTERNAL, CHAIN_APPROX_NONE);
	drawContours(mask, contours, -1, 255, 2);

	// Loop over contours and find the largest ones based on the area
	// This will ensure that small artefacts do not break this down
	vector<Point> largest_contour = contours[0];
	for (size_t i = 0; i<contours.size(); i++) {
		if (contourArea(contours[i]) > contourArea(largest_contour)) {
			largest_contour = contours[i];
		}
	}

	// Now find the minimum enclosing circle
	Point2f center;
	float radius;
	minEnclosingCircle(largest_contour, center, radius);

	int inter_x_center = Intermediate.cols / 2;
	int inter_y_center = Intermediate.rows / 2;

	Rect PasteRect = Rect(inter_x_center - center.x, inter_y_center - center.y, RawImg.cols, RawImg.rows);

	RawImg.copyTo(Intermediate(PasteRect));

	CenteredImg = Intermediate(Rect(inter_x_center - (CenteredImg.cols / 2), inter_y_center - (CenteredImg.rows / 2), CenteredImg.cols, CenteredImg.rows));

	// Draw the centerpoint on the original image
	circle(RawImg, center, 2, Scalar(0, 0, 255), -1);
	circle(RawImg, center, 10, Scalar(0, 255, 0), 2);
	stringstream temp;
	temp << "the radius = " << radius << " /" << " the center = " << center;
	putText(RawImg, temp.str(), Point(50, 50), FONT_HERSHEY_COMPLEX, 1, Scalar(0, 255, 0));
	//imshow("result", RawImg);
	//waitKey(0);
}

void CalcColumnFlow(Mat &SmallFlow, Mat &ColumnFlow, int x_inc, int y_inc, int x_offset, int y_offset, int max_search_distance) {
	if (test_images) {
		ColumnFlow = Mat(Size(2191, 1784), CV_32FC2);
	}
	else {
		ColumnFlow = Mat(Size(SmallFlow.cols * x_inc, SmallFlow.rows * y_inc), CV_32FC2);
	}
	Vec2f defaultval;
	defaultval[0] = 0.0f;
	defaultval[1] = 0.0f;
	ColumnFlow.setTo(defaultval);

	float column_float;
	float flow_pix;
	float x_float;
	float width_pix = ColumnFlow.cols - max_search_distance;
	float flow_final_point;

	for (int x = 0; x < SmallFlow.cols; x++) {
		for (int y = 0; y < SmallFlow.rows; y++) {
			if (SmallFlow.at<Vec2f>(Point(x, y))[0] > 0) {
				flow_final_point = SmallFlow.at<Vec2f>(Point(x, y))[1] / SmallFlow.at<Vec2f>(Point(x, y))[0];
				x_float = float(x*x_inc - x_offset);
				if (incremental_flow){
					flow_pix = flow_final_point;
					column_float = (x_float + flow_pix) / (1.0f + (flow_pix / float(width_pix)));
				}
				else {
					column_float = x_float + flow_final_point;
				}

				if (((y * y_inc) - y_offset > 0) && (column_float > 0) && (column_float < ColumnFlow.cols)) {
					ColumnFlow.at<Vec2f>(Point(int(column_float), (y * y_inc) - y_offset))[0] = 1.0f;
					ColumnFlow.at<Vec2f>(Point(int(column_float), (y * y_inc) - y_offset))[1] = flow_final_point;// corrected_flow_pix;
				}
			}
		}
	}
}

void CalcFinalFlow(Mat &BlurredFlow, Mat &OutputFlow, int max_search_distance) {
	
	float flow_final_point_r;
	float flow_final_point_l;
	float width_pix = BlurredFlow.cols - max_search_distance;
	float flow_pix;

	//Now create the new mat thing
	OutputFlow = Mat(BlurredFlow.size(), CV_32SC3);

	for (int x = 0; x < OutputFlow.cols; x++) {
		for (int y = 0; y < OutputFlow.rows; y++) {
			if (BlurredFlow.at<Vec2f>(Point(x, y))[1] > 0) { //was 5 and 5
				flow_pix = float(BlurredFlow.at<Vec2f>(Point(x, y))[1]) / float(BlurredFlow.at<Vec2f>(Point(x, y))[0]);
				if (incremental_flow){
					flow_final_point_l = flow_pix * float(x) / width_pix;
					flow_final_point_r = flow_pix - flow_final_point_l;
				}
				else {
					flow_final_point_r = flow_pix;
					flow_final_point_l = 0;
				}

				OutputFlow.at<Vec3i>(Point(x, y))[0] = int(flow_final_point_r);
				OutputFlow.at<Vec3i>(Point(x, y))[1] = int(flow_final_point_l);
				OutputFlow.at<Vec3i>(Point(x, y))[2] = 0;
			}
			else {
				OutputFlow.at<Vec3i>(Point(x, y))[0] = 0;
				OutputFlow.at<Vec3i>(Point(x, y))[1] = 0;
				OutputFlow.at<Vec3i>(Point(x, y))[2] = 0;
			}
		}
	}
}

void SmartCut(Mat &Rmat, Mat &Lmat, Mat &OutputMat) {
	OutputMat = Mat(Rmat.size(), CV_8UC4);

	int * cut = new int[Rmat.rows];

	Mat lblurred;
	blur(Lmat, lblurred, Size(20, 20));
	Mat rblurred;
	blur(Rmat, rblurred, Size(20, 20));
	Mat diff_mat;
	absdiff(rblurred, lblurred, diff_mat);

	Mat cut_mat = Mat(diff_mat.size(), CV_8UC1);
	cut_mat.setTo(0);
	
	int last_cut_point = Rmat.cols / 2;

	for (int y = 0; y < Rmat.rows; y++) {
		int min_diff = 10000;
		int min_diff_index = Rmat.cols / 2;
		for (int x = 0; x < Rmat.cols; x++) {
			int diffsum = diff_mat.at<Vec4b>(Point(x, y))[0] + diff_mat.at<Vec4b>(Point(x, y))[1] + diff_mat.at<Vec4b>(Point(x, y))[2] + abs(x - Rmat.cols/2)/2 + abs(x - last_cut_point)/2;
			if (diffsum < min_diff) {
				min_diff_index = x;
				min_diff = diffsum;
			}
		}
		last_cut_point = min_diff_index;
		for (int x = 0; x < min_diff_index; x++) {
			cut_mat.at<uchar>(Point(x, y)) = 255;
		}
	}
	
	Mat cut_mat_blurred;
	blur(cut_mat, cut_mat_blurred, Size(80, 80));

	Vec4b finalpixel;
	float fadevalue;

	for (int y = 0; y < Rmat.rows; y++) {
		for (int x = 0; x < Rmat.cols; x++) {
			fadevalue = float(cut_mat_blurred.at<uchar>(Point(x, y))) / 255.f;

			addWeighted(Lmat.at<Vec4b>(Point(x, y)), fadevalue, Rmat.at<Vec4b>(Point(x, y)), 1.0f - fadevalue, 0, finalpixel);
			OutputMat.at<Vec4b>(Point(x, y)) = finalpixel;
		}
	}
}