/*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 "opencv2/xfeatures2d.hpp"
#include "opencv2/xfeatures2d/nonfree.hpp"
#include "opencv2/features2d.hpp"

using namespace std;
using namespace cv;

vector<Mat> imgs;
vector<Mat> centered_imgs;
vector <Mat> spherical_imgs;

struct feature {
	Point points[3];
	Point orig_points[3];
	int image_index;
};

struct camera_matrix {
	float origin_pt[3];
	float rot_angle[3];
	float center[2];
};

void printUsage();
int parseCmdArgs(int argc, char** argv);
void CreateWarpMat(Mat &WarpMat, Mat &ReverseWarpMat, double k1, double k2, Size map_size);
void ReadInImages(string folder_name, string image_number);
void CenterImg(Mat &RawImg, Mat &CenteredImg, Point2i &readcenter);
void CreateSphericalMap(Mat &SphericalMap, Size map_size);
void MatchFeatures(vector <feature> &features, Mat &SphericalWarpMat, Mat &map1, Mat &map2);
void FindFeatureCoordinates(vector <feature> &features, Mat &Map1, Mat &Map2);
float GetLineDistances(Vec3f vectors[3], Vec3f origins[3]);
void VisualizeVectors(vector <Vec3f> vectors, vector <Vec3f> origins);
float GetParamError(vector <feature> Features, camera_matrix camera_matrices[8], float k1, float k2, float k3, int param_index, int visualize);
void Create360Photo(camera_matrix camera_matrices[8], float k1, float k2, float k3);
void ReadFeatures(vector <feature> &features);
void WriteFeatures(vector <feature> &features);

int bad_counter = 0;
int good_counter = 0;

float err_from_z = 0;
float err_from_x = 0;
float err_from_theta = 0;
float err_from_bad = 0;
float err_from_other = 0;

int main(int argc, char* argv[])
{
	string folder_n;
	string img_num;
	cout << "\nFolder? ";
	cin >> folder_n;
	cout << "\nImage Number? ";
	cin >> img_num;

	ReadInImages(folder_n, img_num);

	//Map features to original coordinates
	
	//FindFeatureCoordinates(Features, sphere_mat, map1);
	
	//Initialize parameters and parameter increments
	camera_matrix camera_matrices[8];
	float *parameters[43];
	float param_lambda[43];
	float derivatives[43];
	float twiddle_factors[43];
	float old_parameters[43];
	float default_twiddles[43];

	vector <int> center_x;
	vector <int> center_y;
	FileStorage fs_center("centers.ssf", FileStorage::READ);
	fs_center["center_x"] >> center_x;
	fs_center["center_y"] >> center_y;
	fs_center.release();

	for (int i = 0; i < 8; i++) {
		camera_matrices[i].rot_angle[0] = 0.0f;
		camera_matrices[i].rot_angle[1] = 90.0f;
		camera_matrices[i].rot_angle[2] = 45.0f * i;

		camera_matrices[i].center[0] = center_x[i];
		camera_matrices[i].center[1] = center_y[i];

		camera_matrices[i].origin_pt[0] = sin(CV_PI * -2.0f * float(i) / 8.0f);
		camera_matrices[i].origin_pt[1] = cos(CV_PI * -2.0f * float(i) / 8.0f);

		parameters[i * 5 + 0] = &(camera_matrices[i].rot_angle[0]);
		parameters[i * 5 + 1] = &(camera_matrices[i].rot_angle[1]);
		parameters[i * 5 + 2] = &(camera_matrices[i].rot_angle[2]);
		parameters[i * 5 + 3] = &(camera_matrices[i].center[0]);
		parameters[i * 5 + 4] = &(camera_matrices[i].center[1]);
		
		default_twiddles[i * 5 + 0] = 0.01f;
		default_twiddles[i * 5 + 1] = 0.01f;
		default_twiddles[i * 5 + 2] = 0.01f;
		default_twiddles[i * 5 + 3] = 1.f;
		default_twiddles[i * 5 + 4] = 1.f;

		twiddle_factors[i * 5 + 0] = 0.01f;
		twiddle_factors[i * 5 + 1] = 0.01f;
		twiddle_factors[i * 5 + 2] = 0.01f;
		twiddle_factors[i * 5 + 3] = 1.f;
		twiddle_factors[i * 5 + 4] = 1.f;
	}

	float k1, k2, k3;
	k1 = 15498.f; //scaled down by 10,000 good at 15500
	k2 = -22000.8f; //scaled down by 100,000 good at -22000
	k3 = 809.f; //good at 820
	
	Create360Photo(camera_matrices, k1, k2, k3);

	parameters[40] = &k1;
	parameters[41] = &k2;
	parameters[42] = &k3;
	twiddle_factors[40] = k1 / 1000.f;
	default_twiddles[40] = k1 / 1000.f;
	twiddle_factors[41] = k2 / 100.f;
	default_twiddles[41] = k2 / 1000.f;
	twiddle_factors[42] = k3 / 100.f;
	default_twiddles[42] = k3 / 1000.f;

	float baseline_error = 1000000.f * 1000.f;
	float prev_baseline_err = 0;
	float total_param_change = 0;
	float expected_err_err = 0;
	float expected_err_err_prev = 0;
	float lambda = 1000.f * 1000.f * 1.f; //was 10 million
	int eval_jacob = 1;
	int time_since_jacob = 0;

	// Match features
	vector <feature> Features;
	Mat sphere_mat, map1, map2;
	MatchFeatures(Features, sphere_mat, map1, map2);

	int write_features;
	cout << "\nR/W/A? ";
	cin >> write_features;
	if (write_features == 0) {
		ReadFeatures(Features);
	}
	else if (write_features == 1){
		WriteFeatures(Features);
	}
	else {
		ReadFeatures(Features);
		WriteFeatures(Features);
	}

	bad_counter = 0;
	good_counter = 0;
	err_from_z = 0;
	err_from_x = 0;
	err_from_theta = 0;
	err_from_bad = 0;
	err_from_other = 0;
	float new_error = GetParamError(Features, camera_matrices, k1, k2, k3, -1, 0);
	float baseline_sum_error = pow(pow(err_from_z, 2) + pow(err_from_x, 2) + pow(err_from_theta, 2) + pow(err_from_bad, 2), 0.5);
	baseline_error = new_error;
	cout << "\nFirst error " << new_error << " se " << baseline_sum_error;
	cout << "\nGood " << good_counter << " bad " << bad_counter << "\nx " << err_from_x << " z " << err_from_z << " bad " << err_from_bad << " theta " << err_from_theta << " other " << err_from_other;

	int i2 = 0;
	int i3 = 0;
	int param_end = 43;
	int param_start = 0;

	while ((i2 < 300) && (i3 < 1000)){
		i3++;
		/*if (i2 > 30) {
			param_end = 43;
			param_start = 40;
			lambda = 1000.f * 1000.f;
		}*/

		if ((eval_jacob == 1) || (time_since_jacob > 30)){
			for (int i = param_start; i < param_end; i++) {
				int param_index = i;
				if (i2 == 0) {
					derivatives[param_index] = 0;
					param_lambda[param_index] = 1;
					//if ((param_index < 40) && (param_index % 5 > 2)) param_lambda[param_index] = 0.1f;
					old_parameters[param_index] = *parameters[param_index];
				}

				float param_old = *parameters[param_index];
				if (abs(twiddle_factors[param_index]) > default_twiddles[param_index]) {
					*parameters[param_index] += twiddle_factors[param_index];
				}
				else {
					if (derivatives[param_index] < 0) {
						*parameters[param_index] += default_twiddles[param_index];
					} 
					else {
						*parameters[param_index] -= default_twiddles[param_index];
					}
				}

				float new_error2 = GetParamError(Features, camera_matrices, k1, k2, k3, -1, 0);

				float err_change = new_error2 - baseline_error;
				float param_change = *parameters[param_index] - param_old;
				float derivative = err_change / param_change;

				if (param_change != 0.f) {
					derivatives[param_index] = derivative;
					//if ((param_index % 5 == 3) || (param_index % 5 == 4)) cout << " d" << param_index << " " << derivative;
					//if ((err_change < 0) && (abs(twiddle_factors[param_index]) >= 0.001f)) {
					if ((err_change < 0) && (abs(twiddle_factors[param_index]) > default_twiddles[param_index])) {
						param_lambda[param_index] *= 0.5f; //good at 0.5 44661 at 0.2 40165.4 at 0.7
					}
					else if (abs(twiddle_factors[param_index]) > default_twiddles[param_index]) {
						param_lambda[param_index] *= 1.2f; //good at 1.2 38405 at 1.5
						//cout << "\nbump " << param_index << " " << param_lambda[param_index];
					}
				}

				*parameters[param_index] = param_old;
			} //end parameter loop
			i2++;
			if (time_since_jacob > 30) {
				lambda = 1000.f * 1000.f;
			}
			time_since_jacob = 0;
		}
		else {
			time_since_jacob++;
		}
		
		for (int i = param_start; i < param_end; i++) {
			float delta_c = derivatives[i] * new_error;
			twiddle_factors[i] = -1.f * delta_c / (derivatives[i] * derivatives[i] + (param_lambda[i] * lambda));
			//twiddle_factors[i] = -1.f * delta_c / (derivatives[i] * derivatives[i] + (lambda));
			/*if ((i > 39) && (abs(twiddle_factors[i]) < 0.5)) {
				if (signbit(derivatives[i])) {
					twiddle_factors[i] = 1.f;
				}
				else {
					twiddle_factors[i] = -1.f;
				}
			}*/

			//if ((i > 39) && (eval_jacob == 1)) {
			//	cout << "\nk" << (i - 39) << " " << derivatives[i] << " tf " << twiddle_factors[i] << " l " << param_lambda[i];
			//}
			//if ((i % 5 == 3) || (i % 5 == 4)) *parameters[i] += twiddle_factors[i];
			*parameters[i] += twiddle_factors[i];
		}

		bad_counter = 0;
		good_counter = 0;
		err_from_z = 0;
		err_from_x = 0;
		err_from_theta = 0;
		err_from_bad = 0;
		err_from_other = 0;
		new_error = GetParamError(Features, camera_matrices, k1, k2, k3, -1, 0);
		float sum_error = pow(pow(err_from_z, 2) + pow(err_from_x, 2) + pow(err_from_theta, 2) + pow(err_from_bad, 2), 0.5);

		//if (new_error < baseline_error) {
		if (sum_error < baseline_sum_error) {
			baseline_sum_error = sum_error;
			cout << "\nIteration " << i2 << " meas error " << new_error << " se " << sum_error;
			cout << "\nGood " << good_counter << " bad " << bad_counter << "\nx " << err_from_x << " z " << err_from_z << " bad " << err_from_bad << " theta " << err_from_theta << " other " << err_from_other;

			float expected_error = baseline_error;
			for (int i = param_start; i < param_end; i++) {
				expected_error += derivatives[i] * twiddle_factors[i];
			}
			//cout << " expc error " << expected_error;
			expected_err_err = abs(expected_error - new_error);
			expected_err_err_prev = expected_err_err;
			prev_baseline_err = baseline_error;
			baseline_error = new_error;

			eval_jacob = 1;
			lambda *= 0.1f;
			cout << " l " << lambda;
			for (int i = 0; i < 43; i++) {
				old_parameters[i] = *parameters[i];
			}
		}
		else {
			for (int i = param_start; i < param_end; i++) {
				*parameters[i] = old_parameters[i];
			}
			lambda *= 1.5f;// 1.414f;// 2.f;
			eval_jacob = 0;
		}
		
		/*for (int i = 0; i < 43; i++) {
			float delta_c = derivatives[i] * new_error;
			twiddle_factors[i] = -1.f * delta_c / (derivatives[i] * derivatives[i] + lambda);
			*parameters[i] += twiddle_factors[i];
		}*/

		/*for (int i = 0; i < 43; i++) {
			int param_index = i;
			if (i2 == 0){
				derivatives[param_index] = 0;
				old_parameters[param_index] = *parameters[param_index];
			}

			float param_old = *parameters[param_index];
			if (twiddle_factors[param_index] != 0.f) {
				*parameters[param_index] += twiddle_factors[param_index];
			}
			else {
				*parameters[param_index] *= 1.01f;
			}
			
			new_error = GetParamError(Features, camera_matrices, k1, k2, k3, -1, 0);

			float err_change = new_error - baseline_error;
			float param_change = *parameters[param_index] - param_old;
			float derivative = err_change / param_change;

			if (param_change != 0.f) {
				//float delta_c = derivative * new_error;
				//twiddle_factors[param_index] = -1.f * delta_c / (derivative * derivative + lambda);

				derivatives[param_index] = derivative;
				*parameters[param_index] = param_old;
			}
			//else {
			//	twiddle_factors[param_index] = *parameters[param_index] / 100.f;
			//}
		} //end parameter loop

		//for (int i = 0; i < 43; i++) {
		//	*parameters[i] += twiddle_factors[i];
		//}*/
	} //end main loop
	
	cout << "\nEnding main loop";

	for (int i = 0; i < 43; i++) {
		*parameters[i] = old_parameters[i];
		twiddle_factors[i] = default_twiddles[i];
	}

	//experimental loop!
	for (int i = 0; i < 1000; i++) {
		if (i % 100 == 0) cout << "\nIter " << i << " BL " << baseline_error << " se " << baseline_sum_error;
		//for (int param_index = 0; param_index < 43; param_index++) {
		int param_index = rand() % 43;
			float param_old = *parameters[param_index];
			*parameters[param_index] += twiddle_factors[param_index];

			err_from_z = 0;
			err_from_x = 0;
			err_from_theta = 0;
			err_from_bad = 0;
			err_from_other = 0;
			float this_error = GetParamError(Features, camera_matrices, k1, k2, k3, -1, 0);
			float sum_error = pow(pow(err_from_z, 2) + pow(err_from_x, 2) + pow(err_from_theta, 2) + pow(err_from_bad, 2), 0.5);

			//if ((this_error >= baseline_error) || (this_error < 0)) {
			if (sum_error >= baseline_sum_error) {
				*parameters[param_index] -= 2.f * twiddle_factors[param_index];

				err_from_z = 0;
				err_from_x = 0;
				err_from_theta = 0;
				err_from_bad = 0;
				err_from_other = 0;
				this_error = GetParamError(Features, camera_matrices, k1, k2, k3, -1, 0);
				sum_error = pow(pow(err_from_z, 2) + pow(err_from_x, 2) + pow(err_from_theta, 2) + pow(err_from_bad, 2), 0.5);

				//if ((this_error < baseline_error) && (this_error > 0)) {
				if (sum_error < baseline_sum_error) {
					twiddle_factors[param_index] *= -2.f;
					baseline_error = this_error;
					baseline_sum_error = sum_error;
				}
				else { 
					*parameters[param_index] = param_old; 
					twiddle_factors[param_index] *= 0.7f; 
				}
			}
			else { 
				baseline_error = this_error; 
				baseline_sum_error = sum_error;
				twiddle_factors[param_index] *= 2.f; 
			}
		//}
	}

	vector <float> rot_angle_0;
	vector <float> rot_angle_1;
	vector <float> rot_angle_2;
	vector <float> center_xs;
	vector <float> center_ys;

	for (int z = 0; z < 8; z++) {
		cout << "\nCamera " << z;
		cout << " :: Angle x " << camera_matrices[z].rot_angle[0] << " y " << camera_matrices[z].rot_angle[1] << " z " << camera_matrices[z].rot_angle[2];
		cout << "  :: Center x " << camera_matrices[z].center[0] << " y " << camera_matrices[z].center[1];
		rot_angle_0.push_back(camera_matrices[z].rot_angle[0]);
		rot_angle_1.push_back(camera_matrices[z].rot_angle[1]);
		rot_angle_2.push_back(camera_matrices[z].rot_angle[2]);
		center_xs.push_back(camera_matrices[z].center[0]);
		center_ys.push_back(camera_matrices[z].center[1]);
	}
	cout << "\nK1 " << k1 << " k2 " << k2 << " k3 " << k3;

	FileStorage fs_cams("cam_positions.ssf", FileStorage::WRITE);
	fs_cams << "rot_angle_0" << rot_angle_0;
	fs_cams << "rot_angle_1" << rot_angle_1;
	fs_cams << "rot_angle_2" << rot_angle_2;
	fs_cams << "center_x" << center_xs;
	fs_cams << "center_y" << center_ys;
	fs_cams << "k1" << k1;
	fs_cams << "k2" << k2;
	fs_cams << "k3" << k3;
	fs_cams.release();

	Create360Photo(camera_matrices, k1, k2, k3);

	GetParamError(Features, camera_matrices, k1, k2, k3, 0, 1);

	return 0;
}

float GetParamError(vector <feature> Features, camera_matrix camera_matrices[8], float k1, float k2, float k3, int param_index, int visualize) {
	float new_error = 0;
	float mu = k3;
	Mat coeffs = Mat(Size(4, 1), CV_32FC1);
	Mat roots;
	coeffs.setTo(0.0f);
	coeffs.at<float>(Point(0, 0)) = k2 / (100.f * 1000.f);
	coeffs.at<float>(Point(2, 0)) =  k1 / (10.f * 1000.f);
	
	coeffs.at<float>(Point(3, 0)) = -1200.f / mu; //was 1280
	solveCubic(coeffs, roots);
	float theta2 = 100;
	for (int i = 0; i < roots.rows; i++) {
		if ((roots.at<float>(Point(0, i)) > 0) && (roots.at<float>(Point(0, i)) < theta2)) {
			theta2 = roots.at<float>(Point(0, i));
		}
	}

	//cout << "\nRoots size " << roots.size();
	//cout << "nums " << roots.at<float>(Point(0, 0)) << " " << roots.at<float>(Point(0, 1)) << " " << roots.at<float>(Point(0, 2));

	new_error = pow(max(theta2 - 1.55f, 0.f), 2) * 5000;
	err_from_theta = new_error;
	if (param_index == 0) {
		cout << " t at 1300: " << theta2 << " addl err " << new_error;
	}
	
	vector <Vec3f> originsa;
	vector <Vec3f> vectorsa;

	//Iterate through features
	for (int i3 = 0; i3 < Features.size(); i3++) {
		Vec3f cart_vects[3];
		Vec3f origins[3];

		//iterate through the three points
		for (int i4 = 0; i4 < 3; i4++) {
			int cam_idx = (Features[i3].image_index + i4) % 8;

			/*if ((i3 < 0) && (param_index == 0)) {
				Mat paster = Mat(Size(400, 200), CV_8UC3);
				cout << "\n\nFeaturePoints old " << Features[i3].points[i4] << " new " << Features[i3].orig_points[i4];
				cout << "\nImage number " << cam_idx << " feature idx " << i4 << " feature num " << i3;

				int lbound = Features[i3].orig_points[i4].x - 100;
				int ubound = Features[i3].orig_points[i4].y - 100;
				imgs[cam_idx](Rect(lbound, ubound, 200, 200)).copyTo(paster(Rect(0, 0, 200, 200)));

				lbound = Features[i3].points[i4].x - 100;
				ubound = Features[i3].points[i4].y - 100;
				spherical_imgs[cam_idx](Rect(lbound, ubound, 200, 200)).copyTo(paster(Rect(200, 0, 200, 200)));
				imshow("2", paster);
				waitKey(0);
				destroyWindow("2");
			}*/

			//convert xy to r, theta, phi
			float r = pow(pow(Features[i3].orig_points[i4].x - camera_matrices[cam_idx].center[0], 2) + pow(Features[i3].orig_points[i4].y - camera_matrices[cam_idx].center[1], 2), 0.5);
			coeffs.at<float>(Point(3, 0)) = -1.f * r / mu;
			solveCubic(coeffs, roots);
			float theta = 100.f;// roots.at<float>(Point(0, 0));// / mu;
			for (int i = 0; i < roots.rows; i++) {
				if ((roots.at<float>(Point(0, i)) > 0) && (roots.at<float>(Point(0, i)) < theta)) {
					theta = roots.at<float>(Point(0, i));
				}
			}

			if (theta == 100.f) {
				theta = r * 1.5f / 1300.f;
				err_from_other++;
				//new_error += 10;
				//err_from_other += 10;
			}// cout << "\nBad theta " << r << " coeffs " << coeffs.at<float>(Point(0, 0)) << " " << coeffs.at<float>(Point(1, 0)) << " " << coeffs.at<float>(Point(2, 0)) << " " << coeffs.at<float>(Point(3, 0));

			float phi = atan2(Features[i3].orig_points[i4].y - camera_matrices[cam_idx].center[1], Features[i3].orig_points[i4].x - camera_matrices[cam_idx].center[0]);
			//add phi for z-rotation
			phi += camera_matrices[cam_idx].rot_angle[0] * CV_PI / 180.f;
			
			//convert to cartesian
			cart_vects[i4][0] = sin(theta) * cos(phi);
			cart_vects[i4][1] = sin(theta) * sin(phi);
			cart_vects[i4][2] = cos(theta);

			float x_angle = -1.f * camera_matrices[cam_idx].rot_angle[1] * CV_PI / 180.0f;
			float z_angle = camera_matrices[cam_idx].rot_angle[2] * CV_PI / 180.0f;

			//first rotate around x:
			//y' = ycos theta - zsin theta
			//z' = ysin theta + zcos theta
			float y = cart_vects[i4][1];
			float z = cart_vects[i4][2];

			cart_vects[i4][1] = y * cos(x_angle) - z * sin(x_angle);
			cart_vects[i4][2] = y * sin(x_angle) + z * cos(x_angle);

			//rotate around z:
			//x' = xcos theta - ysin theta
			//y' = xsin theta + ycos theta
			float x = cart_vects[i4][0];
			y = cart_vects[i4][1];

			cart_vects[i4][0] = x * cos(z_angle) - y * sin(z_angle);
			cart_vects[i4][1] = x * sin(z_angle) + y * cos(z_angle);

			//if ((i3 == 0) && (param_index == 0)) {
			//	cout << "\nRotated XYZ " << cart_vects[i4][0] << " " << cart_vects[i4][1] << " " << cart_vects[i4][2];
			//}

			origins[i4][0] = camera_matrices[cam_idx].origin_pt[0];
			origins[i4][1] = camera_matrices[cam_idx].origin_pt[1];
			origins[i4][2] = 0;

			originsa.push_back(origins[i4]);

			//shift x,y
			cart_vects[i4][0] += camera_matrices[cam_idx].origin_pt[0];
			cart_vects[i4][1] += camera_matrices[cam_idx].origin_pt[1];

			vectorsa.push_back(cart_vects[i4]);
		}

		//if (visualize == 1) VisualizeVectors(vectorsa, originsa);

		float this_feature_err = GetLineDistances(cart_vects, origins);
		new_error += this_feature_err;
	} //end feature loop

	if (visualize == 1) VisualizeVectors(vectorsa, originsa);

	return new_error;
}

void Create360Photo(camera_matrix camera_matrices[8], float k1, float k2, float k3) {
	Mat p360_photo = Mat(Size(2000, 1000), CV_8UC3);
	Vec3f this_pixel;

	for (int x = 0; x < p360_photo.cols; x++) {
		for (int y = 0; y < p360_photo.rows; y++) {
			float phi = float(x) * 360.f / float(p360_photo.cols);
			float theta = float(y) * 180.f / float(p360_photo.rows);

			int closest_cam_idx = -1;
			float closest_cam_diff = 360.f;
			//find closest camera
			for (int i = 0; i < 8; i++) {
				if (abs(camera_matrices[i].rot_angle[2] - phi) < closest_cam_diff) {
					closest_cam_idx = i;
					closest_cam_diff = abs(camera_matrices[i].rot_angle[2] - phi);
				}
			}

			//rotate about z
			float z_angle = -1.f * camera_matrices[closest_cam_idx].rot_angle[2];
			phi += z_angle;

			phi *= CV_PI / 180.f;
			theta *= CV_PI / 180.f;

			//convert to cartesian
			this_pixel[0] = sin(theta) * cos(phi);
			this_pixel[1] = sin(theta) * sin(phi);
			this_pixel[2] = cos(theta);

			//rotate about x
			//float x_angle = camera_matrices[closest_cam_idx].rot_angle[1] * CV_PI / 180.0f;
			//float z2 = this_pixel[2];
			//float y2 = this_pixel[1];
			//this_pixel[1] = y2 * cos(x_angle) - z2 * sin(x_angle);
			//this_pixel[2] = y2 * sin(x_angle) + z2 * cos(x_angle);

			//rotate about y
			float y_angle = -1.f * camera_matrices[closest_cam_idx].rot_angle[1] * CV_PI / 180.0f;
			float z2 = this_pixel[2];
			float x2 = this_pixel[0];
			this_pixel[2] = z2 * cos(y_angle) - x2 * sin(y_angle);
			this_pixel[0] = z2 * sin(y_angle) + x2 * cos(y_angle);

			//convert to polar
			float phi2 = atan2f(this_pixel[1], this_pixel[0]);
			float theta2 = atan2f(pow(pow(this_pixel[1], 2) + pow(this_pixel[0], 2), 0.5), this_pixel[2]);
			//rotate about z
			phi2 -= camera_matrices[closest_cam_idx].rot_angle[0] * CV_PI / 180.0f;

			//convert to x, y
			float k1a = k1 / (10.f * 1000.f);
			float k2a = k2 / (100.f * 1000.f);

			float r = k3 * (k1a * theta2 + k2a * pow(theta2, 3));
			float xa = r * -1.f * sin(phi2);
			float ya = r * cos(phi2);

			xa += camera_matrices[closest_cam_idx].center[0];
			ya += camera_matrices[closest_cam_idx].center[1];

			/*if ((x % 100 == 0) && (y % 100 == 0)) {
				cout << "\n\nx " << x << " y " << y << " cam ind " << closest_cam_idx;
				cout << "\nvector x " << this_pixel[0] << " y " << this_pixel[1] << " z " << this_pixel[2];
				cout << "\nphi " << phi << " theta " << theta << " phi2 " << phi2 << " theta2 " << theta2;
				cout << "\nr " << r << " xa " << xa << " ya " << ya;
			}*/

			if ((xa > -1) && (xa < 2592) && (ya > -1) && (ya < 1944)) {
				p360_photo.at<Vec3b>(Point(x, y)) = imgs[closest_cam_idx].at<Vec3b>(Point(xa, ya));
			}
		}
	}

	imshow("360 photo", p360_photo);
	waitKey(0);
	destroyWindow("360 photo");
}

void VisualizeVectors(vector <Vec3f> vectors, vector <Vec3f> origins) {
	Mat visualization = Mat(Size(1000, 1000), CV_8UC3);
	visualization.setTo(Scalar(0, 0, 0));

	for (int i = 0; i < vectors.size(); i++) {
		line(visualization, Point(vectors[i][0] * 100 + 500, vectors[i][1] * 100 + 500), Point(origins[i][0] * 100 + 500, origins[i][1] * 100 + 500), Scalar(255, 255, 255));
	}

	imshow("viz", visualization);
	waitKey(0);
	destroyWindow("viz");
}

float GetLineDistances(Vec3f vectors[3], Vec3f origins[3]) {
	Vec3f intersections[3];
	float total_err = 0;
	float average_distance = 0;
	float angle_fail = 0;
	float power_mult = 1;
	int badone = 0;
	float max_angle = -1000;
	float min_angle = 1000;

	for (int i = 0; i < 3; i++) {
		//get y intercepts
		/*double a1 = (line1.P1.Y - line1.P2.Y) / (double)(line1.P1.X - line1.P2.X);
		double b1 = line1.P1.Y - a1 * line1.P1.X;

		double a2 = (line2.P1.Y - line2.P2.Y) / (double)(line2.P1.X - line2.P2.X);
		double b2 = line2.P1.Y - a2 * line2.P1.X;

		double x = (b2 - b1) / (a1 - a2);
		double y = a1 * x + b1;*/

		double a1 = (origins[i][1] - vectors[i][1]) / (double)(origins[i][0] - vectors[i][0]);
		double b1 = origins[i][1] - a1 * origins[i][0];

		double a2 = (origins[(i+1)%3][1] - vectors[(i+1)%3][1]) / (double)(origins[(i+1)%3][0] - vectors[(i+1)%3][0]);
		double b2 = origins[(i+1)%3][1] - a2 * origins[(i+1)%3][0];

		double z_x_slope_1 = (vectors[i][2]) / (vectors[i][0] - origins[i][0]);
		double z_intercept_1 = vectors[i][2] - z_x_slope_1 * vectors[i][0];

		double z_x_slope_2 = (vectors[(i + 1) % 3][2]) / (vectors[(i + 1) % 3][0] - origins[(i + 1) % 3][0]);
		double z_intercept_2 = vectors[(i + 1) % 3][2] - z_x_slope_2 * vectors[(i + 1) % 3][0];

		intersections[i][0] = (b2 - b1) / (a1 - a2);
		intersections[i][1] = a1 * intersections[i][0] + b1;
		intersections[i][2] = abs((intersections[i][0] * z_x_slope_1 + z_intercept_1) - (intersections[i][0] * z_x_slope_2 + z_intercept_2));

		float z_angle = atan2f(vectors[i][2], pow(pow(vectors[i][1] - origins[i][1], 2) + pow(vectors[i][0] - origins[i][0], 2), 0.5));
		max_angle = max(z_angle, max_angle);
		min_angle = min(z_angle, min_angle);
		//float distance = pow(pow(intersections[i][1], 2) + pow(intersections[i][0], 2), 0.5);
		//float z_1_angle = atan2(intersections[i][1] * z_x_slope_1 + z_intercept_1, distance);
		//float z_2_angle = atan2(intersections[i][1] * z_x_slope_2 + z_intercept_2, distance);
		//intersections[i][2] = z_1_angle - z_2_angle;
	}

	for (int i = 0; i < 3; i++) {
		//float inter_dist_squared = pow(pow(intersections[i][0] - intersections[(i + 1) % 3][0], 2) + pow(intersections[i][1] - intersections[(i + 1) % 3][1], 2), power_mult / 2.f);
		float distance = pow(pow(intersections[i][0], 2) + pow(intersections[i][1], 2), power_mult / 2.f);
		average_distance += distance;
		float intersection_1_angle = atan2f(intersections[i][1], intersections[i][0]);
		float intersection_2_angle = atan2f(intersections[(i + 1) % 3][1], intersections[(i + 1) % 3][0]);
		float inter_angle_diff_squared = pow(abs(intersection_1_angle - intersection_2_angle), 2);
		
		float inter_angle = atan2f(intersections[i][1] - origins[i][1], intersections[i][0] - origins[i][0]);
		float vector_angle = atan2f(vectors[i][1] - origins[i][1], vectors[i][0] - origins[i][0]);
		float vector2_angle = atan2f(vectors[(i + 1) % 3][1] - origins[(i + 1) % 3][1], vectors[(i + 1) % 3][0] - origins[(i + 1) % 3][0]);

		if (abs(inter_angle - vector_angle) > 0.01f) {
			total_err += pow(abs(vector_angle - vector2_angle), power_mult) * 250 + 10;// was 70, good at 5000
			err_from_bad += pow(abs(vector_angle - vector2_angle), power_mult) * 250 + 10;
			bad_counter++;
			badone++;
		} else {
			//err_from_z += pow(intersections[i][2], power_mult) / distance * 300; //good at 300
			err_from_z += (max_angle - min_angle) * 300;
			//err_from_x += inter_dist_squared / distance;
			//total_err += inter_dist_squared / distance + pow(intersections[i][2], power_mult) / distance * 300;
			total_err += (max_angle - min_angle) * 300;//pow(intersections[i][2], power_mult) / distance * 300;
			good_counter++;
		}
	}

	if (badone == 0){
		float ax = intersections[0][0];
		float ay = intersections[0][1];
		float bx = intersections[1][0];
		float by = intersections[1][1];
		float cx = intersections[2][0];
		float cy = intersections[2][1];
		float tri_area = ax * (by - cy) + bx * (cy - ay) + cx * (ay - by);
		err_from_x += (tri_area / average_distance) * 50.f;
		total_err += (tri_area / average_distance) * 50.f;
		//cout << "\nArea " << tri_area << " dist " << average_distance;
	}

	return total_err;
}

void FindFeatureCoordinates(vector <feature> &features, Mat &Map2, Mat &Map1) {
	cout << "\nMap1 " << Map1.size() << " map2 " << Map2.size();
	cout << "\nFinding coords";
	for (int i = 0; i < features.size(); i++) {
		for (int i2 = 0; i2 < 3; i2++) {
			if ((features[i].points[i2].x > 0) && (features[i].points[i2].y > 0) && (features[i].points[i2].x < Map1.cols) && (features[i].points[i2].y < Map1.rows)) {
				
				int x = Map2.at<Vec2f>(features[i].points[i2])[0];
				int y = Map2.at<Vec2f>(features[i].points[i2])[1];
				//if ((x > 0) && (x < Map2.cols) && (y > 0) && (y < Map2.rows)){
				Point intermediate_point = Point(min(max(x, 0), Map2.cols - 1), min(max(y, 0), Map2.rows - 1));
				features[i].orig_points[i2] = Point(Map1.at<Vec2s>(intermediate_point)[0], Map1.at<Vec2s>(intermediate_point)[1]);
				//}
				//else {
				if ((x > 0) && (x < Map2.cols) && (y > 0) && (y < Map2.rows)) {
						
				}
				else {
					cout << "\nDouble bad point x " << x << " y " << y;
				}
				//}
			}
			else {
				cout << "\nBad point " << features[i].points[i2];
			}
		}
	}
}

void MatchFeatures(vector <feature> &features, Mat &SphericalWarpMat, Mat &map1, Mat &map2) {
	//Mat SphericalWarpMat;
	CreateSphericalMap(SphericalWarpMat, Size(3000, 2000));

	FileStorage fs("out_camera_data.yml", FileStorage::READ);
	Mat distCoeffs, cameraMatrix;
	fs["camera_matrix"] >> cameraMatrix;
	fs["distortion_coefficients"] >> distCoeffs;
	//Mat map1, map2;
	Size newSize = Size(3000, 2000);
	fisheye::initUndistortRectifyMap(cameraMatrix, distCoeffs, Mat(),
		getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, newSize, 1, newSize, 0, true),
		newSize, CV_16SC2, map1, map2);
	fs.release();

	//cout << "\nmap1t " << map1.type() << " 2t " << map2.type();
	//cout << "\n" << map1.at<Vec2s>(Point(1200, 1200)) << " 2 " << map2.at<short>(Point(1200, 1200));
	//cout << "\n" << map1.at<Vec2s>(Point(1201, 1200)) << " 2 " << map2.at<short>(Point(1201, 1200));

	vector <int> center_x;
	vector <int> center_y;
	FileStorage fs_center("centers.ssf", FileStorage::READ);
	fs_center["center_x"] >> center_x;
	fs_center["center_y"] >> center_y;
	fs_center.release();
	
	cout << "\nWarping Images";
	for (int i = 0; i < 8; i++) {
		Mat centered_img;
		CenterImg(imgs[i], centered_img, Point2i(center_x[i], center_y[i]));

		Mat remapped_img, empty;
		remap(centered_img, remapped_img, map1, map2, INTER_LINEAR, BORDER_TRANSPARENT);

		namedWindow("img_matches", WINDOW_NORMAL);
		resizeWindow("img_matches", 1500, 1000);
		imshow("img_matches", remapped_img);
		//waitKey(0);
		destroyWindow("img_matches");

		Mat spherical_img;
		remap(remapped_img, spherical_img, SphericalWarpMat, empty, INTER_LINEAR);
		spherical_imgs.push_back(spherical_img);
	}

	//get keypoints
	std::vector<KeyPoint> keypoints[8];
	Mat descriptors[8];

	cout << "\nGetting Keypoints";
	for (int i = 0; i < 8; i++) {
		int minHessian = 2000; //was 2500

		Ptr<xfeatures2d::SURF> surf = xfeatures2d::SURF::create(minHessian);

		Mat empty;

		surf->detectAndCompute(spherical_imgs[i], empty, keypoints[i], descriptors[i]);
	}

	std::vector< DMatch > good_matches[8];

	cout << "\nGetting Matches";
	for (int i = 0; i < 8; i++) {
		cout << "\n\nImage: " << i;
		FlannBasedMatcher matcher;

		std::vector< DMatch > matches;
		matcher.match(descriptors[i], descriptors[(i + 1) % 8], matches);

		Mat img_matches;
		/*drawMatches(spherical_imgs[i], keypoints[i], spherical_imgs[(i + 1) % 8], keypoints[(i + 1) % 8],
			matches, img_matches, Scalar::all(-1), Scalar::all(-1),
			vector<char>(), DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS);

		namedWindow("img_matches", WINDOW_NORMAL);
		resizeWindow("img_matches", 2500, 1000);
		imshow("img_matches", img_matches);
		waitKey(0);*/

		int x_rejected = 0;
		int y_rejected = 0;
		int distance_rejected = 0;

		cout << " Matches " << matches.size();

		float average_x_diff = 0;
		int x_num = 0;

		for (int i2 = 0; i2 < matches.size(); i2++) {
			if (matches[i2].distance < 0.3) { //was 0.3
				int this_keypoint_y = keypoints[i][i2].pt.y;
				int this_keypoint_x = keypoints[i][i2].pt.x;
				int next_keypoint_y = keypoints[(i + 1) % 8][matches[i2].trainIdx].pt.y;
				int next_keypoint_x = keypoints[(i + 1) % 8][matches[i2].trainIdx].pt.x;
				//cout << "\nQidx: " << matches[i2].queryIdx << " imgidx: " << matches[i2].imgIdx;

				int y_dist = abs(this_keypoint_y - next_keypoint_y);
				int x_dist = abs(next_keypoint_x - this_keypoint_x - 840);

				if (pow(pow(x_dist, 2) + pow(y_dist * 2, 2), 0.5) < 200){
					average_x_diff += next_keypoint_x - this_keypoint_x;
					x_num++;
				//TODO: make these not hardcoded
				//if ((this_keypoint_y > (next_keypoint_y - 60)) && (this_keypoint_y < (next_keypoint_y + 60))) {
				//	if (((this_keypoint_x - next_keypoint_x) < -780) && ((this_keypoint_x - next_keypoint_x) > -880)){ //was 780 and 880
						
						if (good_matches[i].size() > 0){
							int pushed = 0;
							for (int i3 = 0; i3 < good_matches[i].size(); i3++) {
								if (good_matches[i][i3].distance > matches[i2].distance) {
									good_matches[i].insert(good_matches[i].begin() + i3, matches[i2]);
									pushed = 1;
									break;
								}
							}
							if (pushed == 0) {
								good_matches[i].push_back(matches[i2]);
							}
						}
						else {
							good_matches[i].push_back(matches[i2]);
						}
				}
				else { y_rejected++; }
				//	}
				//	else { x_rejected++; }
				//}
				//else { y_rejected++; }
			}
			else { distance_rejected++; }
		}

		cout << "\nDist " << distance_rejected << " x " << x_rejected << " y " << y_rejected;
		cout << " Kept " << good_matches[i].size();
		average_x_diff /= float(x_num);
		cout << "\nXnum " << x_num << " " << average_x_diff;

		/*Mat good_match_mat;
		drawMatches(spherical_imgs[i], keypoints[i], spherical_imgs[(i + 1) % 8], keypoints[(i + 1) % 8],
			good_matches[i], good_match_mat, Scalar::all(-1), Scalar::all(-1),
			vector<char>(), DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS);

		namedWindow("good_img_matches", WINDOW_NORMAL);
		resizeWindow("good_img_matches", 2500, 1000);
		imshow("good_img_matches", good_match_mat);
		waitKey(0);
		destroyWindow("good_img_matches");
		destroyWindow("img_matches");*/
	}

	cout << "\nMatching Matches";
	for (int i = 0; i < 8; i++) {
		int num_triple_points = 0;

		for (int i2 = 0; i2 < good_matches[i].size(); i2++) {
			int next_img_match_idx = good_matches[i][i2].trainIdx;
			int second_img_match_idx = -1;

			int next_img_idx = (i + 1) % 8;
			int second_img_idx = (i + 2) % 8;
			
			for (int i3 = 0; i3 < good_matches[next_img_idx].size(); i3++) {
				if (good_matches[next_img_idx][i3].queryIdx == next_img_match_idx){
					num_triple_points++;
					second_img_match_idx = good_matches[next_img_idx][i3].trainIdx;
					
					feature triple_point_feature;
					triple_point_feature.image_index = i;

					triple_point_feature.points[0] = keypoints[i][good_matches[i][i2].queryIdx].pt;
					triple_point_feature.points[1] = keypoints[next_img_idx][good_matches[next_img_idx][i3].queryIdx].pt;
					triple_point_feature.points[2] = keypoints[second_img_idx][good_matches[next_img_idx][i3].trainIdx].pt;

					features.push_back(triple_point_feature);
					break;
				}
			}

			if (num_triple_points > 40) { //was 20
				break;
			}
		}
		cout << "\nImg " << i << " tps " << num_triple_points;
	}

	//get feature coords
	for (int i = 0; i < features.size(); i++) {
		int bad_feature = 0;
		Mat paste_mat = Mat(Size(300, 100), CV_8UC3);
		for (int i2 = 0; i2 < 3; i2++) {
			if ((features[i].points[i2].x > 0) && (features[i].points[i2].y > 0) && (features[i].points[i2].x < map1.cols) && (features[i].points[i2].y < map1.rows)) {
				int img_index = (features[i].image_index + i2) % 8;
				int x = SphericalWarpMat.at<Vec2f>(features[i].points[i2])[0];
				int y = SphericalWarpMat.at<Vec2f>(features[i].points[i2])[1];
				//if ((x > 0) && (x < SphericalWarpMat.cols) && (y > 0) && (y < SphericalWarpMat.rows)) {
					Point intermediate_point = Point(min(max(x, 0), SphericalWarpMat.cols - 1), min(max(y, 0), SphericalWarpMat.rows - 1));
					features[i].orig_points[i2] = Point(map1.at<Vec2s>(intermediate_point)[0] + center_x[img_index] - 1500, map1.at<Vec2s>(intermediate_point)[1] + center_y[img_index] - 1000);

					if ((features[i].orig_points[i2].x > 50) && (features[i].orig_points[i2].x < 2542) && (features[i].orig_points[i2].y > 50) && (features[i].orig_points[i2].y < 1894)){
						imgs[img_index](Rect(features[i].orig_points[i2].x - 50, features[i].orig_points[i2].y - 50, 100, 100)).copyTo(paste_mat(Rect(100 * i2, 0, 100, 100)));
					}

				if ((x > 0) && (x < SphericalWarpMat.cols) && (y > 0) && (y < SphericalWarpMat.rows)) {
				}
				else {
					cout << "\nDouble bad point x " << x << " y " << y;
					bad_feature++;
				}
			}
			else {
				cout << "\nBad point " << features[i].points[i2];
				bad_feature++;
			}
		}

		if (bad_feature) {
			features.erase(features.begin() + i);
			i--;
		}

		/*if (i % 2 == 0) {
			imshow("paste", paste_mat);
			waitKey(0);
			destroyWindow("paste");
		}*/
	}
}

void ReadInImages(string folder_name, string image_number) {
	for (int i = 0; i < 8; i++) {
		stringstream filename;
		filename << folder_name << "/img" << image_number << "cam" << i << ".bmp";
		//cout << "\nFile: " << filename.str();
		Mat imga = imread(filename.str(), IMREAD_GRAYSCALE);
		Mat img;
		cvtColor(imga, img, CV_BayerGR2BGR_EA);
		namedWindow("img_matches", WINDOW_NORMAL);
		resizeWindow("img_matches", 1500, 1000);
		imshow("img_matches", img);
		waitKey(0);
		destroyWindow("img_matches");
		imgs.push_back(img);
	}
}

void CreateWarpMat(Mat &WarpMat, Mat &ReverseWarpMat, double k1, double k2, Size map_size) {
	WarpMat = Mat(map_size, CV_32FC2);
	ReverseWarpMat = Mat(map_size, CV_32FC2);

	//r is the pixel radius from center = phi * k1 + phi^3 * k2
	//phi is the angle of the ray 

	float x2, y2, x3, y3;
	double r, phi, phi2, theta;

	Point2i center = Point2i(map_size.width / 2, map_size.height / 2);

	Mat coeffs = Mat(Size(4, 1), CV_32FC1);
	coeffs.setTo(0.0f);
	coeffs.at<float>(Point(0, 0)) = k2;
	coeffs.at<float>(Point(2, 0)) = k1;

	Mat roots;

	for (int x = 0; x < WarpMat.cols; x++) {
		for (int y = 0; y < WarpMat.rows; y++) {
			r = pow(pow(x - center.x, 2) + pow(y - center.y, 2), 0.5);
			//phi2 = r * k1 + pow(r, 3) * k2;

			coeffs.at<float>(Point(3, 0)) = -1.f * r;
			solveCubic(coeffs, roots);

			phi = roots.at<float>(Point(0, 0));

			theta = atan2(y - center.y, x - center.x);

			x2 = phi * cos(theta) + center.x;
			y2 = phi * sin(theta) + center.y;

			if ((x2 < WarpMat.cols) && (x2 > -1) && (y2 < WarpMat.rows) && (y2 > -1)) {
				WarpMat.at<Vec2f>(Point(x, y)) = Vec2f(x2, y2);
				ReverseWarpMat.at<Vec2f>(Point(x2, y2)) = Vec2f(x, y);
			}
		}
	}
}

void CreateSphericalMap(Mat &SphericalMap, Size map_size) {
	SphericalMap = Mat(map_size, CV_32FC2);

	float f = 440;
	float s = 1105;
	float x2, y2;

	/*cout << "\nF? ";
	cin >> f;
	cout << " s? ";
	cin >> s;*/

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

			//if ((x2 < SphericalMap.cols) && (x2 > -1) && (y2 < SphericalMap.rows) && (y2 > -1)) {
				SphericalMap.at<Vec2f>(Point(x, y)) = Vec2f(x2, y2);
			//}
		}
	}
}

void CenterImg(Mat &RawImg, Mat &CenteredImg, Point2i &readcenter) {
	Mat Intermediate = Mat(Size(RawImg.cols * 2, RawImg.rows * 2), CV_8UC3);
	Vec3b default_val;
	default_val[0] = 127;
	default_val[1] = 127;
	default_val[2] = 127;
	Intermediate.setTo(default_val);

	Point2i center;
	center.x = RawImg.cols / 2;
	center.y = RawImg.rows / 2;
	Mat draft_center;

	if ((readcenter.x > 0) && (readcenter.y > 0)) {
		center.x = readcenter.x;
		center.y = readcenter.y;
	}

	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 = Mat(Size(3000, 2000), CV_8UC3);
	CenteredImg = Intermediate(Rect(inter_x_center - (CenteredImg.cols / 2), inter_y_center - (CenteredImg.rows / 2), CenteredImg.cols, CenteredImg.rows));
}

void WriteFeatures(vector <feature> &features) {
	FileStorage feature_fs("features.ssf", FileStorage::WRITE);
	vector <Point> origin_points1;
	vector <Point> origin_points2;
	vector <Point> origin_points3;
	vector <int> indexes;
	for (int i = 0; i < features.size(); i++) {
		origin_points1.push_back(features[i].orig_points[0]);
		origin_points2.push_back(features[i].orig_points[1]);
		origin_points3.push_back(features[i].orig_points[2]);
		indexes.push_back(features[i].image_index);
	}
	feature_fs << "origins0" << origin_points1;
	feature_fs << "origins1" << origin_points2;
	feature_fs << "origins2" << origin_points3;
	feature_fs << "indices" << indexes;
	feature_fs.release();
}

void ReadFeatures(vector <feature> &features) {
	FileStorage feature_fs("features.ssf", FileStorage::READ);
	vector <Point> origin_points1;
	vector <Point> origin_points2;
	vector <Point> origin_points3;
	vector <int> indexes;

	feature_fs["origins0"] >> origin_points1;
	feature_fs["origins1"] >> origin_points2;
	feature_fs["origins2"] >> origin_points3;
	feature_fs["indices"] >> indexes;
	feature_fs.release();

	for (int i = 0; i < origin_points1.size(); i++) {
		feature new_feature;
		new_feature.orig_points[0] = origin_points1[i];
		new_feature.orig_points[1] = origin_points2[i];
		new_feature.orig_points[2] = origin_points3[i];
		new_feature.image_index = indexes[i];
		features.push_back(new_feature);
	}
}