package imseProc.graph.shapeFit;

import jafama.FastMath;
import seed.digeom.FunctionND;
import seed.digeom.IDomain;
import seed.digeom.RectangularDomain;

public class CostF extends FunctionND {
	
	private double x[];
	private double d[];
	private double sigma = 1;
	private int funcType;
	private double initP[], minP[], maxP[];
	private boolean paramEnable[];
	private int nSelected;
		
	public CostF(int funcType, double x[], double d[], boolean paramEnable[], 
					double initP[], double minP[], double maxP[]) {
		this.funcType = funcType;
		this.x = x;
		this.d = d;
		this.paramEnable = paramEnable;
		this.initP = initP;
		this.minP = minP;
		this.maxP = maxP;
		for(int i=0; i < initP.length; i++)
			if(paramEnable[i])
				nSelected++; 
	}
	
	public final double[] toSelectedParams(double allParams[]){
		double selectedParams[] = new double[nSelected];
		int j=0;
		for(int i=0; i < initP.length; i++)
			if(paramEnable[i])
				selectedParams[j++] = allParams[i];
		return selectedParams;
	}
	
	public final double[] toAllParams(double selectedParams[]){
		//get full param array from enable ones and inital ones for disabled
		double allParams[] = new double[initP.length];
		int j=0;
		for(int i=0; i < initP.length; i++){
			allParams[i] = paramEnable[i] ? selectedParams[j++] : initP[i];
		}
		return allParams;
	}
	
	@Override
	/** eval, from selected parameters only */
	public final double eval(double[] selectedParams) {
		return evalAllParams(toAllParams(selectedParams));		
	}
	
	/** eval, takes all parameters */
	public final double evalAllParams(double[] p) {
			
		double logP = 0;
		
		double v[] = func(p);
		
		for(int i=0; i < d.length; i++){
			if(x[i] != 0 && !Double.isNaN(x[i]) && !Double.isNaN(d[i]))
				logP += 0.5 * FastMath.pow2((d[i] - v[i])/sigma);
		}
		
		return logP;
	}
		
	public final double[] diff(double p[]){
		double v[] = func(p);
		double ret[] = new double[d.length];
		for(int i=0; i < d.length; i++){
			ret[i] = d[i] - v[i];
		}
		return ret;
	}
		
	public final double[] func(double p[]){
		double f[] = new double[x.length];
		switch(funcType){
			case FuncFitter.FUNC_TRIANGLE:{
				double l = p[0], phs = p[1], A = p[2], y0 = p[3];
				for(int i=0; i < x.length; i++){
					double xx = x[i] + phs*l/(2*Math.PI);
					f[i] = y0 + A*FastMath.signum(xx) * (4*FastMath.abs(((FastMath.abs(xx)/l + 0.75) % 1.0) - 0.5) - 1);
				}
				return f;
				
			}case FuncFitter.FUNC_SAWTOOTH:{
				double l = p[0], phs = p[1], A = p[2], y0 = p[3];
				for(int i=0; i < x.length; i++){
					double xx = x[i] + phs*l/(2*Math.PI);
					if(xx > 0)
						f[i] = y0 + A * (xx/l % 1.0);
					else
						f[i] = y0 + A * (1.0 + (xx/l % 1.0));
				}
				return f;
				
			}case FuncFitter.FUNC_SINE:{
				double l = p[0], phs = p[1], A = p[2], y0 = p[3];
				for(int i=0; i < x.length; i++)
					f[i] = y0 + A*FastMath.sin(2*Math.PI*x[i]/l + phs);

				return f;
			}case FuncFitter.FUNC_ABSSINE:{
				double l = p[0], phs = p[1], A = p[2], y0 = p[3];
				for(int i=0; i < x.length; i++)
					f[i] = y0 + A*FastMath.abs(FastMath.sin(2*Math.PI*x[i]/l + phs));

				return f;
			}case FuncFitter.FUNC_QUADRATIC:{
				double x0 = p[0], a0 = p[1], a1 = p[2], a2 = p[3];
				for(int i=0; i < x.length; i++)
					f[i] = a0 + a1*(x[i] - x0) + a2*(x[i] - x0)*(x[i] - x0);

				return f;
			}case FuncFitter.FUNC_SINESHIFT:{
				double l = p[0], phs = p[1], A = p[2], y0 = p[3], dAdx = p[4], dy0dx = p[5];
				for(int i=0; i < x.length; i++)
					f[i] = y0 + dy0dx * x[i] + (A +dAdx*x[i])*FastMath.sin(2*Math.PI*x[i]/l + phs);

				return f;
				
			}default:
				throw new IllegalArgumentException("Unknown function");
		}
	}
	
	@Override
	public IDomain getDomain() { return new RectangularDomain(toSelectedParams(minP), toSelectedParams(maxP)); }
	
}