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


public class DynCallbackTrackLambda2 implements IDiscreteDynamicsCallback {

	ICommStrengthFunc m_edgeWeightFunc;
	int m_nNumAgents;
	String m_strFilename;
	BufferedWriter m_writer;
	
	int m_nIter = 0;

	Matrix m_laplaceMat;
	
	double m_lfLambdaLast;
	
	IExtraDataWriter m_dataWriter;
	
	public static interface IExtraDataWriter {

		public Iterator<String> getColNames();
		
		public Iterator<String> getColValues();
		
		public void setPosData(double [] arrLfPos);
		
		public void setLogicData(StateBundle [] arrStates);
	
	}
	
	/**
	 * Careful, this class currently only handles robots that live
	 * in two dimensions!!!
	 * @author mds
	 *
	 */
	public static class TrackAngleWriter implements IExtraDataWriter {
		
		int m_nNumAgents;
		int m_nDim;
		Vector<String> m_vecNames;
		Vector<String> m_vecVals;
		
		public TrackAngleWriter() {
			m_vecNames = new Vector<String>();
			m_vecVals = new Vector<String>();
			m_nNumAgents = 0;
			m_nDim = 2; // better be 2!!!
		}
		
		public Iterator<String> getColNames() {
			return m_vecNames.iterator();
		}
		
		public Iterator<String> getColValues() {
			return m_vecVals.iterator();
		}
		
		void setNumAgents(int nAgents) {
			if(nAgents == m_nNumAgents) {
				return;
			}
			m_vecNames.clear();
			m_vecVals.clear();
			for(m_nNumAgents = 0; m_nNumAgents < nAgents; ++m_nNumAgents) {
				String strName = "Agent" + ((Integer)m_nNumAgents).toString();
				m_vecNames.add(strName);
				m_vecVals.add("???");
			}
		}
		
		public void setPosData(double [] arrLfPos) {
			setNumAgents(arrLfPos.length / m_nDim);
			for(int i = 0; i < m_nNumAgents; ++i) {
				Double lfAngle = 
					Math.atan2(arrLfPos[2*i], arrLfPos[2*i+1]);
				m_vecVals.set(i, lfAngle.toString());
			}
		}
		
		public void setLogicData(StateBundle [] arrStates) {}
	
	}
	
	public static class NullDataWriter implements IExtraDataWriter {
		
		Vector<String> m_vecEmpty;
		
		public NullDataWriter() {
			m_vecEmpty = new Vector<String>();
		}
		
		public Iterator<String> getColNames() { 
			return m_vecEmpty.iterator();
		}
		
		public Iterator<String> getColValues() {
			return m_vecEmpty.iterator();
		}
		public void setPosData(double [] arrLfPos){}
		
		public void setLogicData(StateBundle [] arrStates) {}

	
	}
	
	public static class ComposeDataWriters implements IExtraDataWriter {
		
		Vector<IExtraDataWriter> m_vecWriters;
		
		public ComposeDataWriters() {
			m_vecWriters = new Vector<IExtraDataWriter>();
		}
		
		public void addWriter(IExtraDataWriter writer) {
			m_vecWriters.add(writer);
		}
		
		public Iterator<String> getColNames() {
			class Return implements Iterator<String> {
				
				Iterator<IExtraDataWriter> m_iterWriter;
				Iterator<String> m_iterString;
				
				public Return() {
					m_iterWriter = m_vecWriters.iterator();
					m_iterString = null;
				}
				
				void pump() {
					if(m_iterString == null ||
							m_iterString.hasNext() == false) {
						if(m_iterWriter.hasNext()) {
							m_iterString = m_iterWriter.next().getColNames();
						} else {
							m_iterString = null;
						}
					}
				}
				public boolean hasNext() {
					pump();
					return (m_iterString != null && m_iterString.hasNext());
				}
				
				public String next() {
					pump();
					return m_iterString.next();
				}
				
				public void remove() {}
			}
			return new Return();
		}
		
		public Iterator<String> getColValues() {
			class Return implements Iterator<String> {
				
				Iterator<IExtraDataWriter> m_iterWriter;
				Iterator<String> m_iterString;
				
				public Return() {
					m_iterWriter = m_vecWriters.iterator();
					m_iterString = null;
				}
				
				void pump() {
					if(m_iterString == null ||
							m_iterString.hasNext() == false) {
						if(m_iterWriter.hasNext()) {
							m_iterString = m_iterWriter.next().getColValues();
						} else {
							m_iterString = null;
						}
					}
				}
				
				public boolean hasNext() {
					pump();
					return (m_iterString != null && m_iterString.hasNext());
				}
				
				public String next() {
					pump();
					return m_iterString.next();
				}
				
				public void remove() {}
			};
			
			return new Return();
		}
		
		public void setPosData(double [] arrLfPos) {
			Iterator<IExtraDataWriter> iterWriter = 
				m_vecWriters.iterator();
			while(iterWriter.hasNext()) {
				iterWriter.next().setPosData(arrLfPos);
			}
		}
		
		public void setLogicData(StateBundle [] arrStates) {
			Iterator<IExtraDataWriter> iterWriter = 
				m_vecWriters.iterator();
			while(iterWriter.hasNext()) {
				iterWriter.next().setLogicData(arrStates);
			}
		}
		
	}
	
	public static class CountMovingAgents implements IExtraDataWriter {
		Vector<String> m_vecColNames;
		Vector<String> m_vecColVals;
		
		boolean m_bDoPercent;
		
		public CountMovingAgents(boolean bDoPercent) {
			m_bDoPercent = false;
			m_vecColNames = new Vector<String>();
			if(bDoPercent) {
				m_vecColNames.add("Fraction of agents moving");				
			} else {
				m_vecColNames.add("$Number of agents moving");
			}	
			m_vecColVals = new Vector<String>();
			m_vecColVals.add("???");
			m_bDoPercent = bDoPercent;
		}
		
		public CountMovingAgents() {
			m_bDoPercent = false;
			m_vecColNames = new Vector<String>();
			m_vecColNames.add("$n_{\\text{active}}$");
			m_vecColVals = new Vector<String>();
			m_vecColVals.add("???");
		}
		
		public Iterator<String> getColNames() {
			return m_vecColNames.iterator();
		}
		
		public Iterator<String> getColValues() {
			return m_vecColVals.iterator();
		}
		
		public void setPosData(double [] arrLfPos){}
		
		public void setLogicData(StateBundle [] arrStates) {
			Integer nObjCnt = 0;
			
			int nTotal=0;
			for(int i = 0; i < arrStates.length; ++i) {
				if(arrStates[i] == null) {
					continue;
				}
				++nTotal;
				if(arrStates[i].m_controlFunc.getClass()
						!= ControlFuncDoNothing.class) {
					++nObjCnt;
				}
			}
			if(m_bDoPercent) {
				Double lfObjPercent = 
					(double)nObjCnt/
					((double)Math.max(1, nTotal));
				m_vecColVals.set(0, lfObjPercent.toString());
			} else {
				m_vecColVals.set(0, nObjCnt.toString());
			}
		}
		
	}
	
	static CountMovingAgents m_countMovingAgents=null;
	static CountMovingAgents m_percentMovingAgents=null;
	
	
	protected DynCallbackTrackLambda2() {
		m_dataWriter = new NullDataWriter();
	}
	
	Double m_objLfLambdaPlus, m_objLfLambdaMinus;
	
	public DynCallbackTrackLambda2(String strFile, 
			ICommStrengthFunc func, int nNumAgents,
			double lfLambdaPlus, double lfLambdaMinus) {
		m_strFilename = strFile;
		m_edgeWeightFunc = func.makeCopy();
		m_nNumAgents = nNumAgents;
		m_dataWriter = new NullDataWriter();
		m_objLfLambdaPlus = lfLambdaPlus;
		m_objLfLambdaMinus = lfLambdaMinus;
		m_lfLambdaLast = 0.0;
	}
	
	public IExtraDataWriter countMovingDataWriter() {
		if(m_countMovingAgents == null) {
			m_countMovingAgents = new CountMovingAgents();
		}
		return m_countMovingAgents;
	}

	public IExtraDataWriter percentMovingDataWriter() {
		if(m_percentMovingAgents == null) {
			m_percentMovingAgents = new CountMovingAgents(true);
		}
		return m_percentMovingAgents;
	}

	public void setDataWriter(IExtraDataWriter dataWriter) {
		m_dataWriter = dataWriter;
	}
	
	public IExtraDataWriter accessDataWriter() {
		return m_dataWriter;
	}
	
	public void doCallback(ILogicVarBundle vars, double[] arrLfStateCont) {
		if(vars.getId() != 1) {
			return;
		}
		
		++m_nIter;
		
		int nDim = arrLfStateCont.length/m_nNumAgents;
		if(m_laplaceMat == null || 
				m_laplaceMat.getArray().length != m_nNumAgents) {
			m_laplaceMat = new Matrix(m_nNumAgents, m_nNumAgents);			
		}
		for(int i = 0; i < m_nNumAgents; ++i) {
			m_laplaceMat.set(i, i, 0.0);
		}
		for(int i = 0; i < m_nNumAgents; ++i) {
			for(int j = i+1; j < m_nNumAgents; ++j) {
				double lfDist = 0.0;
				for(int nAxis = 0; nAxis < nDim; ++nAxis) {
					double lfTmp = 
						arrLfStateCont[nDim*i+nAxis] -
						arrLfStateCont[nDim*j+nAxis];
					lfDist += lfTmp*lfTmp;
				}
				double lfWeight = 
					m_edgeWeightFunc.getStrength(Math.sqrt(lfDist));
				m_laplaceMat.set(i, j, -lfWeight);
				m_laplaceMat.set(j, i, -lfWeight);
				m_laplaceMat.set(i, i, 
						m_laplaceMat.get(i, i)+lfWeight);
				m_laplaceMat.set(j, j, 
						m_laplaceMat.get(j, j)+lfWeight);
			}
		}
		
		EigenvalueDecomposition matEigs = 
			m_laplaceMat.eig();
		double [] arrLfEigs = 
			matEigs.getRealEigenvalues();
		if(arrLfEigs[1] < 0.0) {
			System.out.println("WTF?"+  
					"No eigenvalues of a valid laplacian should be below zero");
		}
		if(arrLfEigs.length > 1) {
			if(m_lfLambdaLast > arrLfEigs[1] && 
					arrLfEigs[1] < m_objLfLambdaPlus) {
				System.out.println("Sigh, this might be an algorithm bug");
			}
			m_lfLambdaLast = arrLfEigs[1];
			doFileWrite(arrLfEigs[1], m_nIter);
		}
	}
	
	void doFileWrite(double lfLambda2,
			int nIter) {
		if(m_writer == null) {
			try {
				m_writer = 
					new BufferedWriter(new FileWriter(m_strFilename));
			} catch(Exception e) {
				return;
			}
		}
//		
		String strOut = "";
		if(nIter == 1) {
			strOut = m_objLfLambdaMinus.toString() + "\t" + 
				m_objLfLambdaPlus.toString() + "\n";
			strOut += "Number of communication rounds\t" +
				"$\\lambda_2(\\mathcal{G})$";
			Iterator<String> iter = 
				m_dataWriter.getColNames();
			while(iter.hasNext()) {
				strOut += "\t" + iter.next();
			}
			strOut += "\n";
		}
		strOut += String.valueOf(nIter) + "\t" +
			String.valueOf(lfLambda2);
		{
			Iterator<String> iterVals = 
				m_dataWriter.getColValues();
			while(iterVals.hasNext()) {
				strOut += "\t" + iterVals.next();
			}
		}
		try {
			m_writer.write(strOut+"\n");
			if(nIter%5 == 0) {
				m_writer.flush();
			}
		} catch(Exception e) {
			
		}
	}


}