package geometry;

import java.awt.Color;
import java.awt.Graphics;
import java.util.ArrayList;
import java.util.Iterator;

import Jama.Matrix;

import predictor.LinearPredictor;
import predictor.PointPredictor;

public class PenStroke 
{
	ArrayList<Integer> xPosits;
	ArrayList<Integer> yPosits;
	
	ArrayList<Integer> xRenderPosits;
	ArrayList<Integer> yRenderPosits;
	
	ArrayList<Circle> curvatureCircles;
	ArrayList<Circle> oldCurvatureCircles;
	
	PointPredictor predictor;
	
	private static final int NUM_POINTS_RENDERING_HINTS = 50;
	
	public static final boolean DRAW_LINE_VERTICES = false;
	
	public PenStroke(int startX, int startY)
	{
		xPosits = new ArrayList<Integer>();
		yPosits = new ArrayList<Integer>();
		
		xRenderPosits = new ArrayList<Integer>();
		yRenderPosits = new ArrayList<Integer>();
		
		curvatureCircles = new ArrayList<Circle>();
		oldCurvatureCircles = new ArrayList<Circle>();
		
		xPosits.add(new Integer(startX));
		yPosits.add(new Integer(startY));
		
		xRenderPosits.add(new Integer(startX));
		yRenderPosits.add(new Integer(startY));
	}
	
	public PenStroke(PenStroke s)
	{
		xPosits = new ArrayList<Integer>();
		yPosits = new ArrayList<Integer>();
		
		curvatureCircles = new ArrayList<Circle>();
		oldCurvatureCircles = new ArrayList<Circle>();
		
		Iterator<Integer> xIter = s.getxPosits().iterator();
		Iterator<Integer> yIter = s.getyPosits().iterator();
		
		while(xIter.hasNext())
		{
			xPosits.add(xIter.next());
			yPosits.add(yIter.next());
		}
		
		xIter = s.getxRenderPosits().iterator();
		yIter = s.getyRenderPosits().iterator();
		
		while(xIter.hasNext())
		{
			xRenderPosits.add(xIter.next());
			yRenderPosits.add(yIter.next());
		}
	}
	
	public void addPoint(int x, int y)
	{
		Integer iX = new Integer(x);
		Integer iY = new Integer(y);
		
		int index = 0;
		for(Integer i : xPosits)
		{
			if(i.equals(iX))
			{
				if(yPosits.get(index).equals(iY))
					return;  // Already have the point in the pen stroke...no need to repeat it.
			}
			
			index++;
		}
		
		// If we got this far, we know that the point (iX, iY) is not in this penStroke.
		this.xPosits.add(iX);
		this.yPosits.add(iY);
		
		//Yannick : Do prediction here.
		int numPoints = this.xRenderPosits.size(); //Yannick : Maybe put this after you've added the new render position.
		DoubleVector mostLikely = null;
		if(this.predictor != null)
			mostLikely = this.predictor.getMostLikelyPoint(x, -(double)y);
		else
			mostLikely = new DoubleVector(x,y);
		
		DoubleVector headingDirection = this.getHeadingDirection();
		if(headingDirection != null)
		{
			PointPredictor p = new LinearPredictor(new DoubleVector(headingDirection.getX(),-headingDirection.getY()), new DoubleVector(this.xRenderPosits.get(numPoints -1), -this.yRenderPosits.get(numPoints -1)));
			//  Define the array of values we're going to need.
			double[] xUsed = new double[NUM_POINTS_RENDERING_HINTS];
			double[] yUsed = new double[NUM_POINTS_RENDERING_HINTS];
			
			int j = 0;
			//  Fill up the array with the SIZE last points of the stroke.
			for(int i = this.xPosits.size()-1; i > this.xPosits.size() - 1 - NUM_POINTS_RENDERING_HINTS; i--)
			{
				xUsed[j] = this.xPosits.get(i).intValue();
				yUsed[j] = -(double)this.yPosits.get(i).intValue(); //y in pixel space is -y in typical cartesian plane.
				j++;
			}
			
			double err = p.getError(xUsed, yUsed);
			
			if(err < 5)
				this.predictor = p;
			else
				this.predictor = null;
			//System.out.println("ERROR : " + err);
		}
		
		this.xRenderPosits.add((int)Math.round(mostLikely.getX()));
		this.yRenderPosits.add((int)Math.round(mostLikely.getY()));
		
		if(this.xPosits.size() > NUM_POINTS_RENDERING_HINTS)
		{
			this.addCurvatureCircle(numPoints - 1);
		}
	}
	
	private void addCurvatureCircle(int startIndex)
	{
	//  First we define the three points we will be using for our rendering hint calculation.
		int x1 = this.xPosits.get(startIndex);
		int y1 = -this.yPosits.get(startIndex);
		int x2 = this.xPosits.get(startIndex - NUM_POINTS_RENDERING_HINTS/2);
		int y2 = -this.yPosits.get(startIndex - NUM_POINTS_RENDERING_HINTS/2);
		int x3 = this.xPosits.get(startIndex - NUM_POINTS_RENDERING_HINTS + 1);
		int y3 = -this.yPosits.get(startIndex - NUM_POINTS_RENDERING_HINTS + 1);
		
		// Create the circle that passes through the 3 points.
		Circle initCircle = null;
		try {
			initCircle = new Circle(x1,y1,x2,y2,x3,y3);
		} catch (NotACircleException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
		
		//  Define the array of values we're going to need.
		double[] xUsed = new double[NUM_POINTS_RENDERING_HINTS];
		double[] yUsed = new double[NUM_POINTS_RENDERING_HINTS];
		
		int j = 0;
		//  Fill up the array with the SIZE last points of the stroke.
		for(int k = 0; k < NUM_POINTS_RENDERING_HINTS - 1; k++)
		{
			xUsed[j] = this.xPosits.get(startIndex - k).intValue();
			yUsed[j] = -this.yPosits.get(startIndex - k).intValue(); //y in pixel space is -y in typical cartesian plane.
			j++;
		}
		
		Circle goodCircle = new Circle(xUsed,yUsed,initCircle);
		this.curvatureCircles.add(goodCircle);
		this.oldCurvatureCircles.add(initCircle);
	}
	
	public void appendStroke(PenStroke s)
	{
		Iterator<Integer> xIter = s.getxPosits().iterator();
		Iterator<Integer> yIter = s.getyPosits().iterator();
		
		int nextX, nextY;
		
		while(xIter.hasNext())
		{
			nextX = xIter.next().intValue();
			nextY = yIter.next().intValue();
			
			this.addPoint(nextX, nextY);
		}
	}
	
	public void rotateAboutStartPoint(double rot)
	{
		int[] xPos = new int[this.xPosits.size()];
		int[] yPos = new int[this.yPosits.size()];
		
		Iterator<Integer> xIter = this.xPosits.iterator();
		Iterator<Integer> yIter = this.yPosits.iterator();
		int index = 0;
		
		while(xIter.hasNext())
		{
			xPos[index] = xIter.next().intValue();
			yPos[index] = yIter.next().intValue();
			
			index++;
		} 
		
		for(int i = 0;i < yPos.length; i++)
		{
			yPos[i] *= -1;  // Flip the y coords to match the coordinates in a cartesian plane.
		}
		
		int transX =  xPos[0];
		int transY =  yPos[0];
		
		//  Translate all the points so that the first point finds itself at the origin.
		for(int i = 0; i < xPos.length;i++)
		{
			xPos[i] -= transX;
			yPos[i] -= transY;
		}
		
		//  Perform the rotation.
		int[] rotXPos = new int[xPos.length];
		int[] rotYPos = new int[yPos.length];
		
		for(int i = 0; i < xPos.length; i++)
		{
			rotXPos[i] = (int)Math.round(xPos[i]*Math.cos(rot) - yPos[i]*Math.sin(rot));
			rotYPos[i] = (int)Math.round(yPos[i]*Math.cos(rot) + xPos[i]*Math.sin(rot));
		}
		
		//  Translate back to the first point location.
		for(int i = 0; i < xPos.length;i++)
		{
			rotXPos[i] += transX;
			rotYPos[i] += transY;
		}
		
		//  Flip back the y-coords to get back to pixel coordinates.
		for(int i = 0;i < rotYPos.length; i++)
		{
			rotYPos[i] *= -1;
		}
		
		this.xPosits.clear();
		this.yPosits.clear();
		
		for(int i = 0;i < rotXPos.length; i++)
		{
			this.addPoint(rotXPos[i], rotYPos[i]);
		}
	}
	
	public float length()
	{
		float length = 0.0f;
		
		Iterator<Integer> xIter = this.xPosits.iterator();
		Iterator<Integer> yIter = this.yPosits.iterator();
		
		int curX = xIter.next().intValue();
		int curY = yIter.next().intValue();
		int nextX, nextY;
		
		while(xIter.hasNext())
		{
			nextX = xIter.next().intValue();
			nextY = yIter.next().intValue();
			
			length += Math.sqrt((nextX - curX)*(nextX - curX) +  (nextY - curY)*(nextY - curY));
			
			curX = nextX;
			curY = nextY;
		}
		
		return length;
	}
	
	public void calculateCurvatureCircles()
	{
		for(int i = NUM_POINTS_RENDERING_HINTS ; i < this.xPosits.size() ; i++)
		{
			//  First we define the three points we will be using for our rendering hint calculation.
			int x1 = this.xPosits.get(i - 1);
			int y1 = -this.yPosits.get(i - 1);
			int x2 = this.xPosits.get(i - NUM_POINTS_RENDERING_HINTS/2);
			int y2 = -this.yPosits.get(i - NUM_POINTS_RENDERING_HINTS/2);
			int x3 = this.xPosits.get(i - NUM_POINTS_RENDERING_HINTS);
			int y3 = -this.yPosits.get(i - NUM_POINTS_RENDERING_HINTS);
			
			// Create the circle that passes through the 3 points.
			Circle initCircle = null;
			try {
				initCircle = new Circle(x1,y1,x2,y2,x3,y3);
			} catch (NotACircleException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
			
			//  Define the array of values we're going to need.
			double[] xUsed = new double[NUM_POINTS_RENDERING_HINTS];
			double[] yUsed = new double[NUM_POINTS_RENDERING_HINTS];
			
			int j = 0;
			//  Fill up the array with the SIZE last points of the stroke.
			for(int k = 0; k < NUM_POINTS_RENDERING_HINTS; k++)
			{
				xUsed[j] = this.xPosits.get(i - k).intValue();
				yUsed[j] = -this.yPosits.get(i - k).intValue(); //y in pixel space is -y in typical cartesian plane.
				j++;
			}
				
			Circle goodCircle = new Circle(xUsed,yUsed,initCircle);
			this.curvatureCircles.add(goodCircle);
			this.oldCurvatureCircles.add(initCircle);
		}
	}
	
	public PenStroke getSubStroke(int firstPoint, int numPoints)
	{
		PenStroke result = null;
		
		for(int i = firstPoint; i < firstPoint + numPoints; i++)
		{
			if(result == null)
				result = new PenStroke(this.xPosits.get(i), this.yPosits.get(i));
			else
				result.addPoint(this.xPosits.get(i), this.yPosits.get(i));
		}
		
		return result;
	}
	
	public PenStroke getCurvatureHint(float length)
	{
		int SIZE = 6;
		if(this.xPosits.size() >= SIZE)
		{
			PenStroke subStroke = this.getSubStroke(this.xPosits.size() - SIZE, SIZE);
			float sLength = subStroke.length();
			
			PenStroke fullStroke = null;
			float fullLength = 0;
			
			while(fullLength + sLength < length)
			{
				if(fullStroke == null)
					fullStroke = new PenStroke(subStroke);
				else
					fullStroke.appendStroke(subStroke);
				
				fullLength += sLength;
			}
			
			ArrayList<Integer> fullStrokeX = fullStroke.getxPosits();
			ArrayList<Integer> fullStrokeY = fullStroke.getyPosits();
			
			int extX = fullStrokeX.get(1);
			int extY = fullStrokeY.get(1);
			
			int oriX = fullStrokeX.get(0);
			int oriY = fullStrokeY.get(0);
			
			DoubleVector headingDir = this.getHeadingDirection();
			DoubleVector fullStrokeDir = new DoubleVector(extX - oriX, extY - oriY);
			fullStrokeDir.unit();
			
			double rot = headingDir.dot(fullStrokeDir);
			fullStroke.rotateAboutStartPoint(rot);
			
			return fullStroke;
		}
		else
			return null;
	}
	
	public DoubleVector getHeadingDirection()
	{
		//Yannick : THIS NEEDS TO BE WEIGHTED.
		if(this.xPosits.size() >= NUM_POINTS_RENDERING_HINTS)
		{
			//  Define the array of values we're going to need.
			int[] xUsed = new int[NUM_POINTS_RENDERING_HINTS];
			int[] yUsed = new int[NUM_POINTS_RENDERING_HINTS];
			
			int j = 0;
			//  Fill up the array with the SIZE last points of the stroke.
			for(int i = this.xPosits.size()-1; i > this.xPosits.size() - 1 - NUM_POINTS_RENDERING_HINTS; i--)
			{
				xUsed[j] = this.xPosits.get(i).intValue();
				yUsed[j] = -this.yPosits.get(i).intValue(); //y in pixel space is -y in typical cartesian plane.
				j++;
			}
			
			double A00,A01;
			double A10,A11;
			
			double B00;
			double B10;
			
			double length = 0,weight = 0;
			
			A00 = A10 = A01 = A11 = B00 = B10 = 0;
			
			for(int i = 0; i < xUsed.length; i++)
			{
				//  First calculate the length from point i to the first point.
				if(i == 0)
					length = 0;
				else
				{
					DoubleVector v2 = new DoubleVector((xUsed[i] - xUsed[i-1]), (yUsed[i] - yUsed[i-1]));
					length = length + v2.length();
				}
				
				//  This gives us the error weight for this point.
				weight = 1/(1+length*length);
				
				//  Update the coefficients accordingly.
				A00 = A00 + weight*xUsed[i]*xUsed[i];
				A01 = A01 + weight*xUsed[i];
				A10 = A10 + weight*xUsed[i];
				A11 = A11 + weight;
				
				B00 = B00 + weight*xUsed[i]*yUsed[i];
				B10 = B10 + weight*yUsed[i];
			}
			
			double[][] aCoeffs = {{A00,A01},
								  {A10,A11}};
			
			double[][] bCoeffs = {{B00},
								  {B10}};
			
			Matrix A = new Matrix(aCoeffs);
			Matrix B = new Matrix(bCoeffs);
			
			//  Calculate the average x and y position.
			float avgX = 0.0f;
			float avgY = 0.0f;
			
			for(int i = 0; i < NUM_POINTS_RENDERING_HINTS; i++)
			{
				avgX += xUsed[i];
				avgY += yUsed[i];
			}
			
			avgX /= NUM_POINTS_RENDERING_HINTS;
			avgY /= NUM_POINTS_RENDERING_HINTS;
			
			int y1 = this.yPosits.get(this.yPosits.size() - 2);
			int x2 = this.xPosits.get(this.xPosits.size() - 1);
			int y2 = this.yPosits.get(this.yPosits.size() - 1);
			
			if(Math.abs(A.det()) > 0.01)
			{
				// A is not singular, so we can solve the system.
				Matrix X = A.solve(B);
				
				// Line was not vertical, so can calculate a slope which minimises
				// least-squared error.
				double slope = X.get(0, 0);
				slope *= -1;  // Multiply the slope by -1 so that the slope now represents
							  // the slope in pixel space. (Again y in pixel space = -y in cartesian plane).
				
				DoubleVector dir;
				if(x2 > avgX)
					dir = new DoubleVector(1.0f, slope);  // Moving to the right
				else
					dir = new DoubleVector(-1.0f, -slope); // Moving to the left
					
				// Make the direction unit length.
				dir.unit();
			
				return dir;
			}
			else
			{
				//  Vertical line case.
				if(y2 < y1)
					return new DoubleVector(0,-1); //Moving upwards
				else
					return new DoubleVector(0,1); //Moving downwards
			}
		}
		else
			return null;
	}
	
	public void renderStroke(Graphics g, Color c, boolean renderHints)
	{	
		Iterator<Integer> xIter = this.xRenderPosits.iterator();
		Iterator<Integer> yIter = this.yRenderPosits.iterator();
		
		int curX = xIter.next().intValue();
		int curY = yIter.next().intValue();
		int nextX, nextY;
		
		int radius = 4;
		
		if(DRAW_LINE_VERTICES)
		{
			g.setColor(Color.RED);
			g.fillOval(curX - radius, curY - radius, 2*radius, 2*radius);
		}
		
		while(xIter.hasNext())
		{
			nextX = xIter.next().intValue();
			nextY = yIter.next().intValue();
			
			g.setColor(c);
			g.drawLine(curX, curY, nextX, nextY);
			
			if(DRAW_LINE_VERTICES)
			{
				g.setColor(Color.RED);
				g.fillOval(curX - radius, curY - radius, 2*radius, 2*radius);
			}
			
			curX = nextX;
			curY = nextY;
		}
		
//		if(this.xPosits.size() > NUM_POINTS_RENDERING_HINTS + 10)
//		{
//			for(int i = 0; i < 10; i++)
//			{
//				int x1 = this.xPosits.get(NUM_POINTS_RENDERING_HINTS - 1 - i);
//				int y1 = -this.yPosits.get(NUM_POINTS_RENDERING_HINTS - 1 - i);
//				g.setColor(Color.RED);
//				int rad = 4;
//				g.fillOval(x1 - radius, -y1 - rad, 2*rad, 2*rad);
//				
//				if(curvatureCircles.size() > i + 1)
//					this.curvatureCircles.get(i).renderCircle(g, Color.ORANGE);
//				
//				if(oldCurvatureCircles.size() > i + 1)
//					this.oldCurvatureCircles.get(i).renderCircle(g, Color.CYAN);
//			}
//		}
		
//		if(this.xPosits.size() > NUM_POINTS_RENDERING_HINTS)
//		{
//			
//		}
		
		if(this.predictor != null)
			this.predictor.render(g, Color.RED);
		
//		if(this.curvatureCircles.size() > 1)
//			this.curvatureCircles.get(this.curvatureCircles.size() - 1).renderCircle(g, Color.ORANGE);
//		
//		if(this.oldCurvatureCircles.size() > 1)
//			this.oldCurvatureCircles.get(this.oldCurvatureCircles.size() - 1).renderCircle(g, Color.CYAN);
		
		int numPoints = xPosits.size();
		if(numPoints > NUM_POINTS_RENDERING_HINTS && renderHints)
		{
			renderDrawingHints(g,NUM_POINTS_RENDERING_HINTS);
		}
	}

	private void renderDrawingHints(Graphics g, int size)
	{
		//  First we define the three points we will be using for our rendering hint calculation.
		int numPoints = xPosits.size();
		int x1 = this.xPosits.get(numPoints - 1);
		int y1 = -this.yPosits.get(numPoints - 1);
		int x2 = this.xPosits.get(numPoints - size/2);
		int y2 = -this.yPosits.get(numPoints - size/2);
		int x3 = this.xPosits.get(numPoints - size);
		int y3 = -this.yPosits.get(numPoints - size);
		
		//  Draw the 3 points used for the curvature calculation.
		g.setColor(Color.RED);
		int radius = 4;
		g.fillOval(x1 - radius, -y1 - radius, 2*radius, 2*radius);
		g.fillOval(x2 - radius, -y2 - radius, 2*radius, 2*radius);
		g.fillOval(x3 - radius, -y3 - radius, 2*radius, 2*radius);
		
		// Create the circle that passes through the 3 points.
		Circle initCircle = null;
		try {
			initCircle = new Circle(x1,y1,x2,y2,x3,y3);
		} catch (NotACircleException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
		
		double[] xUsed = new double[NUM_POINTS_RENDERING_HINTS];
		double[] yUsed = new double[NUM_POINTS_RENDERING_HINTS];
		
		int j = 0;
		//  Fill up the array with the SIZE last points of the stroke.
		for(int k = 0; k < NUM_POINTS_RENDERING_HINTS; k++)
		{
			xUsed[j] = this.xPosits.get(numPoints - 1 - k).intValue();
			yUsed[j] = -this.yPosits.get(numPoints - 1 - k).intValue(); //y in pixel space is -y in typical cartesian plane.
			j++;
		}
		
		Circle circle = new Circle(xUsed,yUsed,initCircle);
		
		// Get the angular position (in degrees), of point 1 and point 2.
		int startPos = (int)Math.round(circle.anglePosition(x1, y1));
		int prevPos = (int)Math.round(circle.anglePosition(x2, y2));
		// Get the size of the angle to draw a curve of max 100 unit length.
		int arcNeeded = circle.getAngleNeedForArcLength(100);
		
		// Determine, when drawing if we should add arcNeeded or subtract it.
		boolean posDir;
		if(prevPos <= 90 && prevPos >= 0
				&&
		   startPos >=270 && startPos <=360)
		{
			posDir = false;
		}
		else
		{
			if(startPos <= 90 && startPos >= 0
					&&
			   prevPos >=270 && prevPos <=360)
			{
				posDir = true;
			}
			else
			{
				if(prevPos < startPos)
				{
					posDir = true;
				}
				else
					posDir = false;
			}
		}
		
		//  Rotate the obtained circle by 30 degrees.
		double r = circle.getRadius();
		double oldCenterX = circle.getCenterX();
		double oldCenterY = circle.getCenterY();
		
		//g.setColor(Color.ORANGE);
		//g.drawArc((int)(oldCenterX - r), (int)(-oldCenterY - r), (int)(2*r), (int)(2*r), 0, 360);
		
		PenStroke p = new PenStroke(x1,-y1);
		p.addPoint((int)Math.round(oldCenterX), -(int)Math.round(oldCenterY));
		p.rotateAboutStartPoint(Math.PI/6);
		
		double newCenterX = p.getxPosits().get(1);
		double newCenterY = p.getyPosits().get(1);
		
		circle = new Circle(newCenterX,-newCenterY,r);
		
		int cx = (int)circle.getCenterX();
		int cy = -(int)circle.getCenterY();
		int cr = (int)circle.getRadius();
		
		//  Get the new starting position.
		startPos = (int)Math.round(circle.anglePosition(x1, y1));
		
		//  Draw the first drawing hint.
		g.setColor(Color.GREEN);
		if(posDir)
		{
			// Want to move in the positive angle direction.
			g.drawArc(cx - cr, cy - cr, 2*cr, 2*cr, startPos, arcNeeded);
		}
		else
		{
			// Want to move in the negative angle direction.
			g.drawArc(cx - cr, cy - cr, 2*cr, 2*cr, startPos, -arcNeeded);
		}
		
		// Rotate 30 degrees in the other direction (-2 is to compensate for the rotation from before).
		p.rotateAboutStartPoint(-2*Math.PI/6);
		
		newCenterX = p.getxPosits().get(1);
		newCenterY = p.getyPosits().get(1);
		
		circle = new Circle(newCenterX,-newCenterY,r);
		
		cx = (int)circle.getCenterX();
		cy = -(int)circle.getCenterY();
		cr = (int)circle.getRadius();
		
		startPos = (int)Math.round(circle.anglePosition(x1, y1));
		
		// Draw the second rendering hint.
		g.setColor(Color.RED);
		if(posDir)
		{
			// Want to move in the positive angle direction.
			g.drawArc(cx - cr, cy - cr, 2*cr, 2*cr, startPos, arcNeeded);
		}
		else
		{
			// Want to move in the negative angle direction.
			g.drawArc(cx - cr, cy - cr, 2*cr, 2*cr, startPos, -arcNeeded);
		}	
	}
	
	public ArrayList<Integer> getxPosits() {
		return xPosits;
	}

	public ArrayList<Integer> getyPosits() {
		return yPosits;
	}
	
	public ArrayList<Integer> getxRenderPosits() {
		return xRenderPosits;
	}

	public ArrayList<Integer> getyRenderPosits() {
		return yRenderPosits;
	}
}
