package geometry;

import java.awt.Color;
import java.awt.Graphics;

import main.MathHelper;
import main.NoIntersectionException;
import Jama.Matrix;

public class Circle 
{
	private double centerX; // In cartesian coords.
	private double centerY; // In cartesian coords.
	
	private double radius;
	
	public Circle(double x, double y, double r)
	{
		this.centerX = x;
		this.centerY = y;
		this.radius = r;
	}
	
	public Circle(double[] pointsX, double[] pointsY, Circle initGuess)
	{
		//  Based on this initial guess initGuess, fit best possible circle to points.
		boolean passedOnce = false;
		double lastError = 0;
		double curError = this.getError(pointsX, pointsY, initGuess);
		double length = 0,weight;
		
		DoubleVector v1,v2;
		
		double A00,A01,A02;
		double A10,A11,A12;
		double A20,A21,A22;
		
		double B00;
		double B10;
		double B20;
		
		Circle curCircle = initGuess;
		
		while(!passedOnce && Math.abs(curError - lastError) > 0.0001)
		{
			passedOnce = true;
			lastError = curError;
			
			// Reset the matrix coefficients.
			A00 = A01 = A02 = A10 = A11 = A12 = A20 = A21 = A22 = 0;
			B00 = B10 = B20 = 0;
			
			for(int i = 0; i < pointsX.length; i++)
			{
				//  First calculate the length from point i to the first point.
				if(i == 0)
					length = 0;
				else
				{
					v2 = new DoubleVector((pointsX[i] - pointsX[i-1]), (pointsY[i] - pointsY[i-1]));
					length = length + v2.length();
				}
				
				//  This gives us the error weight for this point.
				weight = 1/(1+length*length);
				
				//  Now this is the direction vector from the current center.  We will project on here to estimate our error.
				v1 = new DoubleVector((pointsX[i] - curCircle.getCenterX()), (pointsY[i] - curCircle.getCenterY()));
				v2 = new DoubleVector(pointsX[i], pointsY[i]);
				v1.unit();
				
				//  Update the coefficients accordingly.
				A00 = A00 + weight*v1.getX()           ; A01 = A01 + weight*v1.getY()           ; A02 = A02 + weight;
				A10 = A10 + weight*v1.getX()*v1.getX() ; A11 = A11 + weight*v1.getY()*v1.getX() ; A12 = A12 + weight*v1.getX();
				A20 = A20 + weight*v1.getX()*v1.getY() ; A21 = A21 + weight*v1.getY()*v1.getY() ; A22 = A22 + weight*v1.getY();
				
				B00 = B00 + weight*v1.dot(v2);
				B10 = B10 + weight*v1.getX()*v1.dot(v2);
				B20 = B20 + weight*v1.getY()*v1.dot(v2);
			}
			
			// Create the matrices.
			double[][] Aarray = {{A00,A01,A02},
					 {A10,A11,A12},
		  		     {A20,A21,A22}};

			double[][] Barray = {{B00},
								 {B10},
								 {B20}};

			Matrix A = new Matrix(Aarray);
			Matrix B = new Matrix(Barray);
			
			if(A.det() < 0.0001)
				break;
			
			// Solve the system.
			Matrix X = A.solve(B);
			
			// Update the current circle.
			curCircle = new Circle(X.get(0, 0) , X.get(1, 0) , X.get(2, 0) );
			curError = this.getError(pointsX, pointsY, curCircle);
		}
		
		this.centerX = curCircle.getCenterX();
		this.centerY = curCircle.getCenterY();
		this.radius = curCircle.getRadius();
	}
	
	public Circle(DoublePoint p1, DoublePoint p2, DoubleVector p1Tangent) throws NotACircleException
	{
		DoublePoint midPoint = new DoublePoint((p1.getX() + p2.getX())/2, (p1.getY() + p2.getY())/2);
		
		DoubleVector v1 = new DoubleVector(p1,p2);
    	DoubleVector dir1 = v1.perp();
    	dir1.unit();
    	
    	DoubleVector dir2 = p1Tangent.perp();
    	dir2.unit();
    	
    	DoublePoint circleCenter = new DoublePoint(0,0);
    	
    	try {
			circleCenter = MathHelper.lineIntersect(midPoint, dir1, p1, dir2);
		} catch (NoIntersectionException e) {
			this.centerX = 0;
			this.centerY = 0;
			this.radius = 0;
			throw new NotACircleException();
		}
		
		this.centerX = circleCenter.getX();
		this.centerY = circleCenter.getY();
		this.radius = p1.distanceTo(circleCenter);
	}
	
	public Circle(double x1, double y1, double x2, double y2, double x3, double y3) throws NotACircleException
	{
		//  Assuming the values sent are in cartesian coordinates.
		double sq1 = x1*x1 + y1*y1;
		double sq2 = x2*x2 + y2*y2;
		double sq3 = x3*x3 + y3*y3;
		
		double[][] m11Vals = {{x1,y1,1.0},
							  {x2,y2,1.0},
							  {x3,y3,1.0}};
		
		double[][] m12Vals = {{sq1,y1,1.0},
				  			  {sq2,y2,1.0},
				  			  {sq3,y3,1.0}};
		
		double[][] m13Vals = {{sq1,x1,1.0},
							  {sq2,x2,1.0},
							  {sq3,x3,1.0}};
		
		double[][] m14Vals = {{sq1,x1,y1},
				  			  {sq2,x2,y2},
				  			  {sq3,x3,y3}};
		
		Matrix M11 = new Matrix(m11Vals);
		Matrix M12 = new Matrix(m12Vals);
		Matrix M13 = new Matrix(m13Vals);
		Matrix M14 = new Matrix(m14Vals);
		
		double dM11 = M11.det();
		double dM12 = M12.det();
		double dM13 = M13.det();
		double dM14 = M14.det();
		
		if(Math.abs(dM11) < 0.01)
		{
			this.centerX = 0;
			this.centerY = 0;
			this.radius = 0;
			throw new NotACircleException();
		}
		else
		{
			double x0 = (0.5)*(dM12/dM11);
			double y0 = (-0.5)*(dM13/dM11);
			double r0 = Math.sqrt(x0*x0 + y0*y0 + dM14/dM11);
			
			this.centerX = x0;
			this.centerY = y0;
			
			this.radius = r0;
		}
	}
	
	private double getError(double[] pointsX, double[] pointsY, Circle c)
	{
		DoubleVector v1,v2,v3; 
		double totalError = 0.0;
		double estRad;
		double curError;
		double length = 0;
		double weight;
		
		for(int i = 0; i < pointsX.length ; i++)
		{
			v1 = new DoubleVector((pointsX[i] - c.getCenterX()), (pointsY[i] - c.getCenterY()));
			v2 = new DoubleVector((pointsX[i] - c.getCenterX()), (pointsY[i] - c.getCenterY()));
			
			v1.unit();
			
			estRad = v1.dot(v2);
			
			curError = (estRad - c.getRadius()) * (estRad - c.getRadius());
			
			if(i == 0)
				length = 0;
			else
			{
				v3 = new DoubleVector((pointsX[i] - pointsX[i-1]), (pointsY[i] - pointsY[i-1]));
				length = length + v3.length();
			}
			
			weight = 1/(1+length);
			
			totalError = totalError + weight*curError;
		}
		
		return totalError;
	}
	
	public double circumference()
	{
		return this.radius*2*Math.PI;
	}
	
	public boolean contains(DoublePoint p)
	{
		if(this.getCenter().distanceTo(p) < this.getRadius())
			return true;
		else
			return false;
	}
	
	public DoublePoint pointPosition(double angle)
	{
		// Assumes angle is in degrees. 
		
		double resultAngle = (angle*2*Math.PI)/360;
		
		double resultX = this.getCenterX() + this.getRadius()*Math.cos(resultAngle);
		double resultY = this.getCenterY() + this.getRadius()*Math.sin(resultAngle); 
		
		return new DoublePoint(resultX, resultY);
	}
	
	public double anglePosition(double x, double y)
	{
		//  Expects cartesian coordinates.
		double radAngle = Math.atan2(y - this.centerY, x - this.centerX);
		
		double degAngle = (radAngle*360.0)/(2.0*Math.PI);
		
		return degAngle;
	}
	
	public int getAngleNeedForArcLength(double length)
	{
		double diameter = this.radius*2.0;
		
		double angle = (length*360.0)/(diameter*Math.PI);
		
		int finalAngle = (int)Math.round(angle);
		
		if(finalAngle > 360)
			return 180;
		else
			return finalAngle;
	}
	
	public void renderCircle(Graphics g, Color c)
	{
		int cX = (int)Math.round(this.centerX);
		int cY = -(int)Math.round(this.centerY);
		int rad = (int)Math.round(this.radius);
		
		g.setColor(c);
		g.drawArc(cX - rad, cY - rad, 2*rad, 2*rad, 0, 360);
	}
	
	public DoublePoint getCenter()
	{
		return new DoublePoint(centerX,centerY);
	}
	
	public double getCenterX() {
		return centerX;
	}

	public double getCenterY() {
		return centerY;
	}

	public double getRadius() {
		return radius;
	}
}
