import java.util.Iterator;
import java.util.Vector;
import java.util.Arrays;
import java.util.HashMap;
import Jama.Matrix;
import Jama.EigenvalueDecomposition;


////////10////////20////////30////////40////////50////////60////////70////////80    		


public class LambdaConstraintAgent implements IAgent {
	
	IAgent m_agentWrapped;
	IBcastAlg m_bcastAlg;
	double m_lfVel;
	
	double [] m_arrLfTmpDerivs;
	
	double [] m_arrLfRotDerivs;
	
	ICommStrengthFunc m_commFunc;
	
	double m_lfLambdaMin, m_lfLambdaMax;
	
	double m_lfTCom;
	
	boolean m_bProject;
	
	double m_lfProjectThetaStep;
	
	double m_lfProjectThetaMax;
	
	double m_lfMinLambdaIncrease;
	
	static boolean ms_bTrackBounds = true;
	HashMap<Integer, Vector<Matrix> > m_mapVecMatVelBounds;
	HashMap<Integer, Vector<Matrix>> m_mapVecMatBounds;
	HashMap<Integer, Double> m_mapBoundValidTime;
	HashMap<Integer, Double> m_mapLastLambdaLowBound;
	
	public LambdaConstraintAgent() {
		setLambdaBounds(0.0, 1.0);
		m_commFunc = new ExpCommStrength();
		m_agentWrapped = new RandomAgent(0.0);
		m_bcastAlg = new SimpleAllAllBcast();
		m_lfVel = 0.0;
		m_lfTCom = 0.0;
		m_bProject = false;
		m_lfProjectThetaStep = 0.1;
		m_lfProjectThetaMax = 0.0;
		m_lfMinLambdaIncrease = 0.0;
		m_mapVecMatVelBounds = null;
		m_mapVecMatBounds = null;
	}
	
	public LambdaConstraintAgent(IAgent agentToWrap, double lfMaxVel) {
		setLambdaBounds(0.0, 1.0);
		m_lfVel = lfMaxVel;
		m_agentWrapped = agentToWrap.makeCopy();
		m_commFunc = new ExpCommStrength();
		m_bcastAlg = new SimpleAllAllBcast();
		m_lfTCom = 0.0;
		m_bProject = false;
		m_lfProjectThetaStep = 0.1;
		m_lfProjectThetaMax = 0.0;
		m_lfMinLambdaIncrease = 0.0;
		m_mapVecMatVelBounds = null;
		m_mapVecMatBounds = null;
	}
	
	public LambdaConstraintAgent(IAgent agentToWrap, double lfMaxVel, double lfTCom, 
			ICommStrengthFunc commFunc) {
		setLambdaBounds(0.0, 1.0);
		m_lfVel = lfMaxVel;
		m_agentWrapped = agentToWrap.makeCopy();
		m_commFunc = commFunc.makeCopy();
		m_bcastAlg = new SimpleAllAllBcast();
		m_lfTCom = lfTCom;
		m_bProject = false;
		m_lfProjectThetaStep = 0.1;
		m_lfProjectThetaMax = 0.0;
		m_lfMinLambdaIncrease = 0.0;
		m_mapVecMatVelBounds = null;
		m_mapVecMatBounds = null;
	}
	
	public LambdaConstraintAgent(double lfMaxVel) {
		setLambdaBounds(0.0, 1.0);
		m_lfVel = lfMaxVel;
		m_agentWrapped = new RandomAgent(lfMaxVel);
		m_commFunc = new ExpCommStrength();
		m_bcastAlg = new SimpleAllAllBcast();
		m_lfTCom = 0.0;
		m_bProject = false;
		m_lfProjectThetaStep = 0.1;
		m_lfProjectThetaMax = 0.0;
		m_lfMinLambdaIncrease = 0.0;
		m_mapVecMatVelBounds = null;
		m_mapVecMatBounds = null;
	}
	
	public LambdaConstraintAgent(double lfMaxVel, 
			ICommStrengthFunc commFunc) {
		setLambdaBounds(0.0, 1.0);
		m_lfVel = lfMaxVel;
		m_agentWrapped = new RandomAgent(lfMaxVel);
		m_commFunc = commFunc.makeCopy();
		m_bcastAlg = new SimpleAllAllBcast();
		m_lfTCom = 0.0;
		m_bProject = false;
		m_lfProjectThetaStep = 0.1;
		m_lfProjectThetaMax = 0.0;
		m_lfMinLambdaIncrease = 0.0;
		m_mapVecMatVelBounds = null;
		m_mapVecMatBounds = null;
	}

	
	public LambdaConstraintAgent(LambdaConstraintAgent src) {
		m_agentWrapped = src.m_agentWrapped.makeCopy();
		m_bcastAlg = src.m_bcastAlg.makeCopy();
		m_lfVel = src.m_lfVel;
		m_commFunc = src.m_commFunc.makeCopy();
		m_lfLambdaMin = src.m_lfLambdaMin;
		m_lfLambdaMax = src.m_lfLambdaMax;
		m_lfTCom = src.m_lfTCom;
		m_bProject = src.m_bProject;
		m_lfProjectThetaStep = src.m_lfProjectThetaStep;
		m_lfProjectThetaMax = src.m_lfProjectThetaMax;
		m_lfMinLambdaIncrease = src.m_lfMinLambdaIncrease;
		m_mapVecMatVelBounds = null;
		m_mapVecMatBounds = null;
	}
	
	/**
	 * sets parameters for projecting on to good motion.
	 * @param lfProjMax maximum amount to rotate angle
	 * @param lfProjStep step angle
	 */
	public void setProjectParams(boolean bProject, double lfProjMax, double lfProjStep) {
		m_bProject = bProject;
		m_lfProjectThetaMax = lfProjMax;
		m_lfProjectThetaStep = lfProjStep;
	}
	
	/**
	 * 
	 * @return last calculated Laplacian matrix velocity bounds
	 */
	public Vector<Matrix> getLastVelBounds(int nAgent) {
		if(m_mapVecMatVelBounds != null ) {
			return m_mapVecMatVelBounds.get(new Integer(nAgent));
		}
		return null;
	}
	
	/**
	 * 
	 * @return last calculated Laplacian matrix bounds
	 */
	public Vector<Matrix> getLastMatBounds(int nAgent) {
		if(m_mapVecMatBounds != null) {
			return m_mapVecMatBounds.get(new Integer(nAgent));
		}
		return null;
	}
	
	public Double getLastBoundTime(int nAgent) {
		if(m_mapBoundValidTime != null) {
			return m_mapBoundValidTime.get(new Integer(nAgent));
		}
		return null;
	}
	
	void setLastMatBounds(Vector<Matrix> vecMatVelBounds,
			Vector<Matrix> vecMatBounds, double lfTime, int nAgent) {
		if(ms_bTrackBounds) {
			Integer objRef = new Integer(nAgent);
			if(m_mapVecMatVelBounds == null) {
				m_mapVecMatVelBounds = 
					new HashMap<Integer, Vector<Matrix> >();
			}
			if(m_mapVecMatBounds == null) {
				m_mapVecMatBounds = 
					new HashMap<Integer, Vector<Matrix> >();
			}
			Matrix tmpMat = null;
			Vector<Matrix> vecMatVelBoundsCpy = null;
			if(vecMatVelBounds != null) {
				vecMatVelBoundsCpy = new Vector<Matrix>(2);
				tmpMat = vecMatVelBounds.get(0);
				if(tmpMat != null) {
					tmpMat = tmpMat.copy();
				}
				vecMatVelBoundsCpy.add(0, tmpMat);
				
				tmpMat = vecMatVelBounds.get(1);
				if(tmpMat != null) {
					tmpMat = tmpMat.copy();
				}
				vecMatVelBoundsCpy.add(1, tmpMat);

			}
			m_mapVecMatVelBounds.put(objRef, vecMatVelBoundsCpy);
			
			Vector<Matrix> vecMatBoundsCpy = null;
			if(vecMatBounds != null) {
				vecMatBoundsCpy = new Vector<Matrix>(2);

				tmpMat = vecMatBounds.get(0);
				if(tmpMat != null) {
					tmpMat = tmpMat.copy();
				}
				vecMatBoundsCpy.add(0, tmpMat);
				
				tmpMat = vecMatBounds.get(1);
				if(tmpMat != null) {
					tmpMat = tmpMat.copy();
				}
				vecMatBoundsCpy.add(1, tmpMat);

			}
			m_mapVecMatBounds.put(objRef, vecMatBoundsCpy);
			
			if(m_mapBoundValidTime == null) {
				m_mapBoundValidTime = new HashMap<Integer, Double>();
			}
			
			m_mapBoundValidTime.put(objRef, new Double(lfTime));
		}
	}
	
	void setLastLambdaLowBound(double lfLambdaLowBound, int nAgent) {
		if(m_mapLastLambdaLowBound == null) {
			m_mapLastLambdaLowBound = new HashMap<Integer, Double> ();
		}
		m_mapLastLambdaLowBound.put(new Integer(nAgent), new Double(lfLambdaLowBound));
	}
	
	public double getLastLambdaLowBound(int nAgent) {
		if(m_mapLastLambdaLowBound != null) {
			if(m_mapLastLambdaLowBound.containsKey(new Integer(nAgent))) {
				return m_mapLastLambdaLowBound.get(new Integer(nAgent));
			}
		}
		return 0.0;
	}
	
	public void setLambdaIncrease(double lfLambdaIncrease) {
		m_lfMinLambdaIncrease = lfLambdaIncrease;
	}
	
	/**
	 * Sets lambda_+ and lambda_-
	 */
	public void setLambdaBounds(double lfLambdaMin, double lfLambdaMax) {
		m_lfLambdaMin = lfLambdaMin; m_lfLambdaMax = lfLambdaMax;
	}
	
	/**
     * checks that a particular discrete state is valid for this agent
     */
    public boolean checkStateValidity(StateBundle state) {
    	return true; // temporary hack
    }

    public IAgent getWrappedAgent() {
    	return m_agentWrapped;
    }
    
    public void setWrappedAgent(IAgent agentToWrap) {
    	m_agentWrapped = agentToWrap.makeCopy();
    }
    
    public IBcastAlg getBcastAlg() {
    	return m_bcastAlg;
    }
    
    public void setBcastAlg(IBcastAlg bcastAlg) {
    	m_bcastAlg = bcastAlg;
    }
    
    
    
    
    // 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);
    		PairLogicVarBundle varPair = (PairLogicVarBundle)discreteState;
    		Vector<Iterator<CommLink> > dupLinks = 
    			AgentMsgHelpers.duplicateLinks(channelsToSendOn);
    		m_agentWrapped.getMsgs(varPair.accessFirst(), 
    				NullDynamicsCallback.getInstance(),
    				arrLfStateCont, lfCurrTime, nIdxStateOffset, 
    				dupLinks.elementAt(0));
    		m_bcastAlg.getMsgs(varPair.accessSecond(), 
    				NullDynamicsCallback.getInstance(),
    				arrLfStateCont, lfCurrTime, 
    				nIdxStateOffset, dupLinks.elementAt(1));
    	} catch(java.lang.ClassCastException e) {
    		
    	}
    }

    /**
     * <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> channelsRecieveFrom) {
    	try {
    		PairLogicVarBundle varPair = (PairLogicVarBundle)statePrev;
    		Vector<Iterator<CommLink> > dupLinks = 
    			AgentMsgHelpers.duplicateLinks(channelsRecieveFrom);
    		StateBundle stateCandidate = m_agentWrapped.updateState(
    				varPair.accessFirst(), 
    				arrLfStateCont, lfCurrTime, nIdxStateOffset, env, 
    				dupLinks.elementAt(0));
    		StateBundle stateBcast = m_bcastAlg.updateState(varPair.accessSecond(),
    			arrLfStateCont, lfCurrTime, nIdxStateOffset, env, 
    			dupLinks.elementAt(1));
    		IControlFunc contFunc = stateCandidate.getControlFunc();
    		
    		ILogicVarBundle varCandidate = stateCandidate.getVars();
    		IPosBcast varBcast = (IPosBcast)stateBcast.getVars();
    		
    		PairLogicVarBundle varOut = new PairLogicVarBundle(varCandidate,
    				varBcast);
    		// now we have to generate the matrix
    		
    		int nDim =  varBcast.getDim();
    		int nAgents = varBcast.getNumAgents();
    		
    		if(contFunc.directionConstantQ(lfCurrTime, arrLfStateCont, 
    				null, null, varCandidate, nIdxStateOffset) == false ||
    				varBcast.haveAllInfo() == false) {
    			return new StateBundle(varOut, new ControlFuncDoNothing(nDim));
    		}
    		
   		updateTmpDerivs(arrLfStateCont, contFunc, lfCurrTime, varCandidate, nDim);
    		
    		Vector<Matrix> vecMatBounds = getMatrixBounds(varBcast,lfCurrTime);
    		
    		

    		
    		Matrix matLo = vecMatBounds.elementAt(0);
    		Matrix matHi = vecMatBounds.elementAt(1);
    		
    		EigenvalueDecomposition eigenA = new EigenvalueDecomposition(matLo); 
    		double [] arrLfEigValsA = eigenA.getRealEigenvalues();
    		// Arrays.sort(arrLfEigValsA);
    		
    		this.setLastLambdaLowBound(arrLfEigValsA[1], nIdxStateOffset/nDim);
    		
    		EigenvalueDecomposition eigenB = new EigenvalueDecomposition(matHi); 
    		double [] arrLfEigValsB = eigenB.getRealEigenvalues();
    		
    		double lfLambdaMax = m_lfLambdaMax + 
    			0.0*Math.random();	
    		
    		double lfLambdaMin = Math.max(arrLfEigValsA[1], m_lfLambdaMin);
    		lfLambdaMax = Math.min(arrLfEigValsB[1], lfLambdaMax);
    		
    		Vector<Matrix> vecMatVelBounds = null;
    		
    		double [] arrLfDerivs = makeRotDerivs(0.0, 
					nDim, nIdxStateOffset/nDim);
			
			vecMatVelBounds = 
				getMatrixVelBounds(varBcast, lfCurrTime, arrLfDerivs);
    		
    		if(lfLambdaMin > lfLambdaMax) {
    			setLastMatBounds(vecMatVelBounds, vecMatBounds, lfCurrTime,
						nIdxStateOffset/nDim);
    			return new StateBundle(varOut,contFunc.makeCopy());
    		}

    		
    		
    		int nMinM = Arrays.binarySearch(arrLfEigValsA, lfLambdaMax);
    
    		if(nMinM < 0) {
				nMinM = -nMinM-1; // java binary search has strange return
				// values if it doesn't find the key.
			} else {
				nMinM = nMinM + 1;
				// and gives a return value one off of what I want if it does find the 
				// key
			}

			if(nMinM < 1) {
				nMinM = 1;
			}
			
    		double lfProjectThetaStep = m_lfProjectThetaStep;
    		if(lfProjectThetaStep <= 0.0) {
    			lfProjectThetaStep = 0.1*Math.PI;
    		}
    		
    		double lfProjMax = m_lfProjectThetaMax;
    		if(m_bProject == false) {
    			lfProjMax = 0.0;
    		}
    		
    		double lfMaxMaxCheck = -20.0; // for debugging
    		
    		for(double lfTheta = 0.0; lfTheta <= lfProjMax; 
    		lfTheta += lfProjectThetaStep ) {

    			for(int nSign = -1; nSign <= 1; nSign += 2) {
    				
    				arrLfDerivs = this.makeRotDerivs(nSign*lfTheta, 
    						nDim, nIdxStateOffset/nDim);
    				
    				vecMatVelBounds = 
    					getMatrixVelBounds(varBcast, lfCurrTime, arrLfDerivs);

    				setLastMatBounds(vecMatVelBounds, vecMatBounds, lfCurrTime,
    						nIdxStateOffset/nDim);
    				
    				Matrix matVelLo = vecMatVelBounds.elementAt(0);
    				Matrix matVelHi = vecMatVelBounds.elementAt(1);

    				Matrix matVelDiff = matVelHi.minus(matVelLo);
    				double lfBallRad = 0.5*matVelDiff.normF(); // 
    				Matrix matVelCen = matVelHi.plus(matVelLo);
    				matVelCen.timesEquals(0.5);

    				lfBallRad = 0.0;
    				matVelCen = matVelLo;

    				

    				Matrix matEigVecs = eigenA.getV();
    				// double lfTestNorm = matEigVecs.norm2(); // equals 1.0, correct

    				double lfMaxSCheck = -1.0; 

    				EigenvalueDecomposition matEigsOfVel = 
    					new EigenvalueDecomposition(matVelCen);
    				double [] arrLfEigsOfVel = matEigsOfVel.getRealEigenvalues();

    				double lfVelLambdaMin = arrLfEigsOfVel[0];
    				if(lfVelLambdaMin > 0.0) {
    					lfVelLambdaMin = 0.0;
    				}

    				int nMMin = -1;
    				for(int nCurrM = nMinM; nCurrM <= nAgents; ++nCurrM) {
    					double lfEpsilonA = 0.0;

    					if(nCurrM < nAgents) {
    						lfEpsilonA = ((lfLambdaMax - arrLfEigValsA[1])/
    							(arrLfEigValsA[nCurrM] - arrLfEigValsA[1]));
    						//lfEpsilonA = (lfLambdaMax - arrLfEigValsA[nCurrM-1])/
    						//		(arrLfEigValsA[nCurrM] - arrLfEigValsA[nCurrM-1]);
    					}

    					int nMaxEig = Math.min(nCurrM, nAgents-1);
    					Matrix Mu = matEigVecs.getMatrix(0, nAgents-1, 1, nMaxEig);

    					Matrix matTest = Mu.transpose().times(matVelCen.times(Mu));

    					MathUtils.makeSymmetric(matTest);

    					EigenvalueDecomposition eigenCandMat = 
    						new EigenvalueDecomposition(matTest);

    					double [] arrLfEigs = eigenCandMat.getRealEigenvalues();

    					double lfSCheckTmp = 0.0;
    					
    					lfSCheckTmp = Math.min((1.0-lfEpsilonA)*arrLfEigs[0] + 
    							lfVelLambdaMin*lfEpsilonA, arrLfEigs[0]
    							);

    					if(lfSCheckTmp == 0.0 && lfBallRad == 0.0 &&
    							(arrLfDerivs[0] != 0.0 || arrLfDerivs[1] != 0.0) ) {
    						System.out.println("Here's the wierd case");
    					}
    					
    					if(lfSCheckTmp > lfMaxSCheck) {
    						lfMaxSCheck = lfSCheckTmp;
    						nMMin = nCurrM;
    					}
    					
    					if(lfSCheckTmp >= lfMaxMaxCheck) {
        					lfMaxMaxCheck = lfSCheckTmp;
        				}

    				}

    				
    				if(lfMaxSCheck - lfBallRad >= m_lfMinLambdaIncrease) {
    					ControlFuncMoveDir contFuncOut = 
    						new ControlFuncMoveDir(arrLfDerivs);
    					// System.out.println("agent "+ 
    					// 		nIdxStateOffset/nDim+ " Validated with factor "+
    					// 		(lfMaxSCheck - lfBallRad)+
    					// 		", which is "+lfMaxSCheck+"-"+lfBallRad+", p.s. direction is "+
    					// 		arrLfDerivs[0]+","+arrLfDerivs[1]);
    					// System.out.println("Theta is "+nSign*lfTheta+", nMMin is "+nMMin);
    					return new StateBundle(varOut,contFuncOut);
    				}
    			}
    		}
    		
    		/* if(nIdxStateOffset == 2) {
    			System.out.println("Max increase is "+lfMaxMaxCheck);
    		} */
    		return new StateBundle(varOut,new ControlFuncDoNothing(nDim));
    		
    	} catch(java.lang.ClassCastException e) {
    		
    	}
    	return null; // temp
    }
    
  
    double [] makeRotDerivs(double lfTheta, int nDim, int nOffset) {
    	double lfSinTheta = Math.sin(lfTheta);
    	double lfCosTheta = Math.cos(lfTheta);
    	if(m_arrLfRotDerivs == null || m_arrLfRotDerivs.length != nDim) {
    		m_arrLfRotDerivs = new double[nDim];
    	}
    	m_arrLfRotDerivs[0] = lfCosTheta * m_arrLfTmpDerivs[nOffset*nDim] -
    		lfSinTheta * m_arrLfTmpDerivs[nOffset*nDim+1];
    	m_arrLfRotDerivs[1] = lfCosTheta * m_arrLfTmpDerivs[nOffset*nDim+1] +
    		lfSinTheta * m_arrLfTmpDerivs[nOffset*nDim];
	
    	return m_arrLfRotDerivs;
    }
    
    
    
    void updateTmpDerivs(double arrLfStateCont[], IControlFunc contFunc,
    		double lfCurrTime, ILogicVarBundle logicVars, int nDim) {
    	if(arrLfStateCont == null) {
    		return;
    	}
    	if(m_arrLfTmpDerivs == null || m_arrLfTmpDerivs.length != arrLfStateCont.length) {
    		m_arrLfTmpDerivs = new double[arrLfStateCont.length];
    	}
    	
    	contFunc.getDerivs(lfCurrTime, arrLfStateCont, null, null, logicVars, m_arrLfTmpDerivs, 
    			logicVars.getId()*nDim);
    }
    
    Vector<Matrix> getMatrixVelBounds(IPosBcast bcastPos, double lfCurrTime,
    		double [] arrLfMotion) {
    	int nAgents = bcastPos.getNumAgents();

    	int nDim = bcastPos.getDim();

    	Vector<Matrix> vecResult = new Vector<Matrix>(2);

    	Matrix matLo = new Matrix(nAgents, nAgents);
    	Matrix matHi = new Matrix(nAgents, nAgents);



    	int nId = bcastPos.getId();

    	double [] lfPosI = bcastPos.getPos(nId);
    	double lfRadBallI = m_lfVel*(m_lfTCom+lfCurrTime - bcastPos.getLastTime(nId));
    	// should be zero???
    	for(int j = 0; j < nAgents ; ++j) {

    		if(j == nId) {
    			continue;
    		}
    		double [] lfPosJ = bcastPos.getPos(j);
    		double lfRadBallJ = m_lfVel*(m_lfTCom+ lfCurrTime - bcastPos.getLastTime(j));
    		double lfDist = 0.0;
    		for(int iDim = 0; iDim < bcastPos.getDim(); ++iDim) {
    			double lfDistDim = lfPosJ[iDim] - lfPosI[iDim];
    			lfDist = lfDist + lfDistDim*lfDistDim;
    		}
    		lfDist = Math.sqrt(lfDist);
    		double lfMinD = lfDist - lfRadBallI - lfRadBallJ;
    		double lfMaxD = lfDist + lfRadBallI + lfRadBallJ;
    		double lfMaxDeriv = m_commFunc.maxDerivRange(lfMinD, lfMaxD);
    		double lfMinDeriv = m_commFunc.minDerivRange(lfMinD, lfMaxD);
    		double lfMinRelVel = 0.0;
    		double lfMaxRelVel = 0.0;
    		if(lfMinD <= MathUtils.lfEps && lfMaxD >= -MathUtils.lfEps) {
    			lfMaxRelVel = m_lfVel;
    			lfMinRelVel = -m_lfVel;
    		} else {
    			// double lfSinVelReTheta = (lfRadBallI + lfRadBallJ)/lfDist;
    			// I assert here that abs(lfCosVelTheta) <= 1.0
    			// otherwise previous code branch would be taken
    			double lfVelThetaOff = Math.asin((lfRadBallI + lfRadBallJ)/lfDist);

    			double lfParVelComp = 0.0;
    			double lfVelMag = 0.0;
    			int nOffset = 0; // nId*nDim;
    			for(int nAxis = 0; nAxis < nDim; ++nAxis) {
    				// Here we are taking the rate of change of the edge length from i to j
    				lfParVelComp -= arrLfMotion[nOffset+nAxis] * 
    				(lfPosJ[nAxis]-lfPosI[nAxis]);
    				lfVelMag += arrLfMotion[nOffset+nAxis] *
    				arrLfMotion[nOffset+nAxis];
    			}
    			lfVelMag = Math.sqrt(lfVelMag);
    			lfParVelComp = lfParVelComp/lfDist;

    			if(lfVelMag > MathUtils.lfEps) {
    				double lfVelTheta = Math.acos(lfParVelComp/lfVelMag);

    				double lfCos1 = Math.cos(lfVelTheta + lfVelThetaOff);
    				double lfCos2 = Math.cos(lfVelTheta - lfVelThetaOff);

    				if(Math.abs(lfVelTheta) < Math.abs(lfVelThetaOff)) {
    					lfMaxRelVel = 1.0;
    					lfMinRelVel = Math.min(lfCos1, lfCos2);
    				} else if(Math.abs(Math.abs(lfVelTheta) - Math.PI) < Math.abs(lfVelThetaOff)) {
    					lfMinRelVel = -1.0;
    					lfMaxRelVel = Math.max(lfCos1, lfCos2);
    				} else {
    					lfMaxRelVel = Math.max(lfCos1, lfCos2);
    					lfMinRelVel = Math.min(lfCos1, lfCos2);
    				}

    				lfMinRelVel *= lfVelMag;
    				lfMaxRelVel *= lfVelMag;
    			} else {
    				lfMinRelVel = 0.0;
    				lfMaxRelVel = 0.0;
    			}
    		}
    		double lfDerivMax1 = Math.max(lfMinRelVel*lfMinDeriv, lfMaxRelVel*lfMinDeriv);
    		double lfDerivMax2 = Math.max(lfMinRelVel*lfMaxDeriv, lfMaxRelVel*lfMaxDeriv);
    		double lfDerivMin1 = Math.min(lfMinRelVel*lfMinDeriv, lfMaxRelVel*lfMinDeriv);
    		double lfDerivMin2 = Math.min(lfMinRelVel*lfMaxDeriv, lfMaxRelVel*lfMaxDeriv);
    		matHi.set(nId, j, -Math.max(lfDerivMax1, lfDerivMax2));
    		matLo.set(nId, j, -Math.min(lfDerivMin1, lfDerivMin2));
    		matHi.set(j, nId, matHi.get(nId, j));
    		matLo.set(j, nId, matLo.get(nId, j));
    		matHi.set(nId, nId, matHi.get(nId, nId) - matHi.get(nId, j));
    		matLo.set(nId, nId, matLo.get(nId, nId) - matLo.get(nId, j));


    		matHi.set(j, j, matHi.get(j, j) - matHi.get(nId, j));
    		matLo.set(j, j, matLo.get(j, j) - matLo.get(nId, j));
    	}


    	vecResult.add(0, matLo);
    	vecResult.add(1, matHi);
    	return vecResult;
    }
    
    /**
     * temporarily making this public.  De-publicize later
     * @param bcastPos
     * @param lfCurrTime
     * @return
     */
    public Vector<Matrix> getMatrixBounds(IPosBcast bcastPos, double lfCurrTime) {
    	Vector<Matrix> vecResult = new Vector<Matrix>(2);
    	int nAgents = bcastPos.getNumAgents();
    	Matrix matLo = new Matrix(nAgents, nAgents);
    	Matrix matHi = new Matrix(nAgents, nAgents);
    	
    	for(int i = 0; i < nAgents; ++i) {
    		double [] lfPosI = bcastPos.getPos(i);
    		double lfRadBallI = m_lfVel*(m_lfTCom+lfCurrTime - bcastPos.getLastTime(i));
    		for(int j = i+1; j < nAgents ; ++j) {
    			
    			double [] lfPosJ = bcastPos.getPos(j);
    			double lfRadBallJ = m_lfVel*(m_lfTCom+lfCurrTime - bcastPos.getLastTime(j));
    			double lfDist = 0.0;
    			for(int iDim = 0; iDim < bcastPos.getDim(); ++iDim) {
    				double lfDistDim = lfPosJ[iDim] - lfPosI[iDim];
    				lfDist = lfDist + lfDistDim*lfDistDim;
    			}
    			lfDist = Math.sqrt(lfDist);
    			double lfMinD = lfDist - lfRadBallI - lfRadBallJ;
    			double lfMaxD = lfDist + lfRadBallI + lfRadBallJ;
    			double lfMaxStren = m_commFunc.maxStrengthRange(lfMinD, lfMaxD);
    			double lfMinStren = m_commFunc.minStrengthRange(lfMinD, lfMaxD); 
    			matHi.set(i, j, -lfMaxStren);
    			matLo.set(i, j, -lfMinStren);
    			matHi.set(j, i, -lfMaxStren);
    			matLo.set(j, i, -lfMinStren);
    			matHi.set(i, i,matHi.get(i, i)+lfMaxStren);
    			matLo.set(i, i,matLo.get(i, i)+lfMinStren);
    			

    			matHi.set(j, j,matHi.get(j, j)+lfMaxStren);
    			matLo.set(j, j,matLo.get(j, j)+lfMinStren);
    		}
    	}
    	
    	vecResult.add(0, matLo);
    	vecResult.add(1, matHi);
    	return vecResult;
    }

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

}