package main;

import pointstream.LinearPointStream;
import Jama.Matrix;
import geometry.DoublePoint;
import geometry.DoubleVector;

public class MathHelper
{
	public static double[] quadraticSolve(double a, double b, double c)
	{
		double disc = b*b - 4*a*c;
	    
	    if(disc < 0)
	    {
	        System.out.println("No Solution to : " + a +"x^2 + " + b + "x + " + c + " = 0");
	        return null;
	    }
	    else if(disc < LinearPointStream.DISTANCE_THRESHOLD)
	    {
	    	double[] result = new double[1];
	    	result[0] = -b/(2*a);
	    	
	    	return result;
	    }
	    else
	    {
	    	double[] result = new double[2];
	    	result[0] = (-b + Math.sqrt(disc))/(2*a);
	    	result[1] = (-b - Math.sqrt(disc))/(2*a);
	    	
	    	return result;
	    }
	}
	
	/**
     * Returns the distance of p3 to the segment defined by p1,p2;
     * 
     * @param p1
     *                First point of the segment
     * @param p2
     *                Second point of the segment
     * @param p3
     *                Point to which we want to know the distance of the segment
     *                defined by p1,p2
     * @return The distance of p3 to the segment defined by p1,p2
     */
	public static double segmentDistance(DoublePoint p1, DoublePoint p2, DoublePoint p3)
	{
		double xDelta = p2.getX() - p1.getX();
		double yDelta = p2.getY() - p1.getY();

		if ((xDelta == 0) && (yDelta == 0)) {
		    throw new IllegalArgumentException("p1 and p2 cannot be the same point");
		}

		double u = ((p3.getX() - p1.getX()) * xDelta + (p3.getY() - p1.getY()) * yDelta) / (xDelta * xDelta + yDelta * yDelta);

		DoublePoint closestPoint;
		if (u < 0) {
		    closestPoint = p1;
		} else if (u > 1) {
		    closestPoint = p2;
		} else {
		    closestPoint = new DoublePoint(p1.getX() + u * xDelta, p1.getY() + u * yDelta);
		}

		return closestPoint.distanceTo(p3);
	}
	
	/**
	 * Finds the intersection point between the lines between p1 and p2, and p3 and p4.
	 * @param p1
	 * @param d1
	 * @param p2
	 * @param d2
	 * @return
	 * @throws NoIntersectionException
	 */
	public static DoublePoint lineIntersect(DoublePoint p1, DoublePoint p2, DoublePoint p3, DoublePoint p4) throws NoIntersectionException
	{
		DoubleVector d1 = new DoubleVector(p2.getX() - p1.getX(), p2.getY() - p1.getY());
		DoubleVector d2 = new DoubleVector(p4.getX() - p3.getX(), p4.getY() - p3.getY());
		
		return lineIntersect(p1,d1,p3,d2);
	}
	
	/**
	 * Finds the intersection point between the lines l1 = p1+t*d1 and l2 = p2+s*d2
	 * @param p1
	 * @param d1
	 * @param p2
	 * @param d2
	 * @return
	 * @throws NoIntersectionException
	 */
	public static DoublePoint lineIntersect(DoublePoint p1, DoubleVector d1, DoublePoint p2, DoubleVector d2) throws NoIntersectionException
	{
		double deltaPX = p1.getX() - p2.getX();
		double deltaPY = p1.getY() - p2.getY();
		
		double t;
		DoublePoint intersectionPoint = null;
		
		double[][] pDeltaMatrixVals = {{deltaPX},
								   {deltaPY}};
		
		double[][] dirMatrixVals = {{d2.getX() , -d1.getX()},
					    		{d2.getY() , -d1.getY()}};
										  
		Matrix dirMatrix = new Matrix(dirMatrixVals);
		Matrix pDeltaMatrix = new Matrix(pDeltaMatrixVals);
		
		if(Math.abs(dirMatrix.det()) < 0.0001)
		{
			throw new NoIntersectionException();
		}
		else
		{	
			// Consistent, so return intersection point.
			Matrix paramMatrix = dirMatrix.solve(pDeltaMatrix);
			
			t = paramMatrix.get(1, 0);
			
			double iPX = p1.getX() + t*d1.getX();
			double iPY = p1.getY() + t*d1.getY();
			
			intersectionPoint = new DoublePoint(iPX, iPY);
		}
		
		return intersectionPoint;
	}
}
