package seed.minerva.apps.imse;

import jafama.FastMath;

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

import algorithmrepository.Algorithms;
import binaryMatrixFile.BinaryMatrixWriter;
import oneLiners.OneLiners;
import otherSupport.ColorMaps;
import otherSupport.RandomManager;
import otherSupport.StatusOutput;
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.imse.IMSEOptics135_50_rescaled;
import seed.minerva.optics.Util;
import seed.minerva.optics.collection.IntensityInfo;
import seed.minerva.optics.collection.PlaneAngleInfo;
import seed.minerva.optics.drawing.SVGCylindricalProjection;
import seed.minerva.optics.drawing.SVGRayDrawing;
import seed.minerva.optics.drawing.VRMLDrawer;
import seed.minerva.optics.interfaces.IsoIsoInterface;
import seed.minerva.optics.interfaces.IsoIsoStdFresnel;
import seed.minerva.optics.interfaces.NullInterface;
import seed.minerva.optics.interfaces.Reflector;
import seed.minerva.optics.optics.Box;
import seed.minerva.optics.pointSpread.PointSpreadFunction;
import seed.minerva.optics.surfaces.Plane;
import seed.minerva.optics.surfaces.Square;
import seed.minerva.optics.tracer.Tracer;
import seed.minerva.optics.types.Element;
import seed.minerva.optics.types.Intersection;
import seed.minerva.optics.types.Optic;
import seed.minerva.optics.types.Pol;
import seed.minerva.optics.types.RaySegment;
import seed.minerva.optics.types.Surface;

/** 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 p0 = 0.27; // 0.4 to 0.8 ish
	private final static double p1 = 0.81; // 0.4 to 0.8 ish
	private final static int nPoints = 30;
	//*/
	
	/** What rays to trace */
	public static double minIntensity = 0.01;
	public static boolean traceReflections = false;
	
	private static final double svgRayWidth = 0.0001;
	private static final double svgOpticWidth = 0.0003;
	
	/** 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 = 358; // 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 Plane polarisationPlane = sys.tubeOptics.PEMsFront;
	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);
		
		//rotate CCD by ~10° to IMSE frame for April2013 data
		//sys.imseOptics.ccd.rotate(sys.imseOptics.ccd.getCentre(), 
		//		Algorithms.rotationMatrix(sys.imseOptics.ccd.getNormal(), 10.0 * Math.PI/180));
		
		//sys.mainMirror.shift(new double[]{ 0, 0, 0 });
		//Util.rotateOnZ(sys.mainMirror, sys.mainMirror.getCentre(), -3.0 * Math.PI / 180);
		//sys.mainMirror.rotate(sys.mainMirror.getCentre(), 
		//		Algorithms.rotationMatrix(sys.mainMirror.getRight(), 3.0 * Math.PI/180));
		
		sys.mirrorBox.protectionCoverFront.setInterface(IsoIsoInterface.ideal());
		//sys.mirrorBox.protectionCoverFront.setInterface(IsoIsoStdFresnel.ideal());
		sys.mirrorBox.protectionCoverBack.setInterface(IsoIsoInterface.ideal());
		//sys.mirrorBox.protectionCoverBack.setInterface(IsoIsoStdFresnel.ideal());
		
		VRMLDrawer vrmlOut = new VRMLDrawer(outPath + "/pictures.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-fresnelBoth-sigma.bin", 2 + 8 + 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, Double.NaN, row0);
		
		double globaUp[] = new double[]{ 0, 0, 1 };
		
		//global test element for polarisation definition consistent for all bundles - whatever that means
		double startCentre[] = Util.plus(sys.nbiStartAll[beamIdx], Util.mul(sys.nbiUnitAll[beamIdx], (p0+p1)/2) );
		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 p = p0 + (p1 - p0) * j / (nPoints - 1); 
			double startPos[] = new double[]{ //-1.3, -1.3, 0.1 };
					sys.nbiStartAll[beamIdx][0] + p * sys.nbiUnitAll[beamIdx][0],
					sys.nbiStartAll[beamIdx][1] + p * sys.nbiUnitAll[beamIdx][1],
					sys.nbiStartAll[beamIdx][2] + p * sys.nbiUnitAll[beamIdx][2],
			};
			
			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 VxB[] = 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; i++){
				 do{
					stat.doStatus(i);
					
					Pol.recoverAll();
					
					RaySegment ray = new RaySegment();
					ray.startPos =  startPos.clone();
					ray.dir = Tracer.generateRandomRayTowardSurface(ray.startPos, initRaysTarget);
					
					ray.length = Double.POSITIVE_INFINITY;
					
					
					//ray.up = Util.cross(Util.reNorm(Util.cross(ray.dir, new double[]{ 0, 0, 1 })), ray.dir);
					//ray.up = Util.reNorm(Util.cross(sys.nbiUnitAll[beamIdx], phiDir)); // V x B for toroidal only
					//ray.up = phiDir.clone();
					
					//ray.up = Util.cross(Util.reNorm(Util.cross(ray.dir, sqU)), ray.dir);
					
					// def with btSqU, gtSqU or VxB
					//ray.up = Tracer.generateRayFanConsistentPolarisationDefinition(startPos, ray.dir, initRaysTarget.getBoundarySphereCentre(), btSqU);
					//ray.up = Tracer.generateRayFanConsistentPolarisationDefinition(startPos, ray.dir, initRaysTarget.getBoundarySphereCentre(), gtSqU);
					//ray.up = Util.cross(Util.reNorm(Util.cross(ray.dir, VxB)), ray.dir); // This is MSE-like emission 
					ray.up = Util.cross(Util.reNorm(Util.cross(ray.dir, globaUp)), ray.dir); // This is MSE-like emission
					//ray.up = Util.cross(Util.reNorm(Util.cross(ray.dir, gtSqU)), ray.dir); // This is some wtf
					
					if(nAngs > 1){
						ray.E0 = new double[nAngs][4];
						for(int k=0; k < nAngs; k++) {
							if(maxPitch > 0){ 
								//vs Bz
								
								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[]{ 0, 0, 1, 0 };						
							}else{
								//vs input angle
								double angle = k * Math.PI / (nAngs - 1);
								ray.E0[k] = new double[]{
										FastMath.cos(angle), 0,
										FastMath.sin(angle), 0
									};
							}
						}	
						
					}else{
						ray.E0 = new double[][]{ { 1,0,0,0 }, { 0,0,1,0 } };
					}
					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).posXYZToPlaneUR(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[8 + nAngs*4];
			row[0] = (sumInitPsi/nHit);
			row[1] = FastMath.sqrt((sumInitPsiSq - (sumInitPsi*sumInitPsi)/nHit)/(nHit - 1));
			row[2] = (sumMirrorIncidence/nHit);
			row[3] = FastMath.sqrt((sumMirrorIncidenceSq - (sumMirrorIncidence*sumMirrorIncidence)/nHit)/(nHit - 1));
			row[4] = (sumConeAngle/nHit);
			row[5] = maxConeAng;
			row[6] = sumImgX / nHit;
			row[7] = sumImgY / nHit;
			
			for(int k=0; k < nAngs; k++){
				row[8 + 4*k + PSI_MEAN] = sums[k][PSI_MEAN] / nHit;
				row[8 + 4*k + PSI_STDEV] = FastMath.sqrt((sums[k][PSI_STDEV] - (sums[k][PSI_MEAN]*sums[k][PSI_MEAN])/nHit) / (nHit - 1));
				row[8 + 4*k + CHI_MEAN] = sums[k][CHI_MEAN] / nHit;
				row[8 + 4*k + CHI_STDEV] = FastMath.sqrt((sums[k][CHI_STDEV] - (sums[k][CHI_MEAN]*sums[k][CHI_MEAN])/nHit) / (nHit - 1));
			}

			double pR = FastMath.sqrt(startPos[0]*startPos[0] + startPos[1]*startPos[1]);
			angsOut.writeRow(p, pR, row);
			
			System.out.println("\n---------------------------------------- "+j+" ----------------------------------------");
			System.out.println(j + "(p=" + p + "):\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();
	}

}