package de.desy.acop.video.analysis;
   
   import java.util.Arrays;
   
   import org.apache.commons.math.FunctionEvaluationException;
   import org.apache.commons.math.analysis.DifferentiableMultivariateVectorialFunction;
   import org.apache.commons.math.analysis.MultivariateMatrixFunction;
   import org.apache.commons.math.optimization.OptimizationException;
   import org.apache.commons.math.optimization.VectorialPointValuePair;
  import org.apache.commons.math.optimization.general.LevenbergMarquardtOptimizer;
  
  /**
   * 
   * <code>FittingUtilities</code> provides a set of utilities to fit a 
   * gaussian function to a set of data
   *
   * @author <a href="mailto:jaka.bobnar@cosylab.com">Jaka Bobnar</a>
   *
   */
  public class FittingUtilities {
  
  	private static class GaussWithOffset implements DifferentiableMultivariateVectorialFunction {
  		
  		private static final long serialVersionUID = 1L;
  		private double[] x;
  		
  		/**
  		 * Constructs a new GaussWithOffset, where the values are known for the 
  		 * given horizontal axis.
  		 * 
  		 * @param x the horiontal axis values
  		 */
  		public GaussWithOffset(double[] x) {
  			this.x = x;
  		}
  				
  		/*
  		 * (non-Javadoc)
  		 * @see org.apache.commons.math.analysis.DifferentiableMultivariateVectorialFunction#jacobian()
  		 */
  		public MultivariateMatrixFunction jacobian() {
  			return new MultivariateMatrixFunction(){
  				private static final long serialVersionUID = 1L;
  				public double[][] value(double[] a) throws FunctionEvaluationException, IllegalArgumentException {
  					double[][] J = new double[x.length][a.length];
  					for (int i = 0; i < J.length; i++) {
  						J[i][0] = Math.exp(-a[1]*(x[i]-a[2])*(x[i]-a[2]));
  						J[i][1] = -J[i][0]*a[0]*(x[i]-a[2])*(x[i]-a[2]);
  						J[i][2] = a[0]*J[i][0]*a[1]*2*(x[i]-a[2]);
  						J[i][3] = 1;
  					}
  					return J;
  				}
  			};
  		}
  				
  		/*
  		 * (non-Javadoc)
  		 * @see org.apache.commons.math.analysis.MultivariateVectorialFunction#value(double[])
  		 */
  		public double[] value(double[] a) throws FunctionEvaluationException, IllegalArgumentException {
  			double[] f = new double[x.length];
  			for (int i = 0; i < x.length; i++) {
  				f[i] = a[0] * Math.exp(-a[1]*(x[i]-a[2])*(x[i]-a[2])) + a[3];
  			}
  			return f;
  		}
  	}
  	
  	private static class GaussWithLinear implements DifferentiableMultivariateVectorialFunction {
  		
  		private static final long serialVersionUID = 1L;
  		private double[] x;
  		
  		/**
  		 * Constructs a new GaussWithOffset, where the values are known for the 
  		 * given horizontal axis.
  		 * 
  		 * @param x the horiontal axis values
  		 */
  		public GaussWithLinear(double[] x) {
  			this.x = x;
  		}
  				
  		/*
  		 * (non-Javadoc)
  		 * @see org.apache.commons.math.analysis.DifferentiableMultivariateVectorialFunction#jacobian()
  		 */
  		public MultivariateMatrixFunction jacobian() {
  			return new MultivariateMatrixFunction(){
  				private static final long serialVersionUID = 1L;
  				public double[][] value(double[] a) throws FunctionEvaluationException, IllegalArgumentException {
  					double[][] J = new double[x.length][a.length];
  					for (int i = 0; i < J.length; i++) {
  						J[i][0] = Math.exp(-a[1]*(x[i]-a[2])*(x[i]-a[2]));
  						J[i][1] = -J[i][0]*a[0]*(x[i]-a[2])*(x[i]-a[2]);
  						J[i][2] = a[0]*J[i][0]*a[1]*2*(x[i]-a[2]);
  						J[i][3] = 1;
  						J[i][4] = x[i];
 					}
 					return J;
 				}
 			};
 		}
 				
 		/*
 		 * (non-Javadoc)
 		 * @see org.apache.commons.math.analysis.MultivariateVectorialFunction#value(double[])
 		 */
 		public double[] value(double[] a) throws FunctionEvaluationException, IllegalArgumentException {
 			double[] f = new double[x.length];
 			for (int i = 0; i < x.length; i++) {
 				f[i] = a[0] * Math.exp(-a[1]*(x[i]-a[2])*(x[i]-a[2])) + a[3] + a[4]*x[i];
 			}
 			return f;
 		}
 	}
 	
 	private static class Gauss implements DifferentiableMultivariateVectorialFunction {
 		
 		private static final long serialVersionUID = 1L;
 		private double[] x;
 		
 		/**
 		 * Constructs a new GaussWithOffset, where the values are known for the 
 		 * given horizontal axis.
 		 * 
 		 * @param x the horiontal axis values
 		 */
 		public Gauss(double[] x) {
 			this.x = x;
 		}
 				
 		/*
 		 * (non-Javadoc)
 		 * @see org.apache.commons.math.analysis.DifferentiableMultivariateVectorialFunction#jacobian()
 		 */
 		public MultivariateMatrixFunction jacobian() {
 			return new MultivariateMatrixFunction(){
 				private static final long serialVersionUID = 1L;
 				public double[][] value(double[] a) throws FunctionEvaluationException, IllegalArgumentException {
 					double[][] J = new double[x.length][a.length];
 					for (int i = 0; i < J.length; i++) {
 						J[i][0] = Math.exp(-a[1]*(x[i]-a[2])*(x[i]-a[2]));
 						J[i][1] = -J[i][0]*a[0]*(x[i]-a[2])*(x[i]-a[2]);
 						J[i][2] = a[0]*J[i][0]*a[1]*2*(x[i]-a[2]);
 					}
 					return J;
 				}
 			};
 		}
 				
 		/*
 		 * (non-Javadoc)
 		 * @see org.apache.commons.math.analysis.MultivariateVectorialFunction#value(double[])
 		 */
 		public double[] value(double[] a) throws FunctionEvaluationException, IllegalArgumentException {
 			double[] f = new double[x.length];
 			for (int i = 0; i < x.length; i++) {
 				f[i] = a[0] * Math.exp(-a[1]*(x[i]-a[2])*(x[i]-a[2]));
 			}
 			return f;
 		}
 	}
 	
 //	private static class Gauss2D implements DifferentiableMultivariateVectorialFunction {
 //		
 //		private static final long serialVersionUID = 1L;
 //		private double[] x;
 //		private double[] y;
 //		
 //		/**
 //		 * Constructs a new GaussWithOffset, where the values are known for the 
 //		 * given horizontal and vertical axes.
 //		 * 
 //		 * @param x the horiontal axis values
 //		 * @param y the vertical axis values
 //		 */
 //		public Gauss2D(double[] x, double[] y) {
 //			this.x = x;
 //			this.y = y;
 //		}
 //				
 //		/*
 //		 * (non-Javadoc)
 //		 * @see org.apache.commons.math.analysis.DifferentiableMultivariateVectorialFunction#jacobian()
 //		 */
 //		public MultivariateMatrixFunction jacobian() {
 //			return new MultivariateMatrixFunction(){
 //				private static final long serialVersionUID = 1L;
 //				public double[][] value(double[] a) throws FunctionEvaluationException, IllegalArgumentException {
 //					double[][] J = new double[x.length][a.length];
 //					double d = 0;
 //					for (int i = 0; i < J.length; i++) {
 //						d = Math.exp(-a[1]*(x[i]-a[4])*(x[i]-a[4]) - 2*a[2]*(y[i]-a[5])*(x[i]-a[4]) - a[3]*(y[i]-a[5])*(y[i]-a[5]));
 //						J[i][0] = d;
 //						d *= a[0];
 //						J[i][1] = -d*(x[i]-a[4])*(x[i]-a[4]);
 //						J[i][2] = -d*2*(x[i]-a[4])*(y[i]-a[5]);
 //						J[i][3] = -d*(y[i]-a[5])*(y[i]-a[5]);
 //						J[i][4] = d*(a[1]*2*(x[i]-a[4] + 2*a[2]*(y[i]-a[5])));
 //						J[i][5] = d*(a[3]*2*(y[i]-a[5] + 2*a[2]*(x[i]-a[4])));
 //						J[i][6] = 1;
 //					}
 //					return J;
 //				}
 //			};
 //		}
 //				
 //		/*
 //		 * (non-Javadoc)
 //		 * @see org.apache.commons.math.analysis.MultivariateVectorialFunction#value(double[])
 //		 */
 //		public double[] value(double[] a) throws FunctionEvaluationException, IllegalArgumentException {
 //			double[] f = new double[x.length];
 //			for (int i = 0; i < x.length; i++) {
 //				f[i] = a[0] * Math.exp(-a[1]*(x[i]-a[4])*(x[i]-a[4]) - 2*a[2]*(y[i]-a[5])*(x[i]-a[4]) - a[3]*(y[i]-a[5])*(y[i]-a[5])) + a[6];
 //			}
 //			return f;
 //		}
 //	}
 	
 	/**
 	 * Fits a gauss with offset y = Ae^(-(x-C)^2/(2B^2)) + D to the given data.
 	 * @param x the independent variable data
 	 * @param y the dependent variable data
 	 * @param weights the weights of the measurements
 	 * @param startValues starting values for the parameters
 	 * 
 	 * @return array of calculated parameters ([A,B,C,D])
 	 * 
 	 * @throws OptimizationException
 	 * @throws FunctionEvaluationException
 	 * @throws IllegalArgumentException
 	 * 
 	 * @deprecated ues {@link FittingUtilities#lmLinear(double[], double[], double[], double[])}
 	 */
 	@Deprecated
 	public static double[] lmOffset(double[] x, double[] y, double[] weights, double[] startValues) throws OptimizationException, FunctionEvaluationException, IllegalArgumentException {
 		LevenbergMarquardtOptimizer lmo = new LevenbergMarquardtOptimizer();
 //		GaussNewtonOptimizer lmo = new GaussNewtonOptimizer(true);
 		lmo.setMaxIterations(500);
 		GaussWithOffset f = new GaussWithOffset(x);
 		VectorialPointValuePair pvp = lmo.optimize(f,y,weights,startValues);
 		double[] a = pvp.getPointRef();
 		a[1] = 1./Math.sqrt(2*a[1]);
 		return a;
 	}	
 	
 	/**
 	 * Fits a gauss with offset y = Ae^(-(x-C)^2/(2B^2)) + D + E x to the given data.
 	 * @param x the independent variable data
 	 * @param y the dependent variable data
 	 * @param weights the weights of the measurements
 	 * @param startValues starting values for the parameters
 	 * 
 	 * @return array of calculated parameters ([A,B,C,D,E])
 	 * 
 	 * @throws OptimizationException
 	 * @throws FunctionEvaluationException
 	 * @throws IllegalArgumentException
 	 */
 	public static double[] lmLinear(double[] x, double[] y, double[] weights, double[] startValues) throws OptimizationException, FunctionEvaluationException, IllegalArgumentException {
 		LevenbergMarquardtOptimizer lmo = new LevenbergMarquardtOptimizer();
 		lmo.setInitialStepBoundFactor(0.3);
 		lmo.setMaxIterations(1000);
 		startValues[1] = 1/(2*startValues[1]);
 		GaussWithLinear f = new GaussWithLinear(x);
 		VectorialPointValuePair pvp = lmo.optimize(f,y,weights,startValues);
 		double[] a = pvp.getPointRef();
 		a[1] = 1./Math.sqrt(2*a[1]);		
 		return a;
 	}
 	
 	/**
 	 * Fits a gauss with offset y = Ae^(-(x-C)^2/(2B^2)) to the given data.
 	 * @param x the independent variable data
 	 * @param y the dependent variable data
 	 * @param weights the weights of the measurements
 	 * @param startValues starting values for the parameters
 	 * 
 	 * @return array of calculated parameters ([A,B,C])
 	 * 
 	 * @throws OptimizationException
 	 * @throws FunctionEvaluationException
 	 * @throws IllegalArgumentException
 	 */
 	public static double[] lmGauss(double[] x, double[] y, double[] weights, double[] startValues) throws OptimizationException, FunctionEvaluationException, IllegalArgumentException {
 		LevenbergMarquardtOptimizer lmo = new LevenbergMarquardtOptimizer();
 		lmo.setInitialStepBoundFactor(0.3);
 		lmo.setMaxIterations(1000);
 		startValues[1] = 1/(2*startValues[1]);
 		Gauss f = new Gauss(x);
 		VectorialPointValuePair pvp = lmo.optimize(f,y,weights,startValues);
 		double[] a = pvp.getPointRef();
 		a[1] = 1./Math.sqrt(2*a[1]);
 		return a;
 	}
 		
 	public static void main(String[] args) throws MathException {
 		int n = 1000;
 		double[] x = new double[n];
 		for (int i = 0; i < x.length; i++) {
 			x[i] = 5*Math.exp(-(i-n/2)*(i-n/2)/(2*n*n/100));
 		}
 		double[] res = fit(x,0);
 		System.out.println(Arrays.toString(res));
 	}
 	
 	/**
 	 * Performs a linear fit of exponential function to the y data.
 	 * 
 	 * @param y
 	 * @param constY
 	 * @return
 	 */
 	public static double[] fit(double[] y, double constY) throws MathException {
 		double[][] A = new double[3][3];
 		double[] b = new double[3];
 		double val;
 		double length = y.length;
 		try {
 			int i = 0;
 			double v;
 			double x;
     		for (;;) {
     			//normalize to eliminate overflow
     			val = (y[i]-constY)/length;
     			if (val <= 0) {
     				val = 0;
     			} else {
     				val = Math.log(val);
     			}
     			//extra weight to boost large values instead of small
     			v = y[i]-constY;
     			//normalize to eliminate overflow
     			x = i/length;
     			A[0][0] += x*x*x*x*v;
     			A[0][1] += x*x*x*v;
     			A[0][2] += x*x*v;
     			A[1][2] += x*v;
     			A[2][2] += v;
     			b[0] += val*x*x*v;
     			b[1] += val*x*v;
     			b[2] += val*v;
     			i++;
     		}
 		} catch (ArrayIndexOutOfBoundsException e) {}
 		A[1][1] = A[0][2];
 		A[1][0] = A[0][1];
 		A[2][0] = A[0][2];
 		A[2][1] = A[1][2];
 
 		LUDecomposition lu = new LUDecomposition(A);
 		double[] r = lu.solve(b);
 		double sigma = Math.sqrt(-0.5/r[0])*length;
 		double mean = -0.5*r[1]*length/r[0];
 		double amp = Math.exp(r[2] -0.25*r[1]*r[1]/r[0])*length;
 		
 		return new double[]{amp,sigma,mean,constY,0};
 	}
 	
 	/**
 	 * Checks if any of the values in the arrays is NaN.
 	 * 
 	 * @param values
 	 * @return
 	 */
 	public static boolean isNaN(double[] values) {
 		for (int i = 0; i < values.length; i++) {
 			if (Double.isNaN(values[i])) return true;
 		}
 		return false;
 	}
 	
 	/**
 	 * Smooths the data with averaging.
 	 * 
 	 * @param x data to be smoothed
 	 * @return smoothed data
 	 */
 	public static double[] smooth(double[] x) {
 		return smooth(x,false);
 	}
 		
 	/**
 	 * Smooths the values in the array with averaging and applying a low pass filter.
 	 * 
 	 * @param x the data to smooth
 	 * @param lowPassFilterOn true if low pass filtering should be used
 	 * @return smoothed array
 	 */
 	public static double[] smooth(double[] x, boolean lowPassFilterOn) {
 				
 		int l = x.length;
 		int n = 6;
 		double[] xx = new double[l];
 		for (int i = n; i < l; i++) {
 			for (int j = 0; j < n; j++) { 
 				xx[i] += x[i-j];
 			}
 			xx[i] /= n;
 		}
 		for (int i = 0; i < n; i++) {
 			xx[i] = xx[n];
 		}
 		if (!lowPassFilterOn) return xx;
 		
 		if (l < 16)	l = 16;
 		else if (l < 32) l = 32;
 		else if (l < 64) l = 64;
 		else if (l < 128) l = 128;
 		else if (l < 256) l = 256;
 		else if (l < 512) l = 512;
 		else if (l < 1024) l = 1024;
 		else if (l < 2048) l = 2048;
 		else if (l < 4096) l = 4096;
 		else if (l < 8192) l = 8192;
 		
 		double[] d = new double[l];
 		double[] d1 = new double[l];
 		System.arraycopy(xx,0,d1,0,xx.length);
 		double[][] dd = new double[][]{d1,d};
 		dd = FFT1D(dd,true);
 		int q = (int)(dd[0].length*0.05 + 0.5);
 		int h = dd[0].length/2;
 		for (int i = q; i < h; i++) {
 			dd[0][i] = 0;
 			dd[0][h + i] = 0;
 			dd[1][i] = 0;
 			dd[1][h+i] = 0;
 		}
 		
 		dd = FFT1D(dd,false);
 		double[] a = new double[xx.length];
 		System.arraycopy(dd[0],0,a,0,a.length);
 		return a;
 	}
 	
 	/**
 	 * @param x array of values. First array are real values, second imaginary
 	 * @param forward true for forward and false for inverse transform
 	 * @return Fourier transform of x
 	 * @throws Exception
 	 */
 	public static double[][] FFT1D(double[][] x, boolean forward) {
 		if (x == null)
 			return null;
 		if (x.length != 2 || x[0].length != x[1].length) {
 			throw new IllegalArgumentException("bad x lengths");
 		}
 		int n = x[0].length;
 		int n2 = 1;
 		while (n2 < n) {
 			n2 *= 2;
 			if (n2 > n) {
 				throw new IllegalArgumentException("x length must be power of 2");
 			}
 		}
 		n2 = n / 2;
 		double[][] temp = new double[2][n2];
 		double angle = (-2.0 * Math.PI / n);
 		if (!forward)
 			angle = -angle;
 		for (int i = 0; i < n2; i++) {
 			temp[0][i] = Math.cos(i * angle);
 			temp[1][i] = Math.sin(i * angle);
 		}
 		double[][] y = FFT1D(x, temp);
 		if (!forward) {
 			for (int i = 0; i < n; i++) {
 				y[0][i] /= n;
 				y[1][i] /= n;
 			}
 		}
 		return y;
 	}
 
 	private static double[][] FFT1D(double[][] x, double[][] temp) {
 		int n = x[0].length;
 		int n2 = n / 2;
 		int k = 0;
 		int b;
 		int c = 0;
 		if (n == 1) {
 			double[][] z1 = {{x[0][0]}, {x[1][0]}};
 			return z1;
 		}
 
 		b = (temp[0].length / n2);
 
 		double[][] z = new double[2][n2];
 		double[][] w = new double[2][n2];
 
 		for (k = 0; k < n2; k++) {
 			int k2 = 2 * k;
 			z[0][k] = x[0][k2];
 			z[1][k] = x[1][k2];
 			w[0][k] = x[0][k2 + 1];
 			w[1][k] = x[1][k2 + 1];
 		}
 
 		z = FFT1D(z, temp);
 		w = FFT1D(w, temp);
 
 		double[][] y = new double[2][n];
 		for (k = 0; k < n2; k++) {
 			y[0][k] = z[0][k];
 			y[1][k] = z[1][k];
 
 			double re = w[0][k] * temp[0][c] - w[1][k] * temp[1][c];
 			double im = w[0][k] * temp[1][c] + w[1][k] * temp[0][c];
 			w[0][k] = re;
 			w[1][k] = im;
 
 			y[0][k] += w[0][k];
 			y[1][k] += w[1][k];
 			y[0][k + n2] = z[0][k] - w[0][k];
 			y[1][k + n2] = z[1][k] - w[1][k];
 			c += b;
 		}
 		return y;
 	}
 }