#include <opencv2/opencv.hpp>
#include "/usr/x86_64-w64-mingw32/sys-root/mingw/include/opencv4/opencv2/imgproc.hpp"
#include "/usr/x86_64-w64-mingw32/sys-root/mingw/include/opencv4/opencv2/ximgproc.hpp"
#include "/usr/x86_64-w64-mingw32/sys-root/mingw/include/opencv4/opencv2/core.hpp"
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Eigenvalues>
#include "/usr/x86_64-w64-mingw32/sys-root/mingw/include/opencv4/opencv2/core/eigen.hpp"

#include <chrono>

#include <stdio.h>
#include <sstream>
#include <cmath>
#include <iostream>
#include <stdexcept>
#include <vector>
#include <tuple>
#include <functional>
#include <complex>

#include <fftw3.h>
 
#include "gnuplot-cpp-master/include/gnuplot.h"

using namespace cv;
using namespace std;
using namespace Eigen;

typedef unsigned char uchar;
typedef unsigned int uint;




int main()
{


//initializing and predefining variables 

//Path to reference diffraction image
string path_ref = "";
//Path to observed diffraction images
string path_obs = "";
//Path to results
string path_res = "";
//name of the observed diffraction images, expected naming: beginning{rownumber}middle{columnnumber}ending, in increments of 1 for both rownumber and columnnumber
string beginning = "";
string middle = "_";
string ending = ".tiff";

//mapping and EBSD setup parameters
int tilt_angle = 70; // angle of tilt during the scan
const int seg_size = 256; //segment size
int num_seg = 12; // number of segments
int rows = 50; // number of rows in the whole scan		90
int columns = 50; // number of columns in the whole scan

int stepsize = 20; // step size in nanometer, only square scans are supported atm, only used for scale
stringstream sstepsize;
sstepsize << stepsize*12;

//detectorinfo
int det_x_pix = 13;
int det_y_pix = 10;
float pix_size = 0.0000234; //detector pixel size in METRES/pixel
int system = 1;

double rxp, ryp, rzp;
rxp = 0.493;
ryp = 0.433;
rzp = 0.728;

Array<int,Dynamic,Dynamic> points(4,2);

//peak detection info
const int ROI_center_size = 121;
const int ROI_subpixel_size = 31;

//crystal symmetry
string symmetry = "cubic";
//initial grain orientation
double phi1=333.1249, phi2=2.9297, phi3=72.6599;
//elastic coefficients of silicon in Pa
float c11 = 1.6564*pow(10,11), c12 = 0.6394*pow(10,11), c44 = 0.51*pow(10,11);

//end of variable predefinition


//START OF VARIABLE LOADING
fstream file;
string blank;

file.open("config",ios::in);
if(!file){
	cout<<"Error in opening config file...(this may or may not be on purpose)";
	return 0;
}
else{
	getline(file,blank);
	getline(file,blank);
	getline(file,blank);
	getline(file,blank);
	getline(file,path_ref);
	getline(file,blank);
	getline(file,path_obs);
	getline(file,blank);
	getline(file,path_res);
	getline(file,blank);
	getline(file,beginning);
	getline(file,middle);
	getline(file,ending);
	getline(file,blank);
	file>>tilt_angle;
	getline(file,blank);
	getline(file,blank);
	file>>det_x_pix;
	getline(file,blank);
	getline(file,blank);
	file>>det_y_pix;
	getline(file,blank);
	getline(file,blank);
	file>>pix_size;
	getline(file,blank);
	getline(file,blank);
	file>>rxp;
	getline(file,blank);
	file>>ryp;
	getline(file,blank);
	file>>rzp;
	getline(file,blank);
	getline(file,blank);
	file>>num_seg;
	points = MatrixXi::Zero(num_seg,2);
	getline(file,blank);
	getline(file,blank);
	for(int i; i<num_seg; i++){
	file>>points(i,0);
	file>>points(i,1);
	getline(file,blank);
	}
	getline(file,blank);
	file>>columns;
	getline(file,blank);
	getline(file,blank);
	file>>rows;
	getline(file,blank);
	getline(file,blank);
	file>>stepsize;
	getline(file,blank);
	getline(file,blank);
	file>>phi1;
	getline(file,blank);
	file>>phi2;
	getline(file,blank);
	file>>phi3;
	getline(file,blank);
	getline(file,blank);
	getline(file,symmetry);
	getline(file,blank);
	file>>c11;
	getline(file,blank);
	file>>c12;
	getline(file,blank);
	file>>c44;
	getline(file,blank);
	getline(file,blank);
	file>>system;
}
//END OF VARIABLE LOADING

//correct variable units
c11 = c11 * pow(10,9);
c12 = c12 * pow(10,9);
c44 = c44 * pow(10,9);

phi1 = phi1 * M_PI/180;
phi2 = phi2 * M_PI/180;
phi3 = phi3 * M_PI/180;

int num_points = rows*columns;//	 number of points in the scan
//end of correct variable units



//part handling conversion of the strain coefficient tensor into the sample coordiante system, using the initial grain orientation, put in by the user
//grain orientation and rotation matrix , Bunges notation -> rotation matrix computed as tranverse of ZXZ bunge notation matrix
//according to https://math.stackexchange.com/questions/1821332/transformation-of-fourth-rank-tensor-and-its-matrix-form , mems devices page 259

Matrix<double,3,3> a;
a << cos(phi1)*cos(phi3)-sin(phi1)*sin(phi3)*cos(phi2), -cos(phi1)*sin(phi3)-sin(phi1)*cos(phi3)*cos(phi2), sin(phi1)*sin(phi2),
	  sin(phi1)*cos(phi3)+cos(phi1)*sin(phi3)*cos(phi2), cos(phi1)*cos(phi2)*cos(phi3)-sin(phi1)*sin(phi3) , -cos(phi1)*sin(phi2),
	  sin(phi2)*sin(phi3)				   , cos(phi3)*sin(phi2)			       , cos(phi2);

Matrix<double,6,6> a2;
a2 <<  pow(a(0,0),2), pow(a(0,1),2), pow(a(0,2),2), 2*a(0,1)*a(0,2), 2*a(0,2)*a(0,0), 2*a(0,0)*a(0,1),
	pow(a(1,0),2), pow(a(1,1),2), pow(a(1,2),2), 2*a(1,1)*a(1,2), 2*a(1,2)*a(1,0), 2*a(1,0)*a(1,1),
	pow(a(2,0),2), pow(a(2,1),2), pow(a(2,2),2), 2*a(2,1)*a(2,2), 2*a(2,2)*a(2,0), 2*a(2,0)*a(2,1),
	a(1,0)*a(2,0), a(1,1)*a(2,1), a(1,2)*a(2,2), a(1,1)*a(2,2)+a(1,2)*a(2,1), a(1,0)*a(2,2)+a(1,2)*a(2,0), a(1,1)*a(2,0)+a(1,0)*a(2,1),
	a(2,0)*a(0,0), a(2,1)*a(0,1), a(2,2)*a(0,2), a(0,1)*a(2,2)+a(0,2)*a(2,1), a(0,2)*a(2,0)+a(0,0)*a(2,2), a(0,0)*a(2,1)+a(0,1)*a(2,0),
	a(0,0)*a(1,0), a(0,1)*a(1,1), a(0,2)*a(1,2), a(0,1)*a(1,2)+a(0,2)*a(1,1), a(0,2)*a(1,0)+a(0,0)*a(1,2), a(0,0)*a(1,1)+a(0,1)*a(1,0);

Matrix<double,6,6> a2inverse;
a2inverse <<  pow(a(0,0),2), pow(a(1,0),2), pow(a(2,0),2), 2*a(1,0)*a(2,0), 2*a(2,0)*a(0,0), 2*a(0,0)*a(1,0),
	pow(a(0,1),2), pow(a(1,1),2), pow(a(2,1),2), 2*a(1,1)*a(2,1), 2*a(2,1)*a(0,1), 2*a(0,1)*a(1,1),
	pow(a(0,2),2), pow(a(1,2),2), pow(a(2,2),2), 2*a(1,2)*a(2,2), 2*a(2,2)*a(0,2), 2*a(0,2)*a(1,2),
	a(0,1)*a(0,2), a(1,1)*a(1,2), a(2,1)*a(2,2), a(1,1)*a(2,2)+a(2,1)*a(1,2), a(0,1)*a(2,2)+a(2,1)*a(0,2), a(1,1)*a(0,2)+a(0,1)*a(1,2),
	a(0,2)*a(0,0), a(1,2)*a(1,0), a(2,2)*a(2,0), a(1,0)*a(2,2)+a(2,0)*a(1,2), a(2,0)*a(0,2)+a(0,0)*a(2,2), a(0,0)*a(1,2)+a(1,0)*a(0,2),
	a(0,0)*a(0,1), a(1,0)*a(1,1), a(2,0)*a(2,1), a(1,0)*a(2,1)+a(2,0)*a(1,1), a(2,0)*a(0,1)+a(0,0)*a(2,1), a(0,0)*a(1,1)+a(1,0)*a(0,1);	 

Matrix<double,6,6> elastic_coeff;
elastic_coeff << c11, c12, c12, 0, 0, 0,
		 c12, c11, c12, 0, 0, 0,
		 c12, c12, c11, 0, 0, 0,
		 0,   0,   0, c44, 0, 0,
		 0,   0,   0, 0, c44, 0,
		 0,   0,   0, 0, 0, c44;

Matrix<double,6,6> elastic_coeff_rot;
elastic_coeff_rot = a2*elastic_coeff*a2inverse;
//end of this section, the newly acquired elastric strain coefficients are in the 6x6 matrix elastic_coeff_rot






//some EBSD intial setup, location of the pattern center, location of segments, shift projection correction matrix
vector<double> r1(num_seg);
vector<double> r2(num_seg);
vector<double> r3(num_seg);

MatrixXd Coeff(num_seg*2,8);
vector<Matrix<double, 3, 3>> projectionM(num_seg);
for (int i=0; i<num_seg; i++){
	
		double ys;
		double xs;
		double zs;
		
	switch(system){
		case 1://oxford
		ys = ( points(i,1) + seg_size/2 - (1-ryp)*det_x_pix )*pix_size;//y position of segment center relative to PC on the fluorescent screen in METRES
		xs = -( (points(i,0) + seg_size/2) - rxp*det_x_pix )*pix_size;//x position of segment center relative to PC on the fluorescent screen in METRES, negative because of the mirror transformation
		zs = (rzp*det_x_pix)*pix_size;
		break;
		
		case 2://bruker
		ys = ( points(i,1) + seg_size/2 - ryp*det_y_pix )*pix_size;
		xs = -( (points(i,0) + seg_size/2) - rxp*det_x_pix )*pix_size;
		zs = (rzp*det_y_pix)*pix_size;
		break;
		
		case 3://edax
		ys = ( points(i,1) + seg_size/2 - ryp*det_y_pix )*pix_size;
		xs = -( (points(i,0) + seg_size/2) - rxp*det_x_pix )*pix_size;
		zs = (rzp*det_y_pix)*pix_size;
		break;
	}
	
	double D = pow(ys*ys + xs*xs + zs*zs, 0.5);
	double D3 = pow(ys*ys + xs*xs + zs*zs, 1.5);
		
	projectionM[i] << ys*ys+zs*zs, -ys*xs, 0,
			 -ys*xs, xs*xs+zs*zs, 0,
			 -xs*zs, -ys*zs, 0; 
	
	projectionM[i] = projectionM[i]*zs/D3;	//final projection matrix correction, unitless

	//cout << xs << " " << ys << " " << zs << "\n";
	
	//recalculating the sample-screen vector to sample-projection sphere, only magnitude changes, METRES
	double y = ys * zs/D;
	double x = xs * zs/D;
	double z = zs * zs/D; 
	
	//transformation to sample coordinate system as defined by the oxford/edax/bruker software
	r1[i] = -x;
	r2[i] = cos(M_PI*(90-tilt_angle)/180)*(-1*y) - sin(M_PI*(90-tilt_angle)/180)*zs;
	r3[i] = sin(M_PI*(90-tilt_angle)/180)*(-1*y) + cos(M_PI*(90-tilt_angle)/180)*zs;

	//prepare the Coeff matrix for LLS, different for each segment
	Coeff(2*i,0) = r2[i]*r3[i];
	Coeff(2*i+1,0) = 0;
	Coeff(2*i,1) = r1[i]*r3[i];
	Coeff(2*i+1,1) = 0;
	Coeff(2*i,2) = r3[i]*r3[i];
	Coeff(2*i+1,2) = 0;
	Coeff(2*i,3) = -r1[i]*r2[i];
	Coeff(2*i+1,3) = -r1[i]*r1[i];
	Coeff(2*i,4) = -r2[i]*r2[i];
	Coeff(2*i+1,4) = -r2[i]*r1[i];
	Coeff(2*i,5) = 0;
	Coeff(2*i+1,5) = r1[i]*r3[i];
	Coeff(2*i,6) = 0;
	Coeff(2*i+1,6) = r2[i]*r3[i];
	Coeff(2*i,7) = 0;
	Coeff(2*i+1,7) = r3[i]*r3[i];
}


//construct windowing function
//MatrixXd window(seg_size, seg_size);
//for(int i=0; i< seg_size; ++i){
//	for(int j=0; j< seg_size; ++j){
//		window(i,j) = 1*cos(M_PI*(i-seg_size/2)/seg_size)*cos(M_PI*(j-seg_size/2)/seg_size);
//	}
//}

//construct low pass filter
//array<double, seg_size*(seg_size/2+1)> LPF;
//for(int i=0; i< seg_size; ++i){
//	for(int j=0; j< (seg_size/2+1); ++j){
//		if( sqrt((i-128)*(i-128)+(j-128)*(j-128))<range){
//			LPF[i*(seg_size/2+1) + j] = 1;
//		}
//		else{
//			LPF[i*(seg_size/2+1) + j] = 0;
//		}
//	}
//}


//variables shared between the cores, used for input
vector<fftw_complex [seg_size][(seg_size/2+1)]> vzor_FFT(num_seg); //array of FFT of each segment of the reference image
int counter=0; //progress tracker
MatrixXd strain_tensor(6,num_points);//will hold the strain tensor for every pixel
MatrixXd rotations(3,num_points); //will hold the rotations for every pixel
MatrixXd coeff_determination(rows, columns);//will hold the certainty of LLS for every pixel


//reference image loading must be done before paralelization so that each core has access to the background
//load the reference image and perform dynamic background correction + denoising;
Mat vzor;
imread(path_ref,-1).convertTo(vzor, CV_32F);
Mat background;
background = vzor.clone();//make a copy to blur
int blur_step = 30;
for(int i=0; i<blur_step; i++){
	blur(background,background,Size(51,51)); // blur in 30 steps
}
divide(vzor,background,vzor); //dynamic background
GaussianBlur(vzor,vzor,Size(15,15),0); //denoising
vzor *= 40000; //better find max and upscale to 65535
//end of load the reference image and perform dynamic background correction + denoising;


//fftw pointers, allocating memory, each core uses its own
#pragma omp parallel
{
//variables for image loading
Mat image;
Matrix <double, Dynamic, Dynamic, RowMajor> image_cutM(seg_size, seg_size); //eigen segment
stringstream sj;
stringstream si;
stringstream nub;

//fftw objects
double * pimagef;
fftw_complex * pimageF;
fftw_plan forward;
fftw_plan backward;
fftw_complex * phase_cor;
double * corr_output;

pimagef = (double*) fftw_malloc( sizeof(double)*seg_size*seg_size );
pimageF = (fftw_complex*) fftw_malloc( sizeof(fftw_complex)*seg_size*(seg_size/2+1) );
phase_cor = (fftw_complex*) fftw_malloc( sizeof(fftw_complex)*seg_size*(seg_size/2+1) );
corr_output = (double*) fftw_malloc( sizeof(double)*seg_size*seg_size );

#pragma omp critical
{
forward = fftw_plan_dft_r2c_2d(seg_size, seg_size, pimagef, pimageF, FFTW_MEASURE);
backward = fftw_plan_dft_c2r_2d(seg_size, seg_size, phase_cor, corr_output, FFTW_MEASURE);
}

//eigen matrices for selection of central-most image area
double * pcenterROI;
pcenterROI = (double*) malloc ( sizeof(double)*seg_size*seg_size);
Matrix <double, Dynamic, Dynamic, RowMajor> center(seg_size, seg_size);

//pattern shift registration
int pos_maxy, pos_maxx;
double maxCoeff;
double sum_mx, sum_my, sum_m;
double maxx, maxy, shifti, shiftj;
double shiftxs, shiftys, shiftzs;

//preparing matrix for the LLS eigensolver
MatrixXd Prod(num_seg*2,1);
MatrixXd pred_Prod(num_seg*2,1);
double sum_square_dif;
double sum_square_total;
double R2;
MatrixXd Xs;
int cscanp;






#pragma omp single
{
Matrix <double, Dynamic, Dynamic, RowMajor> vzor_cutM(seg_size, seg_size); //matrix of each segment image data



//load, convert and calculate FFT for each segment of the reference image

for(int i=0; i<num_seg; i++){ 
	
	cv2eigen( vzor( Rect(points(i,0), points(i,1), seg_size, seg_size) ) , vzor_cutM); //converting to eigen
	vzor_cutM.array() -= ( vzor_cutM.sum() / pow(seg_size,2)); //setting mean to zero
	//vzor_cutM = vzor_cutM.cwiseProduct( window ); //applying the windowing function
	
	//cant simply link pimagef, because planning uses physically the memory
	memcpy(pimagef, vzor_cutM.data(), sizeof(double)*seg_size*seg_size);
	//computing the FFT
	fftw_execute(forward);
	//saving
	memcpy(&vzor_FFT[i], pimageF, sizeof(fftw_complex)*seg_size*(seg_size/2+1) );

	//show me the fft
	//Matrix<double, seg_size, seg_size/2 + 1> show_fft;
	//for(int i=0; i<seg_size; ++i){
	//	for(int j=0; j<(seg_size/2+1); ++j){
	//		show_fft(i,j) = LPF[i*(seg_size/2+1)+j]*sqrt( pvzorF[i*(seg_size/2+1) + j][0]*pvzorF[i*(seg_size/2+1) + j][0] + pvzorF[i*(seg_size/2+1) + j][1]*pvzorF[i*(seg_size/2+1) + j][1] )/seg_size;
	//	}
	//}
}


}//pragma omp single

#pragma omp for
for (int i=0; i<columns; i++){
	for(int j=0; j<rows; j++){

	si.str("");
	sj.str("");
	si.clear();
	sj.clear();

	sj << j;
	si << i;

	//image loading and background correction + denosing by Gaussian filtering
	//cout << path_obs + beginning + sj.str() + middle + si.str() + ending << "\n";
	imread(path_obs + beginning + sj.str() + middle + si.str() + ending,-1).convertTo(image, CV_32F);
	divide(image,background,image);
	GaussianBlur(image,image,Size(15,15),0);
	image *= 40000;
	//end of image loading

	//for each segment
	for(int c=0; c<num_seg; c++){

		//cut out segment position from loaded image, convert to eigen matrix
		cv2eigen( image( Rect(points(c,0), points(c,1), seg_size, seg_size) ) , image_cutM); //segmenting & converting to eigen
		image_cutM.array() -= ( image_cutM.sum() / pow(seg_size,2)); //setting mean to zero
		//image_cutM = image_cutM.cwiseProduct( window ); //applying the windowing function

		//linking and computing the FFT
 		memcpy(pimagef, image_cutM.data(), sizeof(double)*seg_size*seg_size);
		fftw_execute(forward);
		
		//phase correlation
		for (int i=0; i<seg_size; i++){
			for (int j=0; j<(seg_size/2+1); j++){
				phase_cor[i*(seg_size/2+1)+j][0] =( vzor_FFT[c][i][j][0]*pimageF[i*(seg_size/2+1)+j][0]+vzor_FFT[c][i][j][1]*pimageF[i*(seg_size/2+1)+j][1] )*pow(-1,i+j);// / pow(  ( pow(vzor_FFT[c][i][j][0],2)+pow(vzor_FFT[c][i][j][1],2) ) * ( pow(pimageF[i*(seg_size/2+1)+j][0],2)+pow(pimageF[i*(seg_size/2+1)+j][1],2) ) ,0.5);//*LPF[i*(seg_size/2+1)+j];
		
				phase_cor[i*(seg_size/2+1)+j][1] =( pimageF[i*(seg_size/2+1)+j][0]*vzor_FFT[c][i][j][1]-vzor_FFT[c][i][j][0]*pimageF[i*(seg_size/2+1)+j][1] )*pow(-1,i+j);// / pow(  ( pow(vzor_FFT[c][i][j][0],2)+pow(vzor_FFT[c][i][j][1],2) ) * ( pow(pimageF[i*(seg_size/2+1)+j][0],2)+pow(pimageF[i*(seg_size/2+1)+j][1],2) ) ,0.5);//*LPF[i*(seg_size/2+1)+j];

				//originally used phase transform, but too noise-sensitive, also no filtering in FFT space at the end
			}
		}
		
		//get reverse FFT phase corellation
		fftw_execute(backward);

		//extract ROI of the center area - middle is at 128x128 size of 15x15  first take 15rows 256 columns of the center, then convert that to ColumnMajor eigen and take 15 columns 15row
		
		memcpy(pcenterROI, corr_output, sizeof(double)*seg_size*seg_size);
		center = Map<Matrix <double, seg_size, seg_size, RowMajor> >(pcenterROI,seg_size,seg_size);	

		
		//maximum value finder, only in square of size ROI_center_size for speed
		int start = seg_size/2 - ROI_center_size;
		int end = seg_size/2 + ROI_center_size + 1; 
		

		maxCoeff=0;
		for(int i=start; i<end; i++){
			for (int j=start; j < end; j++){	
				if(center(i,j) > maxCoeff){
					maxCoeff = center(i,j);
					pos_maxy = i;
					pos_maxx = j;
				}
			}

		}			
		
		
		//interpolation using "center of mass" but with square of each coefficient
		sum_mx = 0;
		sum_my = 0;		
		sum_m = 0;
		for(int y=(pos_maxy-ROI_subpixel_size/2); y <(pos_maxy+ROI_subpixel_size/2+1); y++){
			for (int x=(pos_maxx-ROI_subpixel_size/2); x <(pos_maxx+ROI_subpixel_size/2+1); x++){
				sum_mx = sum_mx + x * center(y,x)* center(y,x);
				sum_my = sum_my + y * center(y,x)* center(y,x);
				sum_m = sum_m + center(y,x)* center(y,x);
				}
		}
		maxx = sum_mx / sum_m;  
		maxy = sum_my / sum_m;
		shifti = -maxx + seg_size/2; //units in pixels
		shiftj = -maxy + seg_size/2; //if pattern shifted to the left, corr peak will be on the right
		
		cout << shifti << " " << shiftj << "\n";
		//end of subpixel shift as center of mass


		
		//visualization of the phase correlation image
		//center = (center.array() - center.minCoeff());
		//center = center*255/center.maxCoeff();	
		//Mat show_IFFT;
		//eigen2cv(center, show_IFFT);
		//show_IFFT.convertTo(show_IFFT,8U);
		//nub << c;
		//imwrite("ROI" + sj.str() + "_"  + si.str() + "seg" + nub.str() + ".tiff", show_IFFT); 
			

		
		//old system - with shiftM containing corr_output - location of the maxCoeFF
		//int maxi, maxj;
		//subpixel shift according to Faroosh et al 2008
		//double shifti, shiftj;
		//shifti = pos_maxx - seg_size/2 + center(pos_maxx+1, pos_maxy)/(maxCoeff + abs(center(pos_maxx+1, pos_maxy))) - maxCoeff/(maxCoeff + abs(center(pos_maxx-1, pos_maxy)));
		//shiftj = pos_maxy - seg_size/2 + center(pos_maxx, pos_maxy+1)/(maxCoeff + abs(center(pos_maxx, pos_maxy+1))) - maxCoeff/(maxCoeff + abs(center(pos_maxx, pos_maxy-1)));
		

		Matrix<double,3,1>shiftA;
		shiftA << -1*shifti, shiftj, 0; //rs in pixels
		shiftA = shiftA * pix_size; //in METRES
		shiftA = projectionM[c] * shiftA;


		//shift conversion to sample coordinate system
		shiftxs = -shiftA(0);
		shiftys = cos(M_PI*(90-tilt_angle)/180)*(-1*shiftA(1))-sin(M_PI*(90-tilt_angle)/180)*shiftA(2);
		shiftzs = sin(M_PI*(90-tilt_angle)/180)*(-1*shiftA(1))+cos(M_PI*(90-tilt_angle)/180)*shiftA(2);

		//prepare Prod with results for this image
		Prod(2*c) = r3[c]*shiftys - r2[c]*shiftzs; 
		Prod(2*c+1) = r3[c]*shiftxs - r1[c]*shiftzs;
		
		
	}//for each segment of each image
	
	Xs = Coeff.bdcSvd(ComputeFullU | ComputeFullV).solve(Prod); //compute the LLS
	//cout << Xs << "\n";
	
	//get R2 for each LLS
	pred_Prod = Coeff * Xs;
	sum_square_dif = (Prod - pred_Prod).array().square().sum();
	Prod.array() -=  Prod.sum()/(2*num_seg); 
	sum_square_total = Prod.array().square().sum();
	R2 = 1-sum_square_dif / sum_square_total;
	//end of R2
	

	//current scan point
	cscanp = j*columns + i;

	//saving the results
	//exx ~ e0, eyy ~ e1, ezz ~ e2, eyz ~ e3, exz ~ e4, exy ~ e5
	strain_tensor(3,cscanp) = (Xs(2) + Xs(4))/2; //eyz
	strain_tensor(4,cscanp) = (Xs(7) + Xs(3))/2; //exz
	strain_tensor(5,cscanp) = (Xs(6) + Xs(1))/2; //exy
	strain_tensor(2,cscanp) = -(elastic_coeff_rot(2,0)*Xs(5) + elastic_coeff_rot(2,1)*Xs(0) + 2*elastic_coeff_rot(2,3)*strain_tensor(3,i) + 2*elastic_coeff_rot(2,4)*strain_tensor(4,cscanp) + 2*elastic_coeff_rot(2,5)*strain_tensor(5,i))/(elastic_coeff_rot(2,0)+elastic_coeff_rot(2,1)+elastic_coeff_rot(2,2)); //ezz
	strain_tensor(1,cscanp) = Xs(0) + strain_tensor(2,cscanp); //eyy
	strain_tensor(0,cscanp) = Xs(5) + strain_tensor(2,cscanp); //exx

	// R_x ~ rotations(0,..) R_y ~ rotations(1,..), R_z = rotations(2,..);
	rotations(0,cscanp) = -(Xs(2)-Xs(4))/2; //phi_x in radians
	rotations(1,cscanp) = (Xs(7)-Xs(3))/2; //phi_y in radians
	rotations(2,cscanp) = -(Xs(6)-Xs(1))/2; //phi_z in radians

	//coeff_determination
	coeff_determination(j,i) = R2;
	//end of result saving
	
	//output current progress
	counter++;
	cout << "Completed " << counter << "/" << num_points << "\n";
	
	}//for each column

}//for each row


}//omp parallel


//converting strain_tensor and rotations to colored images, also resizing to be the same as the original scan in terms of number of rows and columns

Matrix<double, Dynamic, Dynamic, RowMajor> result;
Mat result_grayscale(rows, columns, CV_8U);




//Output into .txt file, with header containing info for gnuplot and some very basic attempt at a scale bar
array<string,6> name_strain {"e_{xx}", "e_{yy}", "e_{zz}", "e_{yz}", "e_{xz}", "e_{xy}"};
array<string,3> name_rot {"phi_x", "phi_y", "phi_z"};
for(int i=0; i<6; ++i){

	ofstream gnnplot;
	stringstream num;
	num << i; 
	gnnplot.open(path_res + "strain_tensor_" + name_strain[i] + ".txt");
	result = strain_tensor.row(i);
	result.resize(rows, columns);
	stringstream max,min,srows,scolumns;
	max << result.maxCoeff();
	min << result.minCoeff();
	srows << rows-0.5+2;
	scolumns << columns-0.5;


	gnnplot << "reset session\n";
	gnnplot << "set title 'Strain " + name_strain[i] +" [-]'\n";
	gnnplot << "unset key\n";
	gnnplot << "set tic scale 0\n";
	gnnplot << "# Color runs from white to green\n";
	gnnplot << "set palette defined ( 0 'blue', 1 'red' )\n";
	gnnplot << "set cbrange ["+ min.str() + ":" + max.str() + "]\n";
	gnnplot << "set size ratio -1\n";
	gnnplot << "set xrange [-0.5:" + scolumns.str() + "]\n";
	gnnplot << "set yrange [" + srows.str() + ":-0.5] reverse\n";	
	gnnplot << "set format xy ''\n";
	gnnplot << "set view map\n";
	gnnplot << "set arrow from 3," + srows.str() +"-1 to 15,"+ srows.str() +"-1 nohead front lw 2\n";
	gnnplot << "set for [i=0:4] arrow from 3+i*12/4," + srows.str() + "-0.5 to 3+i*12/4," + srows.str() + "-1.5 nohead front lw 1.2\n";
	gnnplot << "set label '" + sstepsize.str() + " nm'  at  16," + srows.str() + "-1 front \n";
	gnnplot << "splot '-' matrix with image\n\n" ;
	gnnplot << result;
	
	gnnplot.close();

	GnuplotPipe gp;
    	gp.sendLine("load '"+path_res+"strain_tensor_" + name_strain[i] + ".txt'");

}
for(int i=0; i<3; ++i){
	
	ofstream gnnplot;
	stringstream num;
	num << i; 
	gnnplot.open( path_res + "rotation_" + name_rot[i] + ".txt");
	result = rotations.row(i);
	result.resize(rows, columns);
	stringstream max,min,srows,scolumns;
	max << result.maxCoeff();
	min << result.minCoeff();
	srows << rows-0.5+2;
	scolumns << columns-0.5;


	gnnplot << "reset session\n";
	gnnplot << "set title 'Rotation " + name_rot[i] + " [rad]'\n";
	gnnplot << "unset key\n";
	gnnplot << "set tic scale 0\n";
	gnnplot << "# Color runs from white to green\n";
	gnnplot << "set palette defined ( 0 'blue', 1 'green' )\n";
	gnnplot << "set cbrange ["+ min.str() + ":" + max.str() + "]\n";
	gnnplot << "set size ratio -1\n";
	gnnplot << "set xrange [-0.5:" + scolumns.str() + "]\n";
	gnnplot << "set yrange [" + srows.str() + ":-0.5] reverse\n";	
	gnnplot << "set format xy ''\n";
	gnnplot << "set view map\n";
	gnnplot << "set arrow from 3," + srows.str() +"-1 to 15,"+ srows.str() +"-1 nohead front lw 2\n";
	gnnplot << "set for [i=0:4] arrow from 3+i*12/4," + srows.str() + "-0.5 to 3+i*12/4," + srows.str() + "-1.5 nohead front lw 1.2\n";
	gnnplot << "set label '" + sstepsize.str() + " nm'  at  16," + srows.str() +"-1 front \n";
	gnnplot << "splot '-' matrix with image\n\n" ;
	gnnplot << result;
	
	gnnplot.close();

	GnuplotPipe gp;
    	gp.sendLine("load '"+path_res+"rotation_" + name_rot[i] + ".txt'");
}


	//R2
	ofstream gnnplot;
	gnnplot.open(path_res + "coeff_determination.txt");
	stringstream max,min,srows,scolumns;
	max << coeff_determination.maxCoeff();
	min << coeff_determination.minCoeff();
	srows << rows-0.5+2;
	scolumns << columns-0.5;


	gnnplot << "reset session\n";
	gnnplot << "set title 'Confidence index of the LLS solver'\n";
	gnnplot << "unset key\n";
	gnnplot << "set tic scale 0\n";
	gnnplot << "# Color runs from white to green\n";
	gnnplot << "set palette grayscale\n";
	gnnplot << "set cbrange ["+ min.str() + ":" + max.str() + "]\n";
	gnnplot << "set size ratio -1\n";
	gnnplot << "set xrange [-0.5:" + scolumns.str() + "]\n";
	gnnplot << "set yrange [" + srows.str() + ":-0.5] reverse\n";	
	gnnplot << "set format xy ''\n";
	gnnplot << "set view map\n";
	gnnplot << "set arrow from 3," + srows.str() +"-1 to 15,"+ srows.str() +"-1 nohead front lw 2\n";
	gnnplot << "set for [i=0:4] arrow from 3+i*12/4," + srows.str() + "-0.5 to 3+i*12/4," + srows.str() + "-1.5 nohead front lw 1.2\n";
	gnnplot << "set label '" + sstepsize.str() + " nm'  at  16," + srows.str() +"-1 front \n";
	gnnplot << "splot '-' matrix with image\n\n" ;
	gnnplot << coeff_determination;
	
	gnnplot.close();

	GnuplotPipe gp;
    	gp.sendLine("load '"+path_res+"coeff_determination.txt'");




//end of main
}
