import java.util.Iterator;
import java.util.Vector;


public class TreeConstraintAgent implements IAgent {

	static final boolean m_bDbgDep = false;
	
	double m_lfRad;
	ITreeAlgAgent m_agent;
	
	ITreeMotionConstraint m_motConstraint;
	
	ITreeDepAlg m_depAgent;
	
	SimpAgentTreeTopo m_topoAgent;
	
	public TreeConstraintAgent() {
		m_topoAgent = new SimpAgentTreeTopo();
		m_agent = new TreeDeployAgent();
		m_motConstraint = new ConstraintDoNothing();
		// sensible default for m_agent??
		
		m_depAgent = new TreeDepthAgentDefault();
	}
	
	public TreeConstraintAgent(ITreeAlgAgent agentWrapped, double lfRad) {
		m_topoAgent = new SimpAgentTreeTopo();
		m_agent = agentWrapped.makeCopy();
		m_lfRad = lfRad;
		m_motConstraint = new ConstraintDoNothing();
		m_depAgent = new TreeDepthAgentDefault();
	}
	
	public TreeConstraintAgent(TreeConstraintAgent src) {
		m_topoAgent = new SimpAgentTreeTopo();
		m_agent = src.m_agent.makeCopy();
		m_lfRad = src.m_lfRad;
		m_motConstraint = src.m_motConstraint.makeCopy();

		m_depAgent = src.m_depAgent.makeCopy();
	}
	
	public void setDepAgent(ITreeDepAlg depAgent) {
		m_depAgent = depAgent;
	}
	
	public void setTreeAlgAgent(ITreeAlgAgent agentWrapped) {
		m_agent = agentWrapped.makeCopy();
	}
	
	public void setMotionConstraint(ITreeMotionConstraint constraint) {
		m_motConstraint = constraint;
	}
	
	public boolean checkStateValidity(StateBundle state) {
		ILogicVarBundle vars = state.getVars();
		try {
			PairLogicVarBundle pairVars = (PairLogicVarBundle)vars;
			TreeConstraintState treeState = 
				(TreeConstraintState)pairVars.accessFirst();
			
			return true;
		} catch(Exception e) {
			return false;
		}
	}

	public void getMsgs(ILogicVarBundle discreteState, 
    		IDiscreteDynamicsCallback dynCallback,
    		double[] arrLfStateCont,
			double lfCurrTime, int idxStateOffset,
			Iterator<CommLink> channelsToSendOn) {

		// no exception handling here -- that way exceptions pop
		// up when I debug.
		
		dynCallback.doCallback(discreteState, arrLfStateCont);
		
		PairLogicVarBundle pairVars = (PairLogicVarBundle)discreteState;
		TreeConstraintState treeState = 
			(TreeConstraintState)pairVars.accessFirst();
		ILogicVarBundle algVars = 
			pairVars.accessSecond();
		Vector<Iterator<CommLink> > vecLinkIter = 
			AgentMsgHelpers.duplicateLinks(channelsToSendOn);
		Iterator<CommLink> channelsSendFirst = vecLinkIter.firstElement();
		Iterator<CommLink> channelsSendNext = vecLinkIter.elementAt(1);
		
		m_agent.getMsgs(algVars, NullDynamicsCallback.getInstance(),
				arrLfStateCont, lfCurrTime, 
				idxStateOffset, channelsSendFirst);
		
		IMsg msgAnnotation = 
			m_agent.getMsgAnnotation(algVars, arrLfStateCont, 
					algVars.getId());
		treeState.setMsgAddition(m_agent.getAnnoteType(), msgAnnotation);
		while(channelsSendNext.hasNext()) {
			CommLink link = channelsSendNext.next();
			link.queue().sendMsg((IMsg)treeState);
			link.queue().sendMsg((IMsg)treeState);
			// send it twice rather than explicitly putting send in depth increase
			// agent
		}

	}

	public IAgent makeCopy() {
		return new TreeConstraintAgent(this);
	}
	
	public class DepthInversionException extends Exception {
		
	}

	public StateBundle updateState(ILogicVarBundle statePrev,
			double[] arrLfStateCont, double lfCurrTime, int idxStateOffset,
			IEnvironment env, Iterator<CommLink> channelsRecieveFrom) {
		// no exception handling here -- that way exceptions pop
		// up when I debug.
		
		int nNumAgents = arrLfStateCont.length / env.getDimensionality();
		PairLogicVarBundle pairVars = (PairLogicVarBundle)statePrev;
		TreeConstraintState treeState = 
			(TreeConstraintState)pairVars.accessFirst();
		ILogicVarBundle algVars = 
			pairVars.accessSecond();
		Vector<Iterator<CommLink> > vecLinkIter = 
			AgentMsgHelpers.nDuplicateLinks(channelsRecieveFrom,3);
		Iterator<CommLink> channelsRecvFirst = vecLinkIter.firstElement();
		Iterator<CommLink> channelsRecvNext = vecLinkIter.elementAt(1);
		Iterator<CommLink> channelsRecvTreeDep = vecLinkIter.elementAt(2);
		
		StateBundle candState = 
			m_agent.updateState(algVars, arrLfStateCont, lfCurrTime, 
				idxStateOffset, env, channelsRecvFirst);
		
		// call depth agent here
		
		
		StateBundle depState = m_depAgent.updateState(treeState, arrLfStateCont, 
			lfCurrTime, idxStateOffset, env, channelsRecvTreeDep);

		

		
		treeState = (TreeConstraintState)(depState.getVars());
	

			
		IControlFunc cntrlFunc = candState.getControlFunc();
		// to be modified via projection later.
		Vector<TreeConstraintState> vecTreeNeighbors = 
			new Vector<TreeConstraintState>();
		TreeConstraintState stateParent = null;
		TreeConstraintState stateParentProp = null;
		while(channelsRecvNext.hasNext()) {
			CommLink link = channelsRecvNext.next();
			TreeConstraintState treeStateRecvd = 
				(TreeConstraintState)link.queue().readMsg();
			vecTreeNeighbors.add(treeStateRecvd);
			if(treeStateRecvd.getId() == treeState.getParentId()) {
				stateParent = treeStateRecvd;
			}
			if(treeStateRecvd.getId() == treeState.getParentProp()) {
				stateParentProp = treeStateRecvd;
			}
		}
		
		
		if(stateParent != null && stateParent.getDepthEst() > treeState.getDepthEst()) {
			System.out.println("We have a problem : how did this happen?");
			System.out.println("Roots are  " + stateParent.getRootId() +
					" and " + treeState.getRootId());
			System.out.println("Ids are    " + stateParent.getId() +
					" and " + treeState.getId());
			System.out.println("Depths are " + stateParent.getDepthEst() +
					" and " + treeState.getDepthEst());
			System.out.flush();
			// throw(new DepthInversionException());
		}

		TreeConstraintState treeStateNext = 
			m_topoAgent.stateNext(treeState, vecTreeNeighbors);
		
		if(treeState.getParentId() < 0) {
			treeStateNext.setParentId(treeStateNext.getId());
			treeStateNext.setParentProp(treeStateNext.getParentId());
			treeStateNext.setParDepLess(false);
			stateParent = treeState;
		}
		
		/*
		 * Make sure algorithm on paper reflects this!!! (or vice versa!!!)
		 */
		if(stateParent == null) {
			stateParent = treeState;
			treeStateNext.setParentId(treeStateNext.getId());
			treeStateNext.setRootId(treeStateNext.getId());
		}
		if(stateParentProp == null) {
			stateParentProp = stateParent;
			treeStateNext.setParentProp(treeStateNext.getParentId());
		}
		

		IFuncObj<TreeConstraintState, Boolean> funcSibl = 
			new SelectMsgMatchingDepth(treeState.getDepthEst());

		
		IFuncObj<TreeConstraintState, Boolean> funcChildren = 
			new SelectMsgWithParent(treeState.getId());

		IFuncObj<TreeConstraintState, Boolean> funcPropChildren = 
			new SelectMsgWithPropParent(treeState.getId());
		

		IFuncObj<TreeConstraintState, Boolean> funcCycleCand = 
			new BoolFuncAggregators.BoolFuncAnd<TreeConstraintState>
		(funcSibl, funcPropChildren);
		
		switch(treeState.getCurrRound()%4) {
		case TreeConstraintState.ROUND_ATTACH:	
			if(treeState.getParentProp() != treeState.getParentId()) {
				if(stateParentProp.getDepthEst() == 
					treeState.getDepthEst() &&
					stateParentProp.getRootId() >=
						treeState.getRootId()  ) {
					
					Iterator<TreeConstraintState> iPropChild = 
						AgentMsgHelpers.getFilteredMsgs(funcCycleCand,
								vecTreeNeighbors);
				
					int nMaxPropChild = -1;
					int nMinPropChild = stateParent.getId();
					while(iPropChild.hasNext()) {
						TreeConstraintState propChild = 
							iPropChild.next();
						if(propChild.getId() < nMinPropChild) {
							nMinPropChild = propChild.getId();
						}
						if(propChild.getId() > nMaxPropChild ) {
							nMaxPropChild = propChild.getId();
						}
					}
					
					// DOUBLE CHECK THOROUGHLY
					if((nMaxPropChild < treeState.getId() &&
							stateParentProp.getId() > treeState.getId()) || 
							( stateParentProp.getId() < treeState.getId())) {
						treeStateNext.setParentId(treeState.getParentProp());
					} else {
						treeStateNext.setParentProp(treeState.getParentId());
					}
				} else if(stateParentProp.getDepthEst() <
					treeState.getDepthEst()) {
					treeStateNext.setParentId(treeState.getParentProp());
				} else {
					treeStateNext.setParentProp(treeState.getParentId());
				}
			}
			break;	
		case TreeConstraintState.ROUND_UPDATE_DEPTH:
			if(treeStateNext.getId() != 0) {
				
				if(treeStateNext.getIncreaseDepth()) {
					if(m_bDbgDep) { System.out.println("incr"); }
					treeStateNext.updateDepthEst(treeStateNext, nNumAgents);
				} else {
					treeStateNext.updateDepthEst(stateParent, nNumAgents);
				}
			}
			break;	
		case TreeConstraintState.ROUND_SET_PARLESS:	
			if(treeState.getDepthEst() > 
				stateParent.getDepthEst() ||
				treeState.getRootId() == treeState.getId()) {
				treeStateNext.setParDepLess(true);
			} else {
				treeStateNext.setParDepLess(false);
			}
			break;	
		case TreeConstraintState.ROUND_PROPOSE_NEW:
			// oh right, look up "sort" for vectors
			// and make preferences compatible with it.
//			
			java.util.Collections.sort(vecTreeNeighbors,
					m_agent.getPrefComp(treeState));
			
			Iterator<TreeConstraintState> iterByPref = 
				vecTreeNeighbors.iterator();
			
			boolean bDone = false;
	
			SimpAgentTreeTopo.TreeTopoInfo myTopoInfo = 
				SimpAgentTreeTopo.getInfo(treeStateNext);
			
			TreeConstraintState stateDepTarg = null;
			while(iterByPref.hasNext() && !bDone) {
				TreeConstraintState currItem = iterByPref.next();
				
				if(currItem.getId() == treeState.getParentId()) {
					bDone = true;
					continue;
				}
				
				if(currItem.getDepthEst() < treeState.getDepthEst() || 
						( currItem.getDepthEst() == 
							treeState.getDepthEst()&&
							currItem.getParDepLess())) {
					treeStateNext.setParentProp(currItem.getId());
					bDone = true;
					continue;
				}
				if(stateDepTarg == null && currItem.getId() != 
					treeStateNext.getId() && treeStateNext.getId() != 0) {
		
					if(myTopoInfo != null && myTopoInfo.isDesc(currItem) != true) {
						if(m_depAgent.isViableTarget(treeStateNext, currItem)) {
							stateDepTarg = currItem;
						}
					}
					// Check modification to prefer
					// off-cycle things!!!
				}
			}
			
			if(stateDepTarg != null) {
				if(m_bDbgDep) {
					System.out.println("Depth targ gets set -- right?");
				}
				treeStateNext.setDepTarg(stateDepTarg.getId());
			} else {
				treeStateNext.clearDepTarg();
			}
			
			break;	
			default:
		}
		
		
		if(treeStateNext.getParentId() == treeStateNext.getId()) {
			treeStateNext.setRootId(treeStateNext.getId());
		} else {
			treeStateNext.setRootId(stateParent.getRootId());
		}
		
		treeStateNext.incrRound();
		
		PairLogicVarBundle varsPair = 
			new PairLogicVarBundle(treeStateNext, candState.getVars());
		IControlFunc cntFncNew = 
			m_motConstraint.modifyFunc(cntrlFunc, 
					arrLfStateCont, idxStateOffset, stateParent, 
					AgentMsgHelpers.getFilteredMsgs(funcChildren,
							vecTreeNeighbors), 
					AgentMsgHelpers.getFilteredMsgs(
							new BoolFuncAggregators.BoolFuncNegate<TreeConstraintState>(
									new SelectMsgMatchingRootId(treeState.getRootId())), 
							vecTreeNeighbors));
		return new StateBundle(varsPair, cntFncNew);
	}

	public static class SelectMsgMatchingId implements IFuncObj<TreeConstraintState, Boolean>
	{
		int m_nIdToMatch;

		public SelectMsgMatchingId(int nId) {
			m_nIdToMatch = nId;
		}

		public Boolean doFunc(TreeConstraintState msg) {
			try {
				ILogicVarBundle vars = 
					(ILogicVarBundle)msg;
				return new Boolean(vars.getId() == m_nIdToMatch);
			} catch(java.lang.ClassCastException e) {
				return new Boolean(false);
			}
		}
	}

	public class SelectMsgMatchingRootId implements IFuncObj<TreeConstraintState, Boolean>
	{
		int m_nIdToMatch;

		public SelectMsgMatchingRootId(int nId) {
			m_nIdToMatch = nId;
		}

		public Boolean doFunc(TreeConstraintState msg) {
			try {
				return new Boolean(msg.getRootId() == m_nIdToMatch);
			} catch(java.lang.ClassCastException e) {
				return new Boolean(false);
			}
		}
	}
	
	
	public static class SelectMsgMatchingDepth implements IFuncObj<TreeConstraintState, Boolean>
	{
		int m_nDepToMatch;

		public SelectMsgMatchingDepth(int nDep) {
			m_nDepToMatch = nDep;
		}

		public Boolean doFunc(TreeConstraintState msg) {
			return new Boolean(msg.getDepthEst() == m_nDepToMatch);
		}
	}

	public static class SelectMsgWithParent implements IFuncObj<TreeConstraintState, Boolean>
	{
		int m_nIdToMatch;

		public SelectMsgWithParent(int nId) {
			m_nIdToMatch = nId;
		}

		public Boolean doFunc(TreeConstraintState msg) {
			return new Boolean(msg.getParentId() == m_nIdToMatch);
		}
	}

	
	
	public static class SelectMsgWithPropParent implements IFuncObj<TreeConstraintState, Boolean>
	{
		int m_nIdToMatch;

		public SelectMsgWithPropParent(int nId) {
			m_nIdToMatch = nId;
		}

		public Boolean doFunc(TreeConstraintState msg) {
			return new Boolean(msg.getParentProp() == m_nIdToMatch);
		}
	}

	public static class SelectMsgWithTrueVar implements IFuncObj<TreeConstraintState, Boolean>
	{
		Integer m_nObjVarIdx;

		public SelectMsgWithTrueVar(int nVarIdx) {
			m_nObjVarIdx = new Integer(nVarIdx);
		}

		public Boolean doFunc(TreeConstraintState msg) {
				Boolean bObjResult = msg.getBoolVar(m_nObjVarIdx);
				if(bObjResult == null) {
					return new Boolean(false);
				}
				return bObjResult;
			
		}
	}

}