#if defined(__cplusplus)
extern "C" { 
#endif

#include <stdio.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
//Optical Flow routine
#include "camus.h"

//Drawing libraries
#include "../FW/labImage.h"
#include "../FW/labImgDraw.h"

#include "motion_field.h"

//Draw optical flow field?
#define DISPLAY   1

//The optical flow field
vec_field *optical_flow_camus;

//Counts pictures in ring buffer
int patch_size_alt;
int OF_by_camus(Picture *p_input, 
		float par1, float par2, float par3,
		float par4, float par5, float par6,
		Picture *p_output)
{
 
  //Filter dimensions
  int patch_size=5;
  int ready;
  
  ////////////////////////
  //IMPORTANT PARAMETER!
  //Search in S frames back in the past?
  int S=10; 
  ////////////////////////
  
  //Estimate patch size(must be uneven)  
  patch_size_alt=patch_size;
  patch_size=(int)(par1*10);  
  if(patch_size%2!=1)
    {
      patch_size_alt+=1;
      patch_size+=1;
    }
  
  //Create new optical flow vector field
  if(optical_flow_camus==NULL || 
     p_input->width!=optical_flow_camus->width || 
     patch_size_alt!=patch_size)
    { 
      //release the actual optical flow field
      if(optical_flow_camus!=NULL)
	freeVec_field(optical_flow_camus);
      
      optical_flow_camus=newVec_field();
      resizeVec_field(optical_flow_camus,p_input->width,p_input->height,
		      (patch_size-1)>>1,(patch_size-1)>>1);
      clearVec_field(optical_flow_camus);
    }
  else
    clearVec_field(optical_flow_camus);
  
  ////////////////////////////////////
  //Optical flow routine after Camus
  ////////////////////////////////////
  ready=calculate_optic_flow(p_input, //Input Picture
			     patch_size, //Search space 
			     S, //Time frames
			     optical_flow_camus); //Output Vectorfield

  if(ready==1)
    { 
      
#if DISPLAY
      /////////////////
      //Display routine
      /////////////////
      //Needle plot?
      if( (((int)(par3*10))%2)==0)
	{	  
	  if(p_output->format==pix_grey)
	    { //Overlay picture
	      memcpy((BYTE *)p_output->data,
		     (BYTE *)p_input->data,
		     p_input->datasize);
	      
	      //create color picture
	      p_output=MONO2RGB(p_output);
	    }
	  else
	    {
	      BYTE *ptr1=p_input->data,*ptr2=p_output->data;
	      //Draw in color picture 
	      for(int i=0;i<p_input->datasize;i++)
		{
		  *ptr2++=*ptr1;
		  *ptr2++=*ptr1;
		  *ptr2++=*ptr1++;
		}
	    }
	  //Needle Plot
	  draw_motion_field(p_output, 
			    optical_flow_camus, 
			    0,0, 
			    S+ (int) (par2*10-5), S+ (int) (par2*10-5));
	}
      else
	{
	  //or, Magnitude Plot?
	  draw_motion_field(p_output, 
			    optical_flow_camus, 
			    1,1, 
			    0,0);
	}
#endif
    }
  
  return 0;  
}

#if defined(__cplusplus)
}
#endif

