// Compute 2D Polynomial Fit function. This function fits an polynomial to map the relationship between
// two sets of 2D data. 

// D. R. G. Mitchell, adminnospam@dmscripting.com (remove the nospam to make this address work)
// version:20190905, v1.0, September 2019, www.dmscripting.com


// The function takes as the following arguments
// Input data - an array of data points in the following format:

// [x1, y1, x2, y2 . . xn, yn
// xn+1, yn+1 etc. 
// . . . . . . . . .xfinal, yfinal]

// These are the data being input into the system. In the example here, they are a series of coordinates from a STEM
// scan - being symmetric about the zero position.

// Ouputdata - this is an array of similar format to that above. It contains the corresponding outputs from the system.
// In this case the positions of the STEM (000) beam produced by the aforementioned STEM scan. Due to lens aberrations
// there is not a linear correlation between the two - hence the need for a polynomial fit.

// Inputdata and Outputdata must have the same dimensions.

// This function fits a polynomial to map the relationship between the two data sets. 

// i'= a0+a1*i+a2*j+a3*i^2+a4*i*j+a5*j^2
// j'= b0+b1*i+b2*j+b3*i^2+b4*i*j+b5*j^2

// Here i' and j' are the x and y coordinates respectively of the Outputdata, i and j are the are the x and y
// coordinates of the Inputdata and a0 - a5, and b0 - b5 are the polynomial coefficients of the fit. Higher order
// polynomial fits can produce erratic results with noisy data and here you will see the agreement is OK near the
// the centre of the plots, but in the corners, where the aberrations are most strongly felt, the deviations are significant. 

// This type of analysis might be useful for calibrations, where you need to understand/predict how an input signal
// will affect an output signal. 

//  The function uses a Pseudo-inverse matrix method to compute the polynomial coefficients producing the best
// fit to the data. These coefficients are returned (by reference) to the two passed in images acoefficients and
// bcoefficients. These images are one pixel wide and 6 tall, and contain the a0 - a5 and b0 - b5 values,, top to bottom.

// The function returns 1 if the calculation was successful and 0 if a problem was encountered.

// Use the returned coefficient values in the above expression to calculate Outputdata from input data.


// Function definition

number Fit2DPolynomialFunction(image inputdata, image outputdata, image &acoefficients, image &bcoefficients)
	{
		// Get the size of the input and output images passed in and make sure they are the same size
		
		number inxsize, inysize, outxsize, outysize
		getsize(inputdata, inxsize, inysize)
		getsize(outputdata, outxsize, outysize)

		if(inxsize!=outxsize && inysize!=outysize) 
			{
				showalert("inputdata and outputdata images are not the same size.",2)
				return 0
			}
					
		number nopoints=(inxsize/2)*inysize // Note half of inputxsize, due to x1,y1, x2, y2 . . 


		// Place the input and output data into a single data image for convenience. There are four columns
		// inputx, inputy, outputx, outputy and these run vertically, first point to last

		image alldataincolumns=realimage("", 4, 4, nopoints)

		number x, y, counter=0
		
		for(y=0; y<inysize; y++)
			{
				for(x=0; x<inxsize; x=x+2) // note x columns are seperated by y columns
					{
						number outputx=getpixel(outputdata, x, y)
						setpixel(alldataincolumns, 0, counter, outputx)
						
						number outputy=getpixel(outputdata, x+1, y)
						setpixel(alldataincolumns, 1, counter, outputy)

						number inputx=getpixel(inputdata, x, y)
						setpixel(alldataincolumns, 2, counter, inputx)

						number inputy=getpixel(inputdata, x+1, y)
						setpixel(alldataincolumns, 3, counter, inputy)
						counter=counter+1
					}
			}


		// The data can be fitted to polynomials which have the form:

		// i'= a0+a1*i+a2*j+a3*i^2+a4*i*j+a5*j^2
		// j'= b0+b1*i+b2*j+b3*i^2+b4*i*j+b5*j^2
		
		// where i' and j' are the reciprocal x and y coordinates, i and j are the real x and y coordinates and a0 - a5
		// and b0 - b5 are the polynomial constants

		// see doi:10.4028/www.scientific.net/AMR.628.403 for more information on this method.

		// The constants have to be solved for, and this is done using a pseudo-inverse matrix technique. Non-square matrices
		// mean that normal matrix inversion can not be done - matrices are non-invertible.


		// Create a matrix containing the polynomial factors
		// the rows are [1, inputx, inputy, inputx^2, inputx*inputy, inputy^2]

		image polymatrix=realimage("", 4, 6, nopoints)

		counter=0

		for(y=0; y<inysize; y++)
			{
				for(x=0; x<inxsize; x=x+2)
					{
						number inputx=getpixel(alldataincolumns, 2, counter)
						number inputy=getpixel(alldataincolumns, 3, counter)
						
						setpixel(polymatrix, 0, counter, 1)
						setpixel(polymatrix, 1, counter, inputx)
						setpixel(polymatrix, 2, counter, inputy)
						setpixel(polymatrix, 3, counter, inputx*inputx)
						
						setpixel(polymatrix, 4, counter, inputx*inputy)
						setpixel(polymatrix, 5, counter, inputy*inputy)
						
						counter=counter+1
					}
			}


		// Use a try/catch loop in case there are matrix problems

		try
			{

				// Compute the a and b parameters of the polynomial (in order to calculate the output x and y positions) 
				// using the pseudo-inverse matrix method (A^T.A)^-1.A^T gives the pseudo-inverse matrix of A

				image transposematrix=matrixtranspose(polymatrix)
				image scattermatrix=matrixmultiply(transposematrix, polymatrix)
				image inversescatter=matrixinverse(scattermatrix)
				image pseudoinvmatrix=matrixmultiply(inversescatter, transposematrix)


				// Use the pseudo-inverse matrix to calculate the a and b parameters of the polynomials from the reciprocal data

				acoefficients=matrixmultiply(pseudoinvmatrix, alldataincolumns[0,0,nopoints,1])// the input x values
				bcoefficients=matrixmultiply(pseudoinvmatrix, alldataincolumns[0,1,nopoints,2]) // the input y values
			}
		catch return 0

		return 1
	}



// Main script

// This is an image array which describes a set of input coordinates. These are x, y, x1, y1 etc coordinates of a DigiScan
// scan at 20kx. 

image inputdata:=[22,11]:
	{
		{-4252,-4252,-3401.6,-4252,-2551.2,-4252,-1700.8,-4252,-850.4,-4252,2.27374e-013,-4252,850.4,-4252,1700.8,-4252,2551.2,-4252,3401.6,-4252,4252,-4252},
		{-4252,-3401.6,-3401.6,-3401.6,-2551.2,-3401.6,-1700.8,-3401.6,-850.4,-3401.6,2.27374e-013,-3401.6,850.4,-3401.6,1700.8,-3401.6,2551.2,-3401.6,3401.6,-3401.6,4252,-3401.6},
		{-4252,-2551.2,-3401.6,-2551.2,-2551.2,-2551.2,-1700.8,-2551.2,-850.4,-2551.2,2.27374e-013,-2551.2,850.4,-2551.2,1700.8,-2551.2,2551.2,-2551.2,3401.6,-2551.2,4252,-2551.2},
		{-4252,-1700.8,-3401.6,-1700.8,-2551.2,-1700.8,-1700.8,-1700.8,-850.4,-1700.8,2.27374e-013,-1700.8,850.4,-1700.8,1700.8,-1700.8,2551.2,-1700.8,3401.6,-1700.8,4252,-1700.8},
		{-4252,-850.4,-3401.6,-850.4,-2551.2,-850.4,-1700.8,-850.4,-850.4,-850.4,2.27374e-013,-850.4,850.4,-850.4,1700.8,-850.4,2551.2,-850.4,3401.6,-850.4,4252,-850.4},
		{-4252,2.27374e-013,-3401.6,2.27374e-013,-2551.2,2.27374e-013,-1700.8,2.27374e-013,-850.4,2.27374e-013,2.27374e-013,2.27374e-013,850.4,2.27374e-013,1700.8,2.27374e-013,2551.2,2.27374e-013,3401.6,2.27374e-013,4252,2.27374e-013},
		{-4252,850.4,-3401.6,850.4,-2551.2,850.4,-1700.8,850.4,-850.4,850.4,2.27374e-013,850.4,850.4,850.4,1700.8,850.4,2551.2,850.4,3401.6,850.4,4252,850.4},
		{-4252,1700.8,-3401.6,1700.8,-2551.2,1700.8,-1700.8,1700.8,-850.4,1700.8,2.27374e-013,1700.8,850.4,1700.8,1700.8,1700.8,2551.2,1700.8,3401.6,1700.8,4252,1700.8},
		{-4252,2551.2,-3401.6,2551.2,-2551.2,2551.2,-1700.8,2551.2,-850.4,2551.2,2.27374e-013,2551.2,850.4,2551.2,1700.8,2551.2,2551.2,2551.2,3401.6,2551.2,4252,2551.2},
		{-4252,3401.6,-3401.6,3401.6,-2551.2,3401.6,-1700.8,3401.6,-850.4,3401.6,2.27374e-013,3401.6,850.4,3401.6,1700.8,3401.6,2551.2,3401.6,3401.6,3401.6,4252,3401.6},
		{-4252,4252,-3401.6,4252,-2551.2,4252,-1700.8,4252,-850.4,4252,2.27374e-013,4252,850.4,4252,1700.8,4252,2551.2,4252,3401.6,4252,4252,4252}
	}

// This is the corresponding array of output coordinates - the position of the (000) spot as a function of the above input scan

image outputdata:=[22,11]:
	{
		{29.8136,-10.4224,30.7602,-7.94396,30.9279,-4.52092,30.3943,-0.243581,29.2822,4.30668,27.7096,9.01559,25.65,13.3932,23.2181,17.2621,20.7407,20.1159,18.0103,21.9084,15.3601,22.1441},
		{27.7755,-13.1839,28.146,-10.5917,27.9006,-6.83603,27.0014,-2.2796,25.6636,2.63384,23.8273,7.75578,21.7286,12.5721,19.2992,16.9211,16.9545,20.2789,14.5798,22.4653,12.2252,23.2413},
		{24.5221,-15.9011,24.2902,-13.1144,23.644,-9.21734,22.3554,-4.54512,20.7611,0.744868,18.7796,6.10637,16.6793,11.299,14.2997,15.9488,12.1108,19.739,10.0429,22.3451,8.15743,23.3854},
		{20.1568,-18.4167,19.5801,-15.5918,18.3399,-11.6004,16.8455,-6.75908,14.972,-1.39193,12.9211,4.20432,10.8091,9.58791,8.67584,14.4861,6.70211,18.5173,4.90864,21.4045,3.40155,23.0191},
		{15.2284,-20.4104,14.0687,-17.6683,12.5783,-13.7924,10.7304,-8.96009,8.68649,-3.54758,6.59687,2.13027,4.38033,7.70779,2.54759,12.6696,0.779623,16.9096,-0.640702,20.1162,-1.41805,21.6551},
		{10.0089,-21.8441,8.30863,-19.3172,6.47873,-15.6596,4.30082,-10.958,2.14017,-5.65153,0,0,-1.95758,5.5293,-3.75131,10.6408,-5.09081,14.8938,-6.0337,18.0465,-6.38208,19.8917},
		{4.87928,-22.696,2.76398,-20.5519,0.479574,-17.091,-1.86718,-12.7044,-4.10515,-7.59735,-6.21009,-2.14219,-8.07011,3.28415,-9.5455,8.25601,-10.5446,12.4656,-11.0814,15.7169,-10.7854,17.597},
		{0.097096,-22.7854,-2.34511,-21.0582,-4.9365,-18.0357,-7.39276,-13.9936,-9.79841,-9.20293,-11.8507,-4.07137,-13.4828,1.05441,-14.7267,5.92045,-15.3017,9.96808,-15.2358,13.0158,-14.468,14.7287},
		{-3.8427,-22.0982,-6.65831,-20.8768,-9.37627,-18.3838,-12.0375,-14.7806,-14.3907,-10.4535,-16.3688,-5.74844,-17.8787,-0.935846,-18.7568,3.56241,-18.9583,7.3528,-18.3686,10.1523,-17.0129,11.7571},
		{-6.54386,-20.4793,-9.73599,-19.8543,-12.7099,-17.9451,-15.3802,-14.9492,-17.7529,-11.193,-19.583,-6.9927,-20.8618,-2.69663,-21.3841,1.31095,-21.2053,4.73829,-20.0521,7.26058,-17.9627,8.56994},
		{-7.96862,-17.9804,-11.2833,-18.0143,-14.4438,-16.7233,-17.1762,-14.3987,-19.4488,-11.2958,-21.1194,-7.72068,-22.1718,-4.05167,-22.3267,-0.597405,-21.6493,2.38607,-19.9893,4.43133,-17.3932,5.24699}
	}
	
	
// Create arrays in which to store the polynomial coefficients

image acoefficients, bcoefficients


// Get the size of the input data array

number inxsize, inysize
getsize(inputdata, inxsize, inysize)
number nopoints=(inxsize/2)*inysize


// Call the function 

number functionresult=Fit2DPolynomialFunction(inputdata, outputdata, acoefficients, bcoefficients)

if(functionresult==0)
	{
		showalert("The fitting failed.",2)
		exit(0)
	}


// Extract the six a parameters (a0, a1, a2 etc) from the acoeffmatrix

number a0, a1, a2, a3, a4, a5
a0=getpixel(acoefficients, 0,0)
a1=getpixel(acoefficients, 0,1)
a2=getpixel(acoefficients, 0,2)
a3=getpixel(acoefficients, 0,3)
a4=getpixel(acoefficients, 0,4)
a5=getpixel(acoefficients, 0,5)


// Extract the six b parameters

number b0, b1, b2, b3, b4, b5
b0=getpixel(bcoefficients, 0,0)
b1=getpixel(bcoefficients, 0,1)
b2=getpixel(bcoefficients, 0,2)
b3=getpixel(bcoefficients, 0,3)
b4=getpixel(bcoefficients, 0,4)
b5=getpixel(bcoefficients, 0,5)


// Use the known input coordinates to calculate the output coords using the polynomial fits derived above

// Place the input and output data into a single data image for convenience. There are four columns
// inputx, inputy, outputx, outputy and these run vertically, first point down to last


image alldataincolumns=realimage("", 4, 4, nopoints)

number x, y, counter=0
		
for(y=0; y<inysize; y++)
	{
		for(x=0; x<inxsize; x=x+2) // note x columns are seperated by y columns
			{
				number outputx=getpixel(outputdata, x, y)
				setpixel(alldataincolumns, 0, counter, outputx)
						
				number outputy=getpixel(outputdata, x+1, y)
				setpixel(alldataincolumns, 1, counter, outputy)

				number inputx=getpixel(inputdata, x, y)
				setpixel(alldataincolumns, 2, counter, inputx)

				number inputy=getpixel(inputdata, x+1, y)
				setpixel(alldataincolumns, 3, counter, inputy)
				
				counter=counter+1
			}
	}


// Compute the output data for each input data point - place it in an image

counter=0
number i1
number j1
image calcoutputdata=imageclone(inputdata)*0

for(y=0; y<inysize; y++)
	{
		for(x=0; x<inxsize; x=x+2)
			{
				// Source the input data positions 
				
				number i1=getpixel(alldataincolumns, 2, counter) // input x data
				number j1=getpixel(alldataincolumns, 3, counter) // input y data


				// These are the polynomial fits, iprime and jprime are the output x and y coordinates respectively.
				
				number iprime=a0+(a1*i1)+(a2*j1)+(a3*i1*i1)+(a4*i1*j1)+(a5*j1*j1)
				number jprime=b0+(b1*i1)+(b2*j1)+(b3*i1*i1)+(b4*i1*j1)+(b5*j1*j1)
				
				
				// Create a calculated ouput data image based on the fits
				
				setpixel(calcoutputdata, x, y, iprime)
				setpixel(calcoutputdata, x+1, y, jprime)
				
				counter=counter+1
			}
	}


// Display the original output data

showimage(outputdata)
setname(outputdata, "Original Output Data")


// Display the calculated reciprocal data image

showimage(calcoutputdata)
setname(calcoutputdata, "Calculated Output Data")


// Create a difference between the calculated and original reciprocal data images to show the quality of the fits

image differenceimage=outputdata-calcoutputdata
showimage(differenceimage)
setname(differenceimage, "Original Output Data minus Calculated Output Data")


// Show the a and b coefficient images

showimage(acoefficients)
setname(acoefficients, "a Coefficients")

showimage(bcoefficients)
setname(bcoefficients, "b Coefficients")




