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

/**
 * <p>
 * Implementation of "Move in global frame" formation morphing algorithm
 * rewritten (nearly) from scratch to represent state of the algorithm circa
 * February 2007.
 * </p>
 * @author     Michael Schuresko
 * @version    %I%, %G%
 * @since      1.1
 */
public class MorphAgentGlbl implements IAgent
{




	IMorphSpec m_shapeDesc;

	double m_lfVel;

	double m_lfTCom;
	double m_lfD1, m_lfD2;

	boolean m_bMotionParamsSet;
	boolean m_bVerbose;

	public MorphAgentGlbl()
	{
		m_bVerbose = false;
		m_lfVel = 1.0;
		m_bMotionParamsSet = false;
	}

	public MorphAgentGlbl(double lfRobotVelocity)
	{
		m_bVerbose = false;
		m_lfVel = lfRobotVelocity;
		m_shapeDesc = new MorphSpecGlblRect();
		m_bMotionParamsSet = false;
	}

	public MorphAgentGlbl(MorphAgentGlbl src)
	{
		m_bVerbose = false;
		m_lfVel = src.m_lfVel;
		m_shapeDesc = src.m_shapeDesc; //hopefully these things
		// will not be dynamic!
		m_bMotionParamsSet = src.m_bMotionParamsSet;
	}

	/**
	 * Override the default shape description
	 */
	public void setShapeDesc(IMorphSpec shapeDesc) {
		m_shapeDesc = shapeDesc;
	}

	public void setMotionParams(double lfVel, double lfTCom, double lfRad)
	{
		m_bMotionParamsSet = true;
		m_lfVel = lfVel; m_lfTCom = lfTCom;
		m_lfD2 = lfRad;
		m_lfD1 = m_lfD2 - 4.0*m_lfTCom*m_lfVel;
	}

	/**
	 * checks that a particular discrete state is valid for this agent
	 */
	public boolean checkStateValidity(StateBundle state)
	{

		try {
			TreeAgentState treeState = 
				(TreeAgentState)state.getVars();
			if(treeState != null) {
				return true;
			} else {
				return false;
			}
		} catch(java.lang.ClassCastException e) {
			return false;
		}
	}

	// Discrete state components

	/**
	 * <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) 
	{
		try {
			
			dynCallback.doCallback(discreteState, arrLfStateCont);
			IMsg msg = 
				(IMsg)(new TreeAgentState((TreeAgentState)discreteState));

			while(channelsToSendOn.hasNext()) {
				CommLink linkSendOn = 
					(CommLink)channelsToSendOn.next();
				if(linkSendOn != null && linkSendOn.queue() != null) {
					linkSendOn.queue().sendMsg(msg);
				}
			}
		} catch(java.lang.ClassCastException e) {
			// send nothing
		}
	}


	/**
	 * <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 state bundle containing discrete state of agent
	 * and new control function
	 */
	public StateBundle updateState(ILogicVarBundle statePrev,
			double arrLfStateCont[], double lfCurrTime,
			int nIdxStateOffset, IEnvironment env,
			Iterator<CommLink> channelsReceiveFrom)
	{

		if(!m_bMotionParamsSet) {
			System.out.println("************************************");
			System.out.println("**   Error! Motion params not set **");
		}

		ILogicVarBundle stateNext = getNextState(statePrev,
				channelsReceiveFrom,
				nIdxStateOffset/2);

		IControlFunc controlFuncNext = 
			getNextControlFunc((TreeAgentState)stateNext, arrLfStateCont,
					nIdxStateOffset);

		return new StateBundle(stateNext, controlFuncNext);
	}


	class SelectMsgMatchingId implements IFuncObj<IMsg, Boolean>
	{
		int m_nIdToMatch;

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

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

	class SelectMsgMatchingDepth implements IFuncObj<IMsg, Boolean>
	{
		int m_nDepToMatch;

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

		public Boolean doFunc(IMsg msg) {
			try {
				TreeAgentState vars = 
					(TreeAgentState)msg;
				return new Boolean(vars.getDepthEst() == m_nDepToMatch);
			} catch(java.lang.ClassCastException e) {
				return new Boolean(false);
			}
		}
	}

	class SelectMsgWithParent implements IFuncObj<IMsg, Boolean>
	{
		int m_nIdToMatch;

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

		public Boolean doFunc(IMsg msg) {
			try {
				TreeAgentState vars = 
					(TreeAgentState)msg;
				return new Boolean(vars.getParentId() == m_nIdToMatch);
			} catch(java.lang.ClassCastException e) {
				return new Boolean(false);
			}
		}
	}

	class SelectMsgWithPropParent implements IFuncObj<IMsg, Boolean>
	{
		int m_nIdToMatch;

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

		public Boolean doFunc(IMsg msg) {
			try {
				TreeAgentState vars = 
					(TreeAgentState)msg;
				return new Boolean(vars.getParentProp() == m_nIdToMatch);
			} catch(java.lang.ClassCastException e) {
				return new Boolean(false);
			}
		}
	}

	class SelectMsgWithTrueVar implements IFuncObj<IMsg, Boolean>
	{
		Integer m_nObjVarIdx;

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

		public Boolean doFunc(IMsg msg) {
			try {
				TreeAgentState vars = 
					(TreeAgentState)msg;
				Boolean bObjResult = vars.getBoolVar(m_nObjVarIdx);
				if(bObjResult == null) {
					return new Boolean(false);
				}
				return bObjResult;
			} catch(java.lang.ClassCastException e) {
				return new Boolean(false);
			}
		}
	}

	ILogicVarBundle getNextState(ILogicVarBundle statePrev,
			Iterator<CommLink> channelsReceiveFrom,
			int nAgentId) 
	{
		TreeAgentState treeStateNext = null;

		try {
			TreeAgentState treeStatePrev = 
				(TreeAgentState)statePrev;
			treeStateNext = new TreeAgentState(treeStatePrev);
		} catch(java.lang.ClassCastException e) {
			System.out.println("Error in class cast in MorphAgentGlbl");
			return null;
		}


		treeStateNext.setTargetConfig(m_shapeDesc.getTopoDepth(nAgentId),
				m_shapeDesc.getTopoFirstVisit(nAgentId),
				m_shapeDesc.getTopoLastVisit(nAgentId),
				m_shapeDesc.getNumChildren(nAgentId));

		if(m_bVerbose) {
			if(treeStateNext.getId() == 0) {
				System.out.println("I am root!");
			} else {
				System.out.println("I am "+treeStateNext.getId());
			}
			System.out.println("Anc == (" + treeStateNext.getFirstVisit() +
					"," + treeStateNext.getTargDepth() + "," +
					treeStateNext.getLastVisit() + ")");
		}
		
		

		if(! treeStateNext.getDfsFinished()) {
			treeStateNext = doDfs(treeStateNext, 
					channelsReceiveFrom,
					nAgentId);
			treeStateNext.incrRound();
			return treeStateNext;
		}
		



		IFuncObj<IMsg, Boolean> funcSibl = 
			new SelectMsgMatchingDepth(treeStateNext.getDepthEst());

		IFuncObj<IMsg, Boolean> funcChildren = 
			new SelectMsgWithParent(treeStateNext.getId());

		IFuncObj<IMsg, Boolean> funcParent = 
			new SelectMsgMatchingId(treeStateNext.getParentId());

		IFuncObj<IMsg, Boolean> funcPropParent = 
			new SelectMsgMatchingId(treeStateNext.getParentProp());

		IFuncObj<IMsg, Boolean> funcVar2True =
			new SelectMsgWithTrueVar(2);

		IFuncObj<IMsg, Boolean> funcCntFinalChilds =
			new BoolFuncAggregators.BoolFuncAnd<IMsg>(funcChildren,funcVar2True);


		IFuncObj<IMsg, Boolean> funcOther = new BoolFuncTrue<IMsg>();

		// slight innefficiency introduced here through
		// looking through copies of the same list multiple times
		// However -- ease of coding should be greatly improved

		Vector<IMsg> msgVec = new Vector<IMsg>();
		Collection<IMsg> msgCollection;
		msgCollection = AgentMsgHelpers.accumMsgs(channelsReceiveFrom, 
				msgVec);

		AgentMsgHelpers helperInstance = new AgentMsgHelpers();

		Iterator<IMsg> iParent = 
			helperInstance.getFilteredMsgs(funcParent,
					msgCollection);

		TreeAgentState stateParent = null;

		if(treeStateNext.getId() == 0) {
			treeStateNext.insertBoolVar(new Integer(0), true);
		}

		if(iParent.hasNext()) {
			IMsg msgParent = iParent.next();

			stateParent = (TreeAgentState)msgParent;


			if(stateParent.isAncest(treeStateNext)) {
				treeStateNext.insertBoolVar(new Integer(0), true);
			}
		} else if(treeStateNext.getId() != 0) {
			System.out.println("Agent "+treeStateNext.getId()+
					" has lost touch with parent ("+
					treeStateNext.getParentId()+")");
			treeStateNext.incrRound();
			return treeStateNext;
			// treeStateNext.insertBoolVar(new Integer(0), true);
			// var 0 is always true for root.
		}


		Iterator<IMsg> iChildren = 
			helperInstance.getFilteredMsgs(funcChildren,
					msgCollection);


		//Boolean bObjFinalMove = 
		//    treeStateNext.getBoolVar(new Integer(2));

		Boolean bObjAttachedToAnc =
			treeStateNext.getBoolVar(new Integer(0));

		Boolean bObjExpand =
			treeStateNext.getBoolVar(new Integer(1));

		//boolean bFinalMove = (bObjFinalMove != null &&
		//		       bObjFinalMove.booleanValue());
		boolean bAttachedToAnc = (bObjAttachedToAnc != null &&
				bObjAttachedToAnc.booleanValue());
		boolean bExpand = (bObjExpand != null &&
				bObjExpand.booleanValue());

		if(bExpand && treeStateNext.getId() != 0) {
			if(treeStateNext.getTargDepth() == stateParent.getTargDepth()+1) {
				int nCntChilds = helperInstance.getCountMsg(funcCntFinalChilds, msgCollection);
				int nTargChilds = treeStateNext.getTargNChildren();
				if(nCntChilds == nTargChilds) {
					treeStateNext.insertBoolVar(new Integer(2), true);
				} else {
					treeStateNext.insertBoolVar(new Integer(2), false);
				}
			} else {
				treeStateNext.insertBoolVar(new Integer(2), false);
			}
		}

		if(m_bVerbose && 
				!bAttachedToAnc && treeStateNext.getParentId() == 0) 
		{
			System.out.println("Agent "+treeStateNext.getId()+
			" is attached to root with bad anc info");
			System.out.println("Anc == (" + treeStateNext.getFirstVisit() +
					"," + treeStateNext.getTargDepth() + "," +
					treeStateNext.getLastVisit() + ")");

		}


		// check whether we're in "expand" state yet
		if(bAttachedToAnc) { // && !bExpand) {

			bExpand = true;
			while(bExpand && iChildren.hasNext()) {
				IMsg childMsg = iChildren.next();
				TreeAgentState treeChildMsg = 
					(TreeAgentState)childMsg;
				Boolean bChildExpand = treeChildMsg.getBoolVar(new Integer(1));

				bExpand = bExpand &&
				bChildExpand != null && 
				bChildExpand.booleanValue() == true;
			}
			if(bExpand) {
				treeStateNext.insertBoolVar(new Integer(1), true);
			}
		}

		if(treeStateNext.getCurrRound() == TreeAgentState.ROUND_UPDATE_DEPTH) {
			// do depth update
			// if(stateParent != null)
			// program SHOULD cause null pointer dereference if we cannot find parent
			if(treeStateNext.getId() != 0 && treeStateNext.getParentId() >= 0) {
				treeStateNext.updateDepthEst(stateParent);
			}
		}

		if(treeStateNext.getCurrRound() == TreeAgentState.ROUND_ATTACH) {
			// do check and attach
			if(treeStateNext.getParentProp() != treeStateNext.getParentId()) {
				Iterator<IMsg> iPropParent = 
					helperInstance.getFilteredMsgs(funcPropParent,
							msgCollection);
				if(iPropParent.hasNext()) {
					TreeAgentState statePropParent = 
						(TreeAgentState)iPropParent.next();
					if(statePropParent.getDepthEst() < treeStateNext.getDepthEst() ||
							statePropParent.getId() > treeStateNext.getId()) {
						treeStateNext.setParentId(treeStateNext.getParentProp());
					} else {
						IFuncObj<IMsg, Boolean> funcPropChildren = 
							new SelectMsgWithPropParent(treeStateNext.getId());

						Iterator<IMsg> iPropChildren =
							helperInstance.getFilteredMsgs(funcPropChildren, msgCollection);
						boolean bFoundSmallerPropChild = false;

						while(!bFoundSmallerPropChild && iPropChildren.hasNext()) {
							TreeAgentState statePropChild = 
								(TreeAgentState)iPropChildren.next();
							bFoundSmallerPropChild = 
								(statePropChild.getId() < treeStateNext.getId());
						}

						if(!bFoundSmallerPropChild) {
							treeStateNext.setParentId(treeStateNext.getParentProp());
						}

					}
				}
			}
		}

		if(treeStateNext.getCurrRound() == TreeAgentState.ROUND_SET_PARLESS) {
			// do set parent-less indicator
			if(treeStateNext.getId() == 0) {
				treeStateNext.setParDepLess(true);
			} else if(treeStateNext.getParentId() >= 0) {
				treeStateNext.setParDepLess(stateParent);
				// again, SHOULD cause error if stateParent is null
			}
		}

		// done checking for expand state

		if(treeStateNext.getCurrRound() == 
			TreeAgentState.ROUND_PROPOSE_NEW && 
			treeStateNext.getId() != 0) 
		{
			// do topology re-arrangements

			int nMaxFinalDepthAttach = -1;
			int nIdAttach = -1;

			IMsg msgConsider;
			TreeAgentState treeStateConsider;

			if(true) {
				Iterator<IMsg> iSiblings = 
					helperInstance.getFilteredMsgs(funcSibl,
							msgCollection);

				while(iSiblings.hasNext()) {
					msgConsider = iSiblings.next();
					treeStateConsider = 
						(TreeAgentState)msgConsider;
					if(treeStateConsider.isAncest(treeStateNext) &&
							treeStateConsider.getParDepLess() == true) {


						if(treeStateConsider.getTargDepth() > 
						nMaxFinalDepthAttach &&
						treeStateConsider.getParDepLess()) 
						{
							nMaxFinalDepthAttach = 
								treeStateConsider.getTargDepth();
							nIdAttach = 
								treeStateConsider.getId();
						}
					}
				} // end while(iSiblings.hasNext()) 
			} // end if(bExpand)


			Iterator<IMsg> iOther = 
				helperInstance.getFilteredMsgs(funcOther,
						msgCollection);
			while(iOther.hasNext()) {
				msgConsider = iOther.next();
				treeStateConsider = 
					(TreeAgentState)msgConsider;
				if(treeStateConsider.isAncest(treeStateNext)) {
					Boolean bObjAncExpanding = 
						treeStateConsider.getBoolVar(new Integer(1));
					boolean bAncExpanding = 
						bObjAncExpanding != null &&
						bObjAncExpanding.booleanValue() == true;
					if(treeStateConsider.getTargDepth() > 
					nMaxFinalDepthAttach &&
					(bAncExpanding == bExpand ||
							treeStateConsider.getId() == 0
					)) 
					{
						if(treeStateConsider.getDepthEst() <
								treeStateNext.getDepthEst() ||
								treeStateConsider.getId() ==
									stateParent.getId()) {
							nMaxFinalDepthAttach = 
								treeStateConsider.getTargDepth();
							nIdAttach = 
								treeStateConsider.getId();
						}
					}
				}
			} // while iOther.hasNext


			if(nMaxFinalDepthAttach < 0 && stateParent != null) {
				// now find the parents parent, if its here

				IFuncObj<IMsg, Boolean> funcParentsPar = 
					new SelectMsgMatchingId(stateParent.getParentId());
				Iterator<IMsg> iParPar = 
					helperInstance.getFilteredMsgs(funcParentsPar,
							msgCollection);

				if(iParPar.hasNext()) {
					msgConsider = iParPar.next();
					treeStateConsider = (TreeAgentState)msgConsider;
					nIdAttach = treeStateConsider.getId();

				}

			}

			if(nIdAttach >= 0) {
				treeStateNext.setParentProp(nIdAttach);
			}

		}



		treeStateNext.incrRound();

		return treeStateNext;

	}

	IControlFunc getNextControlFunc(TreeAgentState stateNext,
			double arrLfStateCont[],
			int nIdxStateOffset)
	{

		if(!stateNext.getDfsFinished()) {
			if(m_bVerbose) {
				System.out.println("Agent "+stateNext.getId()+
				" is doing nothing");
			}
			return new ControlFuncDoNothing(2);
		}

		double [] arrLfMotionPoint = null;

		int nParentId = stateNext.getParentId();

		double lfDistToParent = 0.0;
		lfDistToParent = arrLfStateCont[nParentId*2]-
		arrLfStateCont[nIdxStateOffset];
		lfDistToParent *= lfDistToParent;

		double lfTmp = arrLfStateCont[nParentId*2+1]-
		arrLfStateCont[nIdxStateOffset+1];

		lfDistToParent += lfTmp*lfTmp;

		if(lfDistToParent > m_lfD1*m_lfD1) {
			arrLfMotionPoint = new double[2];

			arrLfMotionPoint[0] = arrLfStateCont[2*nParentId];
			arrLfMotionPoint[1] = arrLfStateCont[2*nParentId+1];

			if(m_bVerbose) {
				System.out.println("Moving towards parent!");
				System.out.println("My offset is "+nIdxStateOffset+
						" moving towards agent "+nParentId);
				System.out.println("From point ("+
						arrLfStateCont[nIdxStateOffset]
						               +","+arrLfStateCont[nIdxStateOffset+1]+
						               ") to point ("+
						               arrLfMotionPoint[0]+","+
						               arrLfMotionPoint[1]+")");

				System.out.println("Agent "+stateNext.getId()+
						" is moving towards parent with velocity "
						+m_lfVel);
			}

			return new ControlFuncMoveTowards(arrLfMotionPoint,
					m_lfVel, nParentId);
		}


		if(m_bVerbose) {
			System.out.println("I (agent " + nIdxStateOffset/2+ ") am at (" +
					arrLfStateCont[nIdxStateOffset]+","+
					arrLfStateCont[nIdxStateOffset+1]+")");
		}

		Boolean bFinalMove = 
			stateNext.getBoolVar(new Integer(2));




		if(bFinalMove != null &&
				bFinalMove.booleanValue() == true) {
			// move to final position

			arrLfMotionPoint = 
				m_shapeDesc.getFinalUv(nIdxStateOffset/2);

			if(m_bVerbose) {
				System.out.println("Agent "+stateNext.getId()+
				" is converging on final position");
			}

			return new ControlFuncMoveTowards(arrLfMotionPoint,
					m_lfVel);
			// m_shapeDesc

		}
		arrLfMotionPoint = 
			m_shapeDesc.getFinalUv(0);
		return new ControlFuncMoveTowards(arrLfMotionPoint, m_lfVel);



		// return new ControlFuncDoNothing(2);
	}

	TreeAgentState doDfs(TreeAgentState stateToUpdate,
			Iterator<CommLink> channelsReceiveFrom,
			int nAgentId) 
	{

		if(m_bVerbose) {
			System.out.println("Doing Dfs");
		}

		if(stateToUpdate.getId() == 0) {
			stateToUpdate.setParentId(0);
			stateToUpdate.setDepthEst(0);
		}
		
		// boolean bFoundParent = false;

		if(stateToUpdate.getParentId() < 0) {
			// look for a valid parent

			if(m_bVerbose) {
				System.out.println("Looking for a parent");
			}

			int nParentId = -1;
			int nDep = -2;

			if(m_bVerbose) {
				if(!channelsReceiveFrom.hasNext()) {
					System.out.println("Agent "+stateToUpdate.getId()+
					" has no input channels");
				}
			}

			while(channelsReceiveFrom.hasNext()) {
				CommLink currLink = channelsReceiveFrom.next();
				IMsgChannel msgChannel = currLink.queue();
				IMsg currMsg = msgChannel.readMsg();
				if(currMsg != null) {
					TreeAgentState currTreeMsg = 
						(TreeAgentState)currMsg;

					if(m_bVerbose) {
						System.out.println("Agent "+stateToUpdate.getId()+
								" is looking at agent "+
								currTreeMsg.getId());
					}

					if(currTreeMsg.getParentId() >= 0) {
						if(m_bVerbose) {
							System.out.println("Found a prospective parent");
						}

						if(nParentId < 0 || currTreeMsg.getDepthEst() < nDep &&
								(!currTreeMsg.getDfsFinished() || currTreeMsg.getId() == 0)) {
							nParentId = currLink.from();
							nDep = currTreeMsg.getDepthEst();
						}
					}
				}
			}


			if(m_bVerbose) {
				System.out.println("Setting parent id to "+nParentId);
			}

			stateToUpdate.setParentId(nParentId);
			stateToUpdate.setDepthEst(nDep+1);
		} else {
			// wait for all neighbors to have parents
			// and all children to be done

			boolean bNeighbors = true;
			boolean bChildrenDone = true;

			while(channelsReceiveFrom.hasNext()) {
				CommLink currLink = channelsReceiveFrom.next();
				IMsgChannel msgChannel = currLink.queue();
				int nMyId = currLink.to();
				IMsg currMsg = msgChannel.readMsg();
				if(currMsg == null) {
					bNeighbors = false;
				} else {
					TreeAgentState
					currTreeMsg = (TreeAgentState)currMsg;
					if(currTreeMsg.getParentId() == nMyId &&
							!currTreeMsg.getDfsFinished() ) 
					{
						bChildrenDone = false;
					}
					if(currTreeMsg.getParentId() < 0) {
						bNeighbors = false;
					}
					if(currTreeMsg.getId() == stateToUpdate.getParentId()) {
						// bFoundParent = true;
						if(currTreeMsg.getDfsFinished())  {
							
						}
					}
				}
			}
			stateToUpdate.setDfsFinished(bNeighbors && bChildrenDone);
		}
		return stateToUpdate;
	}

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