import java.util.Iterator;


/**
 * <p>
 * Implementation of "move to centroid of r-limited delaunay cell"
 * algorithm.
 * </p>
 * @author     Michael Schuresko
 * @version    %I%, %G%
 * @since      1.0
 */
public class MoveToCentroidAgent implements IAgent
{

    double m_lfSpeedScale;

    public MoveToCentroidAgent() {
	m_lfSpeedScale = 1.0;
    }

    public MoveToCentroidAgent(double lfSpeed)
    {
	m_lfSpeedScale = lfSpeed;
    }

    /**
     * Set function for parameter to determine speed of convergence to
     * current target.
     * Speed of convergence of algorithm depends on this 
     * and on frequency of communication rounds
     */
    public void setSpeed(double lfSpeed)
    {
	m_lfSpeedScale = lfSpeed;
    }


    public boolean checkStateValidity(StateBundle state)
    {
	return (state != null && state.getControlFunc() != null);
    }


    /**
     * inner class used for flocking messages
     */
    public class SimpFlockMsg implements IMsg
    {
	double [] m_arrLfPt;

	void set(double x, double y) 
	{
	    if(m_arrLfPt == null) {
		m_arrLfPt = new double[2];
	    }
	    m_arrLfPt[0] = x; m_arrLfPt[1] = y;
	}

	public SimpFlockMsg() { set(0.0, 0.0); }
	public SimpFlockMsg(double x, double y) { set(x,y); }
	public SimpFlockMsg(double [] arrLfPt) { set(arrLfPt[0], arrLfPt[1]); }
	public SimpFlockMsg(SimpFlockMsg src) { 
	    set(src.m_arrLfPt[0],src.m_arrLfPt[1]); 
	}
	
	public SimpFlockMsg makeCopy() {
		return new SimpFlockMsg(this);
	}

	/**
	 * adds addMe to this (destructively
	 */
	public SimpFlockMsg addTo(SimpFlockMsg addMe) {
	    set(m_arrLfPt[0]+addMe.m_arrLfPt[0], 
		m_arrLfPt[1]+addMe.m_arrLfPt[1]);
	    return this;
	}

	public double [] getPt() { return m_arrLfPt; }

	public void scale(double lfScaleBy) 
	{
	    if(m_arrLfPt != null) {
		for(int i = 0; i < m_arrLfPt.length; ++i) {
		    m_arrLfPt[i] *= lfScaleBy;
		}
	    }
	}
    }


    /**
     * <p> Given the state of an agent, and the channels
     * it can send messages on, push the next set of messages
     * onto the appropriate channels. 
     * Channels are assumed to be modified by this operation.
     * One weakness of this signature is that it requires
     * programmer discipline not to look at the continuous state
     * of agents other then your neighbors.</p>
     * @param discreteState discrete component of agent state
     * @param arrLfStateCont global vector of continuous state
     * @param nIdxStateOffset offset into global state vector 
     * corresonding to this agent.
     * @param channelsToSendOn Iterator containing objects of type
     * CommLink corresponding to channels to send messages
     * on (neighbor indices can be ascertained from channels).
     * @see CommLink for type handled by iterator
     */
    public void getMsgs(ILogicVarBundle discreteState,
    		IDiscreteDynamicsCallback dynCallback,
			double arrLfStateCont[], 
			double lfCurrTime, 
			int nIdxStateOffset,
			Iterator<CommLink> channelsToSendOn)
    {
    	dynCallback.doCallback(discreteState, arrLfStateCont);
	SimpFlockMsg msg = 
	    new SimpFlockMsg(arrLfStateCont[nIdxStateOffset],
			     arrLfStateCont[nIdxStateOffset+1]);

	while(channelsToSendOn.hasNext()) {
	    CommLink linkSendOn = 
		(CommLink)channelsToSendOn.next();
	    if(linkSendOn != null && linkSendOn.queue() != null) {
		linkSendOn.queue().sendMsg(new SimpFlockMsg(msg) );
	    }
	}

    }

    /**
     * <p> Given the previous state of an agent, and the channels
     * to recieve messages from, recieve messages (destructively)
     * and return new state (non-destructively). </p>
     * @param statePrev previous state (do not modify)
     * @param arrLfStateCont vector of all continuous states (do not modify)
     * @param nIdxStateOffset offset into arrLfStateCont corresponding to
     * start of this agents continuous variables
     * @param channelsRecieveFrom iterator storing CommLink 
     * classes corresponding incoming channels
     * @see CommLink for type handled by iterator
     * @return new discrete state of agent
     */
    public StateBundle updateState(ILogicVarBundle statePrev,
				   double arrLfStateCont[], double lfCurrTime,
				   int nIdxStateOffset,IEnvironment env,
				   Iterator<CommLink> channelsRecieveFrom)
    {
	SimpFlockMsg msgAccum = new SimpFlockMsg(0,0);

	int nCnt = 0;
	while(channelsRecieveFrom.hasNext()) {
	    CommLink linkRecvFrom = 
		(CommLink)channelsRecieveFrom.next();
	    if(linkRecvFrom != null && linkRecvFrom.queue() != null) {
		IMsg currMsg = 
		    linkRecvFrom.queue().readMsg();
		if(currMsg != null) {
		    SimpFlockMsg currFlkMsg = (SimpFlockMsg)currMsg;

		    msgAccum = msgAccum.addTo(currFlkMsg);
		    
		    ++nCnt;
		}
	    }

	}

	ControlFuncMoveTowards func = null;

	if(nCnt > 0) {
	    double lfScale = 1.0/nCnt;
	    msgAccum.scale(lfScale);
	    func = new ControlFuncMoveTowards(msgAccum.getPt(),
					      m_lfSpeedScale);
	}
	else {
	    double [] arrSelf = new double[2];
	    arrSelf[0] = arrLfStateCont[nIdxStateOffset];
	    arrSelf[1] = arrLfStateCont[nIdxStateOffset+1];
	    func = new ControlFuncMoveTowards(arrSelf, 
					      m_lfSpeedScale);
	}
	return new StateBundle(null, func);
    }

    /**
     * in case the agent has some internal state, this 
     * allows syncrhonized wrapper calls to cache a snapshot of the
     * agent at some rational point in time.
     * If the agent has no internal state, this function can just return
     * "this".
     */
    public IAgent makeCopy()
    {
	return (IAgent)(new MoveToCentroidAgent(m_lfSpeedScale));
    }
}