/* 
 *
 * This application uses the gnuplot_i module
 * written by N.Devillard <nDevil@eso.org>.  
 *
 */


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

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

#include "est_and_track.h"
#include "ideal.h"
#include "motion_ideal.h"
#include "ttc.h"
#include "gnuplot_i.h"
#include "mathmi.h"

#define DEROT   1

#define TTC_MAP 0

#define FILTER  1

#define ROBUST  0

#define GNU_ROTATION 1
#define NO_PSEUDO    0

//Gnuplot
#define NPOINTS 300
static double ttc_values[NPOINTS];
static double ttc_values_kal[NPOINTS];
static double ttc_values_median[NPOINTS];
static double ttc_values_pseudo[NPOINTS];
static double ttc_values_mean[NPOINTS];

static double ttc_values_median_derot[NPOINTS];
static double ttc_values_foe[NPOINTS];
static double ttc_values_div[NPOINTS];
static double ttc_values_green[NPOINTS];

static double ttc_values_derot[NPOINTS];
static double ttc_values_kal_derot[NPOINTS];
static double ttc_values_foe_derot[NPOINTS];
static double ttc_values_div_derot[NPOINTS];
static double ttc_values_green_derot[NPOINTS];

static double foe_values_x[NPOINTS];
static double foe_values_y[NPOINTS];

static double foe_values_x_robust[NPOINTS];
static double foe_values_y_robust[NPOINTS];

static double foe_values_x_kal[NPOINTS];
static double foe_values_y_kal[NPOINTS];

static double foe_values_x_median[NPOINTS];
static double foe_values_y_median[NPOINTS];

static double Omx_values[NPOINTS];
static double Omy_values[NPOINTS];
static double Omz_values[NPOINTS];

static double Omx_values_robust[NPOINTS];
static double Omy_values_robust[NPOINTS];
static double Omz_values_robust[NPOINTS];

static double Omx_values_kal[NPOINTS];
static double Omy_values_kal[NPOINTS];
static double Omz_values_kal[NPOINTS];

static double Omx_values_median[NPOINTS];
static double Omy_values_median[NPOINTS];
static double Omz_values_median[NPOINTS];

int gnu_count=0;

//Globals
//TTC - Kalman
static float ttc_p=0, ttc_m;
static float variance_p_ttc=1e10,variance_m_ttc;

static float ttc_p_derot=0, ttc_m_derot;
static float variance_p_ttc_derot=1e10,variance_m_ttc_derot;

gnuplot_ctrl *online_plot;

//Egomotion
static float foe_x, foe_y;
static float foe_x_robust, foe_y_robust;
static float foe_x_kal, foe_y_kal;
static float Omx_est, Omy_est, Omz_est;
static float Omx_robust, Omy_robust, Omz_robust;
static float Omx_est_kal, Omy_est_kal, Omz_est_kal;

int time=0; int save=1;
int motion_ideal(Picture *p_input, 
		 float par1, float par2, float par3,
		 float par4, float par5, float par6,
		 Picture *p_output)
{
  vec_field *motion_field;

  float Tx=21.0, Ty=10.0, Tz=0.4;
  //float Omx=0.0, Omy=0.0, Omz=-0.06;
  float Omx=0, Omy=0, Omz=0;
  float Z0=25.0, Z;
  float average, std_dev, density, outlier,mag_average,mag_std_dev;
  float focal_length=1.0;
  
  //Outlier
  float error1=1.0, error2=0.0, error3=10.0;
  
  //more
  //float error1=1.0, error2=0.0, error3=0.0;
  
  //Weak
  //float error1=5.0, error2=0.0, error3=0.0;
  
  //No error
  //float error1=0.0, error2=0.0, error3=0.0;
  
  int xCenter = p_input->width>>1;
  int yCenter = p_input->height>>1;
  int ready=1;  
  //TTC
  int x0,x1,y0,y1;
  float ttc_est, ttc_foe, ttc_div, ttc_green,ttc_pseudo,ttc_mean;
  float ttc_est_derot, ttc_foe_derot, ttc_div_derot, ttc_green_derot;
  float residual;
  double K;
  
  motion_field=newVec_field();
  
  resizeVec_field(motion_field,p_input->width,p_input->height,2,2);
  clearVec_field(motion_field);
 
  if(time==0)
    {
      online_plot=gnuplot_init(2);
      gnuplot_cmd(online_plot,"set noautoscale");
      gnuplot_cmd(online_plot,"set xrange[%f:%f]",0.0, Z0/Tz-5.0);
      gnuplot_cmd(online_plot,"set yrange[%f:%f]",0.0, Z0/Tz);
    }
  
  //Depth decreases with time
  Z = Z0-Tz*time;
  
  if( Z > focal_length)
    {
      //Increase virtual time
      time++;
      printf("%3.2f",Z);
    
    }
  else
    {
      //Restart time again
      time=0;
      //Z = Z0-Tz*time;
      /*
	error1=(par1) * 20;
	error2=(par2) * 100;
	error3=(par3) * 100;
	
	Tx=(par4-0.5)*20;
	Omz=(par5-0.5);
	Omy=(par6-0.5);
      */
    }
  
  //Produce ideal vector field with errors added 
  make_ideal_field( motion_field, //vector field to build 
		    Tx, Ty, Tz, //Translational Parameters 
		    Z, //Z component
		    Omx, Omy, Omz, //Rotational Parameters
		    error1,error2,error3,//errors
		    &average, //Average error of MF(=Motion Field)  
		    &std_dev, //Standard deviation of M
		    &mag_average,
		    &mag_std_dev,
		    &density, //Density of MF
		    &outlier ); //Percentage of outliers

  //////////////////////////////
  //Estimate and track egomotion
  //////////////////////////////
  ready=est_and_track_egomotion( motion_field, 
				 &foe_x, &foe_y, // Estimated FOE
				 0,0,
				 &foe_x_kal, &foe_y_kal, 
				 // Estimated FOE with Kalman
				 &Omx_est, //Estimated Rotation
				 &Omy_est, 
				 &Omz_est, 
				 0, //Estimated robustRotation
				 0, 
				 0, 
				 &Omx_est_kal,//Estimated Rotation with Kalman 
				 &Omy_est_kal,
				 &Omz_est_kal,
				 0);//Reset Kalman
  //Draw the motion field
  draw_motion_field(p_output, 
		    motion_field, 
		    3, 0, 
		    9, 9);
  //Draw real FOE
  setDrawGValImage(225);
  fillCircleInImage(p_output, (int) (Tx/Tz+xCenter), 
			(int) (-Ty/Tz+yCenter),4);
  if(ready==1)
    //Draw estimated FOE
    draw_foe_bw(p_output, foe_x_kal, foe_y_kal);
  
  /////////
  //Get TTC
  /////////
  int size_ttc=8;

#if 0
  x0=p_output->width/2-size_ttc/2; y0=p_output->height/2-size_ttc/2; 
  x1=x0+size_ttc; y1=y0+size_ttc;
#else
  x0=80; y0=80; x1=x0+size_ttc; y1=y0+size_ttc;
#endif

#if DEROT  
  if(ready==1)
    {
      
      vec_field *tmp;
      tmp = derotate(motion_field,
		     (x0-1), (y0-1), (x1+1), (y1+1),
		     Tx, Ty, Tz, Z,//Debug baby!
		     Omx_est_kal, Omy_est_kal, Omz_est_kal );
      
      ttc_est_derot=ttc(tmp,
			x0, y0, x1, y1,
			foe_x_kal, foe_y_kal, 
			&residual, Tz / Z, 
			&ttc_foe_derot, 
			&ttc_div_derot, 
			&ttc_green_derot,
			&ttc_pseudo, &ttc_mean,
			0);
      //Kalman Filter 
      //Measurements
      variance_m_ttc_derot=residual*residual;
      ttc_m_derot=ttc_est_derot;
      
      //Model
      ttc_p_derot-=1.0;
      variance_p_ttc_derot=variance_p_ttc_derot+variance_m_ttc_derot;
      
      //Simple Kalman - Filter  
      K = variance_p_ttc_derot / (variance_p_ttc_derot + 
				  variance_m_ttc_derot);
      
      ttc_p_derot = ttc_p_derot + K * (ttc_m_derot - ttc_p_derot);
      
      //Update 
      //variance_p_ttc_derot = variance_m_ttc_derot -  
      //K * variance_m_ttc_derot;
      
      variance_p_ttc_derot = K * variance_m_ttc_derot;
      
      freeVec_field(tmp);
    }
  ///DEROT/////////////////
#endif //DEROT

  if(ready==1)
    ttc_est=ttc(motion_field,
		x0, y0, x1, y1,
		foe_x_kal, foe_y_kal, 
		&residual, Tz / Z, 
		&ttc_foe, 
		&ttc_div, 
		&ttc_green,
		&ttc_pseudo,&ttc_mean,
		0);
  else
    ttc_est=ttc(motion_field,
		x0, y0, x1, y1,
		foe_x_kal, foe_y_kal, 
		&residual, Tz / Z, 
		&ttc_foe, 
		&ttc_div, 
		&ttc_green,
		&ttc_pseudo,&ttc_mean,
		5);
    

	//Kalman Filter 
	//Measurements
	variance_m_ttc=residual*residual;
#if NO_PSEUDO
	ttc_m=ttc_est;
#else
	ttc_m=ttc_pseudo;
#endif
	//Model
	ttc_p-=1.0;
	variance_p_ttc=variance_p_ttc+variance_m_ttc;
	
	//Simple Kalman - Filter  
	K = variance_p_ttc / (variance_p_ttc + variance_m_ttc);
	
	ttc_p = ttc_p + K * (ttc_m - ttc_p);
	
	//Update 
	//variance_p_ttc = variance_m_ttc -  K * variance_m_ttc;
	variance_p_ttc = K * variance_m_ttc;
	
	
	//Draw TTC area
	setDrawGValImage(225);
	fillRectInImage(p_output, x0, y0, x1-x0, y1-y0);
	/////////////
	//End of TTC
	/////////////
	
#if TTC_MAP  
	//////////
	//TTC MAP
	/////////
	float **ttc_val;
	int many=2; int size=4;
	
	ttc_val=matrixf(1,many<<1,1,
			( (motion_field->width)-
			  (motion_field->xoffset<<1) )/size);
	
	ttc_map(motion_field,
		foe_x, foe_y, many, size, ttc_val, 
		p_output);
	
	freematrixf(ttc_val,
		    1,many<<1,
		    1,((motion_field->width)-(motion_field->xoffset<<1))/size);
	
#endif TTC_MAP
    

  ////////////////////////////
  //Gnuplot => Graphs to save 
  ////////////////////////////
  if(time!=0)
    {  
      ttc_values[gnu_count]=(double) ttc_est;
      ttc_values_kal[gnu_count]=(double) ttc_p;
      
      ttc_values_pseudo[gnu_count]=(double) ttc_pseudo;
      ttc_values_mean[gnu_count]=(double) ttc_mean;
      
      ttc_values_foe[gnu_count]=(double) ttc_foe;
      ttc_values_div[gnu_count]=(double) ttc_div; 
      ttc_values_green[gnu_count]=(double) ttc_green;

#if DEROT      
      ttc_values_derot[gnu_count]=(double) ttc_m_derot;
      ttc_values_kal_derot[gnu_count]=(double) ttc_p_derot;
      ttc_values_foe_derot[gnu_count]=(double) ttc_foe_derot;
      ttc_values_div_derot[gnu_count]=(double) ttc_div_derot; 
      ttc_values_green_derot[gnu_count]=(double) ttc_green_derot;
#endif
      
      foe_values_x[gnu_count]=(double)foe_x;
      foe_values_y[gnu_count]=(double)foe_y;
      
      foe_values_x_robust[gnu_count]=(double)foe_x_robust;
      foe_values_y_robust[gnu_count]=(double)foe_y_robust;

      foe_values_x_kal[gnu_count]=(double)foe_x_kal;
      foe_values_y_kal[gnu_count]=(double)foe_y_kal;
      if(ready==1)
	{      
	  Omx_values[gnu_count]=(double)Omx_est; 
	  Omy_values[gnu_count]=(double)Omy_est; 
	  Omz_values[gnu_count]=(double)Omz_est;
	  
#if ROBUST
	  Omx_values_robust[gnu_count]=(double)Omx_robust; 
	  Omy_values_robust[gnu_count]=(double)Omy_robust; 
	  Omz_values_robust[gnu_count]=(double)Omz_robust;
#endif
	  
	  Omx_values_kal[gnu_count]=(double)Omx_est_kal;
	  Omy_values_kal[gnu_count]=(double)Omy_est_kal;
	  Omz_values_kal[gnu_count]=(double)Omz_est_kal;
	}
      
      int size=5; float tmp1[size], tmp2[size];
      float tmp1_foe[size], tmp2_foe[size];
      float tmp1_omx[size]; 
      float tmp1_omy[size]; 
      float tmp1_omz[size];

      if(gnu_count >= (size-1))
	{
	  for(int i=0; i<size; i++)
	    {
#if NO_PSEUDO
	      tmp1[i]=(float)ttc_values[gnu_count-size+i+1];
#else
	      tmp1[i]=(float)ttc_values_pseudo[gnu_count-size+i+1];
#endif
	      tmp2[i]=(float)ttc_values_derot[gnu_count-size+i+1];
	      
	      tmp1_foe[i]=(float)foe_values_x[gnu_count-size+i+1];
	      tmp2_foe[i]=(float)foe_values_y[gnu_count-size+i+1];
	      
	      tmp1_omx[i]=(float)Omx_values[gnu_count-size+i+1];
	      tmp1_omy[i]=(float)Omy_values[gnu_count-size+i+1];
	      tmp1_omz[i]=(float)Omz_values[gnu_count-size+i+1];
	    }
	  
	  ttc_values_median[gnu_count]=(double) quick_select(tmp1,size);
	  ttc_values_median_derot[gnu_count]=(double) quick_select(tmp2,size);
	  
	  foe_values_x_median[gnu_count]=(double)
	    quick_select(tmp1_foe, size);
	  
	  foe_values_y_median[gnu_count]=(double)
	    quick_select(tmp2_foe, size);
	  
	  Omx_values_median[gnu_count]=(double)
	    quick_select(tmp1_omx, size);
	  
	  Omy_values_median[gnu_count]=(double)
	    quick_select(tmp1_omy, size);
	  
	  Omz_values_median[gnu_count]=(double)
	    quick_select(tmp1_omz, size); 
	  
	}      
      else
	{
	  ttc_values_median[gnu_count]=0.0;
	  ttc_values_median_derot[gnu_count]=0.0;
	  
	  foe_values_x_median[gnu_count]=0.0;
	  foe_values_y_median[gnu_count]=0.0;
	  
	  Omx_values_median[gnu_count]=0.0; 
	  Omy_values_median[gnu_count]=0.0; 
	  Omy_values_median[gnu_count]=0.0;
	}
      
      //Online TTC - plot
      gnuplot_resetplot(online_plot);
      gnuplot_cmd(online_plot,"set title 'Ang. err.:%2.2f deg  StdDev:%2.2f  Mag. err.:%2.2f pix.  StdDev:%2.2f  Dens.:%2.2f  Outlier:%2.2f'",average,std_dev,mag_average,mag_std_dev,density,outlier);
      gnuplot_setstyle(online_plot, "lines"); 
      gnuplot_plot_slope(online_plot, -1.0, Z0/Tz, "Real TTC");
      gnuplot_setstyle(online_plot, "linespoints") ; 
      gnuplot_plot1d_var1(online_plot, ttc_values, gnu_count, "TTC") ;
      gnuplot_plot1d_var1(online_plot, 
			  ttc_values_kal, gnu_count, "Kalman: TTC") ;
      
      gnuplot_plot1d_var1(online_plot, ttc_values_mean, 
			  gnu_count,"Mean: TTC");
      
      gnuplot_plot1d_var1(online_plot, ttc_values_median, 
			  gnu_count,"Median: TTC");
      
      gnuplot_plot1d_var1(online_plot, ttc_values_pseudo, 
			  gnu_count,"Weighted Median: TTC");
      
       if( gnu_count<NPOINTS && time!=1 )
	{
	  gnu_count++;
	}
      
      if(time==37  && save==1)
	{
	  saveImage("output.pgm",p_output);
	  save=0;
	}

    }
  else
    {
      gnu_count--;
      
      //TTC ALL
      gnuplot_ctrl  *ttc_plot;
      ttc_plot   = gnuplot_init(0); 
      gnuplot_cmd(ttc_plot,"set title 'Ang. err.:%2.2f deg  StdDev:%2.2f Mag. err.:%2.2f pix.  StdDev:%2.2f  Dens.:%2.2f  Outlier:%2.2f'",average,std_dev,mag_average,mag_std_dev,density,outlier);
      gnuplot_setstyle(ttc_plot, "lines") ; 
      gnuplot_plot_slope(ttc_plot, -1.0, Z0/Tz, "Real TTC") ;
      gnuplot_setstyle(ttc_plot, "linespoints");
      
#if NO_PSEUDO
      gnuplot_plot1d_var1(ttc_plot, ttc_values, gnu_count, "Median: TTC") ; 
#else
      gnuplot_plot1d_var1(ttc_plot, ttc_values_pseudo, 
			  gnu_count, "Weighted median: TTC") ;
#endif
      
      gnuplot_plot1d_var1(ttc_plot, 
			  ttc_values_kal, gnu_count, "Kalman: TTC") ; 
      //gnuplot_plot1d_var1(ttc_plot, ttc_values_mean, 
      //		  gnu_count,"Mean: TTC");
      gnuplot_plot1d_var1(ttc_plot, ttc_values_median, 
			  gnu_count,"Temporal Median: TTC"); 
      // gnuplot_plot1d_var1(ttc_plot, ttc_values_pseudo, 
      //		  gnu_count,"Weighted Median: TTC");
      
      save_plot(ttc_plot,"TTC.ps");
      
      //Just
      gnuplot_ctrl  *ttc1_plot;
      ttc1_plot   = gnuplot_init(0); 
      gnuplot_cmd(ttc1_plot,"set title 'Ang. err.:%2.2f deg  StdDev:%2.2f  Mag. err.:%2.2f pix.  StdDev:%2.2f  Dens.:%2.2f  Outlier:%2.2f'",average,std_dev,mag_average,mag_std_dev,density,outlier);
      gnuplot_setstyle(ttc1_plot, "lines") ; 
      gnuplot_plot_slope(ttc1_plot, -1.0, Z0/Tz, "Real TTC") ;
      gnuplot_setstyle(ttc1_plot, "linespoints");
      gnuplot_plot1d_var1(ttc1_plot, ttc_values, gnu_count, "Median: TTC") ; 
      gnuplot_plot1d_var1(ttc1_plot, ttc_values_mean, 
			  gnu_count,"Mean: TTC");
      gnuplot_plot1d_var1(ttc1_plot, ttc_values_pseudo, 
			  gnu_count,"Weighted Median: TTC");
      
      save_plot(ttc1_plot,"TTC_1.ps");



      
      //TTC_FOE
      gnuplot_ctrl  *ttc_plot_foe;
      ttc_plot_foe   = gnuplot_init(0); 
      gnuplot_cmd(ttc_plot_foe,"set title 'Ang. err.:%2.2f deg  StdDev:%2.2f  Mag. err.:%2.2f pix.  StdDev:%2.2f  Dens.:%2.2f  Outlier:%2.2f'",average,std_dev,mag_average,mag_std_dev,density,outlier); 
      gnuplot_setstyle(ttc_plot_foe, "lines") ; 
      gnuplot_plot_slope(ttc_plot_foe, -1.0, Z0/Tz, "Real TTC") ; 
      gnuplot_setstyle(ttc_plot_foe, "linespoints") ; 
      gnuplot_plot1d_var1(ttc_plot_foe, ttc_values_foe,gnu_count,"TTC_FOE"); 
      save_plot(ttc_plot_foe,"TTC_FOE.ps");
      
      //TTC_DIV
      gnuplot_ctrl  *ttc_plot_div;
      ttc_plot_div   = gnuplot_init(0);
      gnuplot_cmd(ttc_plot_div,"set title 'Ang. err.:%2.2f deg  StdDev:%2.2f  Mag. err.:%2.2f pix.  StdDev:%2.2f  Dens.:%2.2f  Outlier:%2.2f'",average,std_dev,mag_average,mag_std_dev,density,outlier); 
      gnuplot_setstyle(ttc_plot_div, "lines") ; 
      gnuplot_plot_slope(ttc_plot_div, -1.0, Z0/Tz, "Real TTC") ;
      gnuplot_setstyle(ttc_plot_div, "linespoints") ; 
      gnuplot_plot1d_var1(ttc_plot_div, ttc_values_div, 
			  gnu_count, "TTC_DIV") ;  
      save_plot(ttc_plot_div,"TTC_Div.ps");
      
      //TTC_GREEN
      gnuplot_ctrl  *ttc_plot_green;
      ttc_plot_green   = gnuplot_init(0);
      gnuplot_cmd(ttc_plot_green,"set title 'Ang. err.:%2.2f deg  StdDev:%2.2f  Mag. err.:%2.2f pix.  StdDev:%2.2f  Dens.:%2.2f  Outlier:%2.2f'",average,std_dev,mag_average,mag_std_dev,density,outlier);
      gnuplot_setstyle(ttc_plot_green, "lines") ; 
      gnuplot_plot_slope(ttc_plot_green, -1.0, Z0/Tz,"Real TTC") ; 
      gnuplot_setstyle(ttc_plot_green, "linespoints") ; 
      gnuplot_plot1d_var1(ttc_plot_green, ttc_values_green,
			  gnu_count,"TTC_Green"); 
      save_plot(ttc_plot_green, "TTC_Green.ps");

#if DEROT    
      //TTC ALL_Derot
      gnuplot_ctrl  *ttc_plot_derot;
      ttc_plot_derot   = gnuplot_init(0); 
      gnuplot_cmd(ttc_plot_derot,"set title 'Avg Error:%2.3f  StdDev:%1.4f  Density:%2.3f  Outlier:%2.3f '",average,std_dev,density,outlier);
      gnuplot_setstyle(ttc_plot_derot, "lines") ; 
      gnuplot_plot_slope(ttc_plot_derot, -1.0, Z0/Tz, "Real TTC") ;
      gnuplot_setstyle(ttc_plot_derot, "lines") ; 
      gnuplot_plot1d_var1(ttc_plot_derot, 
			  ttc_values_derot, gnu_count, "Est: TTC_Derot"); 
      gnuplot_plot1d_var1(ttc_plot_derot, 
			  ttc_values_kal_derot,gnu_count,"Kalman: TTC_Derot"); 
      gnuplot_setstyle(ttc_plot_derot, "points") ; 
      gnuplot_plot1d_var1(ttc_plot_derot, ttc_values_derot, gnu_count,"");
      gnuplot_plot1d_var1(ttc_plot_derot, ttc_values_kal_derot, gnu_count,""); 
       gnuplot_setstyle(ttc_plot_derot, "linespoints") ; 
      gnuplot_plot1d_var1(ttc_plot_derot, ttc_values_median_derot, 
			  gnu_count,"Median: TTC");
      save_plot(ttc_plot_derot,"TTC_Derot.ps");
      

      //TTC_FOE_Derot
      gnuplot_ctrl  *ttc_plot_foe_derot;
      ttc_plot_foe_derot   = gnuplot_init(0);  
      gnuplot_cmd(ttc_plot_foe_derot,"set title 'Avg Error:%2.3f  StdDev:%1.4f  Density:%2.3f  Outlier:%2.3f '",average,std_dev,density,outlier);
      gnuplot_setstyle(ttc_plot_foe_derot, "lines") ; 
      gnuplot_plot_slope(ttc_plot_foe_derot, -1.0, Z0/Tz, "Real TTC") ; 
      gnuplot_setstyle(ttc_plot_foe_derot, "linespoints") ; 
      gnuplot_plot1d_var1(ttc_plot_foe_derot, 
			  ttc_values_foe_derot,gnu_count, "FOE_Derot"); 
      save_plot(ttc_plot_foe_derot,"TTC_FOE_Derot.ps");
      
      //TTC_DIV_Derot
      gnuplot_ctrl  *ttc_plot_div_derot;
      ttc_plot_div_derot   = gnuplot_init(0);
      gnuplot_cmd(ttc_plot_div_derot,"set title 'Avg Error:%2.3f  StdDev:%1.4f  Density:%2.3f  Outlier:%2.3f '",average,std_dev,density,outlier); 
      gnuplot_setstyle(ttc_plot_div_derot, "lines") ; 
      gnuplot_plot_slope(ttc_plot_div_derot, -1.0, Z0/Tz, "Real TTC") ;
      gnuplot_setstyle(ttc_plot_div_derot, "linespoints") ; 
      gnuplot_plot1d_var1(ttc_plot_div_derot, 
			  ttc_values_div_derot, gnu_count, "DIV_Derot") ;  
      save_plot(ttc_plot_div_derot,"TTC_Div_Derot.ps");
      
      //TTC_GREEN_Derot
      gnuplot_ctrl  *ttc_plot_green_derot;
      ttc_plot_green_derot   = gnuplot_init(0);
      gnuplot_cmd(ttc_plot_green_derot,"set title 'Avg Error:%2.3f  StdDev:%1.4f  Density:%2.3f  Outlier:%2.3f '",average,std_dev,density,outlier); 
      gnuplot_setstyle(ttc_plot_green_derot, "lines") ; 
      gnuplot_plot_slope(ttc_plot_green_derot, -1.0, Z0/Tz,"Real TTC") ; 
      gnuplot_setstyle(ttc_plot_green_derot, "linespoints") ; 
      gnuplot_plot1d_var1(ttc_plot_green_derot,
			  ttc_values_green_derot,gnu_count,"Green_Derot"); 
      save_plot(ttc_plot_green_derot,"TTC_Green_Derot.ps");
#endif
      
      //FOE X
      gnuplot_ctrl  *foe_x_plot; 
      foe_x_plot = gnuplot_init(0);
      gnuplot_cmd(foe_x_plot,"set title 'Ang. err.:%2.2f deg  StdDev:%2.2f  Mag. err.:%2.2f pix.  StdDev:%2.2f  Dens.:%2.2f  Outlier:%2.2f'",average,std_dev,mag_average,mag_std_dev,density,outlier);
      gnuplot_setstyle(foe_x_plot, "lines") ;
      gnuplot_plot_slope(foe_x_plot, 0.0, Tx/Tz, "Real Tx/Tz") ;
      gnuplot_setstyle(foe_x_plot, "linespoints");
      gnuplot_plot1d_var1(foe_x_plot, foe_values_x, 
			  gnu_count, "Est.: Tx/Tz ") ;
#if ROBUST
      gnuplot_plot1d_var1(foe_x_plot, foe_values_x_robust, 
			  gnu_count, "Robust: Tx/Tz ") ;
#endif
#if FILTER	 
      gnuplot_plot1d_var1(foe_x_plot, 
			  foe_values_x_kal, gnu_count, "Kalman: Tx/Tz ") ;
      gnuplot_plot1d_var1(foe_x_plot, 
			  foe_values_x_median, 
			  gnu_count, "Median: Tx/Tz ") ;
#endif FILTER
      save_plot(foe_x_plot,"FOE_x.ps");
      
      //FOE Y 
      gnuplot_ctrl  *foe_y_plot;
      foe_y_plot = gnuplot_init(0);
      gnuplot_cmd(foe_y_plot,"set title 'Ang. err.:%2.2f deg  StdDev:%2.2f  Mag. err.:%2.2f pix.  StdDev:%2.2f  Dens.:%2.2f  Outlier:%2.2f'",average,std_dev,mag_average,mag_std_dev,density,outlier);
      gnuplot_setstyle(foe_y_plot, "lines");
      gnuplot_plot_slope(foe_y_plot, 0.0, Ty/Tz, "Real Ty/Tz") ;
      gnuplot_setstyle(foe_y_plot, "linespoints");
      gnuplot_plot1d_var1(foe_y_plot, foe_values_y, 
			  gnu_count, "Est.: Ty/Tz "); 
#if ROBUST      
      gnuplot_plot1d_var1(foe_y_plot, foe_values_y_robust, 
			  gnu_count, "Robust: Ty/Tz ") ;
#endif
#if FILTER	  
      gnuplot_plot1d_var1(foe_y_plot, 
			  foe_values_y_kal, gnu_count, "Kalman: Ty/Tz "); 
      gnuplot_plot1d_var1(foe_y_plot, 
			  foe_values_y_median, 
			  gnu_count, "Median: Ty/Tz ");
#endif FILTER
      save_plot(foe_y_plot,"FOE_y.ps");	

#if GNU_ROTATION      
       //Omx
      gnuplot_ctrl  *Omx_plot;
      Omx_plot   = gnuplot_init(0); 
      gnuplot_cmd(Omx_plot,"set title 'Ang. err.:%2.2f deg  StdDev:%2.2f  Mag. err.:%2.2f pix.  StdDev:%2.2f  Dens.:%2.2f  Outlier:%2.2f'",average,std_dev,mag_average,mag_std_dev,density,outlier);
      gnuplot_setstyle(Omx_plot, "lines") ; 
      gnuplot_plot_slope(Omx_plot, 0.0, Omx, "Real Omega x") ;
      gnuplot_setstyle(Omx_plot, "linespoints");
      gnuplot_plot1d_var1(Omx_plot, Omx_values+1, 
			  gnu_count, "Est. Omega x ") ;
#if ROBUST
      gnuplot_plot1d_var1(Omx_plot, Omx_values_robust, 
			  gnu_count, "Robust: Omega x ");
#endif
#if FILTER	
      gnuplot_plot1d_var1(Omx_plot, Omx_values_kal, 
			  gnu_count, "Kalman: Omega x ");
      gnuplot_plot1d_var1(Omx_plot, 
			  Omx_values_median, 
			  gnu_count, "Median: Omega x ");
#endif FILTER
      save_plot(Omx_plot,"Omega_x.ps");	
      
      //Omy
      gnuplot_ctrl  *Omy_plot;
      Omy_plot   = gnuplot_init(0); 
      gnuplot_cmd(Omy_plot,"set title 'Ang. err.:%2.2f deg  StdDev:%2.2f  Mag. err.:%2.2f pix.  StdDev:%2.2f  Dens.:%2.2f  Outlier:%2.2f'",average,std_dev,mag_average,mag_std_dev,density,outlier);
      gnuplot_setstyle(Omy_plot, "lines") ; 
      gnuplot_plot_slope(Omy_plot, 0.0, Omy, "Real Omega y") ;
      gnuplot_setstyle(Omy_plot, "linespoints");
      gnuplot_plot1d_var1(Omy_plot, Omy_values+1, 
			  gnu_count, "Est. Omega y ") ; 
#if ROBUST
      gnuplot_plot1d_var1(Omy_plot, Omy_values_robust, 
			  gnu_count, "Robust: Omega y ");
#endif 
#if FILTER
      gnuplot_plot1d_var1(Omy_plot, Omy_values_kal, 
			  gnu_count, "Kalman Filter: Omega y ");
      gnuplot_plot1d_var1(Omy_plot, 
			  Omy_values_median, 
			  gnu_count, "Median: Omega y ") ;
#endif FILTER
      save_plot(Omy_plot,"Omega_y.ps");	
      
      //Omz 
      gnuplot_ctrl  *Omz_plot;
      Omz_plot   = gnuplot_init(0); 
      gnuplot_cmd(Omz_plot,"set title 'Ang. err.:%2.2f deg  StdDev:%2.2f  Mag. err.:%2.2f pix.  StdDev:%2.2f  Dens.:%2.2f  Outlier:%2.2f'",average,std_dev,mag_average,mag_std_dev,density,outlier);
      gnuplot_setstyle(Omz_plot, "lines") ; 
      gnuplot_plot_slope(Omz_plot, 0.0, Omz, "Real Omega z") ;
      gnuplot_setstyle(Omz_plot, "linespoints");
      gnuplot_plot1d_var1(Omz_plot, Omz_values+1, 
			  gnu_count, "Est. Omega z ");  
#if ROBUST
      gnuplot_plot1d_var1(Omz_plot, Omz_values_robust, 
			  gnu_count, "Robust: Omega z ");
#endif
#if FILTER
      gnuplot_plot1d_var1(Omz_plot, Omz_values_kal, 
			  gnu_count, "Kalman Filter: Omega z ");
      gnuplot_plot1d_var1(Omz_plot, 
			  Omz_values_median, 
			  gnu_count, "Median: Omega z ") ;
#endif FILTER
      save_plot(Omz_plot,"Omega_z.ps");	
#endif

      //Wait a sec
      sleep(10);
      //Close the plots
      gnuplot_close(online_plot);
      gnuplot_close(ttc_plot);  
      gnuplot_close(ttc1_plot);  
      gnuplot_close(ttc_plot_foe); 
      gnuplot_close(ttc_plot_div);
      gnuplot_close(ttc_plot_green);

#if DEROT      
      sleep(8);
      gnuplot_close(ttc_plot_derot); 
      gnuplot_close(ttc_plot_foe_derot); 
      gnuplot_close(ttc_plot_div_derot);
      gnuplot_close(ttc_plot_green_derot);
#endif 
      
      sleep(7);
      gnuplot_close(foe_x_plot);
      gnuplot_close(foe_y_plot);

#if GNU_ROTATION      
      sleep(2);
      gnuplot_close(Omx_plot); 
      gnuplot_close(Omy_plot); 
      gnuplot_close(Omz_plot);
#endif
      gnu_count=0;
      
      exit(1);
    }

  freeVec_field(motion_field); 

  return 0;
} 



