package seed.minerva.apps.imse;

import java.util.List;

import algorithmrepository.Algorithms;

import mds.GMDSFetcher;
import otherSupport.SettingsManager;
import descriptors.gmds.GMDSSignalDesc;
import imseProc.proc.transform.FeatureTransform;
import imseProc.proc.transform.FeatureTransformCubic;
import jafama.FastMath;
import oneLiners.OneLiners;
import seed.digeom.FunctionND;
import seed.digeom.IDomain;
import seed.digeom.RectangularDomain;
import seed.minerva.MinervaOpticsSettings;
import seed.minerva.aug.mse.AugMSESystem;
import seed.minerva.aug.mse.AugMseFaroData;
import seed.minerva.imse.IMSEOptics135_50;
import seed.minerva.optics.Util;
import seed.minerva.optics.drawing.VRMLDrawer;
import seed.minerva.optics.interfaces.NullInterface;
import seed.minerva.optics.optics.Box;
import seed.minerva.optics.surfaces.Square;
import seed.minerva.optics.tracer.Tracer;
import seed.minerva.optics.types.Intersection;
import seed.minerva.optics.types.Pol;
import seed.minerva.optics.types.RaySegment;
import seed.minerva.optics.types.Surface;
import seed.optimization.BracketingByParameterSpace;
import seed.optimization.ConjugateGradientDirectionFR;
import seed.optimization.GoldenSection;
import seed.optimization.HookeAndJeeves;
import seed.optimization.IStoppingCondition;
import seed.optimization.LineSearchOptimizer;
import seed.optimization.MaxIterCondition;
import seed.optimization.Optimizer;
import seed.optimization.genetic.MSHGPPConfig;
import seed.optimization.genetic.MegaSuperHyperGeneticProblemPacifier;
import signals.gmds.GMDSSignal;

/** Fits mirror rotation (and pos?) to match transform data */
public class FitMirrorToTransform_forwardTrace extends FunctionND {
	private static String outPath;

	// IMSE or AUG pulse number to read transform from GMDS data
	private final static int pulse = 178; // Jan2013 primary
	//private final static int pulse = 351; // Apr2013 primary
	private final static String experiment = "AUG";
	
	public static double minIntensity = 0.01;
	public static boolean traceReflections = false;
	private final static double globalUp[] = new double[]{ 0, 0, 1 };
	private final static double wavelen = 653e-9;
	
	private static AugMSESystem sys = new AugMSESystem(new IMSEOptics135_50());
	private FeatureTransformCubic xform;
	
	private Box cameraBox;
	
	/** zero position of mirror centre and the original normal (used to move it) */ 
	double mirrorZeroPos[],  mirrorZeroNormal[];
	
	public static void main(String[] args) {
		(new FitMirrorToTransform_forwardTrace()).doFit();
	}
	
	private void dumpParams(double p[]) {
		System.out.println("θ = " + p[0]*180/Math.PI +
				", φ = " + p[1]*180/Math.PI +
				", αinit = " + p[2]*180/Math.PI +
				", Δp = " + p[3]);		
	}
		
	public void doFit() {	
		outPath = MinervaOpticsSettings.getAppsOutputPath() + "/rayTracing/augImse/transformFit/p_" + pulse;
		
		//grib the mirror position now, so the shift is relative to it
		mirrorZeroNormal = sys.mainMirror.getNormal().clone();
		mirrorZeroPos = sys.mainMirror.getCentre().clone();
		
		sys.setupForPulse(pulse);
		
		AugMseFaroData faro = new AugMseFaroData();
		double camPos[] = faro.getCameraPos();
		double camSize = 0.05;
		cameraBox = new Box("cameraPos", camPos, camSize,camSize,camSize, null, NullInterface.ideal());
		sys.addElement(cameraBox);
		
		String serverID = SettingsManager.defaultGlobal().getProperty("imseProc.cis.gmdsServerID", "pccis");		
		GMDSFetcher gmds = GMDSFetcher.defaultInstance(serverID);
		
		xform = new FeatureTransformCubic(gmds, experiment, pulse);
			
		for(Surface s : sys.mirrorBox.getSurfaces()){
			s.setInterface(NullInterface.ideal());
		}
		
		/*IStoppingCondition lineSearchStop = new MaxIterCondition(50);
		GoldenSection lineOptimizer = new GoldenSection(lineSearchStop);
		lineOptimizer.setInitialBracketMethod(new BracketingByParameterSpace());
		Optimizer opt = new LineSearchOptimizer(null, new ConjugateGradientDirectionFR(), lineOptimizer);
		//*/
		//HookeAndJeeves opt = new HookeAndJeeves(null);
		MSHGPPConfig cfg = new MSHGPPConfig();
		cfg.populationSize = 20;
		cfg.numChildren = 2;
		cfg.initFromCurrent = false;
		MegaSuperHyperGeneticProblemPacifier opt = new MegaSuperHyperGeneticProblemPacifier(cfg);
		
		System.out.print("mirrorP: "); OneLiners.dumpArray(sys.mainMirror.getCentre());
		System.out.print("mirrorN: "); OneLiners.dumpArray(sys.mainMirror.getNormal());
		System.out.print("ccdU: "); OneLiners.dumpArray(sys.imseOptics.ccd.getUp());
		System.out.print("ccdR: "); OneLiners.dumpArray(sys.imseOptics.ccd.getRight());
		
		double pInit[] = getParams();
		System.out.print("Init: "); dumpParams(pInit);
		setParams(pInit);
		
		System.out.print("mirroP: "); OneLiners.dumpArray(sys.mainMirror.getCentre());
		System.out.print("mirrorN: "); OneLiners.dumpArray(sys.mainMirror.getNormal());
		System.out.print("ccdU: "); OneLiners.dumpArray(sys.imseOptics.ccd.getUp());
		System.out.print("ccdR: "); OneLiners.dumpArray(sys.imseOptics.ccd.getRight());
		
		
		opt.setObjectiveFunction(this);
		
		pInit=getParams();
		pInit = new double[]{
				20.0 * Math.PI / 180, 
				-160.0 * Math.PI / 180, 
				1.0 * Math.PI / 180, 
				0.03,
		};//*/
		
		
		opt.init(pInit);
		double initCost = eval(pInit);
		
		System.out.print("mirroP: "); OneLiners.dumpArray(sys.mainMirror.getCentre());
		System.out.print("mirrorN: "); OneLiners.dumpArray(sys.mainMirror.getNormal());
		System.out.print("ccdU: "); OneLiners.dumpArray(sys.imseOptics.ccd.getUp());
		System.out.print("ccdR: "); OneLiners.dumpArray(sys.imseOptics.ccd.getRight());
			
		for(int i=0; i < 2000; i++) {				
			opt.refine();
			double p[] = opt.getCurrentPos();
			double cost = opt.getCurrentValue();
			eval(p);
			System.out.print(i + ": "); dumpParams(p);			
			System.out.println();
			opt.dumpStatus();
		}
		
		double p[] = opt.getCurrentPos();
		setParams(p);
		
		System.out.println("Init: d=" + initCost + 
				", θ=" + pInit[0]*180/Math.PI +  
				", φ=" + pInit[1]*180/Math.PI);
		//OneLiners.dumpArray(pInit);
		System.out.print("Init: ");	dumpParams(pInit);
		System.out.print("Final: ");	dumpParams(p);
		//OneLiners.dumpArray(p);

		System.out.print("mirrorP: "); OneLiners.dumpArray(sys.mainMirror.getCentre());
		System.out.print("mirrorN: "); OneLiners.dumpArray(sys.mainMirror.getNormal());
		System.out.print("ccdU: "); OneLiners.dumpArray(sys.imseOptics.ccd.getUp());
		double mP[] = sys.mainMirror.getCentre();
		double mN[] = sys.mainMirror.getNormal();
		double cU[] = sys.imseOptics.ccd.getUp();
		
		System.out.println("\n--- vvv ------------------ Cut Here ------------------ vvv ---\n" +
					"// GA optimised, p=" + pulse + ", θ = " + p[0]*180/Math.PI + ", φ = " + p[1]*180/Math.PI + "\n" +
					"//  α = " + p[2]*180/Math.PI + ", (Δr.n) = " + p[3] + "\n" +
					"mainMirror.setCentre(new double[]{ " + mP[0] + ", " + mP[1] + ", " + mP[2] + " });\n" +
					"mainMirror.setNormal(new double[]{ " + mN[0] + ", " + mN[1] + ", " + mN[2] + " });\n" +
					"ccd.setUp(new double[]{ " + cU[0] + ", " + cU[1] + ", " + cU[2] + " });\n" + 
					"--- /666 ------------------ Cut Here ------------------ ^^^ ---\n");
		
	}
	
	protected void setParams(double[] p) { 
		
		sys.mainMirror.setNormal(new double[]{ 
									FastMath.cos(p[0]) * FastMath.cos(p[1]),
									FastMath.cos(p[0]) * FastMath.sin(p[1]),
									FastMath.sin(p[0]) 
								});

		double cosCCDAng = FastMath.cos(p[2]);
		double sinCCDAng = FastMath.sin(p[2]);
		
		double ccdN[] = sys.imseOptics.ccd.getNormal();
		
		//CCD real up frame
		double ccdFR[] = Util.reNorm(Util.cross(ccdN, globalUp));
		double ccdFU[] = Util.reNorm(Util.cross(ccdFR, ccdN));
		
		double ccdU[] = new double[]{
				ccdFU[0] * cosCCDAng + ccdFR[0] * sinCCDAng,
				ccdFU[1] * cosCCDAng + ccdFR[1] * sinCCDAng,
				ccdFU[2] * cosCCDAng + ccdFR[2] * sinCCDAng
			};
		
		sys.imseOptics.ccd.setUp(ccdU);
		
		sys.mainMirror.setCentre(Util.plus(mirrorZeroPos, Util.mul(mirrorZeroNormal, p[3])));
		
	}

	protected double[] getParams() {
		double mirrorP[] = sys.mainMirror.getCentre();
		double mirrorN[] = sys.mainMirror.getNormal();
		
		double ccdN[] = sys.imseOptics.ccd.getNormal();
		double ccdU[] = sys.imseOptics.ccd.getUp();
		double ccdR[] = sys.imseOptics.ccd.getRight();
		
		//CCD real up frame
		double ccdFR[] = Util.reNorm(Util.cross(ccdN, globalUp));
		double ccdFU[] = Util.reNorm(Util.cross(ccdFR, ccdN));
				
		return new double[]{
				FastMath.asin(mirrorN[2]), // mirror normal altitude angle
				FastMath.atan2(mirrorN[1], mirrorN[0]), //mirror normal longitudinal angle
				FastMath.atan2(Util.dot(ccdU, ccdFR), Util.dot(ccdU, ccdFU)),	 // ccd rotation from globalUp nearest
				Util.dot(Util.minus(mirrorP, mirrorZeroPos), mirrorZeroNormal) // mirror centre shift along original normal
			};
	}

	@Override
	public double eval(double x[]) {
		double sigmaDist2 = FastMath.pow2(0.01); //sigma, fraction of image 
				
		double logP = 0;
		double nonHitPenaltyDist = 1.0;
		int nRaysPerPoint = 20;
		int maxAttempts = 2000;
		setParams(x);
		
		for(FeatureTransform.Point p : xform.getPoints()){
			//only fit points marked for fit in IMSEProc
			if(!p.includeInFit())
				continue;
			
			double sumImgX = 0, sumImgY = 0, sumImgX2 = 0, sumImgY2 = 0;
			int nHit = 0;
			
			for(int j=0; j < maxAttempts; j++){
				//trace that back down the system
				RaySegment ray = new RaySegment();
				double ccdC[] = sys.imseOptics.ccd.getCentre();
				double ccdR[] = sys.imseOptics.ccd.getRight();
				double ccdU[] = sys.imseOptics.ccd.getUp();
				ray.startPos =  new double[]{ p.x, p.y, p.z };
					
				//ray.dir = Util.minus(new double[]{0,0,0}, sys.imseOptics.ccd.getNormal()); //
				ray.dir = Tracer.generateRandomRayTowardSurface(ray.startPos, cameraBox);
				
				ray.length = Double.POSITIVE_INFINITY;
				
				ray.up = Util.cross(Util.reNorm(Util.cross(ray.dir, globalUp)), ray.dir); // This is MSE-like emission
				
				ray.E0 = new double[][]{ { 1,0,0,0 } };
			
				ray.wavelength = wavelen; 
									
				Tracer.trace(sys, ray, 100, minIntensity, traceReflections);
				
				
				//VRMLDrawer.dumpRay("/tmp/a.vrml", sys, ray, 0.005);
				
				//make sure it hit the CCD
				List<Intersection> hits = ray.getIntersections(sys.imseOptics.ccd);
				if(hits.size() <= 0){
					//try again
					continue;
					
				}else{			
					
					Intersection imgHit = hits.get(0);
					
					double planePos[] = ((Square)imgHit.surface).posXYZToPlaneUR(imgHit.pos);					
					sumImgX += planePos[1];
					sumImgY += planePos[0];
					sumImgX2 += planePos[1]*planePos[1];
					sumImgY2 += planePos[0]*planePos[0];
				
					nHit++;
				}
				
				if(nHit >= nRaysPerPoint)
					break;
				
				Pol.recoverAll();
			}
			
			double dist2; 
			if(nHit < 4){
				dist2 = nonHitPenaltyDist*nonHitPenaltyDist;
			}else{
				
				//convert to tokamak cartesian physical coordinate (yes, on the CCD, really)
			
				double planePosX = (sumImgX/nHit / sys.imseOptics.ccdWidth) + 0.5; 
				double planePosY = (sumImgY/nHit / sys.imseOptics.ccdHeight) + 0.5;
				
				
				dist2 = FastMath.pow2(planePosX - p.imgX) +
								FastMath.pow2(planePosY - p.imgY);
			}		
			logP -= dist2 / (2*sigmaDist2);
			
		}

		//prior
		double x0[] = new double[]{
				26.8 * Math.PI / 180,
				-162.4 * Math.PI / 180,
				0.0 * Math.PI / 180,
				0.00 };		
		
		double sigX[] = new double[]{
				25.0 * Math.PI / 180,
				25.0 * Math.PI / 180,
				10.0 * Math.PI / 180,
				0.02};
	
		for(int i =0; i < x.length; i++){
			logP -= (x[i] - x0[i])*(x[i] - x0[i]) / (2*sigX[i]*sigX[i]);
		}
		
		 // + prior thing to stop it being silly
		return -logP;
		
	}

	@Override
	public IDomain getDomain() { return new RectangularDomain(
			new double[]{ 26.5*Math.PI/180, -164.0*Math.PI/180, -15.0*Math.PI/180, -0.10 }, 
			new double[]{ 27.2*Math.PI/180, -161.0*Math.PI/180, 5.0*Math.PI/180, 0.10 }
			);	
	}
	
}