import Jama.Matrix;
import Jama.EigenvalueDecomposition;
import java.io.FileWriter;
import java.io.BufferedWriter;
import java.util.Vector;

public class TrackLambda2 implements ISimUICallback {

	ICommStrengthFunc m_strenFunc;
	
	Double m_lastObject;
	
	BufferedWriter m_writer;
	
	BufferedWriter m_compareMatWriter[], m_compareVelWriter[], m_velCompWriter[],
		m_lambdaVelWriter[];
	
	int m_nAgentToTrack;
	
	double m_lfTCom; 
	
	double m_arrLfDerivScratch[];
	
	Matrix m_lastLapMat;
	Matrix m_lastLowerBound;
	
	LambdaConstraintAgent m_testAgent; // take out later
	
	Matrix m_oldLaplac;
	
	public TrackLambda2() {
		m_arrLfDerivScratch = null;
		m_nAgentToTrack = -1;
		m_oldLaplac = null;
	}
	
	/**
	 * 
	 * @param nAgentToTrack
	 * @param lfVelTimesTCom
	 */
	public void setTrackParams(int nAgentToTrack, double lfTCom) {
		m_nAgentToTrack = nAgentToTrack;
		m_lfTCom = lfTCom;
	}
	
	public void setStrengthFunc(ICommStrengthFunc strenFunc) {
		m_strenFunc = strenFunc.makeCopy();
		m_testAgent = new LambdaConstraintAgent(0.125, m_strenFunc);
	}
	
	void growScratchDerivs(int nDesiredSize) {
		if(m_arrLfDerivScratch == null ||
				m_arrLfDerivScratch.length < nDesiredSize) {
			m_arrLfDerivScratch = new double[nDesiredSize];
		}
	}
	
	public boolean doCallback(double[] arrLfState, IAgent agent,
			StateBundle[] arrStates, ISensor[] arrSensors, IEnvironment env,
			CommGraph graph, double lfSimTime) {
		
		int nNumAgents = arrStates.length;
		int nDim = arrLfState.length/nNumAgents;
		Matrix Laplacian = new Matrix(nNumAgents, nNumAgents);
		
		// m_compareMatWriter[], m_compareVelWriter[], m_velCompWriter[];
		if(m_compareMatWriter == null) {
			m_compareMatWriter = new BufferedWriter[nNumAgents];
			m_compareVelWriter = new BufferedWriter[nNumAgents];
			m_velCompWriter = new BufferedWriter[nNumAgents];
			m_lambdaVelWriter = new BufferedWriter[nNumAgents];
		}
		
		if(m_nAgentToTrack < 0) {
			for(int i = 0; i < nNumAgents; ++i) {
				for(int j = i+1; j < nNumAgents; ++j) {
					double lfDistSqrd = 0.0;
					int nOffset1 = j*nDim;
					int nOffset2 = i*nDim;
					for(int nAxis = 0; nAxis < nDim; ++nAxis) {
						double lfTmp = arrLfState[nOffset1+nAxis] - arrLfState[nOffset2+nAxis];
						lfDistSqrd += (lfTmp*lfTmp);
					}
					double lfDist = Math.sqrt(lfDistSqrd);
					double lfStren = m_strenFunc.getStrength(lfDist);
					Laplacian.set(i, j, -lfStren);
					Laplacian.set(j, i, -lfStren);
					Laplacian.set(j,j, Laplacian.get(j, j)+lfStren);
					Laplacian.set(i,i, Laplacian.get(i, i)+lfStren);
				}
			}
			EigenvalueDecomposition eigsLaplace = Laplacian.eig();
			double [] arrLfEigs = eigsLaplace.getRealEigenvalues();
			double lfLambda2 = arrLfEigs[1];
			m_lastObject = new Double(lfLambda2);
			m_writer = doFileWrite(m_writer, lfLambda2, "lambda_track"+".txt");
			return false;
		}
		for(int nCurrAgent = 0; nCurrAgent < nNumAgents; ++nCurrAgent) {

			Laplacian = new Matrix(nNumAgents,nNumAgents);
			for(int i = 0; i < nNumAgents; ++i) {
				for(int j = i+1; j < nNumAgents; ++j) {
					double lfDistSqrd = 0.0;
					int nOffset1 = j*nDim;
					int nOffset2 = i*nDim;
					for(int nAxis = 0; nAxis < nDim; ++nAxis) {
						double lfTmp = arrLfState[nOffset1+nAxis] - arrLfState[nOffset2+nAxis];
						lfDistSqrd += (lfTmp*lfTmp);
					}
					double lfDist = Math.sqrt(lfDistSqrd);
					double lfStren = m_strenFunc.getStrength(lfDist);
					Laplacian.set(i, j, -lfStren);
					Laplacian.set(j, i, -lfStren);
					Laplacian.set(j,j, Laplacian.get(j, j)+lfStren);
					Laplacian.set(i,i, Laplacian.get(i, i)+lfStren);
				}
			}

			Matrix matVel = null;

			IControlFunc contFunc = null;

			try {
				contFunc = arrStates[nCurrAgent].getControlFunc();
				ControlFuncMoveDir contFuncMove = 
					(ControlFuncMoveDir)contFunc;
				double [] arrLfVel = contFuncMove.getVelCopy();

				int i = nCurrAgent;
				matVel = new Matrix(nNumAgents, nNumAgents);
				for(int j = 0; j < nNumAgents; ++j) {
					if(j == i) {
						continue;
					}
					double lfDistSqrd = 0.0;
					int nOffset1 = j*nDim;
					int nOffset2 = i*nDim;
					double lfRelVel = 0.0;
					for(int nAxis = 0; nAxis < nDim; ++nAxis) {
						double lfTmp = arrLfState[nOffset1+nAxis] - arrLfState[nOffset2+nAxis];
						lfDistSqrd += (lfTmp*lfTmp);
						lfRelVel += lfTmp*arrLfVel[nAxis];
					}
					double lfDist = Math.sqrt(lfDistSqrd);
					lfRelVel = lfRelVel/lfDist;

					double lfRateIJ = -lfRelVel*m_strenFunc.getGrad(lfDist);
					// I forget why sign change is needed -- look into this.

					matVel.set(i, j, -lfRateIJ);
					matVel.set(j, i, -lfRateIJ);
					matVel.set(i,i, matVel.get(i, i)+lfRateIJ);
					matVel.set(j,j, matVel.get(j, j)+lfRateIJ);
				}

			} catch(Exception e) {
				matVel = null;
			}


			EigenvalueDecomposition LapEigs = new EigenvalueDecomposition(Laplacian);

			double [] arrLfEigs = LapEigs.getRealEigenvalues();

			double lfLambda2 = arrLfEigs[1];

			if(nCurrAgent >= 0) {

				PairLogicVarBundle pairBundle =
					(PairLogicVarBundle)
					(arrStates[nCurrAgent].getVars());

				LambdaConstraintAgent consAgent = null;
				try {
					consAgent = (LambdaConstraintAgent)agent;
				} catch(Exception e) {
					SwitchedCompoundAgent agentParent =
						(SwitchedCompoundAgent)agent;
					IAgent agentToCast = 
						agentParent.agentFromId(nCurrAgent);
					consAgent = (LambdaConstraintAgent)agentToCast;
				}

				Vector<Matrix> vecLapBounds = 
					consAgent.getLastMatBounds(nCurrAgent);

				Double tValid = 
					consAgent.getLastBoundTime(nCurrAgent);

				if(vecLapBounds != null) {

					Matrix Laplacian2 = vecLapBounds.get(0);

					Matrix LaplacDiff = Laplacian.minus(Laplacian2);

					EigenvalueDecomposition diffEigs = new EigenvalueDecomposition(LaplacDiff);

					double [] arrLfEigsDiff = diffEigs.getRealEigenvalues();

					double lfLambdaDiff = arrLfEigsDiff[0];

					m_lastObject = new Double(lfLambdaDiff);

					System.out.println("lambda2 is "+lfLambda2+" agent is "+nCurrAgent);

					if(lfLambdaDiff < -0.15 || 
							(lfLambda2 < 0.4 && lfSimTime > 1.0)) {
						System.out.println("Way off");
						System.out.println("Time last updated is "+tValid);
						if(m_testAgent != null) {
							ILogicVarBundle varFirst = pairBundle.accessFirst();
							ILogicVarBundle varSecond = pairBundle.accessSecond();
							IPosBcast bcastPos = null;
							try {
								bcastPos = (IPosBcast)varFirst;
							} catch (Exception e) {
								bcastPos = (IPosBcast)varSecond;
							}
							Vector<Matrix> compareUs = 
								m_testAgent.getMatrixBounds(bcastPos, lfSimTime);

							Matrix testLap = null;
							if(m_oldLaplac != null) {
								testLap = Laplacian.minus(m_oldLaplac);
							}

							System.out.println("Now compare stored bounds to real bounds");

						}

						m_oldLaplac = Laplacian.copy();
					}
					m_lastLapMat = Laplacian.copy();
					m_lastLowerBound = Laplacian2.copy();

					m_compareMatWriter[nCurrAgent] = doFileWrite(
							m_compareMatWriter[nCurrAgent], 
							lfLambdaDiff, "lambda_diff"+ nCurrAgent+".txt");

					// return false;

					Vector<Matrix> vecLapVelBounds = 
						consAgent.getLastVelBounds(nCurrAgent);

					Matrix matVelLo = null;
					Matrix matVelHi = null;
					if(vecLapVelBounds != null && matVel != null) {
						matVelLo = vecLapVelBounds.firstElement();
						matVelHi = vecLapVelBounds.elementAt(1);
						if(matVelLo != null) {
							Matrix matVelDiff = matVel.minus(matVelLo);
							EigenvalueDecomposition velDiffEigs = 
								new EigenvalueDecomposition(matVelDiff);
							double [] arrLfVelDiffEigs = velDiffEigs.getRealEigenvalues();

							if(arrLfVelDiffEigs[0] < -0.000000000001) {
								System.out.println("Vel eig problem");
							}

							m_velCompWriter[nCurrAgent] = doFileWrite(
									m_velCompWriter[nCurrAgent], arrLfVelDiffEigs[0],
									"lambdaVelDiff"+ nCurrAgent+".txt");
						} else {
							m_velCompWriter[nCurrAgent] = doFileWrite(
									m_velCompWriter[nCurrAgent], 0.0,
									"lambdaVelDiff"+ nCurrAgent+".txt");
						}
					} else {
						m_velCompWriter[nCurrAgent] = doFileWrite(
								m_velCompWriter[nCurrAgent], 0.0,
								"lambdaVelDiff"+ nCurrAgent+".txt");
					}
					
					if(matVel != null && Laplacian != null) {
						EigenvalueDecomposition lapEigs = new EigenvalueDecomposition(Laplacian);
						Matrix lapEigV = lapEigs.getV();
						Matrix v2 = lapEigV.getMatrix(0, nNumAgents-1, 1, 1);
						Matrix dirTest = v2.transpose().times(matVel.times(v2));
						double lfVal2 = 0.0;
						double lfVal3 = 0.0;
						Matrix vToWatch = null;
						double [] arrLfTestEigs = null;
						Matrix dirTest4 = null;
						Matrix dirTest5 = null;
						EigenvalueDecomposition eigenFullTest = null;
						if(matVelLo != null) {
							Matrix dirTest2 = v2.transpose().times(matVelLo.times(v2));
							lfVal2 = dirTest2.get(0,0);
							
							if(Laplacian2 != null) {
								EigenvalueDecomposition lapEigsLo = new 
									EigenvalueDecomposition(Laplacian2);
								vToWatch = lapEigsLo.getV();
								Matrix v2Alt = vToWatch.getMatrix(0, 
										nNumAgents-1,1,1);
								
								Matrix v2Alt2 = vToWatch.getMatrix(1,1,0, 
										nNumAgents-1);
								
								Matrix dirTest3 = 
									v2Alt.transpose().times(matVelLo.times(v2Alt));
								
								lfVal3= dirTest3.get(0,0);
								
								dirTest4 = 
									vToWatch.transpose().times(matVelLo.times(vToWatch));
								dirTest5 = 
									vToWatch.times(matVelLo.times(vToWatch.transpose()));
								
								MathUtils.makeSymmetric(dirTest4);
								MathUtils.makeSymmetric(dirTest5);
								
								eigenFullTest = 
									new EigenvalueDecomposition(dirTest4);
								arrLfTestEigs = eigenFullTest.getRealEigenvalues();
							}
						}
						double lfVal = dirTest.get(0, 0);
						if(lfVal < -0.0000001 && lfLambda2 < 0.4) {
							System.out.println("This agent is decreasing");
						}
						m_lambdaVelWriter[nCurrAgent] = doFileWrite(
								m_lambdaVelWriter[nCurrAgent], lfVal,
								"lambdaVelCurr"+ nCurrAgent+".txt");
						
					} else {
						m_lambdaVelWriter[nCurrAgent] = doFileWrite(
								m_lambdaVelWriter[nCurrAgent], 0.0,
								"lambdaVelCurr"+ nCurrAgent+".txt");
					}


				} else {
					m_lambdaVelWriter[nCurrAgent] = doFileWrite(
							m_lambdaVelWriter[nCurrAgent], 0.0,
							"lambdaVelCurr"+ nCurrAgent+".txt");
				}
			} 

			

			if(nCurrAgent == 0) {
				m_lastObject = new Double(lfLambda2);
				m_writer = doFileWrite(m_writer, lfLambda2, "lambda_track"+".txt");
			}
		}
		return false;
	}

	BufferedWriter doFileWrite(BufferedWriter writer, double lfLambda2,
			String strFileName) {
		if(writer == null) {
			try {
				writer = 
					new BufferedWriter(new FileWriter(strFileName));
			} catch(Exception e) {
				return null;
			}
		}
		
		
		
		String strOut = String.valueOf(lfLambda2);
		try {
			writer.write(strOut+"\n");
		} catch(Exception e) {
			
		}
		return writer;
	}
	
	public Object getLastObject() {
		// TODO Auto-generated method stub
		return new Double(m_lastObject);
	}

}