/*
Program: FlujoOptico
Descripción: Calcular Flujo Óptico usando Lucas-Kanade,
Procesando en puntos caracteristicos y usando la estrategia Piramidal
Autor: FJHL
Fecha Inicial: 09/Sept/2015
Fecha Actualización: 09/Sept/2015
*/

#include <opencv2\opencv.hpp>

std::vector<cv::Point2f> CalculaCaracteristicas(cv::Mat image);

int main(void){
	//Variables
	cv::Mat Image;
	cv::Mat I_k0; 
	cv::Mat I_k1;
	cv::Mat I_FO;
	std::vector<cv::Point2f> corners_k0;
	std::vector<cv::Point2f> corners_k1;
	cv::VideoCapture capture;
	std::vector<uchar> status;
	std::vector<float> error;
	

	//Ventanas
	cv::namedWindow("Image_k0", CV_WINDOW_NORMAL);
	cv::namedWindow("Image_k1", CV_WINDOW_NORMAL);
	cv::namedWindow("Image_FO", CV_WINDOW_NORMAL);

	//Leer el video
	capture.open("../../../videos/highway.avi");
	if (!capture.isOpened()){
		printf("\nVideo no encontrado...");
		exit(0);
	}
	//Leer un frame
	capture >> Image;
	//Obtener el tamaño
	int nColumnas = Image.cols;
	int nFilas = Image.rows;

	//Crear memoria para las imagenes k0, k1 y FO
	I_k0.create(nFilas, nColumnas, CV_8UC1);
	I_k1.create(nFilas, nColumnas, CV_8UC1);
	I_FO.create(nFilas, nColumnas, CV_8UC3);//Imagen RGB

	//Convertimos a escala de grises
	cv::cvtColor(Image, I_k0, CV_RGB2GRAY);

	Image.copyTo(I_FO);
	//Para los siguientes frames del video
	while (1){
		capture >> Image;
		char key = cv::waitKey(1);
		if (Image.data == NULL || key == 27){
			break;
		}

		//Convertimos a escala de grises
		cv::cvtColor(Image, I_k1, CV_RGB2GRAY);
		
		//Calcular las caracteristicas de la imagen k0
		corners_k0 = CalculaCaracteristicas(I_k0);

		//Calcular Flujo Óptico solo en los puntos característicos de k0
		cv::calcOpticalFlowPyrLK(I_k0, I_k1, corners_k0, corners_k1, status, error, cv::Size(9, 9), 4, 
			                     cv::TermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 30, 0.3), 0, 0.0001);

		for (int i = 0; i < (int)corners_k0.size(); i++){
			if (status[i] == 0 || error[i]>100){
				continue;
			}

			//Dibujar las correspondecnias
			cv::Point p0 = cv::Point(cvRound(corners_k0[i].x), cvRound(corners_k0[i].y));
			cv::Point p1 = cv::Point(cvRound(corners_k1[i].x), cvRound(corners_k1[i].y));
		
			cv::circle(I_FO, p0, 1, CV_RGB(0, 255, 0), -1);
			cv::line(I_FO, p0, p1, CV_RGB(255, 0, 0), 1, 8, 0);
			cv::circle(I_FO, p1, 1, CV_RGB(0, 0, 255), -1);

		}

		//Mostrar imágenes
		cv::imshow("Image_k0", I_k0);
		cv::imshow("Image_k1", I_k1);
		cv::imshow("Image_FO", I_FO);
		
		//Actualización de I_k0
		I_k1.copyTo(I_k0);
		Image.copyTo(I_FO);
	}

	cv::waitKey(0);
	return(0);
}

std::vector<cv::Point2f> CalculaCaracteristicas(cv::Mat image){
	//Parámetros
	double qualityLevel = 0.001;
	double minDistance = 5;
	int blockSize = 5;
	bool useHarrisDetector = false;
	double k = 0.04;
	int MAX_CORNERS = 5000;

	std::vector<cv::Point2f> corners;	

	cv::goodFeaturesToTrack(image, corners, MAX_CORNERS, qualityLevel, minDistance, cv::Mat(), blockSize, useHarrisDetector, k);

	return(corners);
}