// Descripción: Programa que calcula el flujo óptico de dos imágenes
// Autor: Francisco Martín Rico fmartin@gsyc.escet.urjc.es


#include "cv.h"
#include "cvaux.h"
#include "highgui.h"

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <math.h>
#include <float.h>
#include <limits.h>
#include <time.h>
#include <ctype.h>

#define MAX_NUMBER_OF_CORNERS 200

int main( int argc, char** argv )
{
  IplImage *imagen_orig1=0, *imagen_orig2=0;
  
  if(argc!=3)
    {
      fprintf(stderr, "flujo: Usa\tflujo imagen1 imagen2\n");
      return -1;
    }

  if( (imagen_orig1 = cvLoadImage(argv[1],1)) == 0 )
    return -1;
  
  if( (imagen_orig2 = cvLoadImage(argv[2],1)) == 0 )
    return -1;
  
  //Imágenes auxiliares necesarias para cvGoodFeaturesToTrack 
  IplImage *eigen = cvCreateImage(cvSize(imagen_orig1->width,imagen_orig1->height), IPL_DEPTH_32F, 1);
  IplImage *temp = cvCreateImage(cvSize(imagen_orig1->width,imagen_orig1->height), IPL_DEPTH_32F, 1);

  //Tanto cvGoodFeaturesToTrack  como cvCalcOpticalFlowLK, sólo actúan sobre imágenes grises, así que convertimos...
  IplImage *imagen1 = cvCreateImage(cvSize(imagen_orig1->width,imagen_orig1->height), IPL_DEPTH_8U, 1);
  IplImage *imagen2 = cvCreateImage(cvSize(imagen_orig2->width,imagen_orig2->height), IPL_DEPTH_8U, 1);
  cvCvtColor(imagen_orig1,imagen1,CV_BGR2GRAY);
  cvCvtColor(imagen_orig2,imagen2,CV_BGR2GRAY);

  //Aquí se guardarán las esquinitas
  CvPoint2D32f corners[MAX_NUMBER_OF_CORNERS]; 
  int cornerCount; 
  cornerCount = MAX_NUMBER_OF_CORNERS; 

  //Obtenemos buenas features (esquinas) para determinar el movimiento
  //el 6º parámetro indica la calidad mínima de la esquina
  //el 7º parámetro indica la mínima distancia entre esquinas. Tiene que ver con la velocidad del cambio entre imágenes
  cvGoodFeaturesToTrack (imagen1, eigen, temp, corners, &cornerCount, 0.05, 10, NULL);

  CvArr *velx = cvCreateMat( imagen1->height, imagen1->width,  CV_32FC1 );
  CvArr *vely = cvCreateMat( imagen1->height, imagen1->width,  CV_32FC1);
  
  //Calculamos el flujo óptico
  cvCalcOpticalFlowLK( imagen1, imagen2, cvSize(3,3),velx, vely);
  

  //Lo pintamos todo en la imagen original
  for(int i=0;i< cornerCount; i++)
    {
      CvPoint pt1 = {(int)corners[i].x-2,(int)corners[i].y-2};
      CvPoint pt2 = {(int)corners[i].x+2,(int)corners[i].y+2};

      cvRectangle(imagen_orig1, pt1, pt2, CV_RGB(0,255,0), 1);

      double comp_x = cvGetReal2D( velx, (int)corners[i].x, (int)corners[i].x);
      double comp_y = cvGetReal2D( vely, (int)corners[i].y, (int)corners[i].y);
      
      CvPoint ptl1 = {(int)corners[i].x,(int)corners[i].y};
      CvPoint ptl2 = {(int)corners[i].x+(int)comp_x,(int)corners[i].y+(int)comp_y};
      
      cvLine(imagen_orig1, ptl1, ptl2, CV_RGB(255, 0, 0));
    };
    
  //Guardamos el resultado
  cvSaveImage("resultado.jpg",imagen_orig1 );
  

  //liberamos todos los datos
  cvReleaseImage(&imagen1);  
  cvReleaseImage(&imagen2);
  cvReleaseImage(&imagen_orig1);  
  cvReleaseImage(&imagen_orig2);
  cvReleaseImage(&eigen);  
  cvReleaseImage(&temp);
  cvReleaseData(velx);
  cvReleaseData(vely);
  
  return 0;
}