package cz.vutbr.feec.utko.opencv.operators.lineOperations.lineOperationsClasses;
/**
 * Implementation of the RANSAC fit line algorithm
 * 
 * Finding the best model of a line from a input set of the points 
 * 
 * Author: Martin Hasmanda
 */

import static com.googlecode.javacv.cpp.opencv_core.CV_64FC1;
import static com.googlecode.javacv.cpp.opencv_core.CV_64FC2;
import static com.googlecode.javacv.cpp.opencv_core.CV_SVD;
import static com.googlecode.javacv.cpp.opencv_core.cvCreateMat;
import static com.googlecode.javacv.cpp.opencv_core.cvReleaseMat;
import static com.googlecode.javacv.cpp.opencv_core.cvSolve;
import static com.googlecode.javacv.cpp.opencv_core.cvSolveCubic;

import java.awt.Point;
import java.util.Vector;

import com.googlecode.javacv.cpp.opencv_core.CvMat;
import com.googlecode.javacv.cpp.opencv_core.CvPoint;
import com.googlecode.javacv.cpp.opencv_core.CvPoint2D64f;
import static com.googlecode.javacv.cpp.opencv_core.cvPoint2D64f;

/**
 * 
 * @author xhasma01
 * Line model implementation:	y = kx + q 
 */
public class LineModel {
	
	// private members of line
	private double k;
	private double q;
	private double c;
		
	//getter/setters
	public double getK() {
		return k;
	}
	public void setK(double k) {
		this.k = k;
	}
	public double getQ() {
		return q;
	}
	public void setQ(double q) {
		this.q = q;
	}

	public double getC()
	{
		return this.c;		
	}
	
	public void setC(double c) {
		this.c = c;
	}
	
	/**
	 * Constructor
	 */
	public LineModel() {
		this.k = 0;
		this.q = 0;
		this.c = 0;
	}
	
	/**
	 * Overload constructor
	 * @param k
	 * @param q
	 */
	public LineModel(double k, double q) {
		this.k = k;
		this.q = q;
	}

	/**
	 * Overload constructor
	 * @param k
	 * @param q
	 * @param c
	 */
	public LineModel(double k, double q, double c) {
		this.k = k;
		this.q = q;
		this.c = c;
	}
	
	
	/**
	 * Copy constructor
	 * @param model
	 */
	public LineModel(LineModel model)
	{
		this(model.getK(), model.getQ());
	}
	
	
	/**
	 * line approximation f(y)=kx+q
	 * @param pts are points for computation the line's model 
	 * @return array of two items [ k, q ]
	 * 
	 */
	public static LineModel LineApproximation(Point [] pts)
	{
		int n = pts.length;
		double sum_xi_yi = 0, sum_xi = 0, sum_yi = 0, sum_x2=0;
		
		for(int i=0;i<n;i++)
		{
			sum_xi_yi += pts[i].x*pts[i].y;
			sum_xi += pts[i].x;
			sum_yi += pts[i].y;
			sum_x2 += pts[i].x*pts[i].x;
		}
		
		double k = (n*sum_xi_yi-sum_xi*sum_yi)/(n*sum_x2-sum_xi*sum_xi);
		double q = (sum_x2*sum_yi-sum_xi*sum_xi_yi)/(n*sum_x2-sum_xi*sum_xi);
		
		return new LineModel(k, q);
	}
	
	/**
	 * Compute distance point to line by parameters k, q (y = kx+q)
	 * @param pt	-	point
	 * @param k		- 	k parameters of line
	 * @param q		-	q parameters of line
	 * @return		-	distance from a line
	 */
	private double dist_to_line(Point pt, double k, double q)
	{
	    double a = k, b = -1, c = q;
	    return (Math.abs(a*pt.x + b*pt.y + c)/ Math.sqrt(a*a+b*b));
	}
	
	public double PointToLineDist(Point pt, boolean polynom) {
		if(polynom == false)
			return dist_to_line(pt, k, q);
		else
		{
			double val[] = {c,q,k};
			return GetPolynomialDistance(val, cvPoint2D64f(pt.x, pt.y));
		}
	}
	
	/////////////////////////////////////////////////////////////////////
	// Polynomial 3rd order
	/**
	 * Return the polynomial curve by order values
	 * 
	 */
	public static LineModel PolynomialApproximation(Point [] pts)
	{
		Vector<Double> coefs = new Vector<Double>();
		int order = 3;

		if(pts.length == 0)
			return null;

	    // fill matrix
	    CvMat mx = cvCreateMat(pts.length, order, CV_64FC1); 
	    CvMat my = cvCreateMat(pts.length, 1, CV_64FC1);
	    CvMat mz = cvCreateMat(order,1,CV_64FC1);
	    
	    int i, j;
    
	    // soustava
	    for(i=0; i<mx.rows(); i++)
	    {
	        // fill rows
	        for(j=0; j<order; j++)
	        {
	        	mx.put(i,j,(Math.pow(pts[i].x, j)));
	        }
	    }

	    // reseni
	    for(i=0; i<my.rows(); i++)
	    {
	        my.put( i, 0, pts[i].y);
	    }

	    // solve equation
	    cvSolve(mx, my, mz, CV_SVD);

	    for(i=0;i<order;i++)
	    	coefs.add(mz.get(i,0));

	    cvReleaseMat(mx);
	    cvReleaseMat(my);
	    cvReleaseMat(mz);
	    
	    return new LineModel(coefs.get(0), coefs.get(1), coefs.get(2));
	}
	
	/**
	 *	 return distance to 3rd order polynomial curve 
	 */
	public double GetPolynomialDistance(double [] polynom, CvPoint2D64f point) {

//		System.out.println(polynom[0] + ", " + polynom[1] + ", " + polynom[2]);
//		System.out.println(point.x() + ", " + point.y());
		double x0, x1, x2, x3;
		double y, root, dist=0.0;
		
	    x3 = 4*polynom[0]*polynom[0];
	    x2 = 6*polynom[0]*polynom[1];
	    x1 = 2*(1+2*polynom[0]*(polynom[2]-point.y())+polynom[1]*polynom[1]);
	    x0 = 2*(polynom[1]*(-point.y()+polynom[2])-point.x());

	    double params[] = {x3, x2, x1, x0};
	    
//	    System.out.println(x3 + ", " + x2 + ", " + x1 + ", " +x0);
	    
	    CvMat Polynom = CvMat.take(4, 1, CV_64FC1);
	    CvMat Roots = CvMat.take(3, 1, CV_64FC1);
		
	    Polynom.put(params);
	    
	    int cnt = cvSolveCubic(Polynom, Roots);
	    
	    double [] vals = Roots.get();

	    root = vals[0];

	    //System.out.println("reseni: " + cnt);
//	    System.out.println("Root: " + root);
	    
	    y = (polynom[0]*root*root+polynom[1]*root+polynom[2]);
	    double val = point.x()*point.x()-2*point.x()*root+root*root+point.y()*point.y()-2*point.y()*y + y*y;
	    if(Math.abs(val) < 0.01)
	    	val = 0.0;

	    //System.out.println("test: " + val);
	    dist = Math.sqrt(val);
	    
	    //System.out.println("Dist: " + dist);
	    
		return dist;
	}
	
}
