import java.util.Iterator;


public class TreeDeployAgent implements ITreeAlgAgent {

	int m_nDim; //always 2
	double m_lfCommRad;
	double m_lfVel;
	
	public TreeDeployAgent() {
		
		m_nDim = 2;
		m_lfVel = 0.25;
	}
	
	public TreeDeployAgent(double lfCommRad) {
		m_lfCommRad = lfCommRad;
		m_lfVel = 0.25;
		m_nDim = 2;
	}
	
	public TreeDeployAgent(double lfCommRad, double lfVel) {
		m_lfCommRad = lfCommRad;
		m_lfVel = lfVel;
		m_nDim = 2;
	}
	
	public TreeDeployAgent(TreeDeployAgent src) {
		m_lfCommRad = src.m_lfCommRad;
		m_nDim = src.m_nDim;
		m_lfVel = src.m_lfVel;
	}
	
	
	public boolean checkStateValidity(StateBundle state) {
		// TODO Auto-generated method stub
		return false;
	}

	/**
     * <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) );
		    }
		}
	}

	
	public ITreeAlgAgent makeCopy() {
		// TODO Auto-generated method stub
		return new TreeDeployAgent(this);
	}

	
	public StateBundle updateState(ILogicVarBundle statePrev,
			double[] arrLfStateCont, double lfCurrTime, 
			int idxStateOffset, IEnvironment env,
			Iterator<CommLink> channelsRecieveFrom) {
		// TODO Auto-generated method stub
		
		DistributedDelaunayClip clipAgent = 
			new DistributedDelaunayClip(arrLfStateCont[idxStateOffset],
					arrLfStateCont[idxStateOffset+1]);
		while(channelsRecieveFrom.hasNext()) {
			CommLink link = channelsRecieveFrom.next();
			 IMsgChannel channel = link.queue();
			 IMsg msg = channel.readMsg();
			 if(link.from() == link.to()) {
					continue;
				}
				
			 AgentMsgHelpers.PosMsg posMsg = (AgentMsgHelpers.PosMsg)msg;
			 clipAgent.addEdge(link.from(), 
					 posMsg.getPos(0), posMsg.getPos(1));
		}
		double [] arrLfCurrPos = 
			MathUtils.arrDblCpyRange(arrLfStateCont, 
					idxStateOffset, idxStateOffset+2);
		
		//
		// add boundary to make voronoi cells closed
		IBoundary bounds = env.getBoundary();
		double [] arrLfMinBound = bounds.bboxMinCorner();
		double [] arrLfMaxBound = bounds.bboxMaxCorner();
		double [] arrLfTmp = new double[2];
		arrLfTmp[0] = arrLfCurrPos[0];
		arrLfTmp[1] = arrLfMinBound[1] + 
			(arrLfMinBound[1]-arrLfCurrPos[1]);
		clipAgent.addEdge(-1, arrLfTmp[0], arrLfTmp[1]);
		arrLfTmp[1] = arrLfMaxBound[1] + 
			(arrLfMaxBound[1]-arrLfCurrPos[1]);
		clipAgent.addEdge(-1, arrLfTmp[0], arrLfTmp[1]);
		
		arrLfTmp[1] = arrLfCurrPos[1];
		arrLfTmp[0] = arrLfMinBound[0] + 
			(arrLfMinBound[0]-arrLfCurrPos[0]);
		clipAgent.addEdge(-1, arrLfTmp[0], arrLfTmp[1]);
		arrLfTmp[0] = arrLfMaxBound[0] + 
			(arrLfMaxBound[0]-arrLfCurrPos[0]);
		clipAgent.addEdge(-1, arrLfTmp[0], arrLfTmp[1]);
		//
		// end of "add boundary to make voronoi cells closed"
	
		VoronoiHolder<NullLogicVarBundle> stateNext = null;
		stateNext = new VoronoiHolder<NullLogicVarBundle>(
				new NullLogicVarBundle(idxStateOffset/2), 
				clipAgent);
		
		IScalarField scalField = 
			new ScalarFieldUtils.ScalarFieldFromEnv(env,0);
		IScalarField fieldX = 
			new ScalarFieldUtils.LinearScalarField(arrLfCurrPos[0],
					arrLfCurrPos[1], 0, 0.0);
		fieldX = 
			new ScalarFieldUtils.ScalarFieldProduct(fieldX,scalField);
		
		IScalarField fieldY = 
			new ScalarFieldUtils.LinearScalarField(arrLfCurrPos[0],
					arrLfCurrPos[1], 1, 0.0);
		fieldY = 
			new ScalarFieldUtils.ScalarFieldProduct(fieldY,scalField);
		
		double lfIntegralX = 0.0, lfIntegralY = 0.0, lfIntegral = 0.0;
		
		double lfAreaEps = 0.05; // was 0.01
		
		IAreaIntegrator integrator = 
			new NumericalAreaIntegrator(scalField, lfAreaEps);
		IAreaIntegrator integratorX = 
			new NumericalAreaIntegrator(fieldX, lfAreaEps);
		IAreaIntegrator integratorY = 
			new NumericalAreaIntegrator(fieldY, lfAreaEps);
		
		
		
		while(clipAgent.hasNext()) {
			DistributedDelaunayClip.Edge currEdge = clipAgent.next();
			lfIntegral += integrator.integrateClippedTriangle(arrLfCurrPos,
					clipAgent.getEdgeStart(currEdge,100*m_lfCommRad), 
					clipAgent.getEdgeEnd(currEdge,100*m_lfCommRad), 
					m_lfCommRad, arrLfCurrPos);
			lfIntegralX += integratorX.integrateClippedTriangle(arrLfCurrPos,
					clipAgent.getEdgeStart(currEdge,100*m_lfCommRad), 
					clipAgent.getEdgeEnd(currEdge,100*m_lfCommRad), 
					m_lfCommRad, arrLfCurrPos);
			lfIntegralY += integratorY.integrateClippedTriangle(arrLfCurrPos,
					clipAgent.getEdgeStart(currEdge,100*m_lfCommRad), 
					clipAgent.getEdgeEnd(currEdge,100*m_lfCommRad), 
					m_lfCommRad, arrLfCurrPos);
		}
		double lfNewPosX = 0.0;
		double lfNewPosY = 0.0;
		if(Math.abs(lfIntegral) > 0.0) {
			lfNewPosX = lfIntegralX/lfIntegral;
			lfNewPosY = lfIntegralY/lfIntegral;
		}
		IControlFunc ctrlFunc = 
			new ControlFuncMoveTowards(
					lfNewPosX + arrLfCurrPos[0],
					lfNewPosY + arrLfCurrPos[1],
					m_lfVel);
		return new StateBundle(stateNext,
				ctrlFunc);
	}
	
	public IMsg getMsgAnnotation(ILogicVarBundle vars,
			double [] arrLfState, int nIdx) {
		return new AgentMsgHelpers.PosMsg(arrLfState,
				nIdx*m_nDim, m_nDim);
	}
	
	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);
	}
	
	

}