package seed.minerva.aug.mse;

import jafama.FastMath;

import java.util.LinkedList;
import java.util.List;

import binaryMatrixFile.AsciiMatrixFile;

import otherSupport.StatusOutput;

import seed.minerva.MinervaOpticsSettings;
import seed.minerva.optics.Util;
import seed.minerva.optics.interfaces.NullInterface;
import seed.minerva.optics.optics.Box;
import seed.minerva.optics.surfaces.Cylinder;
import seed.minerva.optics.types.Element;
import seed.minerva.optics.types.Optic;
import algorithmrepository.Algorithms;

/** Manipulations of the AUG MSE FARO Line of sight measurements */
public class AugMseFaroData {
	private double losDiameter = 0.03; //(m). Well, kind of
	
	private double data[][];
	
	private double cameraUpVec[];
	private double hFov = 0, vFov = 0;
	private double cameraPos[];
	private double cameraViewVec[];
	private int nChans;
	private int nFibres[];
	
	/** point A on LOS [chan][fibre][x/y/z] */
	private double A[][][];
	/** point B on LOS [chan][fibre][x/y/z] */
	private double B[][][];
	/** unit vec of LOS [chan][fibre][x/y/z] */
	private double u[][][];
	
	public AugMseFaroData() {
		data = AsciiMatrixFile.mustLoad(MinervaOpticsSettings.getMinervaSourcePath() + "/minerva-aug/data/mse-los-faro-nov2013.txt", true);
		
		nChans = (int)Algorithms.max(data[0]);
		nFibres = new int[nChans];
		for(int i=0; i < data[0].length; i++){
			nFibres[(int)data[0][i]-1]++;
		}
		
		A = new double[nChans][][];
		B = new double[nChans][][];
		u = new double[nChans][][];
		for(int ch=0; ch < nChans; ch++){
			A[ch] = new double[nFibres[ch]][];
			B[ch] = new double[nFibres[ch]][];
			u[ch] = new double[nFibres[ch]][];
		}
		
		for(int i=0; i < data[0].length; i++){
			int ch = (int)data[0][i] - 1;
			int f = (int)data[1][i] - 1;
			
			A[ch][f] = new double[]{ data[2][i], data[3][i], data[4][i] };
			B[ch][f] = new double[]{ data[5][i], data[6][i], data[7][i] };
			u[ch][f] = Util.reNorm(Util.minus(B[ch][f], A[ch][f]));
		}
		
		
		calcFAROConvergencePoint();
	}
	
	public Optic makeCylds() { return makeCylds(3.0); }
		
	/** Make cylinders representing the FARO measured MSE LOSs */
	public Optic makeCylds(double length) {
		double diameter = 0.001; 
		
		LinkedList<Element> cylds = new LinkedList<Element>();
		
		for(int i=0; i < data[0].length; i++){
			double u[] = Util.minus(
					new double[]{ data[2][i], data[3][i], data[4][i] },
					new double[]{ data[5][i], data[6][i], data[7][i] });
			double l = length*Util.dot(u,u);
			u = Util.reNorm(u);
				
			Cylinder cyld = new Cylinder(
					"ch"+((int)data[0][i])+"_fibre"+((int)data[1][i]),
					new double[]{
							(data[2][i] + data[5][i])/2,
							(data[3][i] + data[6][i])/2, 
							(data[4][i] + data[7][i])/2
					},
					u,
					diameter/2,
					l,
					NullInterface.ideal());

			cyld.setDrawingDetails(6, 5);
			cylds.add(cyld);
		}
		
		return new Optic("faroCylds", cylds);
	}
	
	/** Return the approximate convergence point of all the FARO measured LOSs, the average unit vector
	 * of view and the horizontal and vertical field of view which makes a LOSFanCamera as close to the
	 * actual MSE view as possible.	 */
	
	public double[] getCameraUpVec(){ return cameraUpVec; }
	public double getHorizFieldOfView(){ return hFov; }
	public double getVertFieldOfView(){ return vFov; }
	public double[] getCameraPos(){ return cameraPos; }
	public double[] getCameraViewVec(){ return cameraViewVec; }
	
	
	/** Calculates the approximate convergence point of all the LOSes,
	 * which is the effective observation position.  
	 * From this, calculates the average vector from that point - the camera view vector.
	 * and then the maximum angles away from the view vector - the field of view.
	 * */
	private void calcFAROConvergencePoint(){
		cameraPos = new double[3];
		cameraViewVec = new double[3];
		int nD = data[0].length;
		int n=0;
		
		
		for(int chA=0; chA < nChans; chA++){
			for(int fA=0; fA < nFibres[chA];fA++){
			
				for(int chB=0; chB < nChans; chB++){
					for(int fB=0; fB < nFibres[chB]; fB++){
						if(chA==chB && fA == fB)
							continue;
				
						double s = Algorithms.pointOnLineNearestAnotherLine(A[chA][fA], u[chA][fA], A[chB][fB], u[chB][fB]);

						cameraPos[0] += A[chA][fA][0] + s * u[chA][fA][0];
						cameraPos[1] += A[chA][fA][1] + s * u[chA][fA][1];
						cameraPos[2] += A[chA][fA][2] + s * u[chA][fA][2];
						n++;
					}
				}

				cameraViewVec[0] += u[chA][fA][0];
				cameraViewVec[1] += u[chA][fA][1];
				cameraViewVec[2] += u[chA][fA][2];
			}
		}

		cameraPos[0] /= n; cameraPos[1] /= n; cameraPos[2] /= n;
		cameraViewVec[0] /= -nD; cameraViewVec[1] /= -nD; cameraViewVec[2] /= -nD;
		cameraViewVec = Util.reNorm(cameraViewVec);
		
		double rightVec[] = Util.cross(cameraViewVec, new double[]{0,0,1});
		cameraUpVec = Util.reNorm(Util.cross(rightVec, cameraViewVec));
		
		for(int chA=0; chA < nChans; chA++){
			for(int fA=0; fA < nFibres[chA];fA++){
				double v = Util.dot(u[chA][fA], cameraUpVec);
				double h = Util.dot(u[chA][fA], rightVec);
				if(v > vFov)
					vFov = v;
				if(h > hFov)
					hFov = h;
			}
		}
		
		vFov = 2*Math.asin(vFov);
		hFov = 2*Math.asin(hFov);
		
	}
	
	public List<Element> intBoxes = new LinkedList<Element>();
	/** Returns points and LOS vectors for intersections of a FARO view cylinder
	 * and the given neutral beam. (for integrating)
	 *  
	 *  @param maxD Intergration step dX/dY/dZ 
	 * @return double[ch][pos/losVec/upVec][idx][x/y/z]
	 */	
	public double[][][][] getPoints(double maxD, double beamStart[], double beamDir[], double beamDiameter, int singleFibre){
		
		int nW = (int)(losDiameter / maxD);
		if(nW <= 0)nW = 1;
		
		double ret[][][][] = new double[nChans][][][];
		//foreach channel
		StatusOutput stat = new StatusOutput(AugMseFaroData.class, nChans*nFibres[0]);
		for(int ch=0; ch < nChans; ch++){
			LinkedList<double[]> posList = new LinkedList<double[]>();
			LinkedList<double[]> losList = new LinkedList<double[]>();
			LinkedList<double[]> upList = new LinkedList<double[]>();
			
			//foreach fibre in channel
			for(int f=0; f < nFibres[ch]; f++){
				if(singleFibre >= 0 && f != singleFibre)
					continue;
				
				double losVec[] = Util.reNorm(new double[]{ -u[ch][f][0],-u[ch][f][1], -u[ch][f][2] });
				double losUp[] = Util.createPerp(losVec);
				double losRight[] = Util.cross(losVec, losUp);
				
				//find point of closest approach to beam
				double s0 = Algorithms.pointOnLineNearestAnotherLine(A[ch][f], losVec, beamStart, beamDir);
				//if(d > (beamDiameter + losDiameter) / 2){
				//	continue; //nowhere near it, try next fibre 
				//}
				
				
				//we want to integrate roughly between +/- beamDiameter/2 of that point on this LOS
				int nS = (int)(beamDiameter / maxD);
				if(nS <= 0)nS = 1;
				for(int iS=0; iS < nS; iS++){ //foreach plabne along fibre LOS
					double s = (nS == 1) ? s0 : s0 - beamDiameter/2 + iS*maxD;
					for(int iQ=0; iQ < nW; iQ++){
						double q = (nW == 1) ? 0 : -losDiameter/2 + iQ*maxD;
						for(int iR=0; iR < nW; iR++){
							double r = (nW == 1) ? 0 : -losDiameter/2 + iR*maxD;
							if(q*q + r*r > losDiameter*losDiameter/4)
								continue;
							
							double pos[] = new double[]{
									A[ch][f][0] + s * losVec[0] + q * losUp[0] + r * losRight[0],
									A[ch][f][1] + s * losVec[1] + q * losUp[1] + r * losRight[1],
									A[ch][f][2] + s * losVec[2] + q * losUp[2] + r * losRight[2],
							};
							
							posList.add(pos);
							
							double actualLOS[] = Util.reNorm(Util.minus(pos, cameraPos));
							double actualRight[] = Util.reNorm(Util.cross(actualLOS, new double[]{0,0,1}));
							double actualUp[] = Util.reNorm(Util.cross(actualRight, actualLOS));
								
							losList.add(actualLOS);
							upList.add(actualUp);
							intBoxes.add(new Box("ch"+ch+"_f"+f+"_s"+iS+"_q"+iQ+"_r"+iR, pos, 0.001, 0.001, 0.001, null, NullInterface.ideal()));
						}	
					}
					
					stat.doStatus(ch*nFibres[0]+f);
				}
			}
			
			ret[ch] = new double[][][]{
					new double[3][posList.size()],
					new double[posList.size()][],
					new double[posList.size()][],
			};
			
			int i = 0;
			for(double pos[] : posList){
				ret[ch][0][0][i] = pos[0];
				ret[ch][0][1][i] = pos[1];
				ret[ch][0][2][i] = pos[2];
				ret[ch][1][i] = losList.poll();
				ret[ch][2][i] = upList.poll();
				i++;
			}
		}
		
		return ret;
	}
	
	/** @return [chan][fibre][R/φ/x/y/z] */
	public double[][][] getBeamClosestApproaches(double beamStart[], double beamDir[]){
		
		double ret[][][] = new double[nChans][][];
		//foreach channel
		for(int ch=0; ch < nChans; ch++){
			ret[ch] = new double[nFibres[ch]][5];
			
			//foreach fibre in channel
			for(int f=0; f < nFibres[ch]; f++){				
				
				double losVec[] = Util.reNorm(new double[]{ -u[ch][f][0],-u[ch][f][1], -u[ch][f][2] });
				
				//find point of closest approach to beam
				double s = Algorithms.pointOnLineNearestAnotherLine(A[ch][f], losVec, beamStart, beamDir);
	
				double pos[] = new double[]{
						A[ch][f][0] + s * losVec[0],
						A[ch][f][1] + s * losVec[1],
						A[ch][f][2] + s * losVec[2],
				};
				
				ret[ch][f][0] = FastMath.sqrt(pos[0]*pos[0] + pos[1]*pos[1]);
				ret[ch][f][1] = FastMath.atan2(pos[1], pos[0]);
				ret[ch][f][2] = pos[0];
				ret[ch][f][3] = pos[1];
				ret[ch][f][4] = pos[2];
			}
		}
		
		return ret;
	}

	/** unit vec of LOS [chan][fibre][x/y/z] */
	public double[][][] getUnitVecs(){ return this.u; }
	/** first point of FARO measurement [chan][fibre][x/y/z] */
	public double[][][] getPosA(){ return this.A; }
	/** first point of FARO measurement [chan][fibre][x/y/z] */
	public double[][][] getPosB(){ return this.B; }
	
}