import java.util.Vector;



/**
 * <p>
 * Grab bag of helper functions for geometric computation
 * </p>
 * @author     Michael Schuresko
 * @version    %I%, %G%
 * @since      1.0
 * @author mds
 *
 */

public class GeometryHelpers {

	double m_lfEps;
	{ m_lfEps = MathUtils.lfEps; }
	

	public static class Circle {
		double [] m_arrLfCenter;
		double m_lfRad;
		
		public Circle() {
			m_arrLfCenter = new double[2];
			m_lfRad = 0.0;
		}
		
		public Circle(int nDim) {	
			m_arrLfCenter = new double[nDim];
			m_lfRad = 0.0;
		}
		
		public Circle(double [] arrLfCen, double lfRad) {
			m_arrLfCenter = MathUtils.arrDblCpy(arrLfCen, 
					arrLfCen.length);
			m_lfRad = lfRad;
		}
		
		public Circle(double [] arrLfToAvg1, double [] arrLfToAvg2, 
				double lfRad) {	
			m_arrLfCenter = new double[arrLfToAvg1.length];
			for(int i = 0; i < arrLfToAvg1.length; ++i) {
				m_arrLfCenter[i] = 0.5*(arrLfToAvg1[i] + arrLfToAvg2[i]);
			}
			m_lfRad = 0.5*lfRad;
		}
		
		public double rad() {
			return m_lfRad;
		}
		
		public double dist(double [] arrLfPt) {
			double lfDist = MathUtils.arrDistSqrd(arrLfPt, m_arrLfCenter);
			return Math.max(0.0, Math.sqrt(lfDist) - m_lfRad);
		}
		
		public double dist(Vector<Double> vecLfPt) {
			double lfDistSqrd=0.0;
			for(int i = 0; i < m_arrLfCenter.length; ++i) {
				double lfTmp = vecLfPt.elementAt(i) - m_arrLfCenter[i];
				lfDistSqrd += lfTmp*lfTmp;
			}
			return Math.max(0.0, Math.sqrt(lfDistSqrd)-m_lfRad);
		}
		
		public void closestPt(double [] arrLfPt, double [] arrLfRslt) {
			if(m_lfRad < MathUtils.lfEps) {
				MathUtils.arrCpy(m_arrLfCenter, arrLfRslt);
				return;
			}
			MathUtils.linCombine(1.0, arrLfPt, -1.0, m_arrLfCenter, arrLfRslt);
			double lfDistSqrd = MathUtils.arrDot(arrLfRslt, arrLfRslt);
			if(lfDistSqrd < m_lfRad*m_lfRad) {
				MathUtils.arrCpy(arrLfPt, arrLfRslt);
				return;
			}
			
			double lfDist = Math.sqrt(lfDistSqrd);
			MathUtils.linCombine(1.0, m_arrLfCenter, m_lfRad/lfDist, 
					arrLfRslt, arrLfRslt);
			return;
		}
		
		public double [] closestPt(double [] arrLfPt) {
			double [] arrLfRslt = new double[arrLfPt.length];
			closestPt(arrLfPt, arrLfRslt);
			return arrLfRslt;
		}
		
		/**
		 * Fails gracelessly if all dimensions are not "2"
		 * @param isectWith circle to intersect with.
		 * @param arrLfPt1 first intersection point
		 * @param arrLfPt2 second intersection point
		 * @param arrLfScratch 2 item scratch array -- can be null
		 * @return true if circles intersect in two distinct points.
		 */
		public boolean isectPts2d(Circle isectWith, double [] arrLfPt1, 
				double [] arrLfPt2, double [] arrLfScratch) {
			if(arrLfScratch == null) {
				arrLfScratch = new double[2];
			}
			MathUtils.linCombine(1.0, isectWith.m_arrLfCenter, 
					-1.0, m_arrLfCenter, arrLfScratch);
			double lfNorm = MathUtils.arrDot(arrLfScratch, arrLfScratch);
			if(lfNorm < MathUtils.lfEps) {
				MathUtils.arrCpy(m_arrLfCenter, arrLfPt2);
				MathUtils.arrCpy(isectWith.m_arrLfCenter, arrLfPt2);
				return false;
			}
			double lfCenDist = Math.sqrt(lfNorm);
			if(lfCenDist + m_lfRad < isectWith.m_lfRad ||
					lfCenDist + isectWith.m_lfRad < m_lfRad) {
				MathUtils.arrCpy(m_arrLfCenter, arrLfPt2);
				MathUtils.arrCpy(isectWith.m_arrLfCenter, arrLfPt2);
				return false;
			}
			lfNorm = 1.0/lfCenDist;
			arrLfScratch[0] *= lfNorm; arrLfScratch[1] *= lfNorm;
			
			double lfDistCen1 = (m_lfRad*m_lfRad - 
					isectWith.m_lfRad*isectWith.m_lfRad + 
					lfCenDist*lfCenDist)/(2.0*lfCenDist);
			double lfHeight = 
				Math.sqrt(m_lfRad*m_lfRad - lfDistCen1*lfDistCen1);
			arrLfPt1[0] = m_arrLfCenter[0] + lfDistCen1*arrLfScratch[0] +
			lfHeight*arrLfScratch[1];
			arrLfPt1[1] = m_arrLfCenter[1] + lfDistCen1*arrLfScratch[1] -
			lfHeight*arrLfScratch[0];
			
			arrLfPt2[0] = m_arrLfCenter[0] + lfDistCen1*arrLfScratch[0] -
			lfHeight*arrLfScratch[1];
			arrLfPt2[1] = m_arrLfCenter[1] + lfDistCen1*arrLfScratch[1] +
			lfHeight*arrLfScratch[0];
			return true;
		}
		
		public double closeness(Vector<Double> vecLfPt) {
			double lfDistSqrd = 0.0; double lfTmp = 0.0;
			for(int i = 0; i < vecLfPt.size(); ++i) {
				lfTmp = vecLfPt.elementAt(i) - m_arrLfCenter[i];
				lfDistSqrd += lfTmp*lfTmp;
			}
			double lfCheck =  lfDistSqrd -
					m_lfRad*m_lfRad - MathUtils.lfEps;
			return lfCheck;
		}

		public boolean ptInCirc(Vector<Double> vecLfPt) {
			double lfDistSqrd = 0.0; double lfTmp = 0.0;
			for(int i = 0; i < vecLfPt.size(); ++i) {
				lfTmp = vecLfPt.elementAt(i) - m_arrLfCenter[i];
				lfDistSqrd += lfTmp*lfTmp;
			}
			double lfCheck =  lfDistSqrd -
					m_lfRad*m_lfRad - MathUtils.lfEps;
			return lfCheck < 0;
		}

		public double closeness(double [] arrLfPt) {
			return MathUtils.arrDistSqrd(arrLfPt, m_arrLfCenter) -
			m_lfRad*m_lfRad - MathUtils.lfEps;
		}
		
		public boolean ptInCirc(double [] arrLfPt) {
			return (MathUtils.arrDistSqrd(arrLfPt, m_arrLfCenter) -
					m_lfRad*m_lfRad - MathUtils.lfEps < 0);
		}
	}

	static void testCirclePrint(double [] arrLf1, double [] arrLf2) {
		System.out.println("Pt1 is (" + 
				(new Double(arrLf1[0])).toString() + "," +
				(new Double(arrLf1[1])).toString() + ")");
		System.out.println("Pt2 is (" + 
				(new Double(arrLf2[0])).toString() + "," +
				(new Double(arrLf2[1])).toString() + ")");
	}
	
	public static void testCircleCode() {
		double [] arrLfCen1 = new double[2];
		double [] arrLfCen2 = new double[2];
		arrLfCen2[0] = 3.0;
		Circle circ1 = new Circle(arrLfCen1, 5.0);
		Circle circ2 = new Circle(arrLfCen2, 4.0);
		
		double [] arrLfPt1 = new double[2];
		double [] arrLfPt2 = new double[2];
		double [] arrLfScratch = new double[2];
		
		circ1.isectPts2d(circ2, arrLfPt1, arrLfPt2, arrLfScratch);
		testCirclePrint(arrLfPt1, arrLfPt2);
		circ2.isectPts2d(circ1, arrLfPt1, arrLfPt2, arrLfScratch);
		testCirclePrint(arrLfPt1, arrLfPt2);
		System.out.println("Closeness is " + 
				(new Double(circ1.closeness(arrLfPt1))).toString() + 
				" or " + 
				(new Double(circ1.closeness(arrLfPt2))).toString() + 
				" alt " + 
				(new Double(circ2.closeness(arrLfPt1))).toString() + 
				" or " + 
				(new Double(circ2.closeness(arrLfPt2))).toString() 
				);
		
		arrLfCen2[0] = 0.0;
		arrLfCen1[1] = -3.0;
		circ1 = new Circle(arrLfCen1, 5.0);
		circ2 = new Circle(arrLfCen2, 4.0);
		
		circ1.isectPts2d(circ2, arrLfPt1, arrLfPt2, arrLfScratch);
		testCirclePrint(arrLfPt1, arrLfPt2);
		circ2.isectPts2d(circ1, arrLfPt1, arrLfPt2, arrLfScratch);
		testCirclePrint(arrLfPt1, arrLfPt2);
		System.out.println("Closeness is " + 
				(new Double(circ1.closeness(arrLfPt1))).toString() + 
				" or " + 
				(new Double(circ1.closeness(arrLfPt2))).toString() + 
				" alt " + 
				(new Double(circ2.closeness(arrLfPt1))).toString() + 
				" or " + 
				(new Double(circ2.closeness(arrLfPt2))).toString() 
				);
		arrLfPt1[0] = 30;
		arrLfPt1[1] = 40;
		System.out.println("Closest pt to 1 is "+ 
				MathUtils.arrToString(circ1.closestPt(arrLfPt1)));
		System.out.println("Closest pt to 2 is "+ 
				MathUtils.arrToString(circ2.closestPt(arrLfPt1)));
		System.out.println("Closeness is " + 
				(new Double(circ1.closeness(circ1.closestPt(arrLfPt1)))).toString() + 
				" or " + 
				(new Double(circ1.closeness(circ1.closestPt(arrLfPt2)))).toString());
	}
	
	/**
	 * 
	 * @param arrLfRayDir direction of ray (not necc. unit length)
	 * @param arrLfRayOrg position of ray origin
	 * @param arrLfCircCen position of circle center
	 * @param lfCircRadSqrd rad^2 of circle
	 * @return distance along ray until it first hits circle.
	 */
	public static double rayToCircle(double [] arrLfRayDir, 
			double [] arrLfRayOrg,
			double [] arrLfCircCen, 
			double lfCircRadSqrd) {
		double [] arrLfScratch = 
				MathUtils.arrDblCpy(arrLfRayOrg, arrLfRayOrg.length); 
		MathUtils.arrSubtrAndStore(arrLfScratch, arrLfCircCen);
		double lfDirMagSqrd = MathUtils.arrDot(arrLfRayDir, arrLfRayDir);
		double lfOrgDotRay = MathUtils.arrDot(arrLfScratch, arrLfRayDir);
		// we have a*b*cosTheta and a^2 and aA and bB we want
		// aA - aBcosTheta
		// which is aA - a*b*cosTheta*bB/b^2
		double lfFactor = lfOrgDotRay/lfDirMagSqrd;
		MathUtils.linCombine(1.0, arrLfScratch, 
				-lfFactor, arrLfRayDir, arrLfScratch);
		double lfPMOff = 0.0; // plus or minus off
		lfPMOff = MathUtils.arrDot(arrLfScratch, arrLfScratch);
		if(lfPMOff > lfCircRadSqrd) {
			System.out.println("Error here");
			lfPMOff = lfCircRadSqrd;
		}
		lfPMOff = Math.sqrt(lfCircRadSqrd - lfPMOff);
		double lfOff = -lfFactor*Math.sqrt(lfDirMagSqrd);
		if(lfOff - lfPMOff < 0.0) {
			return lfOff + lfPMOff;
		} else {
			return lfOff - lfPMOff;
		}
	}
}