/*
 *  Brno University of Technology
 *
 *  Copyright (C) 2009-2010 by Brno University of Technology and the contributors
 *
 *  Complete list of developers available at our web site:
 *
 *       http://spl.utko.feec.vutbr.cz
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU Affero General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU Affero General Public License for more details.
 *
 *  You should have received a copy of the GNU Affero General Public License
 *  along with this program.  If not, see http://www.gnu.org/licenses/.
 */
package cz.vutbr.feec.utko.opencv.operators.imageProcessing;

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.cpp.opencv_video.*;

import com.googlecode.javacpp.Pointer;
import com.googlecode.javacv.cpp.opencv_core.CvPoint2D32f;
import com.googlecode.javacv.cpp.opencv_core.CvSize;
import com.googlecode.javacv.cpp.opencv_core.CvTermCriteria;
import com.googlecode.javacv.cpp.opencv_core.IplImage;
import com.rapidminer.operator.OperatorDescription;
import com.sun.jna.ptr.IntByReference;

public class OpticalFlow extends BaseProcessingOperator2 {

	public OpticalFlow(OperatorDescription description) {
		super(description);
	}

	@Override
	public IplImage doWork(IplImage cvImg1, IplImage cvImg2) {
		IplImage out = cvImg2.clone();
		
		int MAX_CORNERS = 500;
		int win_size = 15;
		
		CvPoint2D32f cornersA = new CvPoint2D32f(MAX_CORNERS);
		CvPoint2D32f cornersB = new CvPoint2D32f(MAX_CORNERS);
		
		IplImage mask = null;
		int[] corner_count = new int[1];
		
		CvSize img_sz = cvGetSize(cvImg1);
		
		// Get the features for tracking
		IplImage eig_image = cvCreateImage(img_sz, IPL_DEPTH_32F, 1);
		IplImage tmp_image = cvCreateImage(img_sz, IPL_DEPTH_32F, 1);
		
		// (opencv_core.CvArr, 		opencv_core.CvArr, 		opencv_core.CvArr, 		opencv_core.CvPoint2D32f, 	int[], double, double, opencv_core.CvArr, int, int, double) 
		// (opencv_core.IplImage, 	opencv_core.IplImage, 	opencv_core.IplImage, 	opencv_core.CvPoint2D32f[], int[], double, double, opencv_core.IplImage, int, int, double)
		
		
		cvGoodFeaturesToTrack(cvImg1, eig_image, tmp_image, cornersA,
				corner_count, 0.05, 5.0, mask, 3, 0, 0.04);
		
		cvGoodFeaturesToTrack(cvImg1, eig_image, tmp_image, cornersA,
				corner_count, 0.05, 5.0, mask, 3, 0, 0.04);

		cvFindCornerSubPix(cvImg1, cornersA, corner_count[0],
				cvSize(win_size, win_size), cvSize(-1, -1),
				cvTermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03)
						);

		// Call Lucas Kanade algorithm
		byte[] features_found = new byte[MAX_CORNERS];
		float[] feature_errors = new float[MAX_CORNERS];

		CvSize pyr_sz = cvSize(cvImg1.width() + 8, cvImg2.height() / 3);

		IplImage pyrA = cvCreateImage(pyr_sz, IPL_DEPTH_32F, 1);
		IplImage pyrB = cvCreateImage(pyr_sz, IPL_DEPTH_32F, 1);
		/*
		 (opencv_core.CvArr, 	opencv_core.CvArr, 		opencv_core.CvArr, 		opencv_core.CvArr, 		opencv_core.CvPoint2D32f, opencv_core.CvPoint2D32f, 	int, opencv_core.CvSize, int, byte[], float[], opencv_core.CvTermCriteria, int) 
		 (opencv_core.IplImage, opencv_core.IplImage, 	opencv_core.IplImage, 	opencv_core.IplImage, 	opencv_core.CvPoint2D32f, opencv_core.CvPoint2D32f[], 	int, opencv_core.CvSize, int, byte[], float[], opencv_core.CvTermCriteria, int) 
		 */
		
		cvCalcOpticalFlowPyrLK(cvImg1, cvImg2, pyrA, pyrB, cornersA, cornersB,
				corner_count[0], cvSize(win_size, win_size), 5,
				features_found, feature_errors,
				cvTermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.3), 0);

		// Make an image of the results

		for (int i = 0; i < corner_count[0]; i++) {
			if (features_found[i] == 0 || feature_errors[i] > 550) {
				System.out.println("Error is " + feature_errors[i] + "/n");
				continue;
			}
			System.out.println("Got it/n");
			CvPoint p0 = cvPoint(Math.round(cornersA.position(i).x()),
					Math.round(cornersA.position(i).y()));
			CvPoint p1 = cvPoint(Math.round(cornersB.position(i).x()),
					Math.round(cornersB.position(i).y()));
			cvLine(out, p0, p1, CV_RGB(255, 0, 0)
					, 2, 8, 0);
		}
		
		return out;
	}
	
}
