/*
 *Time to contact (TTC) Module
 *---------------------------
 *
 * Estimates TTC for an area defined by (x0,y0)-(x1,y1) 
 * with either the FOE, divergence or Greens Theorem. Followed by
 * a temporal median filter (not necessary!) 
 * 
 */

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

#include "ttc.h"
#include "mathmi.h"
#include "gnuplot_i.h"

#define TINY 0.001
#define MAX_TTC 300

//Temporal median filtering?
#define TEMPORAL_MEDIAN 1
//Build average of TTC estimates?
#define AVERAGE         1
//Build variance?
#define VARIANCE        1 
//Temporal plot of TTC (upper scroll bar)?
#define AVERAGE_TTC     1
//Create Gnuplots?
#define ONLINEPLOT      0
#define GNUPLOT_TTC     1

#define PLOT_AVERAGE_TTC 1
#define PSEUDO_VARIANCE  0

#if GNUPLOT_TTC
gnuplot_ctrl *val_ttc;
int start_ttc_gnu=1;
#endif
float ttc(vec_field *field,
	  int x0, int y0, int x1, int y1,
	  float foex, float foey,
	  float *variance, float real_ttc,
	  float *ttc_foe, float *ttc_div, float *ttc_green,
	  float *ttc_pseudo, float *ttc_mean,
	  int mode)
{
  int x,y;
  int x0tmp,y0tmp,x1tmp,y1tmp;
  int i,tmp;
  float xnew,ynew;
  int x_Center=field->width>>1;
  int y_Center=field->height>>1;
  float sum,var,median;
  //All the TTC measurements
  static float ttc[MAX_TTC+1];
  //Output TTC for the region 'int x0, int y0, int x1, int y1'
  float ttc_master=0;
  //Local storage for median filter
  static float tmp_ttc[MAX_TTC+1];   
  
  //How many TTC measurements do we have?
  int count_ttc=0,border1=0,border2=0;
  
#if VARIANCE
  float dev[MAX_TTC+1];  
  float inv_variance_foe;
  float inv_variance_div;
  float inv_variance_green;
  float weighted_ttc;
  double norm;
#endif  

#if GNUPLOT_TTC	
  double dttc[MAX_TTC+1];
  if(start_ttc_gnu==1)
    {
      val_ttc=gnuplot_init(1);
      gnuplot_cmd(val_ttc,"set noautoscale y"); 
      //gnuplot_cmd(val_ttc,"set xrange[%f:%f]",0.0, (x1-x0)*(y1-y0)*3);
      gnuplot_cmd(val_ttc,"set yrange[%f:%f]",0.0, 1.0 / real_ttc);
      
      start_ttc_gnu=0;
    }
#endif
  
  ///////////
  //TTC FOE
  ///////////
  if(mode==0 || mode==1)
    {  //For the area
      for(y=y0; y < y1; y++)
	{
	  for(x=x0; x < x1; x++)
	    { 
	      i=y * field->width +x;
	      
	      xnew=(float) (x - x_Center);
	      ynew=(float) (-y + y_Center);
	      
	      if(field->valid[i]==1)
		{//Calculate TTC and save
		  ttc[count_ttc]=( (field->u[i]/(xnew - foex)) - 
				   (field->v[i]/(ynew - foey)) ) / 2.0;
#if GNUPLOT_TTC
		  dttc[count_ttc]=(double) 1.0 / ttc[count_ttc];
#endif
		  if((ttc[count_ttc]>TINY) && ((count_ttc) < MAX_TTC) )
		    count_ttc++;
		}
	    }
	}
      
      //TTC just from FOE
      if(count_ttc>1)
	{
	  for(i=0; i < (count_ttc-1); i++)
	    tmp_ttc[i]=ttc[i];
	  *ttc_foe= quick_select(tmp_ttc,(count_ttc-1));
	  
	  ttc_master=*ttc_foe;

#if VARIANCE
	  //Calculate Pseudo variance
	  sum=0.0;
	  for(i=0; i < (count_ttc-1); i++ )
	    {
#if PSEUDO_VARIANCE
	      dev[i]=FABS((ttc[i]-*ttc_foe));
#else
	      //sum+=(ttc[i]-*ttc_foe)*(ttc[i]-*ttc_foe);
	      sum+=FABS(ttc[i]-*ttc_foe);
#endif
	    }
#if PSEUDO_VARIANCE
	  inv_variance_foe=(count_ttc-1) / 
				   quick_select(dev,(count_ttc-1));
#else
	  inv_variance_foe=(count_ttc-2) / sum;
#endif
#endif
	  *ttc_foe=1.0 / *ttc_foe;
	  ttc_master=*ttc_foe;
	}
      else
	{
	  inv_variance_foe=0;
	  *ttc_foe=0;
	  ttc_master=0;
      }
    }
  else
    {
      inv_variance_foe=0;
      *ttc_foe=0;
      ttc_master=0;
    }
  
  //////////////////
  //TTC DIVERGENCE 
  /////////////////
  if(mode==0 || mode==2 || mode==5)
    {
      border1=count_ttc;
      for(y=y0; y < y1; y++)
	{
	  for(x=x0; x < x1; x++)
	    { 
	      {
		ttc[count_ttc]=divergence(field, x, y);
#if GNUPLOT_TTC	      
		dttc[count_ttc]=(double) 1.0 / ttc[count_ttc];
#endif
		if(ttc[count_ttc]>TINY &&  (count_ttc)< MAX_TTC )
		  {
		    count_ttc++;
		  }
	      }
	    }
	}
       
      //TTC just from Div
      if((count_ttc-border1) > 0)
	{ 
	  for(i=0; i < (count_ttc-border1-1); i++)
	    tmp_ttc[i]=*(ttc+border1+i);
	  *ttc_div = quick_select(tmp_ttc,(count_ttc-border1-1));

#if VARIANCE
	  //Calculate Pseudo variance
	  sum=0.0;
	  for(i=0; i < (count_ttc-border1-1); i++ )
	    {
#if PSEUDO_VARIANCE
	    dev[i]=FABS( (*(ttc+border1+i) - *ttc_div) );
#else
	    //sum+=(*(ttc+border1+i) - *ttc_div)*(*(ttc+border1+i) - *ttc_div);
	    sum+=FABS(*(ttc+border1+i) - *ttc_div);
#endif
	    }
	  
#if PSEUDO_VARIANCE
	  inv_variance_div=(count_ttc-border1-1)/
	    quick_select(dev,(count_ttc-border1-1));
#else
	  inv_variance_div=(count_ttc-border1-2) / sum;
#endif
	  
#endif
	  *ttc_div = 1.0 / *ttc_div;
	  ttc_master=*ttc_div;
	}
      else 
	{
	  inv_variance_div=0;
	  *ttc_div=0;
	  ttc_master=0;
	}
    }
  else
    {
      inv_variance_div=0;
      *ttc_div=0;
      ttc_master=0;
    }
  
  ////////////////////////////////////////
  //Green's Theorem
  /////////////////////////////////////////
  if(mode==0 || mode==3 || mode==5)
    {
      border2=count_ttc;
      x0tmp=x0; y0tmp=y0;
      x1tmp=x1; y1tmp=y1;
      tmp=(MIN((x1-x0),(y1-y0))>>1)-2;
      for(i=0; i < tmp; i++)
	{
	  ttc[count_ttc]=rect(field,x0tmp,y0tmp,x1tmp,y1tmp); 
	  x0tmp++; y0tmp++; x1tmp--; y1tmp--;
#if GNUPLOT_TTC	
	  dttc[count_ttc]=(double) 1.0 / ttc[count_ttc];
#endif
	  if((ttc[count_ttc] > TINY) && (count_ttc < MAX_TTC))
	    {
	      count_ttc++;
	    }
	}
      
      x0tmp=x0; y0tmp=y0;
      x1tmp=x1; y1tmp=y1;
      tmp=MIN((x1-x0),(y1-y0)) / 2;
      for(i=0; i<tmp; i++)
	{
	  ttc[count_ttc]=rect(field,x0tmp,y0tmp,x1tmp,y1tmp); 
	  x0tmp++; y0tmp++;
#if GNUPLOT_TTC	
	  dttc[count_ttc]=(double) 1.0 / ttc[count_ttc];
#endif
	  if((ttc[count_ttc] > TINY) && (count_ttc < MAX_TTC))
	    {
	      count_ttc++;
	    }
	}
      
      x0tmp=x0; y0tmp=y0;
      x1tmp=x1; y1tmp=y1;
      tmp=MIN((x1-x0),(y1-y0)) / 2;
      for(i=0; i<tmp; i++)
	{
	  ttc[count_ttc]=rect(field,x0tmp,y0tmp,x1tmp,y1tmp); 
	  x1tmp--; y1tmp--;
#if GNUPLOT_TTC	
	  dttc[count_ttc]=(double) 1.0 / ttc[count_ttc];
#endif
	  if((ttc[count_ttc] > TINY) && (count_ttc < MAX_TTC))
	    {
	      count_ttc++;
	    }
	}
      
      x0tmp=x0; y0tmp=y0;
      x1tmp=x1; y1tmp=y1;
      tmp=(y1-y0)-3;
      for(i=0; i < tmp; i++)
	{
	  ttc[count_ttc]=rect(field,x0tmp,y0tmp,x1tmp,y1tmp); 
	  y0tmp++;
#if GNUPLOT_TTC	
	  dttc[count_ttc]=(double) 1.0 / ttc[count_ttc];
#endif
	  if((ttc[count_ttc] > TINY) && (count_ttc < MAX_TTC))
	    {
	      count_ttc++;
	    }
	}
      
      x0tmp=x0; y0tmp=y0;
      x1tmp=x1; y1tmp=y1;
      tmp=(y1-y0)-3;
      for(i=0; i < tmp; i++)
	{
	  ttc[count_ttc]=rect(field,x0tmp,y0tmp,x1tmp,y1tmp); 
	  y1tmp--;
#if GNUPLOT_TTC	
	  dttc[count_ttc]=(double) 1.0 / ttc[count_ttc];
#endif
	  if((ttc[count_ttc] > TINY) && (count_ttc < MAX_TTC))
	    {
	      count_ttc++;
	    }
	}
  
      x0tmp=x0; y0tmp=y0;
      x1tmp=x1; y1tmp=y1;
      tmp=(x1-x0)-3;
      for(i=0; i < tmp; i++)
	{
	  ttc[count_ttc]=rect(field,x0tmp,y0tmp,x1tmp,y1tmp); 
	  x0tmp++;
#if GNUPLOT_TTC	
	  dttc[count_ttc]=(double) 1.0 / ttc[count_ttc];
#endif
	  if((ttc[count_ttc] > TINY) && (count_ttc < MAX_TTC))
	    {
	      count_ttc++;
	    }
	}
      
      x0tmp=x0; y0tmp=y0;
      x1tmp=x1; y1tmp=y1;
      tmp=(x1-x0)-3;
      for(i=0; i < tmp; i++)
	{
	  ttc[count_ttc]=rect(field,x0tmp,y0tmp,x1tmp,y1tmp); 
	  x1tmp--;
#if GNUPLOT_TTC	
	  dttc[count_ttc]=(double) 1.0 / ttc[count_ttc];
#endif
	  if((ttc[count_ttc] > TINY) && (count_ttc < MAX_TTC))
	    {
	      count_ttc++;
	    }
	}
      
      /*  
      x0tmp=x0; y0tmp=y0+1;
      x1tmp=x1; y1tmp=y0tmp+3;
      tmp=((x1-x0)>>1)-3;
      for(i=0; i < tmp; i++)
      {
      ttc[count_ttc]=rect(field,x0tmp,y0tmp,x1tmp,y1tmp); 
      y0tmp++;y1tmp=y0tmp+3;
      
      dttc[count_ttc]=(double) ttc[count_ttc];
      
      if((ttc[count_ttc] > TINY) && (count_ttc < MAX_TTC))
      count_ttc++;
      }

      x0tmp=x0+1; y0tmp=y0;
      x1tmp=x0tmp+3; y1tmp=y1;
      tmp=((x1-x0)>>1)-3;
      for(i=0; i < tmp; i++)
      {
       ttc[count_ttc]=rect(field,x0tmp,y0tmp,x1tmp,y1tmp); 
       x0tmp++;x1tmp=x0tmp+3;
       
       dttc[count_ttc]=(double) ttc[count_ttc];
 
       if((ttc[count_ttc] > TINY) && (count_ttc < MAX_TTC))
	 count_ttc++;
	 } */
  
      //TTC just from Green
      if((count_ttc - border2) > 0)
	{
	  for(i=0; i < (count_ttc-border2-1); i++)
	    tmp_ttc[i]=*(ttc+border2+i);
	  *ttc_green=quick_select(tmp_ttc,(count_ttc-border2-1));
	  
#if VARIANCE
	  //Calculate Pseudo variance
	  sum=0.0;
	  for(i=0; i < (count_ttc-border2-1); i++ )
	    {
#if PSEUDO_VARIANCE
	    dev[i]=FABS( (*(ttc+border2+i) - *ttc_green) );
#else
	    //sum+=(*(ttc+border2+i) - *ttc_green)*
	    //(*(ttc+border2+i) - *ttc_green) ;
	    sum+=FABS(*(ttc+border2+i) - *ttc_green);
#endif
	    }
	#if PSEUDO_VARIANCE  
	  inv_variance_green=(count_ttc-border2-1)/ 
	    quick_select(dev,(count_ttc-border2-1));
#else
	  inv_variance_green=  (count_ttc-border2-2)/sum;
#endif
#endif
	  *ttc_green=1.0 / *ttc_green;
	  ttc_master=*ttc_green;  
	}
      else
	{
	  inv_variance_green=0;
	  *ttc_green=0;
	  ttc_master=0;
	}
    }
  else
    {
      inv_variance_green=0;
      *ttc_green=0;
      ttc_master=0;
    }
  //////////END TTC - Green's Theorem////////// 
  
  //////////////
  //Combined TTC
  //////////////
#if AVERAGE
  //Average
  sum=0.0;
  for(i=0;i < (count_ttc-1); i++ )
    sum+=ttc[i];
  *ttc_mean = (count_ttc-1) / sum;
  
  //Calculate variance
  var=0.0;  
  for(i=0;i < (count_ttc-1); i++ )
    var+=(ttc[i] - (1.0/(*ttc_mean)))*(ttc[i] - (1.0 /(*ttc_mean)));
  *variance = var / (count_ttc-2) ;
#endif
  
  //TTC from ALL
  if(mode==0 || mode==5)
   {
     if((count_ttc > 1))
       {
	 median=quick_select(ttc,(count_ttc-1));
	 ttc_master = 1.0 / median;
       }
     else
       ttc_master=0;
   }
  
#if VARIANCE  
  //Calculate Pseudo variance
  sum=0.0;
  for(i=0; i < (count_ttc-1); i++ )
    {
#if PSEUDO_VARIANCE
      dev[i]=FABS((ttc[i]-median));
#else
      sum+=(ttc[i]-median)*(ttc[i]-median);
#endif
    }
#if PSEUDO_VARIANCE
  *variance=(count_ttc-1)*quick_select(dev,(count_ttc-1));
#else
  *variance = sum / (count_ttc-2);
#endif
  
#endif

#if GNUPLOT_TTC 
  gnuplot_resetplot(val_ttc); 
  gnuplot_cmd(val_ttc,"set arrow from %d,0 to %d,60 nohead lt 0 lw 2",
	      border1, border1);
  gnuplot_cmd(val_ttc,"set arrow from %d,0 to %d,60 nohead lt 0 lw 2",
	      border2, border2);
  gnuplot_setstyle(val_ttc, "linespoints") ;
  gnuplot_plot1d_var1(val_ttc, dttc, count_ttc-1, "TTC Measurements");
  gnuplot_setstyle(val_ttc, "lines") ;
  gnuplot_plot_slope(val_ttc, 0.0, 1.0 / real_ttc , "Real TTC");
  gnuplot_setstyle(val_ttc, "lines") ;
  gnuplot_plot_slope(val_ttc, 0.0, 1.0 / median , "Median");
  gnuplot_setstyle(val_ttc, "lines") ;
  gnuplot_plot_slope(val_ttc, 0.0, *ttc_mean , "Mean");
  gnuplot_setstyle(val_ttc, "lines") ;
#if VARIANCE 
  norm=1.0 / ((inv_variance_foe + inv_variance_div+ inv_variance_green));
  
  weighted_ttc=norm*inv_variance_foe*(*ttc_foe)+
    norm*inv_variance_div*(*ttc_div)+
    norm*inv_variance_green*(*ttc_green);  
  
  *ttc_pseudo=weighted_ttc;
  
  gnuplot_plot_slope(val_ttc, 0.0, weighted_ttc , "Weighted Median");
#endif
  save_plot(val_ttc,"val_ttc.ps");
  gnuplot_cmd(val_ttc,"set noarrow"); 
#endif 
  
  //Finally return TTC
  return ttc_master;
  
}
//////////////////////////////////////////
//Calculates the divergence at point (x,y)
//////////////////////////////////////////
float divergence(vec_field *field,int x, int y)
{
  double dx;
  double dy,tmp;
  int i;
  
  i = y * field->width + x;
  
  if( (field->valid[i-1]==1) && 
      (field->valid[i+1]==1) &&
      (field->valid[i-field->width]==1) && 
      (field->valid[i+field->width]==1) )
    {
      dx= (field->u[i+1]-field->u[i-1]) / 2;
      dy= (field->v[i+field->width]-field->v[i-field->width]) / 2;
      
      tmp=(dx+dy) / 2.0;
      
      return (float) tmp;
    }
  
  else
    return -1.0;
}

////////////////////////////////////////////
//Sums the horzontal and vertical components
//////////////////////////////////////////// 
float rect(vec_field *field,int x0, int y0, int x1, int y1)
{
  int x,y,i,fail=0;
  float sum[4];
  
  y=y0; x=x0; sum[0] = 0.0;
  do
    {
      i=y * field->width +x;
      if(field->valid[i]==1)
	sum[0]+=-field->v[i];
      else
	fail++;
    } while( (x++) < x1);
  
  x--; 
  sum[1] = 0.0;
  do
    {
      i=y *field->width +x;
      if(field->valid[i]==1)
	sum[1]+=field->u[i];
      else
	fail++;
    } while( (y++) < y1);

  y--; 
  sum[2] = 0.0;
  do
    {
      i=y *field->width +x;
      if(field->valid[i]==1)
	sum[2]+=field->v[i];
      else
	fail++;
    } while( (x--) > x0);

  x++; 
  sum[3] = 0.0;
  do
    {
      i=y *field->width +x;
      if(field->valid[i]==1)
	sum[3]+=-field->u[i];
      else
	fail++;
    } while( (y--) > y0);

  if(fail==0)
    return (sum[0]+sum[1]+sum[2]+sum[3]) / ( 2*(x1-x0)*(y1-y0) );
  else
    return -1.0;
}

///////////////////////////////////////////////////
//Fast Median filter -> 'Numerical Recipes for C'
////////////////////////////////////////////////////
#define ELEM_SWAP(a,b) { register float t=(a);(a)=(b);(b)=t; }
float quick_select(float arr[], int n) 
{
  int low, high ;
    int median;
    int middle, ll, hh;

    low = 0 ; high = n-1 ; median = (low + high) / 2;
    for (;;) {
        if (high <= low) /* One element only */
            return arr[median] ;

        if (high == low + 1) {  /* Two elements only */
            if (arr[low] > arr[high])
                ELEM_SWAP(arr[low], arr[high]) ;
            return arr[median] ;
        }

    /* Find median of low, middle and high items; swap into position low */
    middle = (low + high) / 2;
    if (arr[middle] > arr[high])    ELEM_SWAP(arr[middle], arr[high]) ;
    if (arr[low] > arr[high])       ELEM_SWAP(arr[low], arr[high]) ;
    if (arr[middle] > arr[low])     ELEM_SWAP(arr[middle], arr[low]) ;

    /* Swap low item (now in position middle) into position (low+1) */
    ELEM_SWAP(arr[middle], arr[low+1]) ;

    /* Nibble from each end towards middle, swapping items when stuck */
    ll = low + 1;
    hh = high;
    for (;;) {
        do ll++; while (arr[low] > arr[ll]) ;
        do hh--; while (arr[hh]  > arr[low]) ;

        if (hh < ll)
        break;

        ELEM_SWAP(arr[ll], arr[hh]) ;
    }

    /* Swap middle item (in position low) back into correct position */
    ELEM_SWAP(arr[low], arr[hh]) ;

    /* Re-set active partition */
    if (hh <= median)
        low = ll;
        if (hh >= median)
        high = hh - 1;
    }
}

//Counter for averaging
int loop=0;
//Array of average values
unsigned char *average;
//Indicate start of routine
int start_ttc=1;

#if PLOT_AVERAGE_TTC
double ttc_average[200];
int average_counter=0;
#endif

gnuplot_ctrl *online_plot_ttc;
#define MEDIAN_SIZE 300
static float **ttc_values[MEDIAN_SIZE];
//Array of actual TTC estimates
float **ttc_val; 
int median_count=0,stop_counter=0;
int ttc_map(vec_field *field, //Optical flow field
	    float foex, float foey, int valid,//Input positon of FOE 
	    int many, int size,  //How many rows and what size
	    Picture *p_in, //Input picture
	    int mode,//0=ALL, 1=DIV, 2=Green
	    int thresh ) 
  
{
  int i,j,nr_horiz, nr_vert,nr_rects;
  int nr_horiz_ttc;
  int x0,x1,y0,y1;
  float ttc_foe, ttc_div, ttc_green, ttc_pseudo, ttc_mean;
  float variance;
  int tmp=0;
  int stop_ttc=0;

#if TEMPORAL_MEDIAN 
  int filter_size=9;
  float tmp1[filter_size];
#endif

  //Nr. of horizontal rectangular areas
  nr_horiz=((field->width)-(field->xoffset<<1)) / size;
  nr_horiz_ttc=(field->width)/size;

  //Nr. of vertical rectangular areas
  if((unsigned) (many)*size < (field->height>>1)-field->yoffset)
    nr_vert= many<<1;
  else
    nr_vert= (((field->height>>1)-field->yoffset) / size)<<1;
  
  //How many rectangular areas at all;
  nr_rects=nr_vert*nr_horiz;
  
  //First time the function is called, it 
  // has to allocated memory!
  if(start_ttc==1)
    {
      if(average!=NULL)
	free(average);
      
      for(i=0; i< MEDIAN_SIZE; i++)
	{
	  if(ttc_values[i]!=NULL)
	    freematrixf(ttc_values[i],1, nr_vert, 1, nr_horiz);
	}      
      
      if(ttc_val!=NULL)
	freematrixf(ttc_val,1, nr_vert, 1, nr_horiz);
      
      ttc_val=matrixf(1, nr_vert, 1, nr_horiz);
      
#if ONLINEPLOT
      online_plot_ttc=gnuplot_init();
      gnuplot_cmd(online_plot_ttc,"set title 'TTC - Map for %dnd row'",
		  (nr_vert>>1));
      gnuplot_cmd(online_plot_ttc,"set noautoscale y");
      //gnuplot_cmd(online_plot_ttc,"set xrange[%f:%f]",0.0, 100);
      gnuplot_cmd(online_plot_ttc,"set yrange[%d:%d]",0, 200);
#endif

      //Malloc for TTC estimates
      for(i=0; i< MEDIAN_SIZE; i++)
	ttc_values[i]=matrixf(1, nr_vert, 1, nr_horiz);
      
#if AVERAGE_TTC
      average=(unsigned char *) malloc(nr_horiz_ttc*sizeof(unsigned char));
#endif
      start_ttc=0; 
    } 
  
  //Start values
  x0=field->xoffset; x1=x0+size;
  y0=(field->height>>1)-many*size; y1=y0+size;
  
  //Build TTC - Hazard map
  for(j=1; j <= nr_vert; j++)
    {
      for(i=1; i <= nr_horiz; i++)
	{
	  //FOE stimates valid? lets get TTC
	  if(mode==2 || mode==3 || mode==5 || (mode==1 && valid==1))
	    ttc_values[median_count][j][i]=ttc(field,x0, y0, x1, y1,
					       foex, foey,
					       &variance, 0,
					       &ttc_foe, &ttc_div, 
					       &ttc_green ,
					       &ttc_pseudo, &ttc_mean, 
					       mode);
	  else if(valid!=1 && mode==0)
	    {//if not, just use Green and divergence
	      ttc_values[median_count][j][i]=ttc(field,x0, y0, x1, y1,
						 foex, foey,
						 &variance, 0,
						 &ttc_foe, &ttc_div, 
						 &ttc_green, 
						 &ttc_pseudo, &ttc_mean, 
						 5);
	    }
	  else
	    ttc_values[median_count][j][i]=0;
	  
	  x0=x1; x1=x0+size;
	}
      
      x0=field->xoffset; x1=x0+size;
      y0=y1; y1=y0+size;
    }

  //Temporal median filtering 
#if TEMPORAL_MEDIAN
  if(median_count >= (filter_size-1))
    {
      for(j=1; j <= nr_vert; j++)
	{
	  for(i=1; i <= nr_horiz; i++)
	    {
	      //Get the last 'filter_size' measurements and filter them
	      for(int u=0; u < filter_size; u++)
		tmp1[u]=ttc_values[median_count-filter_size+u+1][j][i];
	      
	      //Median filtered TTC estimates
	      ttc_val[j][i]=quick_select(tmp1,filter_size);
	    }
	}
    }
  else
#else
    {
      for(j=1; j <= nr_vert; j++)
	{
	  for(i=1; i <= nr_horiz; i++)
	    {
	      ttc_val[j][i]=ttc_values[median_count][j][i];
	    }
	}
    }
#endif

#if ONLINEPLOT
  //Online plot of TTC Values 
  double ttc_tmp[nr_horiz]; int c=0;
  double ttc_tmp_median[nr_horiz];
  for(i=1; i <= nr_horiz; i++)
    {
      ttc_tmp[c]=(double)ttc_values[median_count][nr_vert>>1][i];
      ttc_tmp_median[c++]=(double)ttc_val[nr_vert>>1][i];
    }
  c--;
  gnuplot_resetplot(online_plot_ttc);
  gnuplot_setstyle(online_plot_ttc, "impulses"); 
  gnuplot_plot1d_var1(online_plot_ttc, ttc_tmp, c,"Est: TTC");
  gnuplot_setstyle(online_plot_ttc, "points"); 
  gnuplot_plot1d_var1(online_plot_ttc, ttc_tmp, c,"");
  gnuplot_setstyle(online_plot_ttc, "impulses"); 
  gnuplot_plot1d_var1(online_plot_ttc, ttc_tmp_median, c,"Median: TTC");
  gnuplot_setstyle(online_plot_ttc, "points"); 
  gnuplot_plot1d_var1(online_plot_ttc, ttc_tmp_median, c,"");
#endif

  //////////////////
  //Draw Hazard Map
  //////////////////
  //Grey scale image -> Color
  if(p_in->format==pix_grey)
    p_in=MONO2RGB(p_in);
  
  x0=field->xoffset; x1=x0+size;
  y0=(field->height>>1)-many*size; y1=y0+size;
  int sum_col=0, nr_sum_col=0;
  //For all columns
  for(j=1; j <= nr_vert; j++)
    {//For all rows
      for(i=1; i <= nr_horiz; i++)
	{
	  if(ttc_val[j][i] > 1.0)
	    {//Scale up
	      tmp=((int) ttc_val[j][i])<<2;
	      
	      if(tmp>225.0)
		{//Too big
		  setDrawColorImageR((unsigned char) 30);
		  setDrawColorImageB((unsigned char) 30);
		  
		  setDrawColorImageG((unsigned char) 30);
		}
	      else if(tmp<5.0)
		{//Too small
		  setDrawColorImageR((unsigned char) 255);
		  setDrawColorImageB((unsigned char) 255);
		  setDrawColorImageG((unsigned char) 255);
		}		
	      else
		{//That is what we are looking for
		  setDrawColorImageR((unsigned char)(250-tmp));
		  setDrawColorImageB((unsigned char)(250-tmp));
		  setDrawColorImageG((unsigned char) 0);
		  
		  sum_col+=(unsigned char)(250-tmp);
		  nr_sum_col++;
		}
	      //Draw rectangular area 
	      fillRectInImage(p_in, x0, y0, size, size);
	    }	  
	  //calc. new column
	  x0=x1; x1=x0+size;
	} 
      //calc. new row
      x0=field->xoffset; x1=x0+size;
      y0=y1; y1=y0+size;
    }
  
#if AVERAGE_TTC 
  //Draw average TTC
  int joho=0,prev;
  if(nr_sum_col!=0)
    {
      average[loop]=sum_col/nr_sum_col;

#if PLOT_AVERAGE_TTC
      ttc_average[average_counter++]=250-average[loop];
      
      if(average_counter>197)
	average_counter=0;
#endif

    }
  else
    {
      average[loop]=0;
    }
  
  stop_counter=0; stop_ttc=0;
  for(i=0; i < nr_horiz_ttc ; i++)
    {
      //Save previous index
      prev=joho;
      //new index
      joho=(nr_horiz_ttc+i+loop+1)%nr_horiz_ttc;
      
      if(p_in->format==pix_grey)
	setDrawGValImage(average[joho]);
      else
	{ 
	  setDrawColorImageR((unsigned char) (average[joho]));
	  setDrawColorImageB((unsigned char) (average[joho]));
	  setDrawColorImageG((unsigned char) 0);
	}
      
      //TTC really to close
      if((average[joho] > 210) /*&& (average[prev] < average[joho])*/ )
	{
	  if( (stop_counter++) > thresh)
	    { //Oh, collison might be imminent!!
	      drawLineInImage(p_in,0, p_in->height,p_in->width, 0, 2);
	      drawLineInImage(p_in,0, 0, p_in->width, p_in->height, 2);
	      stop_ttc=1;
	    }
	  
	  setDrawColorImageR((unsigned char) (stop_counter*40+50));
	  setDrawColorImageB((unsigned char) 0);
	  setDrawColorImageG((unsigned char) 0);
	}
      else
	{//No, false alarm!
	  stop_counter=0;
	  stop_ttc=0;
	}
      //Update sroll bar at the top of image
      fillRectInImage(p_in, i*size, 0, size, size);
    }    
  
  //index of new entry
  loop=(loop+1)%nr_horiz_ttc;  
#endif
  
#if TEMPORAL_MEDIAN
  //Gnuplot purposes
  median_count++;
  if(median_count > MEDIAN_SIZE-1)
    {
      median_count=0;
      //fprintf(stdout,"Nah, too big, reset median counter!\n ");
    }
#endif  
  
  return stop_ttc;
}

  


