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



public class ConstraintClosestLegalPoint implements ITreeMotionConstraint {

	double m_lfRad;
	double m_lfTCom;
	
	public ConstraintClosestLegalPoint() {
		m_lfTCom = 0.0;
		m_lfRad = 0.0; // we need sensible defaults for all these things
	}
	
	public ConstraintClosestLegalPoint(double lfRad, double lfTCom) {
		m_lfRad = lfRad;
		m_lfTCom = lfTCom;
	}
	
	public ConstraintClosestLegalPoint(ConstraintClosestLegalPoint src) {
		m_lfRad = src.m_lfRad;
		m_lfTCom = src.m_lfTCom;
	}
	
	public ITreeMotionConstraint makeCopy() {
		return new ConstraintClosestLegalPoint(this);
	}
	
	class AccumCircs implements IFuncObj<TreeConstraintState, Boolean> {
		
		Vector<GeometryHelpers.Circle > m_vecCircs;
		Vector<Vector<Double> > m_vecCandPts;
		
		double [] m_arrLfPt1;
		double [] m_arrLfPt2;
		double [] m_arrLfPt3;
		
		double m_lfRad;
		double m_lfRadSqrd;
		double m_lfMaxLen;
		double [] m_arrLfRayOrig;
		double [] m_arrLfRayDir;
		double [] m_arrLfRayTarget;
		double [] m_arrLfGlblState;
		
		public AccumCircs(double lfRad, double lfTravelRad, 
				double lfMaxLen,
				double [] arrLfRayOrig, double [] arrLfRayDir,
				double [] arrLfRayTarget,
				double [] arrLfGlblState)
		{
			m_arrLfPt1 = new double[2];
			m_arrLfPt2 = new double[2];
			m_arrLfPt3 = new double[2];
			m_arrLfRayTarget = new double[2];
			m_lfRad= lfRad;
			m_lfRadSqrd = lfRad*lfRad;
			m_lfMaxLen = lfMaxLen;
			m_arrLfRayOrig = arrLfRayOrig;
			m_arrLfRayDir = arrLfRayDir;
			m_arrLfGlblState = arrLfGlblState;
			m_arrLfRayTarget = arrLfRayTarget;
			
			m_vecCircs = new Vector<GeometryHelpers.Circle>();
			m_vecCandPts = 
				new Vector<Vector<Double> >();
			GeometryHelpers.Circle circTravelRad = 
				new GeometryHelpers.Circle(m_arrLfRayOrig, lfTravelRad);
			addCirc(circTravelRad);
		}
		
		void pushPtAsVec(double [] arrLfPt) {
			Vector<Double> vecNew = new Vector<Double>(2);
			vecNew.add(arrLfPt[0]);
			vecNew.add(arrLfPt[1]);
			m_vecCandPts.add(vecNew);
		}
		
		public void addCirc(GeometryHelpers.Circle circ) {
			Iterator<GeometryHelpers.Circle> circIter = 
				m_vecCircs.iterator();
			while(circIter.hasNext()) {
				GeometryHelpers.Circle isectWith = circIter.next();
				if(circ.isectPts2d(isectWith, m_arrLfPt1, m_arrLfPt2, 
						m_arrLfPt3)) { // pt3 is "scratch"
					pushPtAsVec(m_arrLfPt1);
					pushPtAsVec(m_arrLfPt2);
				}
			}
			circ.closestPt(m_arrLfRayTarget, m_arrLfPt3);
			pushPtAsVec(m_arrLfPt3);
			m_vecCircs.add(circ);
			
		}
		
		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);
			GeometryHelpers.Circle newCirc = 
				new GeometryHelpers.Circle(m_arrLfRayOrig, arrLfCircCen, 
					0.5*m_lfRad);
			addCirc(newCirc);
			return Boolean.FALSE;
		}
		
		double distFromTargSqrd(Vector<Double> vecPt) {
			double lfRslt = 0.0;
			double lfTmp = vecPt.elementAt(0) - m_arrLfRayTarget[0];
			lfRslt += lfTmp*lfTmp;
			lfTmp = vecPt.elementAt(1) - m_arrLfRayTarget[1];
			lfRslt += lfTmp*lfTmp;
			return lfRslt;
		}
		
		public double [] getProjDir() {
			double [] arrLfRslt = 
				MathUtils.arrDblCpy(m_arrLfRayOrig, 2);
			Iterator<Vector<Double>> ptIter =
				this.m_vecCandPts.iterator();
			double lfMinDistSqrd = 4*m_lfRad*m_lfRad;
			boolean bFound = false;
			boolean bSomethingInside = false;
			double lfMinMaxDist = Math.sqrt(lfMinDistSqrd);
			while(ptIter.hasNext()) {
				Vector<Double> vecLfCurrPt = ptIter.next();
				boolean bInside = true;
				Iterator<GeometryHelpers.Circle> circIter = 
					m_vecCircs.iterator();
				double lfMaxDist = lfMinMaxDist;
				while(bInside && circIter.hasNext()) {
					GeometryHelpers.Circle circToCheck= circIter.next();
					bInside = circToCheck.ptInCirc(vecLfCurrPt);
					if(!bInside) {
						// not really max dist anymore, is it?
						// oops.
						lfMaxDist = circToCheck.dist(vecLfCurrPt);
					}
				}
				if(!bInside) {
					if(!bSomethingInside) {
						if(lfMaxDist < lfMinMaxDist) {
							lfMinMaxDist = lfMaxDist;
						}
					}
					continue;
				}
				lfMinMaxDist = 0.0;
				bSomethingInside = true;
				double lfCurrDistSqrd = distFromTargSqrd(vecLfCurrPt);
				if(lfCurrDistSqrd < lfMinDistSqrd) {
					bFound = true;
					lfMinDistSqrd = lfCurrDistSqrd;
					arrLfRslt[0] = vecLfCurrPt.elementAt(0);
					arrLfRslt[1] = vecLfCurrPt.elementAt(1);
				}
			}
			if(!bFound) {
				if(!bSomethingInside) {
					System.out.println("Not even inside!!! Dist is " +
							(new Double(lfMinMaxDist)).toString());
				}
				System.out.println("Not found");
			} 
			return arrLfRslt;
		}
	}
	
	// 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 [] arrLfTargetPt;
		
		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]);
			}
			arrLfTargetPt = arrLfTmp;
			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);
				arrLfTargetPt = new double[2];
				lfMaxLen = m_lfRad;
				MathUtils.linCombine(1.0, arrLfCurrPos, 
						m_lfTCom*lfVel, arrLfVectorDir, arrLfTargetPt);
			} 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);
		
		AccumCircs projectHelper = new AccumCircs(m_lfRad, 
				lfVel*m_lfTCom, lfMaxLen,
				arrLfCurrPos, arrLfVectorDir, arrLfTargetPt,
				arrLfState);

		
		AgentMsgHelpers.applyFunc(projectHelper, iterAll);
		
		arrLfVectorDir = projectHelper.getProjDir();
		
		return new ControlFuncMoveTowards(arrLfVectorDir, lfVel);
	}

}