import java.util.Random;
import java.util.Vector;

/**
 * <p>
 * Creates a random flock of robots in an n-dimensional box
 * </p>
 * @author     Michael Schuresko
 * @version    %I%, %G%
 * @since      1.0
*/
public class RandomConnectedInitOfAtLeastN implements IInitializer
{

    int m_nNumAgents, m_nDim;
    int m_nNumAgentsActual;
    double [] m_arrLfMin;
    double [] m_arrLfMax;
    double m_lfConnectRad;
    
    double [] m_arrLfIniContState;

    ILogicVarBundle m_logicVarsToClone;
    IControlFunc m_controlFuncToClone;

    Random m_randomGen;

    boolean m_bVerbose;
    
    boolean m_bResetOnContStateRequest;
    boolean m_bResetOnNumAgentsRequest;

    /**
     * constructor specifying number of agents and
     * dimensionality in which they live.
     * @param nAgents number of agents
     * @param nDim dimensionality in which they live
     */
    public RandomConnectedInitOfAtLeastN(int nAgents, int nDim, double lfRad)
    {
    	m_bResetOnContStateRequest =true;
    	m_bResetOnNumAgentsRequest =true;
    	m_bVerbose = false;
    	m_randomGen = new Random();
    	m_nNumAgents = nAgents;
    	m_nDim = nDim;
    	m_lfConnectRad = lfRad;
    	m_arrLfMin = new double[nDim];
		m_arrLfMax = new double[nDim];
		for(int i = 0; i < nDim; ++i) {
			m_arrLfMin[i] = -0.5;
			m_arrLfMax[i] = 0.5;
		}
		m_logicVarsToClone = new NullLogicVarBundle();
		m_controlFuncToClone = new ControlFuncDoNothing(nDim);
    }

    /**
     * constructor specifying number of agents, the
     * dimensionality in which they live, and the size
     * of the containing box along each dimension.
     * @param nAgents number of agents
     * @param arrLfSizes length, height, width, etc of box
     * in which agents live-arrLfSizes.length is dimensionality.
     */
    public RandomConnectedInitOfAtLeastN(int nAgents, double [] arrLfSizes, double lfRad)
    {
    	m_bResetOnContStateRequest =true;
    	m_bResetOnNumAgentsRequest =true;
    	m_bVerbose = false;
    	m_randomGen = new Random();
    	m_nNumAgents = nAgents;
    	m_lfConnectRad = lfRad;
    	m_nDim = arrLfSizes.length;
    	m_arrLfMin = new double[m_nDim];
    	m_arrLfMax = new double[m_nDim];
    	for(int i = 0; i < m_nDim; ++i) {
    		m_arrLfMin[i] = -0.5*arrLfSizes[i];
    		m_arrLfMax[i] = 0.5*arrLfSizes[i];
    	}	
    	m_logicVarsToClone = new NullLogicVarBundle();
    	m_controlFuncToClone = new ControlFuncDoNothing(m_nDim);
    }
    
    /**
     * constructor specifying number of agents, the
     * dimensionality in which they live, and the size
     * of the containing box along each dimension.
     * @param nAgents number of agents
     * @param arrLfMin min corner of box in which agents live.
     * @param arrLfMax max corner of box in which agents live.
     * 
     */
    public RandomConnectedInitOfAtLeastN(int nAgents, double [] arrLfMin, 
			       double [] arrLfMax, double lfRad)
    {
    	m_bResetOnContStateRequest =true;
    	m_bResetOnNumAgentsRequest =true;
    	m_bVerbose = false;
    	m_randomGen = new Random();
    	m_nNumAgents = nAgents;
	
    	m_lfConnectRad = lfRad;
    	m_nDim = arrLfMin.length;
    	m_arrLfMin = new double[m_nDim];
    	m_arrLfMax = new double[m_nDim];
    	for(int i = 0; i < m_nDim; ++i) {
    		m_arrLfMin[i] = arrLfMin[i];
    		m_arrLfMax[i] = arrLfMax[i];
    	}
    	m_logicVarsToClone = new NullLogicVarBundle();
    	m_controlFuncToClone = new ControlFuncDoNothing(m_nDim);
    }

    public void setDefaultIniVars(ILogicVarBundle copyMe)
    {
    	m_logicVarsToClone = copyMe.makeCopy();
    }

    public void setDefaultIniControlFunc(IControlFunc copyMe)
    {
    	m_controlFuncToClone = copyMe.makeCopy();
    }

    
    
    /**
     * Initial continuous state vector
     */
    public double [] getInitContState() 
    { 
    	return m_arrLfIniContState;
    }
    
    public void resetState() {
    	m_arrLfIniContState = createIniContState();
    	m_nNumAgentsActual = m_arrLfIniContState.length;
    }
    
    double [] createIniContState() 
    {
    	Vector<Vector<Double> > vecPoints;
    	vecPoints = getIniVec();
    	double [] arrLfOut = new double[vecPoints.size()*m_nDim];
    	for(int i = 0; i < arrLfOut.length; ++i) {
    		for(int j = 0; j < m_nDim; ++j) {
    			arrLfOut[i*m_nDim+j] = vecPoints.get(i).get(j);
    		}
    	}
		return arrLfOut;
    }

    Integer getDsuRep(Vector<Integer> vecReps, int i) {
    	Integer objJ = new Integer(i);
		while(vecReps.get(objJ).compareTo(objJ) != 0) {
	    	objJ = vecReps.get(objJ);
		}
		Integer objResult = objJ;
		Integer objNext = objJ;
		for(objJ = new Integer(i); objJ.compareTo(objResult) != 0; objJ = objNext) {
	    	objNext = vecReps.get(objJ);
	    	vecReps.set(objJ.intValue(), objResult);
		}
		return objResult;
    }

    void doDsuMerge(Vector<Integer> vecReps, Vector<Integer> vecCounts, int i, int j) {
    	Integer objIRep = getDsuRep(vecReps, i);
    	Integer objJRep = getDsuRep(vecReps, j);
    	if(objIRep.compareTo(objJRep) == 0) {
    		return;
    	}
    	vecCounts.set(objJRep, vecCounts.get(objJRep)+ vecCounts.get(objIRep));
    	vecReps.set(objIRep.intValue(), objJRep);
    }

    Vector<Vector<Double> > getIniVec() {
    	
	Vector<Integer> vecIndices = new Vector<Integer>(m_nNumAgents, m_nNumAgents);
	Vector<Integer> vecCounts = new Vector<Integer>(m_nNumAgents, m_nNumAgents);
	
	Vector<Vector<Double> > vecTmpPos = 
		new Vector<Vector<Double> >(m_nNumAgents, m_nNumAgents);
	
	boolean bConnected = false;
	
	int i;
	for(i = 0; !bConnected; ++i) {
		vecIndices.add(i, i);
		vecCounts.add(i,1);
		Vector<Double> vecNextPos = new Vector<Double>(m_nDim);
		int nAxis = 0;
		for(nAxis = 0; nAxis < m_nDim; ++nAxis) {
			Double objLfCurrPos = 
				new Double(m_arrLfMin[nAxis] +
					    m_randomGen.nextDouble()*
					    (m_arrLfMax[nAxis]-m_arrLfMin[nAxis]));
			vecNextPos.add(nAxis, objLfCurrPos);
		}
		vecTmpPos.add(i, vecNextPos);
		
		for(int j = 0; j < i; ++j) {
			double lfDistSum = 0.0;
			for(nAxis = 0; nAxis < m_nDim; ++nAxis) {
				double lfAxisDist = 
					vecTmpPos.get(i).get(nAxis) -
					vecTmpPos.get(j).get(nAxis);
				lfDistSum += lfAxisDist*lfAxisDist;
			}
			if(lfDistSum < m_lfConnectRad*m_lfConnectRad) {
				doDsuMerge(vecIndices, vecCounts, i, j);
			}
	    }
		bConnected = (vecCounts.get(i) >= m_nNumAgents);
	}

	Vector<Vector<Double> > vecResult = 
		new Vector<Vector<Double> >(vecCounts.get(vecCounts.size()-1));
	Integer objSetRep = vecIndices.get(vecIndices.size()-1);
	int nAddPos = 0;
	for(i = 0; i < vecCounts.size(); ++i) {
		if(objSetRep.compareTo(vecIndices.get(i)) == 0) {
			vecResult.add(nAddPos, vecTmpPos.get(i));
			++nAddPos;
		}
	}
	return vecResult;
    }

    public IEnvironment getEnv()
    {
	return new NdTrivialEnv(m_arrLfMin, m_arrLfMax);
    }
    


    public int getNumAgents() 
    {
    	return m_nNumAgentsActual;
    }
    

    /**
     * convenient to have this...
     */
    public StateBundle [] getInitDiscreteState()
    {
        StateBundle [] arrStates = new StateBundle[m_nNumAgentsActual];
    	if(m_bVerbose) {
    		System.out.println("Initializing to "+m_nNumAgentsActual+" agents\n");
    	}
    	for(int i = 0; i < m_nNumAgentsActual; ++i)
	    {
    		ILogicVarBundle currCopy = m_logicVarsToClone.makeCopy();
    		currCopy.setId(i);
    		currCopy.init();
    		arrStates[i] = new StateBundle(currCopy,
    				m_controlFuncToClone.makeCopy()
    		);
	    }
    	return arrStates;
    }

    public boolean isAgentCompatible(IAgent agent)
    {
    	return (agent != null);
    }
}