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 RandomConstrainedAgent implements IAgent
{

    double m_lfSpeedScale;
    
    double m_lfMinRad;
    
    
    
    double [] m_arrLfVelVec;
    

    public RandomConstrainedAgent() {
	m_lfSpeedScale = 1.0;
	m_arrLfVelVec = new double[2];
	m_lfMinRad = m_lfSpeedScale;
    }

    public RandomConstrainedAgent(double lfSpeed)
    {
	m_lfSpeedScale = lfSpeed;
	m_arrLfVelVec = new double[2];
	m_lfMinRad = m_lfSpeedScale;
    }
    
   
    
    public RandomConstrainedAgent(RandomConstrainedAgent src) {
    	m_lfSpeedScale = src.m_lfSpeedScale;
    	m_arrLfVelVec = new double[2];
    	m_arrLfVelVec[0] = src.m_arrLfVelVec[0];
    	m_arrLfVelVec[1] = src.m_arrLfVelVec[1];  
    	m_lfMinRad = src.m_lfMinRad;
    	
    }

    /**
     * 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)
    {
    	System.out.println("Set speed of radom constrained agent");
    	m_lfSpeedScale = lfSpeed;
    }
    
    public void setMinRad(double lfMinRad) {
    	m_lfMinRad = lfMinRad;
    }


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

	dynCallback.doCallback(discreteState, arrLfStateCont);
	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;
	
	m_arrLfVelVec[0] = 0.5*m_lfSpeedScale*Math.cos(lfTheta);
	m_arrLfVelVec[1] = 0.5*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;
	    		double [] arrLfPosJ = posMsg.getPosArrCpy();
	    		double lfDist = 0.0;
	    		for(int i = 0; i < arrLfPosJ.length; ++i) {
	    			arrLfPosJ[i] -= arrLfStateCont[nIdxStateOffset+i];
	    			lfDist += arrLfPosJ[i]*arrLfPosJ[i];
	    		}
	    		if(lfDist < this.m_lfMinRad*this.m_lfMinRad && lfDist > 0.0) {
	    			lfDist = Math.sqrt(lfDist);
	    			
	    			double lfOneOverDist = 1.0/lfDist;
	    			double lfDistDot = 0.0;
	    			for(int i = 0; i < arrLfPosJ.length; ++i) {
	    				arrLfPosJ[i] *= lfOneOverDist;
	    				lfDistDot += m_arrLfVelVec[i]*arrLfPosJ[i];
	    			}
	    			
	    			if(lfDistDot > 0.0) {
	    				for(int i = 0; i < arrLfPosJ.length; ++i) {
	    					m_arrLfVelVec[i] = 0.0; //-= lfDistDot*arrLfPosJ[i];
	    					
	    				}
	    			}
	    		}
	    		
	    		
	    		
	    	} 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 RandomConstrainedAgent(this));
    }
}