package cz.vutbr.feec.utko.opencv.operators.lineOperations.lineOperationsClasses;

import ij.process.ImageProcessor;

import java.awt.Color;
import java.awt.Point;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Random;
import java.util.Vector;


import com.googlecode.javacv.FrameGrabber.Array;


import static com.googlecode.javacv.cpp.opencv_core.*;
import static com.googlecode.javacv.cpp.opencv_imgproc.*;
import static com.googlecode.javacv.cpp.opencv_highgui.*;


public class RansacFitLine {
	private int m_max_iter;
	private int m_min_iter;
	private double m_p;
	private boolean m_bPolynom;
	
	private int m_last_iter;
	
	private InlierParams m_best;
	private Point [] m_testData;
		
	/**
	 * Constructor
	 */
	public RansacFitLine() {
		
		// initialize parameters
		m_p = 0.99;				// the likelihood for better model 
		m_max_iter = 10000;		// maximum iteration
		m_last_iter = 0;
		m_bPolynom = false;
	}
	
	/**
	 * Setup a parameters for Ransac 
	 * @param maxIter	... max iteration
	 * @param p			...	likelihood
	 * @param e
	 */
	public void SetupParameters(int maxIter, int minIter, double p, boolean enablePolynom)
	{
		m_p = p;
		m_max_iter = maxIter;
		m_min_iter = minIter;
		m_bPolynom = enablePolynom;
	}
	
	LineModel ComputeModel(Point [] points)
	{
//		System.out.println(m_bPolynom);
		if(m_bPolynom == false)
		{
			return  LineModel.LineApproximation(points);
		}
		else
		{
			return LineModel.PolynomialApproximation(points);
		}	
	}
	
	/**
	 * Compute line from two points
	 * @param pt1x
	 * @param pt1y
	 * @param pt2x
	 * @param pt2y
	 * @return {q, k} from y = kx+q
	 */
	private double[] CreateLine(double pt1x, double pt1y, double pt2x, double pt2y)
	{
		double tmp = (double)(pt2y-pt1y)/(pt2x-pt1x);
		double res[] = new double[2];

		res[1] = -pt1x*tmp+pt1y;
		res[0] = tmp;
		
		return res;
	}
	
	public Point [] GetLine(IplImage image, InlierParams params)
	{
		int y1 = (int)params.q;
		int y2 = (int)(params.k*image.width()+params.q);
		return new Point[]{ new Point(0, y1), new Point(image.width(), y2)};
	}
	
	public Vector<Point> DrawLine(IplImage image, InlierParams params, double line_size ) {
		Vector<Point> ret = new Vector<Point>();
		
		for (int i = 0; i < image.width(); i++)
		{
			ret.add(new Point(i,  (int)(params.k*i+params.q)));
		}
		
		// y = kx + q
		int y1 = (int)params.q;
		int y2 = (int)(params.k*image.width()+params.q);

		double tmpK = params.k;

		double distMax = line_size / (Math.sin( 2.0*Math.PI-Math.atan(tmpK) ) );
		double distMin = line_size / (Math.sin( Math.PI-Math.atan(tmpK) ) );

		cvLine(image, cvPoint(0, y1), cvPoint(image.width(), y2), CV_RGB(255,0,0), 1, 8, 0);
		
		// low
		double [] kq = CreateLine(distMax, y1, image.width()+distMax, y2);
		
		int y11 = (int) kq[1];
		int y12 = (int) (kq[0]*image.width()+kq[1]);

		cvLine(image, cvPoint(0, y11), cvPoint(image.width(), y12), CV_RGB(0,255,0), 1, 8, 0);
		
		// high
		kq = CreateLine(distMin, y1, image.width()+distMin, y2);
		
		y11 = (int) kq[1];
		y12 = (int) (kq[0]*image.width()+kq[1]);

		cvLine(image, cvPoint(0, y11), cvPoint(image.width(), y12), CV_RGB(0,255,0), 1, 8, 0);
		
		return ret;
	}
	
	public Vector<Point> DrawPolynom(IplImage image, InlierParams params, double line_size ) {
		Vector<Point>  ret = new Vector<Point>();
		
		// y = kx^2 + qx + c
		double y = 0;
		
		for (int i = 0; i < image.width(); i++) {
			y = params.c*i*i+params.q*i+params.k;
			cvCircle(image, cvPoint(i, /*image.height()-*/(int)y), 3, CV_RGB(0,255,0), -1, 8, 0);
			
			//ret.add(new Point(i, (int)y));
		}
		
		
		for (int i = 0; i < image.width(); i+=5) {
			y = params.c*i*i+params.q*i+params.k;
			cvCircle(image, cvPoint(i, /*image.height()-*/(int)(y-line_size/2)), 1, CV_RGB(255,0,0), -1, 8, 0);
			cvCircle(image, cvPoint(i, /*image.height()-*/(int)(y+line_size/2)), 1, CV_RGB(255,0,0), -1, 8, 0);
			
			ret.add(new Point(i, (int)y));
		}
		
		return ret;
	}
	
	/***
	 * Draw line to image
	 * @param ip - image processor for output image
	 * @param params - parameters of a found model
	 * @param line_size - maximal distance from a line
	 */
	public void DrawLine(ImageProcessor ip, InlierParams params, double line_size) {
				
		// y = kx + q
		int y1 = (int)params.q;
		int y2 = (int)(params.k*ip.getWidth()+params.q);

		double tmpK = params.k;

		double distMax = line_size / (Math.sin( 2.0*Math.PI-Math.atan(tmpK) ) );
		double distMin = line_size / (Math.sin( Math.PI-Math.atan(tmpK) ) );

		ip.setColor(Color.red);
		ip.drawLine(0, y1, ip.getWidth(), y2);
		
		// low
		double [] kq = CreateLine(distMax, y1, ip.getWidth()+distMax, y2);
		
		int y11 = (int) kq[1];
		int y12 = (int) (kq[0]*ip.getWidth()+kq[1]);

		ip.setColor(Color.green);
		ip.drawLine(0, y11, ip.getWidth(), y12);
		
		ip.drawLine(0, y11, ip.getWidth(), y12);
		
		// high
		kq = CreateLine(distMin, y1, ip.getWidth()+distMin, y2);
		
		y11 = (int) kq[1];
		y12 = (int) (kq[0]*ip.getWidth()+kq[1]);

		ip.drawLine(0, y11, ip.getWidth(), y12);
		
	}
	
	
	// Estimate model .. get parameters
	public InlierParams EstimateModel(Point [] points, LineModel act_model, double dist) {
	    
	
		
		double sum = 0, distance;
	    InlierParams params = new InlierParams();
	    
	    for(int i=0;i<points.length;i++)
	    {
	    
	    	
	    	distance = act_model.PointToLineDist(points[i], m_bPolynom);
	    	//System.out.println(i + ": "+ distance);
	        sum += distance;
	        if(distance<dist)
	        {
	        	params.act_inliers.add(true);
	        	params.num_inliers++;
	        }
	        else
	        	params.act_inliers.add(false);
	    }

	    
	    params.distance = sum;
	    params.k = act_model.getK();
	    params.q = act_model.getQ();
	    params.c = act_model.getC();
	    return new InlierParams(params);
	}
	
	@SuppressWarnings("rawtypes")
	private class SortedPoints implements Comparable
	{
		private Point m_pts;
		
		public SortedPoints(Point pts) {
			m_pts = new Point(pts);
		}
				
		public Point GetPoint(){return m_pts;}
		
		public int compareTo(Object o) {
			return this.m_pts.x-((SortedPoints)o).GetPoint().x;
		}
	};
	
	private Point [] SplitPoints(Point [] input)
	{
		Point [] pts = new Point[input.length];
		SortedPoints [] sortPts = new SortedPoints[input.length];
		
		for (int i = 0; i < sortPts.length; i++) {
			sortPts[i] = new SortedPoints(input[i]);
		}
		
		Arrays.sort(sortPts);
		
		for (int i = 0; i < sortPts.length; i++) {
			pts[i] = new Point(sortPts[i].GetPoint());
		}
				
		return pts;
	}

	public class Range
	{
		public Range(double a, double b) {
			this.a = a;
			this.b = b;
		}
		
		public double getA() {
			return a;
		}

		public void setA(double a) {
			this.a = a;
		}

		public double getB() {
			return b;
		}

		public void setB(double b) {
			this.b = b;
		}

		private double a,b;
	};
	
	public Range [] GetInliersRange(double stepBorder, double stepCenter)
	{
		Range [] res = new Range[3];
		
		double width = 1.0;
		double centerDiff = (stepCenter/2.0)*width;
		
		// borders
		res[0] = new Range(0.0, stepBorder*width);
		res[2] = new Range(width-stepBorder*width, width);
				
		// center
		res[1] = new Range(width/2.0-centerDiff, width/2.0+centerDiff);
		
		return res;
	}
	
	public Point [] GetPointsFromRange(Point [] points, Range range)
	{
		int a = (int)(range.a*points.length);
		int b = (int)(range.b*points.length);
		
		Point [] res = new Point[b-a];
		
		for (int i = a, j=0; i < b; i++, j++) {
			res[j] = points[i];
		}
		
		return res;
	}
	
	/**
	 * /**
	 * Computation of Ransac algorithm
	 * @param input 	...		data are actual data
	 * @param min_pts	...		min points for choice
	 * @param error		...		min distance from model (recognition for inliers)		
	 * @return			inliers for best model
	 */
	public InlierParams Run(Point [] input, int min_pts, double error) {
		
		m_testData = new Point[input.length];
		System.arraycopy(input, 0, m_testData, 0, input.length);
		
		System.out.println("Starting Ransac algorithm");
		// parameters
		int loop = m_max_iter;
		int cnt = 0;
		m_min_iter = 0;
		
		double p = m_p, e = 1.0, pOutlier;
		boolean iterUpdate = false;
		
		LineModel actModel;
		
		// test of inputs parameters
		if(input.length < min_pts || min_pts < 0) return null;
		
		// the random generated points
		Random g = new Random();
		
		Point [] act_vals = new Point[min_pts];
		InlierParams params = new InlierParams();
		InlierParams best = new InlierParams();
				
		while(((loop > cnt || cnt < m_min_iter) && cnt < m_max_iter) && loop != 0)
		//while(cnt < m_max_iter)
		{
			iterUpdate = false;
			
			// generate new points
			//while(true)
			{
				// to sort a points
				Point [] sortedPts = SplitPoints(input);

				Range [] rangeVals = GetInliersRange(0.99, 0.99);
				
				for(int i=0; i<min_pts; i++)
				{
					Point [] availablePts = GetPointsFromRange(sortedPts, rangeVals[i]);
					//System.out.println(availablePts.length);
					act_vals[i] = availablePts[g.nextInt(availablePts.length)];
					
					System.out.print(act_vals[i] + ", ");
				}
				System.out.println();
			}
			
			// compute actual model
			actModel = ComputeModel(act_vals);
			
			if (actModel == null)
				continue;
			
			// test model
			params = EstimateModel(input, actModel, error);
			
			//System.out.println("dist:" + params.distance);
			
			// test for inlier
			
			//System.out.println("iter");
			
			
			
//			if(best.num_inliers < params.num_inliers && Math.abs(params.c) < 0.0001){
			if(best.RecalcNumInliersForEval() < params.RecalcNumInliersForEval() && Math.abs(params.c) < 0.0001){
			
				//if(best.distance > params.distance)
				{
					// save the best parameters (for now)
					best = new InlierParams(params);
					iterUpdate = true;
				}
            } 
            
			if(iterUpdate)
			{
				pOutlier = 1 - ( (double)best.num_inliers / input.length );
				if( e >= pOutlier) e = pOutlier;
					loop = (int) (Math.log( 1 - p ) / Math.log( 1 - Math.pow( (1 - e) , 3) ));
				System.out.println(pOutlier);
				System.out.println(params.num_inliers);
				System.out.println(loop);
			}
						
			++cnt;
		}

		System.out.println("iters:" + cnt);
		
		m_last_iter = cnt;
		
		System.out.println(best.k + ", " + best.q + ", " + best.c);
		
		m_best = new InlierParams(best);
		
		return new InlierParams(m_best);
	}
	
	int GetLastIterations()
	{
		return m_last_iter;
	}
	
	public Point [] GetInliers()
	{
		Point [] inl = new Point[m_best.num_inliers];
		
		for (int i = 0, j=0; i < m_best.act_inliers.size(); i++) {
			if(m_best.act_inliers.get(i))
				inl[j++] = new Point(m_testData[i].x, m_testData[i].y);
		}
		
		return inl;
	}
	
	public InlierParams ApproximateInliers()
	{
		LineModel actModel = ComputeModel(GetInliers());
		InlierParams params = EstimateModel(m_testData, actModel, 22);
		
		return params;
	}
}
