package imseProc.proc.transform;

import java.util.ArrayList;

import descriptors.gmds.GMDSSignalDesc;

import oneLiners.OneLiners;

import jafama.FastMath;
import algorithmrepository.Algorithms;
import algorithmrepository.CubicInterpolation2D;
import seed.digeom.Function;
import seed.digeom.FunctionND;
import seed.digeom.IDomain;
import seed.digeom.RectangularDomain;
import seed.optimization.HookeAndJeeves;
import signals.gmds.GMDSSignal;
import mds.GMDSFetcher;

/** Linear and cubic version of IMSETransform */
public class FeatureTransformCubic extends FeatureTransform {
	
	/** range info for lat/lon */
	private double minLon, maxLon, minLat, maxLat;
	
	/** Axes of the cubic interpolators */
	private double pLon[], pLat[];
	
	/** Interpolators for the cubic transform */
	private CubicInterpolation2D interpX, interpY;

	/** Transform matrix of linear version */
	private double linXYtoLL[][], linLLtoXY[][];
	
	public FeatureTransformCubic() {
		super();
	}
	
	public FeatureTransformCubic(GMDSFetcher gmds, String experiment, int pulse) {
		super(gmds, experiment, pulse);
		
		loadLinear(gmds, experiment, pulse);
		loadKnots(gmds, experiment, pulse);
	}
	
	public void saveToSignal(GMDSFetcher gmds, String experiment, int pulse){
		super.saveToSignal(gmds, experiment, pulse);
		if(isLinearValid()) saveLinear(gmds, experiment, pulse);
		if(isCubicValid()) saveKnots(gmds, experiment, pulse);
	}
	
	public void loadLinear(GMDSFetcher gmds, String experiment, int pulse){
		GMDSSignalDesc sigDesc = new GMDSSignalDesc(pulse, experiment, "Transform/linearXYtoLL");		
		GMDSSignal sig = (GMDSSignal)gmds.getSig(sigDesc);
		linXYtoLL = (double[][])sig.getData();

		linLLtoXY = Algorithms.inv3x3(linXYtoLL);		
	}

	public void loadKnots(GMDSFetcher gmds, String experiment, int pulse){
		pLon = (double[])((GMDSSignal)gmds.getSig(new GMDSSignalDesc(pulse, experiment, "Transform/cubicKnotsLon"))).getData();
		pLat = (double[])((GMDSSignal)gmds.getSig(new GMDSSignalDesc(pulse, experiment, "Transform/cubicKnotsLat"))).getData();
		
		double kX[][] = (double[][])((GMDSSignal)gmds.getSig(new GMDSSignalDesc(pulse, experiment, "Transform/cubicKnotsX"))).getData();
		double kY[][] = (double[][])((GMDSSignal)gmds.getSig(new GMDSSignalDesc(pulse, experiment, "Transform/cubicKnotsY"))).getData();
		
		interpX = new CubicInterpolation2D(pLon, pLat, kX);
		interpY = new CubicInterpolation2D(pLon, pLat, kY);

	}
	
	public void saveLinear(GMDSFetcher gmds, String experiment, int pulse){
		gmds.writeToCache(new GMDSSignal(new GMDSSignalDesc(pulse, experiment, "Transform/linearXYtoLL"), linXYtoLL));
		gmds.writeToCache(new GMDSSignal(new GMDSSignalDesc(pulse, experiment, "Transform/linearLLtoXY"), linLLtoXY));				
	}

	public void saveKnots(GMDSFetcher gmds, String experiment, int pulse){
		gmds.writeToCache(new GMDSSignal(new GMDSSignalDesc(pulse, experiment, "Transform/cubicKnotsLon"), pLon));
		gmds.writeToCache(new GMDSSignal(new GMDSSignalDesc(pulse, experiment, "Transform/cubicKnotsLat"), pLat));
		gmds.writeToCache(new GMDSSignal(new GMDSSignalDesc(pulse, experiment, "Transform/cubicKnotsX"), interpX.getfp()));
		gmds.writeToCache(new GMDSSignal(new GMDSSignalDesc(pulse, experiment, "Transform/cubicKnotsY"), interpY.getfp()));				
	}

	@Override
	public double[] xyToLatLon(double X, double Y) {
		return xyToLatLon(X, Y, Double.NaN, Double.NaN);
	}
	
	/** Find XY of a given AB, this is numeric, quite slow and surprisingly bad */
	public double[] xyToLatLon(double X, double Y, double initA, double initB) {
		if(Double.isNaN(initA) || interpX == null){
			initA = linXYtoLL[0][0]*X + linXYtoLL[0][1]*Y + linXYtoLL[0][2];// + RandomManager.instance().nextUniform(-0.05, 0.05);
			initB = linXYtoLL[1][0]*X + linXYtoLL[1][1]*Y + linXYtoLL[1][2];// + RandomManager.instance().nextUniform(-0.05, 0.05);
		
			if(interpX == null){
				return new double[]{ initA, initB };
			}
		}
		
		final double targX = X, targY = Y;
		Function costF = new FunctionND() {
			@Override
			public double eval(double ab[]) {
				double X = interpX.eval(ab[0], ab[1]);
				double Y = interpY.eval(ab[0], ab[1]);
				
				return (X - targX)*(X - targX) + (Y - targY)*(Y - targY); 
			}
			public IDomain getDomain() { 
				return new RectangularDomain(
					new double[]{ 0, 0 }, 
					new double[]{ 1, 1 }); 
			}			
		};
		HookeAndJeeves opt = new HookeAndJeeves(costF);
		/*ConjugateGradientDirectionFR cg = new ConjugateGradientDirectionFR();
		CoordinateDescentDirection cd = new CoordinateDescentDirection();
		//GoldenSection ls = new GoldenSection(new MaxIterCondition(500));
		NewtonsMethod1D ls = new NewtonsMethod1D(new MaxIterCondition(50));
				
		LineSearchOptimizer opt = new LineSearchOptimizer(null, cd, ls);//*/
		opt.setObjectiveFunction(costF);
		
		double maxDistSq = 0.0001 * 0.0001;
		
		opt.init(new double[]{ initA, initB });
		
		int n=0;
		while(opt.getCurrentValue() > maxDistSq && n < 1000){
			opt.refine();
			//double pos[] = opt.getCurrentPos();
			//System.out.println(pos[0] + "\t" + pos[1] + "\t" + opt.getCurrentValue());
			n++;
		}
		
		//System.out.println("XY-->AB in " + n + "steps");
		
		return opt.getCurrentPos();
	}
	
	public double[][][] calcPixelVectors(int width, int height){
		double pixelVectors[][][] = new double[height][width][3];
		
		double AB[] = new double[]{ Double.NaN, Double.NaN };
		for(int ipy = 0; ipy < height; ipy++){
			for(int ipx = 0; ipx < width; ipx++){
				AB = xyToLatLon((double)ipx / width, (double)ipy / height, AB[0], AB[1]);
				if(!Double.isNaN(AB[0]) && !Double.isNaN(AB[1])){
					
			        pixelVectors[ipy][ipx] = new double[]{
							FastMath.cos(AB[1]) * FastMath.cos(AB[0]),
							FastMath.cos(AB[1]) * FastMath.sin(AB[0]),
							FastMath.sin(AB[1]),
					};

				}else{					
					pixelVectors[ipy][ipx] = new double[]{ Double.NaN, Double.NaN, Double.NaN };
				}
			}	
		}
		
		return pixelVectors;
	}
	
	public void initCubic(int nLon, int nLat) {
		if(pLon == null || nLon != pLon.length || nLat != pLat.length){			
			pLon = OneLiners.linSpace(minLon, maxLon, nLon);
			pLat = OneLiners.linSpace(minLat, maxLat, nLat);
			interpX = new CubicInterpolation2D(pLon, pLat, new double[nLon][nLat]);
			interpY = new CubicInterpolation2D(pLon, pLat, new double[nLon][nLat]);			
		}		
	}
	
	/** Use linear transform of four corners x,y to calc range of R,Z */
	private void calcABRanges(){
		//and for ranges, start with X matrix of the four corners of the image
		double CA[] = new double[]{
				linXYtoLL[0][0]*0 + linXYtoLL[0][1]*0 + linXYtoLL[0][2], //top left
				linXYtoLL[0][0]*1 + linXYtoLL[0][1]*0 + linXYtoLL[0][2], //top right
				linXYtoLL[0][0]*0 + linXYtoLL[0][1]*1 + linXYtoLL[0][2], //bot left
				linXYtoLL[0][0]*1 + linXYtoLL[0][1]*1 + linXYtoLL[0][2], //bot right
		};
		double CB[] = new double[]{
				linXYtoLL[1][0]*0 + linXYtoLL[1][1]*0 + linXYtoLL[1][2], //top left
				linXYtoLL[1][0]*1 + linXYtoLL[1][1]*0 + linXYtoLL[1][2], //top right
				linXYtoLL[1][0]*0 + linXYtoLL[1][1]*1 + linXYtoLL[1][2], //bot left
				linXYtoLL[1][0]*1 + linXYtoLL[1][1]*1 + linXYtoLL[1][2], //bot right
		};
		minLon = Math.min(Math.min(CA[0], CA[1]), Math.min(CA[2], CA[3]));
		maxLon = Math.max(Math.max(CA[0], CA[1]), Math.max(CA[2], CA[3]));
		minLat = Math.min(Math.min(CB[0], CB[1]), Math.min(CB[2], CB[3]));
		maxLat = Math.max(Math.max(CB[0], CB[1]), Math.max(CB[2], CB[3]));
	
		this.pLon = null;
		this.pLat = null;
		this.interpX = null;
		this.interpY = null;
	}
	
	/** (re)calculate the lat/lon angles from the camera position of each of the fitting points */
	public void calcPointsLatLon() {
		double cameraPos[] = getViewPosition();
		if(cameraPos == null){
			throw new IllegalArgumentException("No view position in point data");
		}
		
		for(FeatureTransform.Point p : points){
			double u[] = new double[]{
					p.x - cameraPos[0],
					p.y - cameraPos[1],
					p.z - cameraPos[2],
			};
			u = seed.minerva.optics.Util.reNorm(u);
			
			p.lat = FastMath.asin(u[2]);
			p.lon = FastMath.atan2(u[1], u[0]); //might need to do some rotation here if phi becomes discontinuous			
		}
		
	}
	
	/** Use the first 3 marked points to calculate the approx linear transform from Lat/Lon to XY and vice versa */
	public void calcLinear() {	
			double linPointsXY[][] = new double[3][3];
			double linPointsLL[][] = new double[3][3];
			
			int nLinearFit = 0;
			for(FeatureTransform.Point p : points){				
				if(p.enable == FeatureTransform.EN_FIT_AND_MATRIX){ //use for initial
					linPointsXY[0][nLinearFit] = p.imgX;
					linPointsXY[1][nLinearFit] = p.imgY;
					linPointsXY[2][nLinearFit] = 1;
					linPointsLL[0][nLinearFit] = p.lon;
					linPointsLL[1][nLinearFit] = p.lat;
					linPointsLL[2][nLinearFit] = 1;
					nLinearFit++;
				}
				
				if(nLinearFit >= 3)
					break;
			}
			
			if(nLinearFit < 3){
				throw new IllegalArgumentException("Less than 3 points available for linear transform inversion");					
			}
			
			// calc initial linear trasnform
			double invX[][] = Algorithms.inv3x3(linPointsXY);
			linXYtoLL = Algorithms.mul3x3(linPointsLL, invX);
			linLLtoXY = Algorithms.inv3x3(linXYtoLL);
			
			calcABRanges();
	}

	public boolean isLinearValid() { return (linLLtoXY != null); }
	public boolean isCubicValid() { return (interpX != null); }
	
	@Override
	public boolean isValid() { return isLinearValid(); }
	
	@Override
	public final double[] latlonToXY(double lon, double lat){
		return isCubicValid() 
				? new double[]{ interpX.eval(lon, lat), interpY.eval(lon, lat) } 
				: latlonToXYLinear(lon, lat);
	}
	
	public final double[][] latlonToXY(double lon[], double lat[]){
		return isCubicValid() 
				? new double[][]{ interpX.eval(lon, lat), interpY.eval(lon, lat) } 
				: latlonToXYLinear(lon, lat);
	}
	
	public double[] latlonToXYLinear(double lon, double lat) {
		return new double[]{  
				linLLtoXY[0][0] * lon + linLLtoXY[0][1] * lat + linLLtoXY[0][2],
				linLLtoXY[1][0] * lon + linLLtoXY[1][1] * lat + linLLtoXY[1][2]						
		//	1 = rztoImage[2][0] * pointData[IMSETransform.PD_BI_R] + rztoImage[2][1] * pointData[IMSETransform.PD_BI_Z] + rztoImage[2][2];
			};
	}
	
	public double[][] latlonToXYLinear(double lon[], double lat[]) {
		double xy[][] = new double[2][lon.length];
		for(int i=0; i < lon.length; i++){  
				xy[0][i] = linLLtoXY[0][0] * lon[i] + linLLtoXY[0][1] * lon[i] + linLLtoXY[0][2];
				xy[1][i] = linLLtoXY[1][0] * lat[i] + linLLtoXY[1][1] * lat[i] + linLLtoXY[1][2];						
		}
		return xy;
	}	

	public double[][] getXYtoLLMat() { return linXYtoLL; }
	public double[][] getLLtoXYMat() { return linLLtoXY; }

	public final double getMinLon() { return minLon; }
	public final double getMaxLon() { return maxLon; }
	public final double getMinLat() { return minLat; }
	public final double getMaxLat() { return maxLat; }

	public double[][] getKnotValsX() { return (interpX != null) ? interpX.getfp() : null; }; 
	public double[][] getKnotValsY() { return (interpY != null) ? interpY.getfp() : null; }
	
	public void setKnotValsX(double knotValsX[][]){ interpX.setfp(knotValsX); }
	public void setKnotValsY(double knotValsY[][]){ interpY.setfp(knotValsY); }
	
	public int getNKnotsLon(){ return pLon.length; }
	public int getNKnotsLat(){ return pLat.length; }
	
	public void initCubicToLinear() {
				
		double pX[][] = new double[pLon.length][pLat.length];
		double pY[][] = new double[pLon.length][pLat.length];
		
		for(int iB = 0; iB < pLat.length; iB++) {
			for(int iA = 0; iA < pLon.length; iA++) {				
				pX[iA][iB] = linLLtoXY[0][0]*pLon[iA] + linLLtoXY[0][1]*pLat[iB] + linLLtoXY[0][2];
				pY[iA][iB] = linLLtoXY[1][0]*pLon[iA] + linLLtoXY[1][1]*pLat[iB] + linLLtoXY[1][2];				
			}				
		}
		
		interpX.setfp(pX);
		interpY.setfp(pY);
		
	}

	public void invalidateCubic() { interpX = null; interpY = null; }; 	
	
}