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 MorphAgentTestAlg implements IAgent
{

    IMorphSpec m_shapeDesc;
    
    double m_lfVel;

    double m_lfTCom;
    double m_lfD1, m_lfD2;

    public MorphAgentTestAlg()
    {
	m_lfVel = 1.0;
    }

    public MorphAgentTestAlg(double lfRobotVelocity)
    {
	m_lfVel = lfRobotVelocity;
	m_shapeDesc = new MorphSpecCombRect(); // dumb default
    }

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

    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; }
	TreeAgentState stateCast = (TreeAgentState)(state.getVars());
	if(stateCast != null &&
	   state.getControlFunc() != null &&
	   state.getControlFunc().getNumStateVars() == 2) 
	    {
		return true;
	    }
	
	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<CommLink> 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(stateOut.getAlgVars().getBoolVar(new Integer(1)).booleanValue()
	       == false) {
		doPhase1(stateOut, arrLfTarget, 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();
	    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) {
	    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) {
			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) {
			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)
	    {
		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);
    }


    void doPhase1(TreeAgentState stateToUpdate,
		  double [] arrLfNewTarget,
		  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) {
		    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())
			{
			    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.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;
	    System.out.println("Phase 1, root bot\n");
	}
	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);
	    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) {
		    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;
	    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 MorphAgentTestAlg(this); }
}