package cz.vutbr.feec.utko.opencv.operators.lineOperations.lineOperationsClasses;
/**
 * Implementation of the gradient line boundary method
 * 
 * The functions implements method for a line operations 
 * and finding the maximum gradient on the perpendicular vector
 * 
 * Author: Martin Hasmanda
 */

import java.io.BufferedWriter;
import java.io.FileWriter;
import java.io.IOException;
import java.nio.ByteBuffer;
import java.util.Vector;

import com.googlecode.javacv.cpp.opencv_core.CvMat;
import com.googlecode.javacv.cpp.opencv_core.CvPoint2D64f;
import com.sun.jna.Structure.ByValue;

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


// import static com.googlecode.javacv.jna.cxcore.*;
// import static com.googlecode.javacv.jna.cv.v20or21.*;


public class GradientLineBoundary {

	
	/**
	 * Represent a line by equation y=kx+q
	 * @author xhasma01
	 *
	 */
	public class Line
	{
		double k, q;
		CvPoint ptBegin, ptEnd; 
		
		public Line() {
		}
		
		public Line(double k, double q) {
			this.k = k;
			this.q = q;
		}
	}

	/**
	 * Create line from two points
	 * @param begin
	 * @param end
	 * @return
	 */
	public Line CreateLine(CvPoint begin, CvPoint end) {
		Line line = new Line();
	    double tmp = (double)(end.y()-begin.y())/(end.x()-begin.x());
	    
	    line.k = tmp; line.q =-begin.x()*tmp+begin.y();
	    line.ptBegin = begin; line.ptEnd = end;
	    
	    return line;
	}
	
	
	/**
	 * Return paralel line 
	 * @param imgSize
	 * @param src
	 * @param line_dist (line_size / (sin( 2.0*CV_PI-atan(in.k) ) ))
	 * @return 
	 */
	public Line ParallelByDist(CvSize imgSize, Line src, double line_dist) {
		int y1 = (int)(src.q);
		int y2 = (int)(src.k*imgSize.width()+src.q);
		
		return CreateLine(cvPoint((int)line_dist, y1), cvPoint((int)(imgSize.width()+line_dist), y2));
	}
	
	/**
	 * Return perpendicular line
	 * @param src
	 * @param pt
	 * @return
	 */
	Line PerpendicularLine(Line src, CvPoint pt)
	{
		double k = -1.0/src.k;
	    double q = pt.y()+pt.x()/src.k;    // pty+(-1.0/k1)*ptx
	    
	    return new Line(k, q);
	}
	
	/**
	 * Finding intersect point
	 * @param src1
	 * @param src2
	 * @return
	 */
	CvPoint IntersectLines(Line src1, Line src2)
	{
	    double a = src1.k, b = -1, c = src1.q;
	    double a2 = src2.k, b2 = -1, c2 = src2.q;

	    double ptx = (b*c2 - c * b2) / (a*b2 - a2*b);
	    double pty = -(a*c2 - a2 * c) / (a*b2 - a2*b);
	    
	    return cvPoint((int)ptx, (int)pty);
	}
	
	/**
	 * Sampling a line by sample count
	 * @param src
	 * @param sample
	 * @return
	 */
	Vector<CvPoint> SampleLine(Line src, int sample)
	{
		Vector<CvPoint> pts = new Vector<CvPoint>();
	    // step
	    double stepx = (double)(src.ptEnd.x()-src.ptBegin.x())/sample;
	    double stepy = (double)(src.ptEnd.y()-src.ptBegin.y())/sample;
	    
	    int i;
	    for(i=0;i<=sample;i++)
	    {
	        pts.add(cvPoint((int)(i*stepx+src.ptBegin.x()+0.5), (int)(i*stepy+src.ptBegin.y()+0.5)));
	    }
	    
	    return pts;
	}
	
	/**
	 * Drawing a gradient line
	 * @param sample
	 * @param x
	 * @param y
	 * @param x2
	 * @param y2
	 * @param grad_val
	 * @return
	 */
	CvPoint drawingline(IplImage sample, int x,int y,int x2, int y2, int grad_val) {
		
		
		/*int len = Math.max(x2-x+1, y2-y+1);
		byte[] buffer = new byte[len];
		cvSampleLine(sample, new CvPoint(x,y), new CvPoint(x2, y2), buffer, 8);
		*/
		
		
		int w = x2 - x ;
	    int h = y2 - y ;
	    int dx1 = 0, dy1 = 0, dx2 = 0, dy2 = 0 ;
	    if (w<0) dx1 = -1 ; else if (w>0) dx1 = 1 ;
	    if (h<0) dy1 = -1 ; else if (h>0) dy1 = 1 ;
	    if (w<0) dx2 = -1 ; else if (w>0) dx2 = 1 ;
	    
	    int longest = Math.abs(w) ;
	    int shortest = Math.abs(h) ;
	    if (!(longest>shortest)) {
	        longest = Math.abs(h) ;
	        shortest = Math.abs(w) ;
	        if (h<0) dy2 = -1 ; else if (h>0) dy2 = 1 ;
	        dx2 = 0 ;            
	    }
	    int numerator = longest >> 1 ;
	    int prev = 0;

	    boolean init = true;
	    int res = 0;
	    
	    int step = sample.widthStep();
	    
	    Vector<Integer> hodnoty = new Vector<Integer>();
	    
	    for (int i = 0; i < 10; i++)
	    	hodnoty.add(0);
	    
	    
	    for (int i=0;i<=longest;i++) {

	        // check line
	        if(x<0 || x>sample.height()-1 || y<0 || y>sample.width()-1)
	            continue;

	        ByteBuffer buff = sample.getByteBuffer();

	        if(!init)
	        {
	            // compute gradient
	        	
	            int pr = buff.get(y+step*x);
	            
	            //hodnoty.remove(0);
	            //hodnoty.add(pr);
	            
	            
	          /*  for (int k = 0; k < hodnoty.size()-1; k++) {
					if (hodnoty[i] )
				}
	            */
	          
	            res = pr-prev;
	            
            
	            
	            if(res>grad_val && prev != 0)
	                return cvPoint(y,x);
            
	            prev = pr;
	        }

	        
	        
	        numerator += shortest ;
	        if (!(numerator<longest)) {
	            numerator -= longest ;
	            x += dx1 ;
	            y += dy1 ;
	        } else {
	            x += dx2 ;
	            y += dy2 ;
	        }

	        init = false;
	    }
	    return null;
	}

	/**
	 * Draw Line by DDA algorithm 
	 * @param sample
	 * @param pt1
	 * @param pt2
	 * @param grad_val
	 * @return
	 */
	CvPoint DrawDDALine(IplImage sample, CvPoint pt1, CvPoint pt2, int grad_val)
	{
		
		return drawingline(sample, pt1.y(), pt1.x(), pt2.y(), pt2.x(), grad_val);
	}
	
	public double GetPolynomialDistance(double [] polynom, CvPoint2D64f point) {

		double x0, x1, x2, x3;
		
	    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());

	    CvMat Polynom = CvMat.take(4, 1, CV_64FC1);
	    CvMat Roots = CvMat.take(3, 1, CV_64FC1);
		
	    cvSolveCubic(Polynom, Roots);
	    
		return 0;
	}
	
	
	/**
	 * Return the approximates points 
	 * @param image
	 * @param coefs
	 * @param samples
	 * @param order
	 * @return
	 */
	public Vector<CvPoint> GetApproximatePoints(IplImage image, Vector<Double> coefs, int samples, int order) {
		Vector<CvPoint> res = new Vector<CvPoint>();
		
		if(coefs.size()<order-1)
			return res;
			
		double step = (double)(image.width()-1)/samples;
		double y;
		int xx, yy;
		
		for (double i = 0; i < image.width(); i+=step) {
			y = 0;
			// compute polynom's y
			for (int j = 0; j < coefs.size(); j++) {
				y += coefs.get(j)*Math.pow(i, j);
			}
			
			xx = (int)(i+0.5);
			yy = (int)(y+0.5);
			
			res.add(cvPoint(xx, yy));
 		}
		
		return res;
	}


	/**
	 * Return the polynomial curve by order values
	 * 
	 * 	maximal order is 5, minimal is 2
	 */
	public Vector<Double> GetPolynomialPoints(Vector<CvPoint> pts, int order)
	{
		for (CvPoint cvPoint : pts) {
			System.out.println("px: " + cvPoint.x() + " py: " + cvPoint.y());
		}
		
		
		Vector<Double> coefs = new Vector<Double>();

		if(pts.size() == 0)
			return coefs;
		
	    if(order>6)
	        return coefs;

	    // fill matrix
	    CvMat mx = cvCreateMat(pts.size(), order, CV_64FC1); 
	    CvMat my = cvCreateMat(pts.size(), 1, CV_64FC1);
	    CvMat mz = cvCreateMat(order,1,CV_64FC1);
	    
	    int i, j;

	    System.out.println(pts.size());
	    
	    // soustava
	    for(i=0; i<mx.rows(); i++)
	    {
	        // fill rows
	        for(j=0; j<order; j++)
	        {
	        	mx.put(i,j,(Math.pow(pts.get(i).x(), j)));
	        }
	    }

	    // reseni
	    for(i=0; i<my.rows(); i++)
	    {
	        my.put( i, 0, pts.get(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 coefs;
	}
	
	/**
	 * Draw a points of the polynom 
	 * @param sample
	 * @param coef
	 * @param order
	 * @param color
	 */
	void PrintPolynomPoints(IplImage sample, Vector<Double> coef, int order, CvScalar color, int cislo)
	{
		int prevX=-1, prevY=-1;

        
		
		  
		
	        
	      
	        
		
		for(double i=0; i<sample.width();i+=0.5)
	    {
	        int yy = 0;

	        for(int j=0;j<order;j++)
	        {
	            if(j==0)
	                yy += coef.get(j);
	            else
	                yy += coef.get(j)*Math.pow(i, j);
	        }

	        
			
	        
	        if(yy>=0 && yy<sample.height() && (int)(i)<sample.width())
	        {
	        	ByteBuffer buff = sample.getByteBuffer();
	            
	            int step = sample.widthStep();
	            if(sample.nChannels()>1)
	            {
	            	int ii = (int)(i);
	            	int jj = yy;
	            	
	            	if(prevX>-1)
	            	{
	            		// save only new pts
	            		if(!(prevX == ii && prevY == jj))
	            		{
		            		// save act
		            		buff.put(((ii*sample.nChannels())+step*yy), (byte)color.blue());
			            	buff.put(((ii*sample.nChannels()+1)+step*yy),(byte)color.green());
			            	buff.put(((ii*sample.nChannels()+2)+step*yy), (byte)color.red());
	            		} 
	            	}
	            	
	            	prevX = ii;
	            	prevY = jj;
	            	
	            }
	            else
	            {
	            	buff.put((int)(i+step*yy), (byte)255);
	            }
	        }
	    }
		
		
	}
	
	/////////////////////////////////////////////////////////////////
	// Implementation methods
	/////////////////////////////////////////////////////////////////
	/**
	 * Class specify a line boundary parameters
	 */
	public class LineBoundaryParams
	{
		Line line;							// the origin line 
		Line l2, l3, p2, p3;				// l2, l3 ... rovnobezky, p2, p3 ... kolmice
		Vector<CvPoint> pts, pts2, pts3;	// segmentovane body na kolmici
		public Vector<CvPoint> res2;			// gradienty na kolmici k primce
		public Vector<CvPoint> res3;
		Vector<Double>	coef2, coef3;		// approximovane body funkci
		
		int lineSample;
		double dist_to_line;
		int gradMaxVal;
		int lineOrder;
	};

	/**
	 * Crate line boundary parameters
	 * @param imgSize
	 * @param src
	 * @param dist_to_line
	 * @param lineSample
	 * @return
	 */
	public LineBoundaryParams CreateLineSample(CvSize imgSize, Line src, double dist_to_line, int lineSample) {
		LineBoundaryParams params = new LineBoundaryParams();
		params.lineSample = lineSample;
		params.dist_to_line = dist_to_line;
		params.line = src;
		
		// create parallel lines
		params.l2 = ParallelByDist(imgSize, src, (dist_to_line / (Math.sin( 2.0*Math.PI-Math.atan(src.k) ))));
		params.l3 = ParallelByDist(imgSize, src, (dist_to_line / (Math.sin( Math.PI-Math.atan(src.k) ))));
		
		// perpendicular lines
		params.p2 = PerpendicularLine(src, src.ptBegin);
		params.p3 = PerpendicularLine(src, src.ptEnd);
		
		// intersect points
		CvPoint P1 = IntersectLines(params.l2, params.p2);
		CvPoint P2 = IntersectLines(params.l2, params.p3);
		CvPoint P3 = IntersectLines(params.l3, params.p2);
		CvPoint P4 = IntersectLines(params.l3, params.p3);
		
		// points for perpendicular lines
		params.p2.ptBegin = P1; params.p2.ptEnd = P3;
		params.p3.ptBegin = P2; params.p3.ptEnd = P4;

		
		// point for a parallel lines
		params.l2.ptBegin = P1; params.l2.ptEnd = P2;
		params.l3.ptBegin = P3; params.l3.ptEnd = P4;

		// sampling lines
		params.pts = SampleLine(src, lineSample);
		params.pts2 = SampleLine(params.l2, lineSample);
		params.pts3 = SampleLine(params.l3, lineSample);
				
		return params;
	}
	
	/**
	 * Finding the gradient with a determined maximum value
	 * @param sample
	 * @param params
	 * @param gradMaxVal
	 */
	public void FindGradients(IplImage sample, LineBoundaryParams params, int gradMaxVal)
	{
		params.gradMaxVal = gradMaxVal;
		params.res2 = new Vector<CvPoint>();
		params.res3 = new Vector<CvPoint>();
		
	    for(int i=0;i<params.pts.size();i++)
	    {
	            CvPoint pt = DrawDDALine(sample, params.pts.get(i), params.pts2.get(i), gradMaxVal);
	            if (pt != null)
	            	params.res2.add(pt);
	
	           // pt = DrawDDALine(sample, params.pts.get(i), params.pts3.get(i), gradMaxVal);
	           // params.res3.add(pt);
	    }
	}
	
	/**
	 * Approximate a line by polynomial
	 * @param sample
	 * @param params
	 * @param lineOrder
	 */
	public void ApproximateLine(IplImage sample, LineBoundaryParams params, int lineOrder) {
		params.lineOrder = lineOrder;
		params.coef2 = new Vector<Double>();
		params.coef3 = new Vector<Double>();
		params.coef2= GetPolynomialPoints(params.res2, lineOrder);
		//params.coef3 = GetPolynomialPoints(params.res3, lineOrder);
	}
	
	/**
	 * The simple method for drawing line and their points 
	 * @param sample
	 * @param line
	 * @param color
	 */
	
	private void DrawLine(IplImage sample, Line line, CvScalar color)
	{
		cvLine(sample, line.ptBegin, line.ptEnd, color, 1, CV_AA, 0);
		cvCircle(sample, line.ptBegin, 3, color, 1, 1, 0);
		cvCircle(sample, line.ptEnd, 3, color, 1, 1, 0);
	}
	
	/**
	 * Drawing a debug informations
	 * @param sample
	 * @param params
	 */
	public void PrintDebugInfo(IplImage sample ,LineBoundaryParams params) {
		// text
		CvFont font = new CvFont();
		cvInitFont(font, CV_FONT_HERSHEY_TRIPLEX, 0.4,0.4,0,1,8);
		// line length
		Double distToLine = params.dist_to_line;
		Integer lineSample = params.lineSample;
		Integer gradMaxVal = params.gradMaxVal;
		Integer lineOrder = params.lineOrder;
		
		cvPutText(sample, "Dist. to line", cvPoint(20,20), font, CV_RGB(255,0,0));
		cvPutText(sample, distToLine.toString(), cvPoint(160,20), font, CV_RGB(255,0,0));

		cvPutText(sample, "Line samples", cvPoint(20,40), font, CV_RGB(128,255,255));
		cvPutText(sample, lineSample.toString(), cvPoint(160,40), font, CV_RGB(128,255,255));
		
		cvPutText(sample, "Gradient Boundary", cvPoint(20,60), font, CV_RGB(255,0,255));
		cvPutText(sample, gradMaxVal.toString(), cvPoint(160,60), font, CV_RGB(255,0,255));
		
		cvPutText(sample, "Line Approx. order", cvPoint(20,80), font, CV_RGB(255,255,0));
		cvPutText(sample, lineOrder.toString(), cvPoint(160,80), font, CV_RGB(255,255,0));		
	}
	
	/**
	 * Create the image with a debug informations
	 * @param sample
	 * @param params
	 * @param drawPts
	 * @param drawAppPts
	 * @param drawLines
	 * @param drawApproxLines
	 * @param drawDebugInfo
	 */
	public void CreateDebugImage(
			IplImage sample, LineBoundaryParams params, 
			boolean drawPts, 
			boolean drawAppPts, 
			boolean drawLines, 
			boolean drawApproxLines,
			boolean drawDebugInfo)
	{
		
		if(drawLines)
		{
			DrawLine(sample, params.line, CV_RGB(255, 0, 0));
			DrawLine(sample, params.l2, CV_RGB(0, 255, 0));
			DrawLine(sample, params.l3, CV_RGB(0, 255, 0));
			DrawLine(sample, params.p2, CV_RGB(255, 0, 0));
			DrawLine(sample, params.p3, CV_RGB(255, 0, 0));
		}
		
		if(drawPts)
		{
		    for (CvPoint p : params.pts) {
		    	cvCircle(sample, p, 3, CV_RGB(255,255,255), 1, 1, 0);
			}
		    for (CvPoint p : params.pts2) {
		    	cvCircle(sample, p, 3, CV_RGB(0,255,255), 1, 1, 0);
			}
		    /*
		    for (CvPoint p : params.pts3) {
		    	cvCircle(sample, p, 3, CV_RGB(0,255,255), 1, 1, 0);
			}*/		    
		}
		
		if(drawAppPts)
		{
		    for (CvPoint p : params.res2) {
		    	cvCircle(sample, p, 3, CV_RGB(255,0,255), 1, 1, 0);
			}
	/*	    for (CvPoint p : params.res3) {
		    	cvCircle(sample, p, 3, CV_RGB(255,0,255), 1, 1, 0);
			}*/
		}
	
		if(drawApproxLines)
		{   
		    PrintPolynomPoints(sample, params.coef2, params.lineOrder, CV_RGB(255,255,0),0);
//		    PrintPolynomPoints(sample, params.coef3, params.lineOrder, CV_RGB(255,255,0),1);
		}
		
		if(drawDebugInfo)
			PrintDebugInfo(sample, params);
	}
	
	
	/**
	 * Create image mask from the result line
	 * @param sample only for the copy the image parameters 
	 * @param params parameters of the model
	 * @return the mask image
	 */
	public IplImage CreateMaskLine(IplImage sample, LineBoundaryParams params)
	{
		IplImage res = IplImage.create(cvGetSize(sample), sample.depth(), 3);
		
		// set the black image
		cvSetZero(res);
		
		// draw the white paths
		PrintPolynomPoints(res, params.coef2, params.lineOrder, CV_RGB(255,255,255),0);
	    //PrintPolynomPoints(res, params.coef3, params.lineOrder, CV_RGB(255,255,255),1);
		
	    // draw the white space
	    double x = (params.line.ptEnd.x()-params.line.ptBegin.x())/2;
	    double y = params.line.k*x + params.line.q;
	    
	    
	    if (x >= 0 && y >= 0 && x < res.width() && y < res.height())
	    {
		    CvPoint center = new CvPoint((int)x,(int)y);
		    
		    // Smooth for the smoothing the borders
		    cvSmooth(res, res, CV_GAUSSIAN, 3, 3, 3.0, 3.0);
		    cvFloodFill(res, center, CV_RGB(255,255,255), cvScalarAll(0), cvScalarAll(0), null, 4, null);
		    
		    // for the remove a smoothing edges
		    IplImage resGray = IplImage.create(cvGetSize(sample), sample.depth(), 1);
		    cvCvtColor(res, resGray, CV_BGR2GRAY);
		    cvThreshold(resGray, resGray, 10, 255, CV_THRESH_BINARY);
		    cvCvtColor(resGray, res, CV_GRAY2BGR);
		    cvReleaseImage(resGray);
	    }
		return res;
	}
	
	
	
}

