import java.util.Iterator;


/**
 * <p>
 * Implementation of IAgent to handle a simple flocking algorithm.
 * </p>
 * @author     Michael Schuresko
 * @version    %I%, %G%
 * @since      1.0
 */
public class SimpleFlockingAgent implements ITreeAlgAgent // IAgent
{

    double m_lfSpeedScale;
    double m_lfHeadingRate;
    
    
    public SimpleFlockingAgent() {
	m_lfSpeedScale = 1.0;
	m_lfHeadingRate = 0.1;
    }

    public SimpleFlockingAgent(double lfSpeed)
    {
	m_lfSpeedScale = lfSpeed;
	m_lfHeadingRate = 0.1;
    }

    /**
     * Set function for parameter to determine speed of robots
     */
    public void setSpeed(double lfSpeed)
    {
	m_lfSpeedScale = lfSpeed;
    }
    
    /**
     * Sets rate of convergence of heading
     */
    public void setHeadingRage(double lfRate) {
    	m_lfHeadingRate = lfRate;
    }


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


    /**
     * inner class used for flocking state
     */
    public static class SimpFlockState implements ILogicVarBundle
    {
    	double m_lfHeading;
    	int m_nId;
    	
    	public SimpFlockState() {
    		m_nId = -1; m_lfHeading = Math.PI*(2.0*Math.random()-1.0);
    	}
    	
    	public SimpFlockState(double lfHeading) {
    		m_nId = -1; m_lfHeading = lfHeading;
    	}
    	
    	public SimpFlockState(SimpFlockState src) {
    		m_nId = src.m_nId; m_lfHeading = src.m_lfHeading;
    	}

    	public void init() {
    		m_lfHeading = Math.PI*(2.0*Math.random()-1.0);
    	}
    	
    	/**
         * makes a copy and returns it
         */
        public SimpFlockState makeCopy() 
        { return new SimpFlockState(this); }

        public Integer getIntVar(Object refObject) { return null;}

        public boolean insertIntVar(Object refObject, int nValue) 
        {return false;}

        public Boolean getBoolVar(Object refObject) {return null;}

        public boolean insertBoolVar(Object refObject, boolean bValue) 
        {return false;}
        
        /**
         * Removes any values indexed by refObject
         * @param refObject remove values associated with this.
         */
        public void removeVar(Object refObject) {
        	return;
        }


        /**
         * Gets unique Id of this agent
         */
        public int getId() { return m_nId; }

        /**
         * Sets unique Id of this agent
         */
        public void setId(int nId) { m_nId = nId; }
        

        public double getHeading() { return m_lfHeading; }
        public void setHeading(double lfHeading) {m_lfHeading = lfHeading; }
        
        public double getHeadingDelta(double lfHead, double lfRate) {
        	double lfDiff = lfHead - m_lfHeading;
        	while(lfDiff >= Math.PI) {
        		lfDiff -= 2.0*Math.PI;
        	}
        	while(lfDiff < -Math.PI) {
        		lfDiff += 2.0*Math.PI;
        	}
        	return lfRate*lfDiff;
        }
    }
    
    /**
     * inner class used for flocking messages
     */
    public class SimpFlockMsg implements IMsg
    {
    	double m_lfHeading;

    	void set(double lfHeading) 
    	{
    		m_lfHeading = lfHeading;
    	}

    	public SimpFlockMsg() { set(0.0); }
    	public SimpFlockMsg(double lfTheta) { set(lfTheta); }
    	public SimpFlockMsg(SimpFlockMsg src) { 
    		set(src.m_lfHeading); 
    	}

    	public SimpFlockMsg(SimpFlockState src) {
    		set(src.getHeading());
    	}

    	public SimpFlockMsg makeCopy() {
    		return new SimpFlockMsg(this);
    	}
    	
    	public double getHeading() { return m_lfHeading; }
    }


    /**
     * <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)
    {
    	SimpFlockState flockState = (SimpFlockState)discreteState;
    	SimpFlockMsg msg = 
    		new SimpFlockMsg(flockState);

    	dynCallback.doCallback(discreteState, arrLfStateCont);
    	
    	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)
    {
    	try {
    		SimpFlockState flockState = (SimpFlockState)statePrev;
    		double lfAngleDeviance = 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) {
    					// right now only works
    					// if all agents in swarm are flockers
    					SimpFlockMsg currFlkMsg = (SimpFlockMsg)currMsg;
    					lfAngleDeviance += 
    						flockState.getHeadingDelta(currFlkMsg.getHeading(), 
    								m_lfHeadingRate);
    					++nCnt;

    				}

    			}
    		}


    		ControlFuncMoveDir func = null;

    		SimpFlockState stateNext = new SimpFlockState(flockState);

    		stateNext.setHeading(stateNext.getHeading() + lfAngleDeviance);

    		func = new ControlFuncMoveDir(
    				m_lfSpeedScale*Math.cos(stateNext.getHeading()),
    				m_lfSpeedScale*Math.sin(stateNext.getHeading()));
    		return new StateBundle(stateNext, func);
    	} catch(java.lang.ClassCastException e) {
    		SimpFlockState stateNext = new SimpFlockState();
    		stateNext.setId(statePrev.getId());
    		return new StateBundle(stateNext, 
    				new ControlFuncDoNothing(2));
    	}
    }

    /**
     * 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 ITreeAlgAgent makeCopy()
    {
    	return (ITreeAlgAgent)(new SimpleFlockingAgent(m_lfSpeedScale));
    }
    
	public IMsg getMsgAnnotation(ILogicVarBundle vars,
			double [] arrLfState, int nIdx) {
		return new AgentMsgHelpers.PosMsg(arrLfState,
				nIdx*2, 2);
	}
	
	public java.lang.reflect.Type getAnnoteType() {
		return AgentMsgHelpers.PosMsg.class;
	}
		
	
	class lclCompare implements java.util.Comparator<TreeConstraintState> {
		
		TreeConstraintState m_agentSorting;
		AgentMsgHelpers.PosMsg m_posMsg;
		
		public lclCompare(TreeConstraintState agentSorting)
		{ 
			m_agentSorting = agentSorting;
			m_posMsg = (AgentMsgHelpers.PosMsg)
			 (m_agentSorting.getMsgAddition(AgentMsgHelpers.PosMsg.class));
		}
		
		/**
		 * smaller number means more preference.
		 */
		public int compare(TreeConstraintState treeState1, 
				TreeConstraintState treeState2) {
			IMsg attach1 = treeState1.getMsgAddition(AgentMsgHelpers.PosMsg.class);
			IMsg attach2 = treeState2.getMsgAddition(AgentMsgHelpers.PosMsg.class);
			
			// prefer to attach to things other than yourself
			if(treeState1.getId() == m_agentSorting.getId() &&
		  			treeState2.getId() != m_agentSorting.getId()) {
		  		return 1;
		  	} else if(treeState2.getId() == m_agentSorting.getId() &&
		  			treeState1.getId() != m_agentSorting.getId()) {
		  		return -1;
		  	}
			
			if(treeState1.getRootId() >= 0 && 
					treeState1.getRootId() < treeState2.getRootId()) {
				return -1;
			}
			
			if(treeState2.getRootId() >= 0 && 
					treeState2.getRootId() < treeState1.getRootId()) {
				return 1;
			}
			
			try {
				AgentMsgHelpers.PosMsg pos1 = 
					(AgentMsgHelpers.PosMsg) attach1;
				AgentMsgHelpers.PosMsg pos2 = 
					(AgentMsgHelpers.PosMsg) attach2;
			
				double lfDist1Sqrd = m_posMsg.getDistSqrd(pos1);
				double lfDist2Sqrd = m_posMsg.getDistSqrd(pos2);
				if(lfDist1Sqrd < lfDist2Sqrd) {
					return -1;
				}
				if(lfDist1Sqrd > lfDist2Sqrd) {
					return 1;
				}
			} catch(Exception e) {
			  	
			}
			if(treeState1.getId() == m_agentSorting.getParentId() &&
					treeState2.getId() != m_agentSorting.getParentId()) {
				return -1;
			}
			if(treeState2.getId() == m_agentSorting.getParentId() &&
					treeState1.getId() != m_agentSorting.getParentId()) {
				return 1;
			}
			return 0; // equality if all else fails
		}
	}
	
	/**
	 * gets a comparator for sorting ITreeAlgAgent nodes by preference
	 * @return Comparator -- smaller number means more preference
	 */
	public java.util.Comparator<TreeConstraintState> 
		getPrefComp(TreeConstraintState agentSorting) {
		
		return new lclCompare(agentSorting);
	}

}