import java.util.LinkedList;
import java.util.Random;
import Jama.Matrix;

public class Ransac
{
	private LinkedList<Point[]> couples = new LinkedList<Point[]>();

	public Ransac(LinkedList<Point[]> couples)
	{
		this.couples = couples;
	}

	public LinkedList<Point[]> runRansac(int iter, double maxerror)
	{
		LinkedList<Point[]> newCouples = new LinkedList<Point[]>();
		Matrix model = algorithm(iter, maxerror);
		for(int i = 0; i < couples.size(); i++)
		{
			if(couples.get(i)[1].getDistance() < maxerror)
			{
				newCouples.add(couples.get(i));
			}
		}
		return newCouples;
	}

	private Matrix algorithm(int iter, double maxerror)
	{
		Matrix bestmodel = null;
		double bestscore = 0;
		double error = 0;

		for(int i = 0; i < iter; i++)
		{
			Matrix model = null;
			while(model == null)
			{
				Point[][] samples = heuristicChoice(8, 200);
				System.out.println("Punkt kontrolny " + i);
				model = calculateModel(samples);
			}
			System.out.println("Koniec pętli while");
			double score = 0;
			for(int j = 0; j < couples.size(); j++)
			{
				error = modelError(model, couples.get(j)[0], couples.get(j)[1]);
				couples.get(j)[1].setDistance(error);
				if(error < maxerror)
					score++;
			}
			if(score > bestscore)
			{
				bestscore = score;
				bestmodel = model;
			}
			System.out.println("Koniec iteracji " + i);
		}
		return bestmodel;
	}

	private Point[][] randomChoice()
	{
		Point[][] chosen = new Point[3][2];
		Random rand = new Random();
		int position = 0;
		for(int i = 0; i < 3; i++)
		{
			position = rand.nextInt(couples.size());
			chosen[i][0] = couples.get(position)[0];
			chosen[i][1] = couples.get(position)[1];
		}
		return chosen;
	}

	private Point[][] heuristicChoice(double r, double R)
	{
		System.out.println("Obliczanie heurystyki - start");
		Point[][] chosen = new Point[3][2];
		Random rand = new Random();
		int position = rand.nextInt(couples.size());
		System.out.println("Obliczanie heurystyki - 1");
		chosen[0][0] = couples.get(position)[0];
		chosen[0][1] = couples.get(position)[1];
		System.out.println("Obliczanie heurystyki - 2");
		position = rand.nextInt(couples.size());
		while(!checkPoints(chosen[0][0], chosen[0][1], couples.get(position)[0], couples.get(position)[1], r, R))
		{
			position = rand.nextInt(couples.size());
		}
		chosen[1][0] = couples.get(position)[0];
		chosen[1][1] = couples.get(position)[1];
		System.out.println("Obliczanie heurystyki - 3");
		position = rand.nextInt(couples.size());
		while(!checkPoints(chosen[0][0], chosen[0][1], couples.get(position)[0], couples.get(position)[1], r, R) && checkPoints(chosen[1][0], chosen[1][1], couples.get(position)[0], couples.get(position)[1], r, R))
			position = rand.nextInt(couples.size());
		chosen[2][0] = couples.get(position)[0];
		chosen[2][1] = couples.get(position)[1];
		System.out.println("Obliczanie heurystyki - koniec");
		return chosen;
	}

	private boolean checkPoints(Point p1, Point p2, Point p3, Point p4, double r, double R)
	{
		double condition1 = Math.pow((p1.getX() - p3.getX()), 2) + Math.pow((p1.getY() - p3.getY()), 2);
		double condition2 = Math.pow((p2.getX() - p4.getX()), 2) + Math.pow((p2.getY() - p4.getY()), 2);
		System.out.println("Obliczanie heurystyki - 2a");
		return condition1 > r * r && condition1 < R * R && condition2 > r * r && condition2 < R * R;
	}

	private Matrix calculateModel(Point[][] samples)
	{
		double[][] array = new double[][]
				{
					{samples[0][0].getX(),samples[0][0].getY(),1,0,0,0},
					{samples[1][0].getX(),samples[1][0].getY(),1,0,0,0},
					{samples[2][0].getX(),samples[2][0].getY(),1,0,0,0},
					{0,0,0,samples[0][0].getX(),samples[0][0].getY(),1},
					{0,0,0,samples[1][0].getX(),samples[1][0].getY(),1},
					{0,0,0,samples[2][0].getX(),samples[2][0].getY(),1},
				};
		Matrix matrix1 = new Matrix(array);
		try
		{
			matrix1 = matrix1.inverse();
		}
		catch(Exception e)
		{
			e.printStackTrace();
			return null;
		}
		array = new double[][]
				{
					{samples[0][1].getX()},
					{samples[1][1].getX()},
					{samples[2][1].getX()},
					{samples[0][1].getY()},
					{samples[1][1].getY()},
					{samples[2][1].getY()}
				};
		Matrix matrix2 = new Matrix(array);
		double[][] result = matrix1.times(matrix2).getArray();

		array = new double[][]
				{
					{result[0][0], result[1][0], result[2][0]},
					{result[3][0], result[4][0], result[5][0]},
					{0, 0, 1}
				};

		return new Matrix(array);
	}

	private double modelError(Matrix model, Point p1, Point p2)
	{
		double[][] mod = model.getArray();
		double error = (mod[0][0] * p1.getX() + mod[0][1] * p1.getY() + mod[0][2]
				- p2.getX()) * (mod[0][0] * p1.getX() + mod[0][1] * p1.getY() + mod[0][2] - p2.getX())
				+ (mod [1][0] * p1.getX() + mod[1][1] * p1.getY() + mod[1][2]
				- p2.getY()) * (mod[1][0] * p1.getX() + mod[1][1] * p1.getY() + mod[1][2] - p2.getY());
		return Math.sqrt(error);
	}
}