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.imse.IMSEOptics135_50;
import seed.minerva.imse.IMSEOptics135_50_rescaled;
import seed.minerva.optics.Util;
import seed.minerva.optics.drawing.VRMLDrawer;
import seed.minerva.optics.interfaces.NullInterface;
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 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_rescaled());
	private FeatureTransformCubic xform;
	
	private static int maxAttempts = 50;
	private static int minHitsPerPoint = 10;
	//private static double fireRadiusAtObjLens = 20;

	/** zero position of mirror centre and the original normal (used to move it) */ 
	double mirrorZeroPos[],  mirrorZeroNormal[];
	double ccdUpZero[];
	
	public static void main(String[] args) {
		(new FitMirrorToTransform()).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);
		
		// before messing anymore, we need to know how the model thinks the CCD is rotated
		// this defines the 'zero' of the CCD. We rotate the CCD relative to this in this program.
		// but actually we want to rotate the whole MSE tube by that much, because it probably comes
		// from the PEMs mount being at that angle on the port flange.
		ccdUpZero = sys.imseOptics.ccd.getUp();
		
		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());
		}
		sys.mainMirror.setWidth(0.400);
		sys.mainMirror.setHeight(0.400);
		
		
		/*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 = 50;
		cfg.numChildren = 5;
		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("cost: " + initCost); 
		System.out.print("mirroP: "); OneLiners.dumpArray(sys.mainMirror.getCentre());
		System.out.print("mirrorN: "); OneLiners.dumpArray(sys.mainMirror.getNormal());
			
		for(int i=0; i < 700; i++) {				
			opt.refine();
			double p[] = opt.getCurrentPos();
			double cost = opt.getCurrentValue();
			//eval(p);
			System.out.print(i + ": "); dumpParams(p);			
			System.out.print(i + ": cost=" + cost);
			System.out.println();
			opt.dumpStatus();
			
			if( ((i+1) % 100) == 0 ){

				double mP[] = sys.mainMirror.getCentre();
				double mN[] = sys.mainMirror.getNormal();
				double cU[] = sys.imseOptics.ccd.getUp();
				setParams(opt.getCurrentPos());
				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");
				
			}
		}
		
		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, ccdUpZero));
		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); //expect sigma = 2cm ish 
				
		double logP = 0;
		setParams(x);
		
		for(FeatureTransform.Point p : xform.getPoints()){
			//only fit points marked for fit in IMSEProc
			if(!p.includeInFit())
				continue;
			
			//convert to tokamak cartesian physical coordinate (yes, on the CCD, really)
			double planePosX = (p.imgX - 0.5) * sys.imseOptics.ccdWidth;
			double planePosY = 	(p.imgY - 0.5) *  sys.imseOptics.ccdHeight;
			
			int nHit = 0;
			double sumD = 0;
			RaySegment ray = null;
			for(int j=0; j < maxAttempts; j++){
				
				//trace that back down the system
				ray = new RaySegment();
				double ccdC[] = sys.imseOptics.ccd.getCentre();
				double ccdN[] = sys.imseOptics.ccd.getNormal();
				double ccdR[] = sys.imseOptics.ccd.getRight();
				double ccdU[] = sys.imseOptics.ccd.getUp();
				
				ray.startPos =  new double[]{
						ccdC[0] + planePosX * ccdR[0] + planePosY * ccdU[0] - 10 * Tracer.reHitTolerance * ccdN[0],
						ccdC[1] + planePosX * ccdR[1] + planePosY * ccdU[1] - 10 * Tracer.reHitTolerance * ccdN[1],
						ccdC[2] + planePosX * ccdR[2] + planePosY * ccdU[2] - 10 * Tracer.reHitTolerance * ccdN[2], 
				};
				//ray.dir = Util.minus(new double[]{0,0,0}, sys.imseOptics.ccd.getNormal()); //
				//ray.dir = Util.reNorm(Util.minus(sys.imseOptics.objLens.getBoundarySphereCentre(), sys.imseOptics.ccd.getCentre())); 
				//ray.dir = Tracer.generateRandomRayTowardSurface(ray.startPos, sys.imseOptics.objLens);
				ray.dir = Tracer.generateRandomRayTowardSurface(ray.startPos, sys.tubeOptics.fibreEnds);
				
				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 mirror
				List<Intersection> hits = ray.getIntersections(sys.mainMirror);
				if(hits.size() <= 0){
					//
					int dummy=0; 
					
				}else{
									
					RaySegment finalRay = ray;
					while(finalRay.endHit != null){
						if(finalRay.endHit.transmittedOrdinary != null)
							finalRay = finalRay.endHit.transmittedOrdinary;
						else if(finalRay.endHit.reflectedOrdinary != null)
							finalRay = finalRay.endHit.reflectedOrdinary;
						else
							break;
					}
					
					sumD += Algorithms.distLinePoint(finalRay.startPos, 
								Util.plus(finalRay.startPos, finalRay.dir), 
								new double[]{ p.x, p.y, p.z });
					nHit++;
												
				}
				
				Pol.recoverAll();
				
				if(nHit >= minHitsPerPoint)					
					break;				
			}
			if(nHit < minHitsPerPoint){
				System.out.println("No ray from "  + p.name + " hit the mirror.");
				ray.dumpPath();
				VRMLDrawer.dumpRay("/tmp/a.vrml", sys, ray, 0.005);
				// this should anyway be dealt with by the prior thing at the bottom
				
				return Double.POSITIVE_INFINITY;
			}
			
			double avgD = sumD / nHit;			
			logP -= avgD*avgD / (2*sigmaDist2);			
		}

		//prior
		double x0[] = new double[]{
				26 * Math.PI / 180, 	//mirror latitude angle
				-162 * Math.PI / 180, 	//mirror longitude angle
				0.0 * Math.PI / 180, 	//CCD rotate
				0.0 }; 				// mirror position shift along normal
		
		double sigX[] = new double[]{
				25.0 * Math.PI / 180,
				25.0 * Math.PI / 180,
				0.05 * Math.PI / 180, 
				0.001};
	
		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[]{ 24.0*Math.PI/180, -165.0*Math.PI/180, -15.0*Math.PI/180, -0.10 }, 
			new double[]{ 28.0*Math.PI/180, -161.0*Math.PI/180, 5.0*Math.PI/180, 0.10 }
			);	
	}
	
}