import java.util.Iterator;


/**
 * <p>
 * Implementation of IAgent to move randomly, but not decrease inter-robot distances
 * once they drop below some threshold
 * </p>
 * @author     Michael Schuresko
 * @version    %I%, %G%
 * @since      1.0
 */
public class MoveCircleAgent implements IAgent
{

    double m_lfSpeedScale;
    
    double m_lfMinRad;
    
    double [] m_arrLfVelVec;
    
    AgentMsgHelpers m_agentMsgHelpers;
    
    boolean m_bClockwise;

    public MoveCircleAgent() {
	m_lfSpeedScale = 1.0;
	m_arrLfVelVec = new double[2];
	m_agentMsgHelpers = new AgentMsgHelpers();
	m_lfMinRad = m_lfSpeedScale;
	m_bClockwise = false;
    }

    public MoveCircleAgent(double lfSpeed)
    {
	m_lfSpeedScale = lfSpeed;
	m_arrLfVelVec = new double[2];
	m_agentMsgHelpers = new AgentMsgHelpers();
	m_lfMinRad = m_lfSpeedScale;
	m_bClockwise = false;
    }
    
    public MoveCircleAgent(MoveCircleAgent src) {
    	m_lfSpeedScale = src.m_lfSpeedScale;
    	m_arrLfVelVec = new double[2];
    	m_agentMsgHelpers = new AgentMsgHelpers();  
    	m_lfMinRad = src.m_lfMinRad;
    	m_bClockwise = src.m_bClockwise;
    }

    /**
     * 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 void setMinRad(double lfMinRad) {
    	m_lfMinRad = lfMinRad;
    }
    
    public void setClockwise(boolean bClockwise) {
    	m_bClockwise = bClockwise;
    }


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


  

    /**
     * <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);
	AgentMsgHelpers.PosMsg msg = 
	    new AgentMsgHelpers.PosMsg(arrLfStateCont,
			     nIdxStateOffset,2);

	while(channelsToSendOn.hasNext()) {
	    CommLink linkSendOn = 
		(CommLink)channelsToSendOn.next();
	    if(linkSendOn != null && linkSendOn.queue() != null) {
		linkSendOn.queue().sendMsg(new AgentMsgHelpers.PosMsg(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);

	


	
	double lfTheta = 2.0*Math.random()*Math.PI;
	
	double lfRad = arrLfStateCont[nIdxStateOffset] * arrLfStateCont[nIdxStateOffset];
	lfRad += arrLfStateCont[nIdxStateOffset+1] * arrLfStateCont[nIdxStateOffset+1];
	
	double lfRadCutoff = m_lfMinRad;
	
	if(lfRad < 0.1*lfRadCutoff) {
		lfTheta = 0.0;
	} else {
		lfTheta = Math.atan2(-arrLfStateCont[nIdxStateOffset], 
				arrLfStateCont[nIdxStateOffset+1]) +
				2*(lfRadCutoff-lfRad);
	}
	
	if(m_bClockwise) {
		if(lfRad < 0.1*lfRadCutoff) {
			lfTheta = Math.PI;
		} else {
			lfTheta = Math.atan2(arrLfStateCont[nIdxStateOffset], 
					-arrLfStateCont[nIdxStateOffset+1]) -
					2*(lfRadCutoff-lfRad);
		}
	}
	
	m_arrLfVelVec[0] = m_lfSpeedScale*Math.cos(lfTheta);
	m_arrLfVelVec[1] = m_lfSpeedScale*Math.sin(lfTheta);
	
	ControlFuncMoveDir func = null;

	
	while(channelsRecieveFrom.hasNext()) {
	    CommLink linkRecvFrom = 
		(CommLink)channelsRecieveFrom.next();
	    if(linkRecvFrom != null && linkRecvFrom.queue() != null) {
	    	try {
	    		// IMsg msg = 
	    		linkRecvFrom.queue().readMsg();
	    		// AgentMsgHelpers.PosMsg posMsg = (AgentMsgHelpers.PosMsg)msg;
	    		
	    		
	    		
	    		
	    	} catch (java.lang.ClassCastException e) {

	    		m_arrLfVelVec[0] = 0.0;
	    		m_arrLfVelVec[1] = 0.0;
	    		// func = new ControlFuncMoveDir(m_arrLfVelVec);
	    	}
	    	
	    	

	    	
	    }

	}	

	
	
	
	
	
	func = new ControlFuncMoveDir(m_arrLfVelVec);
	
	
	return new StateBundle(new NullLogicVarBundle(nIdxStateOffset/2), 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 MoveCircleAgent(this));
    }
}