#include <stdio.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>

#include "optical_flow_liu.h"

//Optical Flow routine
#include "liu.h"
  
//Drawing libraries
#include "../FW/labImage.h"
#include "../FW/labImgDraw.h"

#include "motion_field.h"

//Draw optical flow field?
#define DISPLAY_LIU  1
  
//The optical flow field
vec_field *optical_flow_liu;
int OF_by_liu(Picture *p_input, 
		float par1, float par2, float par3,
		float par4, float par5, float par6,
		Picture *p_output)
{
  //Filter dimensions 
  int W_x=21; int W_y=21;
  int W_t=17;
  //Estimate area to calculate the optical flow
  int x0=W_x; int y0=W_x;
  int x1=p_input->width-W_x;
  int y1=p_input->height-W_x; 
  x0=0; y0=0; x1=p_input->width; y1=p_input->height;
  
  //Confidence Measurement
  int stepx=(int) (par1*7), stepy=(int) (par1*7);
  float lampda_min=par3*3; 
  //float residual_min=par4*10;
  float residual_min=0.0;
  //float cond=par4*10;
  float cond=0.0;
  //Flag for the routines
  int ready,ok;
  
  //Create new optical flow vector field
  if(optical_flow_liu==NULL || 
     p_input->width!=optical_flow_liu->width )
    { 
      //release the actual optical flow field
      if(optical_flow_liu!=NULL)
	freeVec_field(optical_flow_liu);
      
      optical_flow_liu=newVec_field();
      resizeVec_field(optical_flow_liu,p_input->width,p_input->height,0,0);
      clearVec_field(optical_flow_liu);
    }
  else
    clearVec_field(optical_flow_liu);
  
  ////////////////////////////////////
  //Optical flow routine after Camus
  ////////////////////////////////////
  ready=calculate_optic_flow_liu(p_input, 
				 W_x, W_y, W_t,
				 x0, y0, x1, y1,
				 stepx, stepy,
				 lampda_min,
				 residual_min,
				 cond,
				 optical_flow_liu, 
				 p_output);
  if(ready==1)
    { 
#if DISPLAY_LIU
      /////////////////
      //Display routine
      /////////////////
      if(p_output->format==pix_grey)
	{ 
	  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;
	  
	  for(int i=0;i<p_input->datasize;i++)
	    { 
	      //Draw in color picture 
	      *ptr2++=*ptr1;
	      *ptr2++=*ptr1;
	      *ptr2++=*ptr1++;
	    }
	}
      
      if(0.3 > par2)
	//Magnitude
	draw_motion_field(p_output, optical_flow_liu, 1, 1, 1,1);
      else if(0.3<=par2 && par2<0.6)
	//Angle
	draw_motion_field(p_output, optical_flow_liu, 2, 1, 1, 1);
      else
	//Needle
	draw_motion_field(p_output, optical_flow_liu, 0, 0, 1, 1);
#endif      
    }
  
  return 0;  
}


