import java.util.Iterator;


public class ConstraintParentChildOther implements ITreeMotionConstraint {

	double m_lfRad;
	
	public ConstraintParentChildOther() {
		m_lfRad = 0.0; // we need sensible defaults for all these things
	}
	
	public ConstraintParentChildOther(double lfRad) {
		m_lfRad = 0.5*lfRad;
	}
	
	public ConstraintParentChildOther(ConstraintParentChildOther src) {
		m_lfRad = src.m_lfRad;
	}
	
	public ITreeMotionConstraint makeCopy() {
		return new ConstraintParentChildOther(this);
	}
	
	class ClipDir implements IFuncObj<TreeConstraintState, Boolean> {
		
		double m_lfRadSqrd;
		double m_lfMaxLen;
		double [] m_arrLfRayOrig;
		double [] m_arrLfRayDir;
		double [] m_arrLfGlblState;
		
		public ClipDir(double lfRad, double lfMaxLen,
				double [] arrLfRayOrig, double [] arrLfRayDir,
				double [] arrLfGlblState)
		{
			m_lfRadSqrd = lfRad*lfRad;
			m_lfMaxLen = lfMaxLen;
			m_arrLfRayOrig = arrLfRayOrig;
			m_arrLfRayDir = arrLfRayDir;
			m_arrLfGlblState = arrLfGlblState;
		}
		
		double [] CntrFromState(double [] arrLfAgent,TreeConstraintState
				stateAgent2) {
			int nIdxState = stateAgent2.getId();
			double [] arrLfRslt = 
				MathUtils.arrDblCpyRange(m_arrLfGlblState, 2*nIdxState, 
						2*(nIdxState+1));
			MathUtils.linCombine(0.5, arrLfRslt, 0.5, arrLfAgent, 
					arrLfRslt);
			return arrLfRslt;
		}
		
		public Boolean doFunc(TreeConstraintState msg) {
		
			double [] arrLfCircCen;
			arrLfCircCen = CntrFromState(m_arrLfRayOrig, msg);
			double lfTmp = GeometryHelpers.rayToCircle(m_arrLfRayDir, 
					m_arrLfRayOrig, 
					arrLfCircCen, m_lfRadSqrd);
			if(lfTmp < m_lfMaxLen) {
				m_lfMaxLen = lfTmp;
			}
			return Boolean.FALSE;
		}
		
		public double getMaxLen() { return m_lfMaxLen; }
	}
	
	// override?
	public IControlFunc modifyFunc(IControlFunc funcIn,
			double [] arrLfState, int nIdxOff,
			TreeConstraintState stateParent,
			Iterator<TreeConstraintState> iterChildStates,
			Iterator<TreeConstraintState> iterOtherRoot) {
		
		
		double [] arrLfVectorDir;
		
		double [] arrLfCurrPos = 
			MathUtils.arrDblCpyRange(arrLfState, nIdxOff, 
					nIdxOff + funcIn.getNumStateVars());
		
		double lfMaxLen, lfVel;
		
		// why even bother if it takes so many arguments?
		// stuff is too tied together here.
		//if(!funcIn.directionConstantQ(lfCurrTime, arrLfStateSrc, sensors, env, logicVars, nIdxOffsetState)
		
		double lfMagSqrd = 0.0;
		try {
			ControlFuncMoveTowards funcMoveTo = 
				(ControlFuncMoveTowards)(funcIn);
			double [] arrLfTmp = funcMoveTo.getTowards();
			arrLfVectorDir = new double[arrLfCurrPos.length];
			for(int i = 0; i < arrLfCurrPos.length; ++i) {
				arrLfVectorDir[i] = arrLfTmp[i] - arrLfCurrPos[i];
				lfMagSqrd += (arrLfVectorDir[i] * arrLfVectorDir[i]);
			}
			lfMaxLen = Math.sqrt(lfMagSqrd);
			MathUtils.arrMults(arrLfVectorDir, 1.0/lfMaxLen);
			lfVel = funcMoveTo.getVel();
		} catch(java.lang.ClassCastException e) {
			try {
				ControlFuncMoveDir funcMoveDir = 
					(ControlFuncMoveDir)(funcIn);
				
				arrLfVectorDir = 
					MathUtils.arrDblCpy(funcMoveDir.getVel(),
							funcMoveDir.getNumStateVars());
				lfMagSqrd = 0.0;
				for(int i = 0; i < arrLfVectorDir.length; ++i) {
					lfMagSqrd += arrLfVectorDir[i]*arrLfVectorDir[i];
				}
				lfVel = Math.sqrt(lfMagSqrd);
				MathUtils.arrMults(arrLfVectorDir, 1.0/lfVel);
				lfMaxLen = m_lfRad;
			} catch ( java.lang.ClassCastException e2) {
				return new ControlFuncDoNothing(
						funcIn.getNumStateVars());
			}
		}
	
	
		Iterator<TreeConstraintState> iterNotParent = 
			new AgentMsgHelpers.appendIter<TreeConstraintState>(
					iterChildStates,iterOtherRoot);
	
		Iterator<TreeConstraintState> iterAll = 
			new AgentMsgHelpers.consIter<TreeConstraintState>(
					stateParent,iterNotParent);
		
		ClipDir clipHelper = new ClipDir(m_lfRad, lfMaxLen,
				arrLfCurrPos, arrLfVectorDir, arrLfState);

		
		AgentMsgHelpers.applyFunc(clipHelper, iterAll);
		
		double lfNewDist = clipHelper.getMaxLen();
		
		MathUtils.linCombine(1.0, arrLfCurrPos, lfNewDist, 
				arrLfVectorDir, arrLfVectorDir);
		
		return new ControlFuncMoveTowards(arrLfVectorDir, lfVel);
	}

}