package predictor;

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

import geometry.DoubleVector;
import geometry.Line;

public class LinearPredictor extends PointPredictor {

	double m;
	double b;
	
	DoubleVector predictionDirection;  // All this is in cartesian coords.
	DoubleVector predictionIntercept;
	
	public LinearPredictor(DoubleVector headedDir, DoubleVector interceptPoint)
	{
		if(Math.abs(headedDir.length() - 1) > 0.0001)
			System.out.println("Headed dir not unit. Vector : (" + headedDir.getX() + " , " + headedDir.getY() + ") Lenght : " + headedDir.length());
		
		this.m = headedDir.getY()/headedDir.getX();
		this.b = interceptPoint.getY() - m*interceptPoint.getX();
		
		this.predictionDirection = headedDir;
		this.predictionIntercept = interceptPoint;
		
		this.predictionDirection.unit();
	}
	
	public DoubleVector getMostLikelyPoint(double x, double y) 
	{
		//Yannick : THIS NEEDS TO BE FIXED UP !!!
		DoubleVector toPoint = new DoubleVector(x - predictionIntercept.getX(),y - predictionIntercept.getY());
		toPoint.unit();
		
		double angle = this.predictionDirection.dot(toPoint);
		
		if(angle < PointPredictor.PREDICTION_ANGLE && angle > -PointPredictor.PREDICTION_ANGLE)
		{
			//  The predictor can predict where the point should go, so use the predicted value as the proper one.
			double num = toPoint.dot(this.predictionDirection);
			double denom = this.predictionDirection.length();
			denom = denom*denom;
			double coeff = num/denom;
			
			DoubleVector returnPoint = new DoubleVector(this.predictionIntercept.getX(), this.predictionIntercept.getY());
			System.out.println("Return Point : " + returnPoint.getX() + "," + returnPoint.getY());
			DoubleVector dirAddition = new DoubleVector(this.predictionDirection.getX(), this.predictionDirection.getY());
			System.out.println("Return Point : " + returnPoint.getX() + "," + returnPoint.getY());
			dirAddition.multiply(coeff);
			
			returnPoint.add(dirAddition);
			
			return new DoubleVector(returnPoint.getX(), -returnPoint.getY());
		}
		else
		{
			return new DoubleVector(x,-y);
		}
	}

	public double getResidual(double x, double y) 
	{
		return y - (m*x + b);
	}

	public void render(Graphics g, Color c) 
	{
		g.setColor(c);
		
		int curX = (int)Math.round(this.predictionIntercept.getX());
		int curY = (int)Math.round(this.predictionIntercept.getY());
		
		double endDX = this.predictionIntercept.getX() + this.predictionDirection.getX()*100;
		double endDY = this.predictionIntercept.getY() + this.predictionDirection.getY()*100;
		
		int endX = (int)Math.round(endDX);
		int endY = (int)Math.round(endDY);
		
		g.drawLine(curX, -curY, endX, -endY);
	}

}
