import java.util.Iterator;


/**
 * <p>
 * Implementation of IAgent for formation morphing algorithm.
 * </p>
 * <p>
 * Associated state is described in TreeAgentState
 * </p>
 * @author     Michael Schuresko
 * @version    %I%, %G%
 * @since      1.0
 * @see TreeAgentState
*/
public class MorphAgent implements IAgent
{

    IMorphSpec m_shapeDesc;
    
    double m_lfVel;

    double m_lfTCom;
    double m_lfD1, m_lfD2;

    boolean m_bGlobalFrameMode;
    boolean m_bVerbose;

    public MorphAgent()
    {
	m_bVerbose = false;
	m_lfVel = 1.0;
    }

    public MorphAgent(double lfRobotVelocity)
    {
	m_bVerbose = false;
	m_lfVel = lfRobotVelocity;
	m_shapeDesc = new MorphSpecCombRect(); 
	    // MorphSpecCombRect(); // dumb default
	m_bGlobalFrameMode = false;
    }

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

    /**
     * Override the default shape description
     * (useful if switching between global and local
     * algorithm modes)
     */
    public void setShapeDesc(IMorphSpec shapeDesc) {
	m_shapeDesc = shapeDesc;
    }

    public void setGlobalFrameMode(boolean bMode) {
	m_bGlobalFrameMode = bMode;
    }

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

    /**
     * checks that a particular discrete state is valid for this agent.
     */
    public boolean checkStateValidity(StateBundle state)
    {
	if(state == null) { return false; }
	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 state 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 CommLink corresponding to channels to send messages
     * on (neighbor indices can be ascertained from channels).
     * @see CommLink
     * @since 1.0
     */
    public void getMsgs(ILogicVarBundle state,
    		IDiscreteDynamicsCallback dynCallback,
			double [] arrLfStateCont, 
			double lfCurrTime, 
			int nIdxStateOffset,
			Iterator<CommLink> channelsToSendOn)
    {
	try {
		dynCallback.doCallback(state, arrLfStateCont);
	    TreeMsg msg = 
		new TreeMsg((TreeAgentState)state,
			    arrLfStateCont,
			    nIdxStateOffset/2);
	    
	    while(channelsToSendOn.hasNext()) {
		CommLink linkSendOn = 
		    (CommLink)channelsToSendOn.next();
		if(linkSendOn != null && linkSendOn.queue() != null) {
		    linkSendOn.queue().sendMsg(new TreeMsg(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
     * @return new discrete state of agent
     */
    public StateBundle updateState(ILogicVarBundle statePrev,
				   double [] arrLfStateCont, double lfCurrTime,
				   int nIdxStateOffset, IEnvironment env,
				   Iterator<CommLink> channelsRecieveFrom)
    {
	if(statePrev == null) {
	    return new StateBundle(new TreeAgentState(),
				   new ControlFuncDoNothing(2));
	}
	TreeAgentState state = null;
	try {
	    state = (TreeAgentState)statePrev;
	} catch(java.lang.ClassCastException e) {
	    return new StateBundle(new TreeAgentState(),
				   new ControlFuncDoNothing(2));
	}

	TreeAgentState stateOut = new TreeAgentState(state);

	double [] arrLfTarget = new double[2];

	arrLfTarget[0] = arrLfStateCont[nIdxStateOffset];
	arrLfTarget[1] = arrLfStateCont[nIdxStateOffset+1];

	int nId = nIdxStateOffset/2;
	stateOut.setTargetConfig(m_shapeDesc.getTopoDepth(nId),
				 m_shapeDesc.getTopoFirstVisit(nId),
				 m_shapeDesc.getTopoLastVisit(nId),
				 m_shapeDesc.getNumChildren(nId));

	switch(stateOut.getCurrRound()) {
	case TreeAgentState.ROUND_ATTACH :
	    break;
	case TreeAgentState.ROUND_UPDATE_DEPTH :
	    break;
	case TreeAgentState.ROUND_SET_PARLESS :
	    break;
	case TreeAgentState.ROUND_PROPOSE_NEW :
	    break;
	default :
	    // somehow flag this as an error
	    break;
	}

	if(stateOut.getDfsFinished() == false) {
	    doDfsPhase(stateOut, arrLfTarget, channelsRecieveFrom);
	} 
	else if(m_bGlobalFrameMode) {
	    if(stateOut.getAlgVars().getBoolVar(new Integer(1)).booleanValue()
	       == false) {
		if(m_bVerbose) {
		    System.out.println("Phase 1 : agent=="+nIdxStateOffset/2+
				       "\n");
		}
		arrLfTarget[0] = arrLfStateCont[0];
		arrLfTarget[1] = arrLfStateCont[1];
		arrLfTarget = m_shapeDesc.getFinalUv(0);
		doGlblPhase1(stateOut, nId, channelsRecieveFrom);
		/* } else if(stateOut.getAlgVars().getBoolVar(new Integer(2)).
		      booleanValue() == false) {
		
		System.out.println("Phase 2 : agent=="+nIdxStateOffset/2+
				   "\n");
		arrLfTarget[0] = arrLfStateCont[0];
		arrLfTarget[1] = arrLfStateCont[1];
		doGlblPhase2(stateOut, nId, channelsRecieveFrom);
		*/
	    } else{
		if(m_bVerbose) {
		    System.out.println("Phase 2 : agent=="+nIdxStateOffset/2+
				       "\n" +
				       "\tParent == "+stateOut.getParentId() 
				       +" , depth est == "
				       +stateOut.getDepthEst() + "\n");
		}
		double [] arrLfTargTmp;
		arrLfTargTmp = 
		    m_shapeDesc.getFinalUv(nIdxStateOffset/2);
		arrLfTarget[0] = arrLfTargTmp[0];
		arrLfTarget[1] = arrLfTargTmp[1];
 
	    }
	    
	} else {
	    if(stateOut.getAlgVars().getBoolVar(new Integer(1)).booleanValue()
	       == false) {
		doPhase1(stateOut, arrLfTarget, 
			 arrLfStateCont, nIdxStateOffset,
			 channelsRecieveFrom);
		if(stateOut.getAlgVars().getBoolVar(new 
						    Integer(1)).booleanValue() 
		   == true) {
		    arrLfTarget[0] = arrLfStateCont[nIdxStateOffset];
		    arrLfTarget[1] = arrLfStateCont[nIdxStateOffset+1];
		}
	    }
	    else {
		doPhase2(stateOut, arrLfTarget, 
			 arrLfStateCont, nIdxStateOffset,
			 channelsRecieveFrom);
		if(false) {
		    arrLfTarget[0] = arrLfStateCont[nIdxStateOffset];
		    arrLfTarget[1] = arrLfStateCont[nIdxStateOffset+1];
		}
	    }
	}	    
	int nParentId = stateOut.getParentId();
	if(nIdxStateOffset != 0 && stateOut.getDfsFinished() == true) {
	    double lfSqrdDst = (arrLfStateCont[nIdxStateOffset]-
				arrLfStateCont[2*nParentId])*
		(arrLfStateCont[nIdxStateOffset]-
		 arrLfStateCont[2*nParentId]) +
		(arrLfStateCont[nIdxStateOffset+1]-
		 arrLfStateCont[2*nParentId+1])*
		(arrLfStateCont[nIdxStateOffset+1]-
		 arrLfStateCont[2*nParentId+1]);
	    
	    if(Math.sqrt(lfSqrdDst) > m_lfD1) {
		arrLfTarget[0] = arrLfStateCont[2*nParentId];
		arrLfTarget[1] = arrLfStateCont[2*nParentId+1];
	    }
	}

	

	if(nIdxStateOffset == 0) {
	    if(m_bGlobalFrameMode) {
		arrLfTarget = m_shapeDesc.getFinalUv(0);
	    } else {
		arrLfTarget[0] = arrLfStateCont[nIdxStateOffset];
		arrLfTarget[1] = arrLfStateCont[nIdxStateOffset+1];
	    }
	}

	return new StateBundle(stateOut,
			       new ControlFuncMoveTowards(arrLfTarget,
							  m_lfVel));
    }


    void doDfsPhase(TreeAgentState stateToUpdate,
		    double [] arrLfNewTarget,
		    Iterator<CommLink> channelsRecieveFrom)
    {
	//	try {
	int nId = -1;
	if(stateToUpdate.getParentId() < 0) {
	    // wait till we find a parent
	    while(channelsRecieveFrom.hasNext()) {
		CommLink linkRecvFrom = 
		    (CommLink)channelsRecieveFrom.next();
		nId = linkRecvFrom.to();
		if(linkRecvFrom != null && linkRecvFrom.queue() != null) {
		    IMsg currMsg = 
			linkRecvFrom.queue().readMsg();
		    if(currMsg != null&& 
		       linkRecvFrom.to() != linkRecvFrom.from()) {
			TreeMsg currTreeMsg = (TreeMsg)currMsg;
			TreeAgentState senderState = 
			    (TreeAgentState)(currTreeMsg.getState());
			if(senderState.getParentId() >= 0 ||
			   linkRecvFrom.from() == 0) {
			    if(linkRecvFrom.to() > 0 &&
			       stateToUpdate.getParentId() < 0) {
				stateToUpdate.setParentId(linkRecvFrom.from());
				stateToUpdate.setDepthEst(senderState.
							  getDepthEst()+1);
							  
			    }
			}
		    }
		}
	    }
	}
	else {
	    // wait till our children all settle down
	    boolean bSettled = true;
	    while(channelsRecieveFrom.hasNext()) {
		CommLink linkRecvFrom = 
		    (CommLink)channelsRecieveFrom.next();
		if(linkRecvFrom != null && linkRecvFrom.queue() != null) {
		    IMsg currMsg = 
			linkRecvFrom.queue().readMsg();
		    if(currMsg != null&& 
		       linkRecvFrom.to() != linkRecvFrom.from()
		       ) {
			TreeMsg currTreeMsg = (TreeMsg)currMsg;
			TreeAgentState senderState = 
			    (TreeAgentState)(currTreeMsg.getState());
			if(senderState.getParentId() < 0)
			    {
				bSettled = false;
			    }
			if(senderState.getParentId() == 
			   linkRecvFrom.to() &&
			   linkRecvFrom.to() != 0 && 
			   senderState.getDfsFinished() != true)
			    {
				bSettled = false;
			    }
			if(linkRecvFrom.from() == stateToUpdate.getParentId()
			   && linkRecvFrom.to() != 0)
			    {
				stateToUpdate.setDepthEst(senderState.
							  getDepthEst()+1);
			    }
		    }
		}
	    }
	    stateToUpdate.setDfsFinished(bSettled);
	}

	if(nId == 0)
	    {
		if(m_bVerbose) {
		    System.out.println("Phase Dfs, root bot\n");
		}
		stateToUpdate.setParentId(0);
		stateToUpdate.setDepthEst(0);
	    }
	//	} catch(java.lang.ClassCastException e) {
	//    // if we recieve an invalid message, abort silently
	//}
	stateToUpdate.getAlgVars().insertBoolVar(new Integer(1), false);
	stateToUpdate.getAlgVars().insertBoolVar(new Integer(2), false);
    }


    void doGlblPhase1(TreeAgentState stateToUpdate,
		      int nAgentId,
		      Iterator<CommLink> channelsRecieveFrom)
    {

	int nNewParent = stateToUpdate.getParentId();

	// double [] arrLfTargTmp = null;



	int nMaxDepth = -1;
	int nDepthEst = stateToUpdate.getDepthEst();

	
	
	boolean bChildrenFinished = true;

	int nCountChildren = 0;

	while(channelsRecieveFrom.hasNext()) {
	    CommLink linkRecvFrom = 
		(CommLink)channelsRecieveFrom.next();
	    if(linkRecvFrom != null && linkRecvFrom.queue() != null) {
		IMsg currMsg = 
		    linkRecvFrom.queue().readMsg();
		if(currMsg != null&& 
		   linkRecvFrom.to() != linkRecvFrom.from()
		   ) {
		    TreeMsg currTreeMsg = (TreeMsg)currMsg;
		    TreeAgentState senderState = 
			(TreeAgentState)(currTreeMsg.getState());
		    
		    
		    if(senderState.getParentId() == nAgentId) {
			++nCountChildren;
		    }
		    if(linkRecvFrom.from() == stateToUpdate.getParentId())
			{
			    nDepthEst = senderState.getDepthEst()+1;
			}
		    if(senderState.isAncest(stateToUpdate) &&
		       senderState.getTargetDepth() > nMaxDepth)
			{
			    if(senderState.getDepthEst() < 
			       stateToUpdate.getDepthEst() ||
			       senderState.getParentId() ==
			       stateToUpdate.getParentId() ||
			       linkRecvFrom.from() == 
			       stateToUpdate.getParentId()) {
				nNewParent = linkRecvFrom.from();
				nMaxDepth = senderState.getTargetDepth();
				nDepthEst = senderState.getDepthEst()+1;
			    }
			}
		    else if(senderState.getParentId() == nAgentId) {
			// ++nCountChildren;
			
			if(stateToUpdate.isAncest(senderState)) {
			    if(senderState.getTargetDepth() !=
			       stateToUpdate.getTargetDepth() +1 ||
			       senderState.getAlgVars().
			       getBoolVar(new Integer(1)).
			       booleanValue() == false) {
				bChildrenFinished = false;
			    } else {

			    }
			}
			else {
			    bChildrenFinished = false;
			}
		    }
		}
	    }
	}
	if(nMaxDepth >= 0) {
	    stateToUpdate.setParentId(nNewParent);
	}

	if(m_bVerbose) {
	    System.out.println("nChilds == "+nCountChildren+", finished == "+
			       bChildrenFinished+", targ children == "+
			       stateToUpdate.getTargNChildren()+", Id == "+
			       nAgentId);
	    System.out.println("depth diff is "+
			       (stateToUpdate.getTargetDepth()-nMaxDepth) +
			       "depth est is "+stateToUpdate.getDepthEst() +
			       ", Parent is "+stateToUpdate.getParentId());
	}

	if(bChildrenFinished == true &&
	   nCountChildren == stateToUpdate.getTargNChildren() &&
	   stateToUpdate.getTargetDepth() == nMaxDepth + 1) {
	    stateToUpdate.getAlgVars().insertBoolVar(new Integer(1),
						     true);
	}
	if(nAgentId == 0) {
	    nDepthEst = 0;
	}
	stateToUpdate.setDepthEst(nDepthEst);

    }

    void doGlblPhase2(TreeAgentState stateToUpdate,
		      int nAgentId,
		      Iterator<CommLink> channelsRecieveFrom)
    {

	int nNewParent = stateToUpdate.getParentId();

	// double [] arrLfTargTmp = null;

	int nId = -1;

	int nMaxDepth = -1;
	int nDepthEst = stateToUpdate.getDepthEst();

	
	
	boolean bChildrenFinished = true;

	boolean bParentFinished = true;

	int nCountChildren = 0;

	while(channelsRecieveFrom.hasNext()) {
	    CommLink linkRecvFrom = 
		(CommLink)channelsRecieveFrom.next();
	    if(linkRecvFrom != null && linkRecvFrom.queue() != null) {
		IMsg currMsg = 
		    linkRecvFrom.queue().readMsg();
		if(currMsg != null && 
		   linkRecvFrom.to() != linkRecvFrom.from()
		   ) {
		    TreeMsg currTreeMsg = (TreeMsg)currMsg;
		    TreeAgentState senderState = 
			(TreeAgentState)(currTreeMsg.getState());
		    nId = linkRecvFrom.to();
		    
		    if(senderState.getParentId() == nId ) {
			++nCountChildren;
		    }
		    if(linkRecvFrom.from() == stateToUpdate.getParentId())
			{
			    nDepthEst = senderState.getDepthEst()+1;
			    bParentFinished = 
				senderState.getAlgVars().
				getBoolVar(new Integer(2)).booleanValue();
			}
		    if(senderState.isAncest(stateToUpdate) &&
		       senderState.getTargetDepth() > nMaxDepth)
			{
			    if(senderState.getDepthEst() < 
			       stateToUpdate.getDepthEst() ||
			       senderState.getParentId() ==
			       stateToUpdate.getParentId() ||
			       linkRecvFrom.from() == 
			       stateToUpdate.getParentId() ) {
				nNewParent = linkRecvFrom.from();
				nMaxDepth = senderState.getTargetDepth();
				nDepthEst = senderState.getDepthEst()+1;
			    }
			}
		
		    /* else if(senderState.getParentId() == nAgentId) {
		       
		    
		    if(stateToUpdate.isAncest(senderState)) {
		    if(senderState.getTargetDepth() !=
		    stateToUpdate.getTargetDepth() +1 ||
		    senderState.getAlgVars().
		    getBoolVar(new Integer(1)).
		    booleanValue() == false) {
		    bChildrenFinished = false;
		    } else {
		    
		    }
		    }
		    else {
		    bChildrenFinished = false;
		    }
		    }
		    */
		    
		    
		}
	    }
	}
	if(nMaxDepth >= 0) {
	    stateToUpdate.setParentId(nNewParent);
	}
	if(m_bVerbose) {
	    System.out.println("nChilds == "+nCountChildren+", finished == "+
			       bChildrenFinished+", targ children == "+
			       stateToUpdate.getTargNChildren()+", Id == "+
			       nAgentId);
	    System.out.println("depth diff is "+
			       (stateToUpdate.getTargetDepth()-nMaxDepth) +
			       "depth est is "+stateToUpdate.getDepthEst() +
			       ", Parent is "+stateToUpdate.getParentId());
	}

	if(bChildrenFinished == true &&
	   nCountChildren == stateToUpdate.getTargNChildren() &&
	   bParentFinished == true) {
	    stateToUpdate.getAlgVars().insertBoolVar(new Integer(2),
						     true);
	}
	if(nAgentId == 0) {
	    nDepthEst = 0;
	}
	stateToUpdate.setDepthEst(nDepthEst);

    }

    void doPhase1(TreeAgentState stateToUpdate,
		  double [] arrLfNewTarget,
		  double [] arrLfWorldState, int nIdxAgentOffset,
		  Iterator<CommLink> channelsRecieveFrom)
    {

	// int nNewParent = stateToUpdate.getParentId();

	double [] arrLfTargTmp = null;

	int nId = -1;

	int nMaxDepth = -1;
	int nDepthEst = stateToUpdate.getDepthEst();

	boolean bChildrenFinished = true;

	while(channelsRecieveFrom.hasNext()) {
	    CommLink linkRecvFrom = 
		(CommLink)channelsRecieveFrom.next();
	    if(linkRecvFrom != null && linkRecvFrom.queue() != null) {
		IMsg currMsg = 
		    linkRecvFrom.queue().readMsg();
		if(currMsg != null&& 
		   linkRecvFrom.to() != linkRecvFrom.from()
		   ) {
		    TreeMsg currTreeMsg = (TreeMsg)currMsg;
		    TreeAgentState senderState = 
			(TreeAgentState)(currTreeMsg.getState());
		    nId = linkRecvFrom.to();
		    if(senderState.isAncest(stateToUpdate) &&
		       senderState.getTargetDepth() > nMaxDepth)
			{
			    if(
			       senderState.getDepthEst() <
			       stateToUpdate.getDepthEst() &&
			       senderState.getDfsFinished() == true) {
				nMaxDepth = senderState.getTargetDepth();
				stateToUpdate.setParentId(linkRecvFrom.from());
			    }
			}
		    if(senderState.getDepthEst() < 
		       stateToUpdate.getDepthEst() -1 &&
		       nMaxDepth < 0 &&
		       senderState.getDfsFinished() == true) 
			{
			    stateToUpdate.setParentId(linkRecvFrom.from());
			}
		    if(linkRecvFrom.from() == stateToUpdate.getParentId())
			{
			    if(m_bVerbose) {
				System.out.println(nId+" -- "+
						   senderState.getFirstVisit() +
						   ", "+
						   senderState.getLastVisit()+
						   "\t|\t"+
						   stateToUpdate.getFirstVisit()
						   +", "+
						   stateToUpdate.getLastVisit());
				System.out.println("children finished == "+
						   bChildrenFinished +
						   " and isAncest == " +
						   senderState.isAncest(stateToUpdate));
			    }
			    
			    nDepthEst = 
				senderState.getDepthEst()+1;
			    if(nMaxDepth < 0) {
				arrLfNewTarget[0] = 0.0;
				arrLfNewTarget[1] = 0.0;

				arrLfTargTmp =
				    currTreeMsg.
				    getPhase2Targ(arrLfWorldState, 
						  nIdxAgentOffset,
						  m_lfVel*
						  m_lfTCom
						  );
				
				
				// currTreeMsg.getPhase1Targ(m_lfD1);
				arrLfNewTarget[0] = arrLfTargTmp[0];
				arrLfNewTarget[1] = arrLfTargTmp[1];
			    }
			} else if(senderState.getParentId() ==
			    linkRecvFrom.to() &&
				  linkRecvFrom.from() != linkRecvFrom.to()) {
			    nId = linkRecvFrom.to();
			    boolean bPhase1 = senderState.getAlgVars().
				getBoolVar(new Integer(1)).booleanValue();
			    bChildrenFinished = bChildrenFinished &&
				bPhase1;

			}
		}
	    }
	}

	if(nId == 0) {
	    nDepthEst = 0; 
	    nMaxDepth = 0;
	    if(m_bVerbose) {
		System.out.println("Phase 1, root bot\n");
	    }
	}
	if(m_bVerbose) {
	    if(nDepthEst - stateToUpdate.getDepthEst() > 1) {
		System.out.println("Illogical Jump\n");
	    }
	
	    System.out.println("Robot "+nId+" goes from "+
			       stateToUpdate.getDepthEst()+
			       " to "+nDepthEst +
			       " parentId is "+
			       stateToUpdate.getParentId()+
			       " nMaxDetph is "+nMaxDepth);
	}

	stateToUpdate.setDepthEst(nDepthEst);
	if(bChildrenFinished && nMaxDepth >= 0) {
	    stateToUpdate.getAlgVars().insertBoolVar(new Integer(1), true);
	    if(m_bVerbose) {
		System.out.println("Robot "+nId+" has completed phase 1\n");
	    }
	}
    }

    void doPhase2(TreeAgentState stateToUpdate,
		  double [] arrLfNewTarget,
		  double [] arrLfWorldState, int nIdxAgentOffset,
		  Iterator<CommLink> channelsRecieveFrom)
    {
	 
	// int nNewParent = stateToUpdate.getParentId();

	double [] arrLfTargTmp = null;

	int nMaxDepth = -1;
	int nDepthEst= stateToUpdate.getDepthEst();



	

	int nId = -1;

	while(channelsRecieveFrom.hasNext()) {
	    CommLink linkRecvFrom = 
		(CommLink)channelsRecieveFrom.next();
	    nId = linkRecvFrom.to();
	    if(linkRecvFrom != null && linkRecvFrom.queue() != null) {
		IMsg currMsg = 
		    linkRecvFrom.queue().readMsg();
		if(currMsg != null&& 
		   linkRecvFrom.to() != linkRecvFrom.from()
		   ) {
		    TreeMsg currTreeMsg = (TreeMsg)currMsg;
		    TreeAgentState senderState = 
			(TreeAgentState)(currTreeMsg.getState());
		    if(senderState.isAncest(stateToUpdate) &&
		       senderState.getTargetDepth() > nMaxDepth &&
		       ( senderState.getParentId() == 
			 stateToUpdate.getParentId() ||
			 senderState.getDepthEst() < 
			 stateToUpdate.getDepthEst()
			 ) &&
		       senderState.getDfsFinished() == true)
			{
			    nMaxDepth = senderState.getTargetDepth();
			    stateToUpdate.setParentId(linkRecvFrom.from());
			}
		    if(linkRecvFrom.from() == stateToUpdate.getParentId())
			{
			    nDepthEst = senderState.getDepthEst() +1;
			    if(nMaxDepth < stateToUpdate.getTargetDepth()-1) {
				arrLfNewTarget[0] = 0.0;
				arrLfNewTarget[1] = 0.0;
				arrLfTargTmp = 
				    currTreeMsg.getPhase2Targ(arrLfWorldState, 
							      nIdxAgentOffset,
							      m_lfVel*
							      m_lfTCom
							      );

				arrLfNewTarget[0] = arrLfTargTmp[0];
				arrLfNewTarget[1] = arrLfTargTmp[1];
			    } else {
				arrLfTargTmp = 
				    m_shapeDesc.getFinalUv(nIdxAgentOffset/2);
				//	arrLfTargTmp[0] = 0.0;
				//	arrLfTargTmp[1] = 0.0;
				arrLfTargTmp = 
				    currTreeMsg.uvToWorld(arrLfTargTmp,
							  0);
				arrLfNewTarget[0] = arrLfTargTmp[0];
				arrLfNewTarget[1] = arrLfTargTmp[1];
			    }
			} 
		}
	    }
	}

	if(nId == 0) {
	    nDepthEst = 0;
	    if(m_bVerbose) {
		System.out.println("Phase 2, root bot\n");
	    }
	}
	
	stateToUpdate.setDepthEst(nDepthEst);
    }

    /**
     * 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 new MorphAgent(this); }


}