import java.util.Set;
import java.util.HashSet;
import java.util.Iterator;


/**
 *
 */

public class NonInteractiveSim {
    
    ISimUICallback m_callbackAgent;

    INumericalIntegrator m_numInt;
    int m_nNumAgents, m_nDim;
    double m_lfTComm;
    //    ISimUICallback m_callbackAgent, m_callbackHalt;
    IEnvironment m_env;
    //    IProjection m_proj;
    double [] m_arrLfInitialPositions;
    double [] m_arrLfPositions;
    double [] m_arrLfPositionsLast;
    double [] m_arrLfPositionsLast2;
    
    StateBundle [] m_arrStates;
    StateBundle [] m_arrStatesLast;
    StateBundle [] m_arrStatesLast2;
    ISensor [] m_arrSensors;

    ICommunicationScheduler m_sched;
    IProxGraph m_proxGraph;
    IProxGraphOptWrapper m_opt;

    IInitializer m_init;
    IAgent m_agent;

    public NonInteractiveSim() {
	trivSetupNagents(12, 100, 1.0, new ControlFuncDoNothing(2), 
			 new LogicVarIntLookup());	
    }

    public IInitializer getInit() { return m_init; }

    void xferPositions() {
    	m_arrLfInitialPositions = new double[m_arrLfPositions.length];
    	for(int i = 0; i < m_arrLfPositions.length; ++i) {
    		m_arrLfInitialPositions[i] = m_arrLfPositions[i];
    	}
    }
    
    void doStateCopy() {
    	int i;
    	if(m_arrStatesLast != null) {
    		if(m_arrStatesLast2 == null) {
    			m_arrStatesLast2 = new StateBundle[m_arrStatesLast.length];
    		}
    		if(m_arrStatesLast2.length < m_arrStatesLast.length) {
    			m_arrStatesLast2 = new StateBundle[m_arrStatesLast.length];	
    		}


    		for(i = 0; i < m_arrStatesLast.length; ++i) {
    			m_arrStatesLast2[i] = m_arrStatesLast[i];
    		}
    	}

    	if(m_arrLfPositionsLast != null) {
    		if(m_arrLfPositionsLast2 == null) {
    			m_arrLfPositionsLast2 = new double[m_arrLfPositionsLast.length];
    		}
    		if(m_arrLfPositionsLast2.length < m_arrLfPositionsLast.length) {
    			m_arrLfPositionsLast2 = new double[m_arrLfPositionsLast.length];	
    		}

    		for(i = 0; i < m_arrLfPositionsLast.length; ++i) {
    			m_arrLfPositionsLast2[i] = m_arrLfPositionsLast[i];
    		}
    	}
    	
    	if(m_arrStatesLast == null) {
    		m_arrStatesLast = new StateBundle[m_arrStates.length];
    	}
    	if(m_arrStatesLast.length < m_arrStates.length) {
    		m_arrStatesLast = new StateBundle[m_arrStates.length];	
    	}
    	
    	if(m_arrLfPositionsLast == null) {
    		m_arrLfPositionsLast = new double[m_arrLfPositions.length];
    	}
    	if(m_arrLfPositionsLast.length < m_arrLfPositions.length) {
    		m_arrLfPositionsLast = new double[m_arrLfPositions.length];	
    	}
    	
    	
    	for(i = 0; i < m_arrLfPositions.length; ++i) {
    		m_arrLfPositionsLast[i] = m_arrLfPositions[i];
    	}
    	
    	for(i = 0; i < m_arrStates.length; ++i) {
    		m_arrStatesLast[i] = m_arrStates[i];
    	}
    }
    
    public double [] getIniPos() 
    { 
    	double [] arrLfOut = new double[m_arrLfInitialPositions.length];
    	for(int i = 0; i < arrLfOut.length; ++i) {
    		arrLfOut[i] = m_arrLfInitialPositions[i];
    	}
    	return arrLfOut;
    }

	public static interface IAgentInit {
		public int getNumAgents();
		
		public ILogicVarBundle getVars(int nAgent);
	}

    
    public void trivSetupNagents(int n, double lfSize, double lfAgentRad,
				 IControlFunc cntFncDefault,
				 ILogicVarBundle logicDefault) {
	m_callbackAgent = null;
	
	if(lfSize > n*lfAgentRad) {
		lfSize = n*lfAgentRad;
	}
	
	double arrLfMin[] = new double[2];
	double arrLfMax[] = new double[2];
	arrLfMin[0] = arrLfMin[1] = -0.5*lfSize;
	arrLfMax[0] = arrLfMax[1] = 0.5*lfSize;
	
	m_env = new NdTrivialEnv(arrLfMin, arrLfMax);
	m_arrLfPositions = new double[2*n];

	m_arrSensors = new ISensor[n];

	m_nDim = 2; m_nNumAgents = n;

	if(m_lfTComm > 0.0) {
	    m_sched = new SynchronousScheduler(m_lfTComm, n);
	}
	m_init = new RandomConnectedInit(m_nNumAgents, arrLfMin,
					 arrLfMax, lfAgentRad);
	
	m_init.resetState();
	
	m_nNumAgents = m_init.getNumAgents();
					 
	m_init.setDefaultIniControlFunc(cntFncDefault);
	m_init.setDefaultIniVars(logicDefault);

	setScheduleAndGraph(0.01, new RdiskGraph(lfAgentRad));
	m_agent = null;

	m_numInt = new Rk4Naive(0.001);

		
	m_arrLfPositions = m_init.getInitContState();
	m_arrStates = m_init.getInitDiscreteState();
	
	xferPositions();

    }

    public void setAgent(IAgent agent) {
	m_agent = agent;
    }

    public void setScheduleAndGraph(double lfTime, IProxGraph proxGraph) {
	m_lfTComm = lfTime;
	m_proxGraph = proxGraph;
	if(m_nNumAgents > 0) {
	    m_sched = new SynchronousScheduler(m_lfTComm, m_nNumAgents);
	}
    }

    public NonInteractiveSim(ISimUICallback callbackAgent) {
	m_callbackAgent = callbackAgent;
    }

    public void setEnv(IEnvironment env) {
	m_env = env;
    }

    public void setupSim() {
    }

    public boolean checkRdiskValid(double lfRad, double [] arrLfPos, 
    		int nDim, CommGraph graph) 
    {	
    	boolean bResult = true;
    	int nBadCount = 0;
    	
    	int nAgents = arrLfPos.length/nDim;
    	
    	for(int i = 0; i < nAgents; ++i) {
    		Iterator<CommLink> currIter = graph.getEdgesTo(i);
    		
    		while(currIter.hasNext()) {
    			CommLink linkNext = currIter.next();
    			int nIdxFrom = linkNext.from();
    			
    			double lfDist = 0.0;
    			for(int nAxis = 0; nAxis < nDim; ++nAxis) {
    				double lfTmp = arrLfPos[i*nDim+nAxis] -
    					arrLfPos[nIdxFrom*nDim+nAxis];
    				lfDist += lfTmp*lfTmp;
    			}
    			if(lfDist > lfRad*lfRad) {
    				bResult = false;
    				++nBadCount;
    			}
    		}
    	}
    	
    	return bResult;
    }
    
    public void runSim(ISimUICallback callbackAgent, double lfMaxTime) {
	if(callbackAgent != null) {
	    m_callbackAgent = callbackAgent;
	}

	if(m_callbackAgent == null) {
	    System.out.println("No callback agent provided : halting");
	    return;
	}

	// now write main sim loop here
	
		double lfSimTime = 0.0;
	double [] arrLfDerivScratch = new double[m_nNumAgents*m_nDim];

	CommGraph graphCurr = new CommGraph(m_nNumAgents);

	int nIterCnt = 0;

	boolean bQuit = false;
	
	NullDynamicsCallback dynCallback = new NullDynamicsCallback();

	while(!bQuit && 
	      (lfSimTime < lfMaxTime || lfMaxTime < 0.0)) {
	    

	    
	    Set<Integer> setActiveAgents = new HashSet<Integer>(2017);
	    
	    double lfNextTime = 
		m_sched.getNextRoundAndFiringSchedule(lfSimTime,
						      m_nNumAgents, 
						      setActiveAgents);

	    System.out.println("next time is "+lfNextTime);
	    System.out.println("sim time is "+lfSimTime);
	    
	    nIterCnt = (nIterCnt+1)%20;


	    // now integrate till lfNextTime

	    lfSimTime = 
		m_numInt.integrateUntil(lfNextTime, lfSimTime,
					m_arrLfPositions, 
					arrLfDerivScratch,
					m_agent, m_arrStates, 
					m_arrSensors, m_env,
					null, m_callbackAgent);
	    if(Math.abs(lfSimTime-lfNextTime) < 0.0000001) {
		// now do communication
		graphCurr = m_proxGraph.getGraph(m_nDim, 
						 m_arrLfPositions,
						 m_opt, graphCurr, m_env);


		bQuit = m_callbackAgent.doCallback(m_arrLfPositions,
						   m_agent, m_arrStates,
						   m_arrSensors, m_env,
						   graphCurr, lfNextTime);
		
		Iterator<Integer> setIter = setActiveAgents.iterator();
		while(setIter.hasNext()) {
		    Integer intIdxAgent = setIter.next();
		    int nIdxAgent = intIdxAgent.intValue();
		    m_agent.getMsgs(m_arrStates[nIdxAgent].getVars(),
		    		dynCallback,
				    m_arrLfPositions, lfSimTime, m_nDim*nIdxAgent,
				    graphCurr.getEdgesFrom(nIdxAgent));
		}

		boolean bValid = checkRdiskValid(1.0, m_arrLfPositions, m_nDim, graphCurr);
		
		if(!bValid) {
			graphCurr = m_proxGraph.getGraph(m_nDim, 
					 m_arrLfPositions,
					 m_opt, graphCurr, m_env);
		}
		
		setIter = setActiveAgents.iterator();
		while(setIter.hasNext()) {
		    Integer intIdxAgent = setIter.next();
		    int nIdxAgent = intIdxAgent.intValue();
		    m_arrStates[nIdxAgent] = 
			m_agent.updateState(m_arrStates[nIdxAgent].getVars(),
					    m_arrLfPositions, lfSimTime, 
					    m_nDim*nIdxAgent, m_env,
					    graphCurr.getEdgesTo(nIdxAgent));
		    lfSimTime = lfNextTime;
		}

		doStateCopy();
	    }
	}
    }


}