package ipp.imse.fusionOptics.apps.imse;

import ipp.imse.fusionOptics.imse.IMSEOptics135_50;
import ipp.imse.fusionOptics.imse.IMSEOptics135_50_rescaled;
import ipp.imse.fusionOptics.mse.AugMSESystem;
import ipp.imse.fusionOptics.mse.AugMseFaroData;
import jafama.FastMath;

import java.util.Arrays;
import java.util.List;

import fusionOptics.MinervaOpticsSettings;
import fusionOptics.Util;
import fusionOptics.collection.IntensityInfo;
import fusionOptics.collection.PlaneAngleInfo;
import fusionOptics.drawing.SVGCylindricalProjection;
import fusionOptics.drawing.SVGRayDrawing;
import fusionOptics.drawing.VRMLDrawer;
import fusionOptics.interfaces.IsoIsoInterface;
import fusionOptics.interfaces.IsoIsoStdFresnel;
import fusionOptics.interfaces.NullInterface;
import fusionOptics.interfaces.Reflector;
import fusionOptics.optics.Box;
import fusionOptics.pointSpread.PointSpreadFunction;
import fusionOptics.surfaces.Plane;
import fusionOptics.surfaces.Square;
import fusionOptics.tracer.Tracer;
import fusionOptics.types.Element;
import fusionOptics.types.Intersection;
import fusionOptics.types.Optic;
import fusionOptics.types.Pol;
import fusionOptics.types.RaySegment;
import fusionOptics.types.Surface;

import algorithmrepository.Algorithms;
import binaryMatrixFile.BinaryMatrixWriter;
import oneLiners.OneLiners;
import otherSupport.ColorMaps;
import otherSupport.RandomManager;
import otherSupport.StatusOutput;

/** try to work out the various forms of θ=0 at the CCD */
public class WhichWayIsUp {

	private final static String outPath = MinervaOpticsSettings.getAppsOutputPath() + "/rayTracing/augImse/whichWayIsUp/";
	private final static int nRays = 1000;
	private final static int nRaysDraw = 1;
	private final static double R0 = 1.5;
	private final static double R1 = 2.2;
	private final static int nPoints = 25; //every 2cm
	//*/
	
	public static boolean includeProtectionCoverFresnel = false;
	
	/** What rays to trace */
	public static double minIntensity = 0.01;
	public static boolean traceReflections = false;
	
	/** Optical system setup */
	private static AugMSESystem sys = new AugMSESystem(new IMSEOptics135_50_rescaled());
	
	// IMSE or AUG pulse number for matched hardware config
	//private final static int pulse = 178; // Jan2013 primary
	private final static int pulse = 351; // Apr2013 primary

	private static int beamIdx = AugMSESystem.BEAM_Q3;
	
	private static Plane imagePlane = sys.imseOptics.ccd;
	private static Plane polarisationPlane = sys.imseOptics.plate1;
	private static Optic cameraBox = new Box("camera", (new AugMseFaroData()).getCameraPos(), 0.05, 0.05, 0.05, null, NullInterface.ideal());
	private static Element initRaysTarget = sys.mainMirror; //sys.mirrorBox.holeGlassFront;
	
	private static final double cols[][] = ColorMaps.jet(nPoints);
	
	private final static double wavelen = 653e-9;
	
	private final static double maxPitch = 20 * Math.PI / 180;
	private final static int nAngs = 20; 
	
	public static void main(String[] args) {
		run();
	}
	
	private static final int PSI_MEAN = 0;
	private static final int PSI_STDEV = 1;
	private static final int CHI_MEAN = 2;
	private static final int CHI_STDEV = 3;
	
	private static void run(){
		
		sys.setupForPulse(pulse);
		sys.addElement(cameraBox);
		
		sys.mirrorBox.protectionCoverFront.setInterface(includeProtectionCoverFresnel ? IsoIsoStdFresnel.ideal() : IsoIsoInterface.ideal());
		sys.mirrorBox.protectionCoverBack.setInterface(includeProtectionCoverFresnel ? IsoIsoStdFresnel.ideal() : IsoIsoInterface.ideal());
		
		VRMLDrawer vrmlOut = new VRMLDrawer(outPath + "/rays.vrml");
		vrmlOut.setSmallLineLength(0.002);
		vrmlOut.setDrawPolarisationFrames(true);
		vrmlOut.addVRML(AugMSESystem.vrmlScaleToAUGDDD);
		vrmlOut.setSkipRays(9);
		
		//BinaryMatrixWriter angsOut = new BinaryMatrixWriter(outPath + "/polAngs-final-vs-init-BzScan-noFresnel-sigma.bin", 2 + 8 + nAngs*4);
		BinaryMatrixWriter angsOut = new BinaryMatrixWriter(outPath + "/polAngs-final-vs-init-BzScan-sigma.bin", 9 + nAngs*4);
		double row0[] = new double[8 + nAngs*4];
		for(int k=0; k < nAngs; k++){
			row0[8 + 4*k + PSI_MEAN] = k * (maxPitch < 0 ? Math.PI : maxPitch) / nAngs;
		}
		angsOut.writeRow(Double.NaN, row0);
		
		double globaUp[] = new double[]{ 0, 0, 1 };
		
		//global test element for polarisation definition consistent for all bundles - whatever that means
		double midR = (R0 + R1) / 2;
		double midT = Algorithms.min(Algorithms.cylinderLineIntersection(AugMSESystem.nbiStartAll[beamIdx], AugMSESystem.nbiUnitAll[beamIdx],
				new double[]{ 0, 0, 0 }, new double[]{ 0, 0, 1 },midR*midR));
		double startCentre[] = Util.plus(AugMSESystem.nbiStartAll[beamIdx], Util.mul(AugMSESystem.nbiUnitAll[beamIdx], midT));
		double cPath[] = Util.minus(cameraBox.getBoundarySphereCentre(), startCentre);
		double gtSqN[] = Util.reNorm(cPath);
		double gtSqC[] = Util.plus(startCentre, Util.mul(gtSqN, 2*Util.length(cPath)/3) );
		double gtSqU[] = Util.reNorm(Util.cross(Util.cross(gtSqN, globaUp), gtSqN));
		Square globalTestSquare = new Square("testSquare", gtSqC, gtSqN, gtSqU, 0.3, 0.3, null, null, NullInterface.ideal());
		sys.addElement(globalTestSquare);
		
						
		StatusOutput stat = new StatusOutput(WhichWayIsUp.class, nPoints*nRays);		
		for(int j=0; j < nPoints; j++){
			double R = R0 + (R1 - R0) * j / (nPoints - 1);
			double t = Algorithms.min(Algorithms.cylinderLineIntersection(sys.nbiStartAll[beamIdx], sys.nbiUnitAll[beamIdx], 
					new double[]{ 0, 0, 0 }, new double[]{ 0, 0, 1 }, R*R));
			
			double startPos[] = Util.plus(sys.nbiStartAll[beamIdx], Util.mul(sys.nbiUnitAll[beamIdx], t));
			
			int nAttempts = 0, nHit = 0;
			
			double sums[][] = new double[nAngs][4];
			
			double sumConeAngle = 0, sumConeAngleSq = 0, maxConeAng = 0;; //angle of incidence on mirror
			
			double sumMirrorIncidence = 0, sumMirrorIncidenceSq = 0; //angle of incidence on mirror
			double sumInitPsi = 0, sumInitPsiSq = 0; // VxBφ - 'up' at beam
			double sumImgX=0, sumImgY=0, sumImgXSq=0, sumImgYSq=0;
			double sumPCAng = 0;
			
			//Bφ field direction at emission point
			double Bphi[] = Util.reNorm(Util.cross(startPos, new double[]{ 0, 0, 1 }));
			
			// VxBφ direction at emission, for Q3
			double VxBphi[] = Util.reNorm(Util.cross(sys.nbiUnitAll[beamIdx], Bphi)); // V x B for toroidal only
			
			
			//test element for polarisation consistent for bundle only
			double bPath[] = Util.minus(cameraBox.getBoundarySphereCentre(), startPos);
			double btSqN[] = Util.reNorm(bPath);
			double btSqC[] = Util.plus(startPos, Util.mul(btSqN, 1*Util.length(bPath)/3) );
			double btSqU[] = Util.reNorm(Util.cross(Util.cross(btSqN, globaUp), btSqN));
			Square bundleTestSquare = new Square("bundleTestSquare", btSqC, btSqN, btSqU, 0.05, 0.05, null, null, NullInterface.ideal());
			sys.addElement(bundleTestSquare);
			
			//sqU is now the general 'up' direction for this emission point 		
						
			for(int i=0; i < (nRays-2); i++){
				 do{
					stat.doStatus(j*nRays + i);
					
					Pol.recoverAll();
					
					RaySegment ray = new RaySegment();
					ray.startPos =  startPos.clone();
					ray.dir = Tracer.generateRandomRayTowardSurface(ray.startPos, initRaysTarget);
					
					ray.length = Double.POSITIVE_INFINITY;
					
					//start with global up aligned - this should not change the output
					ray.up = Util.cross(Util.reNorm(Util.cross(ray.dir, globaUp)), ray.dir);
					
					ray.E0 = new double[nAngs][4];
					for(int k=0; k < nAngs; k++) {
						if(maxPitch > 0){ 
							double pitch = k * maxPitch / (nAngs - 1);
							//B with Bphi plus some Bz (downwards is normal +ve Ip)
							double B[] = Util.plus( Util.mul(Bphi, FastMath.cos(pitch)), new double[]{ 0, 0, -FastMath.sin(pitch) });
							
							double starkE[] = Util.reNorm(Util.cross(sys.nbiUnitAll[beamIdx], B)); // V x B
							
							ray.rotatePolRefFrame(starkE);
							
							ray.E0[k] = new double[]{ 1, 0, 0, 0 }; //sigma-like						
						}
					}
					ray.wavelength = wavelen; 
										
					Tracer.trace(sys, ray, 100, minIntensity, traceReflections);
					
					stat.doStatus(j*nRays+i);
					
					List<Intersection> hits = ray.getIntersections(imagePlane); 
									
					if(hits.size() > 0){
						if(nHit < nRaysDraw){
							vrmlOut.drawRay(ray, cols[j]);
						}
						
						Intersection imgHit = hits.get(0);
						
						double planePos[] = ((Square)imgHit.surface).posXYZToPlaneRU(imgHit.pos);
						sumImgX += planePos[0];
						sumImgXSq += planePos[0]*planePos[0];
						sumImgY += planePos[1];
						sumImgYSq += planePos[1]*planePos[1];
						
						Intersection polHit = imgHit.incidentRay.findFirstEarlierIntersection(polarisationPlane);
						if(polHit == null)
							continue;
						
						Intersection initHit = imgHit.incidentRay.findFirstEarlierIntersection(bundleTestSquare);
						double coneAngle, initPsi;
						if(initHit != null){
	
							coneAngle = FastMath.acos(Util.dot(initHit.incidentRay.dir, ((Plane)initHit.surface).getNormal()));
													
							double Eproj0[][] = Pol.projectToPlanesView(initHit, false);						
							//ray.rotatePolRefFrame(new double[]{ 0, 0, 1 });						
							initPsi = Pol.psi(Eproj0[0]);
						}else{
							coneAngle = Double.NaN;
							initPsi = Double.NaN;
						}
						
						double Eproj[][] = Pol.projectToPlanesView(polHit, false);
						//hits.get(0).incidentRay.rotatePolRefFrame(new double[]{ 0, 0, 1 }); //sys.tubeOptics.fibreEnds.getUp());						
						//double Eproj[][] = hits.get(0).incidentRay.E1;
						
						
						for(int k=0; k < nAngs; k++){
							double psi = Pol.psi(Eproj[k]);
							double chi = Pol.chi(Eproj[k]);
							sums[k][PSI_MEAN] += psi;
							sums[k][PSI_STDEV] += psi * psi;
							sums[k][CHI_MEAN] += chi;
							sums[k][CHI_STDEV] += chi * chi; 
						}
													
						Intersection mirrorHit = polHit.incidentRay.findFirstEarlierIntersection(sys.mainMirror);
						double mirrorIncidence = Math.PI - FastMath.acos(Util.dot(mirrorHit.incidentRay.dir, mirrorHit.normal));

						sumInitPsi += initPsi;
						sumInitPsiSq += initPsi * initPsi;
						
						sumMirrorIncidence += mirrorIncidence;
						sumMirrorIncidenceSq += mirrorIncidence * mirrorIncidence;
						
						sumConeAngle += coneAngle;
						sumConeAngleSq += coneAngle * coneAngle;
						maxConeAng = Math.max(maxConeAng, coneAngle);
						
						Intersection glassHit = mirrorHit.incidentRay.findFirstEarlierIntersection(sys.mirrorBox.protectionCoverFront);
						double Eprj[][] = Pol.projectToPlanesView(glassHit, false);
						double polAtGlassAng = Pol.psi(Eprj[0]);
						sumPCAng += polAtGlassAng;
						
						nHit++;
						break;
					}
					if(nAttempts > 100000){
						break;
					}
					nAttempts++;
				}while(true);
			}
			
			double row[] = new double[9 + nAngs*4];
			row[0] = R;
			row[1] = (sumInitPsi/nHit);
			row[2] = FastMath.sqrt((sumInitPsiSq - (sumInitPsi*sumInitPsi)/nHit)/(nHit - 1));
			row[3] = (sumMirrorIncidence/nHit);
			row[4] = FastMath.sqrt((sumMirrorIncidenceSq - (sumMirrorIncidence*sumMirrorIncidence)/nHit)/(nHit - 1));
			row[5] = (sumConeAngle/nHit);
			row[6] = maxConeAng;
			row[7] = sumImgX / nHit;
			row[8] = sumImgY / nHit;
			
			for(int k=0; k < nAngs; k++){
				row[9 + 4*k + PSI_MEAN] = sums[k][PSI_MEAN] / nHit;
				row[9 + 4*k + PSI_STDEV] = FastMath.sqrt((sums[k][PSI_STDEV] - (sums[k][PSI_MEAN]*sums[k][PSI_MEAN])/nHit) / (nHit - 1));
				row[9 + 4*k + CHI_MEAN] = sums[k][CHI_MEAN] / nHit;
				row[9 + 4*k + CHI_STDEV] = FastMath.sqrt((sums[k][CHI_STDEV] - (sums[k][CHI_MEAN]*sums[k][CHI_MEAN])/nHit) / (nHit - 1));
			}

			angsOut.writeRow(row);
			
			System.out.println("\n---------------------------------------- "+j+" ----------------------------------------");
			System.out.println(j + "(R=" + R + "):\t" + nAttempts + "\t" + nHit);
			System.out.println("psi vs up at beam = " + row[0]*180/Math.PI + " ± " + row[1]*180/Math.PI );
			System.out.println("psi = " + row[PSI_MEAN]*180/Math.PI + " ± " + row[PSI_STDEV]*180/Math.PI );
			System.out.println("chi = " + row[CHI_MEAN]*180/Math.PI + " ± " + row[CHI_STDEV]*180/Math.PI );
			System.out.println("mirror ang = " + row[2]*180/Math.PI + " ± " + row[3]*180/Math.PI );
			System.out.println("pol0 at prot glass = " + (sumPCAng/nHit)*180/Math.PI);
			
		}

		angsOut.close();
		vrmlOut.drawOptic(sys);
		vrmlOut.addVRML("}"); //end of rotate/transform
		vrmlOut.destroy();
		stat.done();
	}
}
