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

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

#include "motion_field.h"
#include "gnuplot_i.h"
#include "ttc.h"

//malloc vector field
vec_field *allocVec_field()
{
  return (vec_field *)malloc(sizeof(vec_field));
}

//initialise vector field
vec_field *initVec_field(vec_field *field)
{
  if (field)
    {
      field->width  = 0;
      field->height = 0;
      
      field->xoffset  = 0;
      field->yoffset  = 0;
      
      field->u      = NULL;
      field->v      = NULL;
      
      field->valid  = NULL;
    }
  return field;
}

//new vector field 
vec_field *newVec_field()
{
  return initVec_field(allocVec_field());
}

void freeVec_field(vec_field *field)
{
  if (field)
    {
      resizeVec_field(field, 0, 0, 0, 0);
      free(field);
      field=NULL;
    }
}

//Malloc for data in vector field 
int resizeVec_field(vec_field *field, 
		    unsigned int width, 
		    unsigned int height, 
		    unsigned int xoffset,
		    unsigned int yoffset)
{
  if (field)
  {
    
    if ((width>0) && (height>0) )
      {
      
	field->width  = width;
	field->height = height;
       
	field->xoffset  = xoffset;
	field->yoffset  = yoffset;
	
	field->datasize=field->width*field->height;      
	
	if (field->u!=NULL)
        field->u=(float *)
	  realloc(field->u,field->datasize * 
		  sizeof(field->u));
      else
        field->u=(float *)
	  malloc(field->datasize * sizeof(field->u));      
	
	if (field->v!=NULL)
	  field->v=(float *)
	  realloc(field->v,field->datasize * 
		  sizeof(field->v));
	else
	  field->v=(float *)
	    malloc(field->datasize * sizeof(field->v));
	
	if (field->valid!=NULL)
	  field->valid=(unsigned char *)
	    realloc(field->valid,field->datasize * 
		    sizeof(field->valid));
	else
	  field->valid=(unsigned char *)
	    malloc(field->datasize * sizeof(field->valid));
	
	//Malloc or realloc failed!
	if (!(field->u) || !(field->v) || !(field->valid))
	  {
	    field->width    = 0;
	    field->height   = 0;
	    field->datasize = 0;
	    field->u=NULL;
	    field->v=NULL;
	    field->valid=NULL;
	    return(0);
	  }
	//Malloc success
	return(1);
      }
    else
      {
	field->width  = 0;
	field->height = 0;
	
	field->xoffset  = 0;
	field->yoffset  = 0;
	
	field->datasize = 0;
	
	if (field->u!=NULL) 
	  free(field->u);
	
	if (field->v!=NULL) 
	  free(field->v);
	
	if (field->valid!=NULL) 
	  free(field->valid);
	
	field->u = NULL;
	field->v = NULL;
	field->valid = NULL;
      }      
  }
  return(0);
}

//Set all valid flags to zero
void clearVec_field(vec_field *field)
{
  if (field)
    memset(field->valid,'\0', field->datasize*sizeof(field->valid)); 	
}

//Draw an vector_field
void draw_motion_field(Picture  *p_image, 
		       vec_field *field, 
		       int draw_mode, int normalize, 
		       int stepx, int stepy)
{
  unsigned int i,anzahl=0,x,y;
  float norm,min=1000.0,max=0.0;
  float col,ave;
  //FILE *fp;
  //fp = fopen("uv.txt", "wb");

  if(stepx<=0 || stepy<=0 )
    {
      stepx=1; stepy=1;
    }
  
  if(normalize==1)
    {
      //Scale vector
      ave=0.0;
      for(i=0; i < field->datasize; i++)
	{
	  if(field->valid[i]==1)
	    {
	      norm = sqrt(field->u[i] * field->u[i] + 
			  field->v[i] * field->v[i]);
	      
	      if(max < norm)
		max=norm; 
	      
	      if(min > norm)
		min=norm;
	      ave+=norm;
	      anzahl++;
	    }
	}
      ave/=anzahl;
    }
    
  //fprintf(stdout,"min:%f max:%f average:%f\n",min,max,ave);

  if(draw_mode==3)
    clearImage_white(p_image);
  
  for(y=field->yoffset; y < field->height-field->yoffset; y+=stepy)
    {
      for(x=field->xoffset; x < field->width-field->xoffset; x+=stepx)
	{
	  i=y * field->width + x;
	  
	  if(field->valid[i]==1)
	    {
	      switch(draw_mode)
		{
		  //Needle Plot
		case(0):
		  {
		    
		    setDrawColorImageR((unsigned char) 250);
		    setDrawColorImageB((unsigned char) 0);
		    setDrawColorImageG((unsigned char) 0);
		    
		    //Draw optic flow vector
		    PutBigPixelInImage(p_image,x,y,1);
		    
		    // col=(float) (10.0 * 
		    //sqrt(field[i].u*field[i].u+field[i].v*field[i].v) * ave );
		    //fprintf(fp,"%2.3f %2.3f\n",field->u[i],field->v[i]);
		    //if((col=stepx-3) < 0)
		     col=5;
		    //col=5.0 / sqrt(field->u[i]*field->u[i]+field->v[i]*field->v[i]);
		    //Scale vector
		    drawLineInImage(p_image,x,y,
				    (x+(int) (field->u[i] * col)),
				    (y+(int) (field->v[i] * col)),1);	
		  } 
		  break;
		  
		  //Magnitude Plot
		case(1):
		  {
		    col=(float) (200.0 * sqrt(field->u[i] * field->u[i] + 
					      field->v[i] * field->v[i]) / (max-min) );
		    if(p_image->format==pix_rgb24)
		      {
			setDrawColorImageR((unsigned char) 0);
			setDrawColorImageB((unsigned char) (col+50.0));
			setDrawColorImageG((unsigned char) 0);
		      }
		    else
		      setDrawGValImage((unsigned char) (col+50.0));
		    
		    PutPixelInImage(p_image,x,y);
		  } break;
		  
	      //Angle plot
		case(2):
		  {
		    col = 200.0 * atan(field->v[i]/field->u[i]) / 3.14;
		    
		    setDrawColorImageR((unsigned char) 0);
		    setDrawColorImageB((unsigned char) 0);
		    setDrawColorImageG((unsigned char) (col+50.0));
		    
		    PutPixelInImage(p_image,x,y);
		  } break;
		  
		case(3):
		  {
		    setDrawGValImage(150);
		    //Draw optic flow vector
		    PutBigPixelInImage(p_image,x,y,1);
		    col=1;
		    //Scale vector
		    drawLineInImage(p_image,x,y,
				    (x+(int) (field->u[i]*col) ),
				    (y+(int) (field->v[i]*col) ),1);	
		  } break;
		
		}
	    }
	}  
    }
  //fclose(fp);
  
}

//Copy an vector field
vec_field *copy_vec_field(vec_field *in_field)
{
  vec_field *tmp=NULL;
  
  if(in_field)
    {
      tmp=newVec_field();
      
      resizeVec_field(tmp,
		      in_field->width,in_field->height,
		      in_field->xoffset,in_field->yoffset);
  
      //Copy all values
      memcpy(tmp->u,in_field->u,
	     in_field->datasize * sizeof(in_field->u)); 
      
      memcpy(tmp->v,in_field->v,
	     in_field->datasize * sizeof(in_field->v));
      
      memcpy(tmp->valid,
	     in_field->valid,in_field->datasize * sizeof(in_field->valid));
    }
  
  return tmp;
}




//Gnuplot
#ifndef NPOINTS
#define NPOINTS 300
#endif
static double foe_values_x[NPOINTS];
static double foe_values_y[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_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];
#define ROTATION 0
#define MEDIAN   0
//Plot some graphs!
int plotting_ego(int *counter,//Gimme a global counter!
		 float foe_x,float foe_y,
		 float foe_x_kal,float foe_y_kal,
		 float Omx_est, float Omy_est, float Omz_est,
		 float Omx_est_kal, float Omy_est_kal, float Omz_est_kal,
		 int force_plot)
{
  
  if(*counter < NPOINTS && force_plot==0)
    {
      foe_values_x[*counter]=(double)foe_x;
      foe_values_y[*counter]=(double)foe_y;
      foe_values_x_kal[*counter]=(double)foe_x_kal;
      foe_values_y_kal[*counter]=(double)foe_y_kal;
      
      Omx_values[*counter]=(double)Omx_est; 
      Omy_values[*counter]=(double)Omy_est; 
      Omz_values[*counter]=(double)Omz_est;
      
      Omx_values_kal[*counter]=(double)Omx_est_kal;
      Omy_values_kal[*counter]=(double)Omy_est_kal;
      Omz_values_kal[*counter]=(double)Omz_est_kal;

#if MEDIAN
      //Median filtered version
      int median_size=11; 
      float tmp1[median_size]; float tmp2[median_size];
      float tmp1_omx[median_size]; 
      float tmp1_omy[median_size]; 
      float tmp1_omz[median_size];
      
      if(*counter >= (median_size-1))
	{
	  //median destroys my field :(
	  for(int i=0; i<median_size; i++)
	    {
	      tmp1[i]=foe_values_x[(*counter)-median_size+i+1];
	      tmp2[i]=foe_values_y[(*counter)-median_size+i+1];
	      
	      tmp1_omx[i]=Omx_values[(*counter)-median_size+i+1];
	      tmp1_omy[i]=Omy_values[(*counter)-median_size+i+1];
	      tmp1_omz[i]=Omz_values[(*counter)-median_size+i+1];
	    }
	  
	  foe_values_x_median[*counter]=(double)
	    quick_select(tmp1, median_size);
	  
	  foe_values_y_median[*counter]=(double)
	    quick_select(tmp2, median_size);
	  
	  Omx_values_median[*counter]=(double)
	    quick_select(tmp1_omx, median_size);

	  Omy_values_median[*counter]=(double)
	    quick_select(tmp1_omy, median_size);
	  
	  Omz_values_median[*counter]=(double)
	    quick_select(tmp1_omz, median_size); 
	}      
      else
	{
	  foe_values_x_median[*counter]=0.0;
	  foe_values_y_median[*counter]=0.0;
	  
	  Omx_values_median[*counter]=0.0; 
	  Omy_values_median[*counter]=0.0; 
	  Omy_values_median[*counter]=0.0;
	}
#endif //MEDIAN
      
      (*counter)++;
    }
  else
    {
      (*counter)--;
      
      //FOE X
      gnuplot_ctrl  *foe_x_plot; 
      foe_x_plot = gnuplot_init(0);
      //      gnuplot_cmd(foe_x_plot,"set title 'Avg Error:%2.3f  StdDev:%1.4f  Density:%2.3f  Outlier:%2.3f '",average,std_dev,density,outlier);
      gnuplot_setstyle(foe_x_plot, "linespoints");
      gnuplot_plot1d_var1(foe_x_plot, foe_values_x, 
			  *counter, "Est. Tx/Tz ") ;
      gnuplot_plot1d_var1(foe_x_plot, 
			  foe_values_x_kal, 
			  *counter, "Kalman: Tx/Tz ") ;
#if MEDIAN
      gnuplot_plot1d_var1(foe_x_plot, 
			  foe_values_x_median, 
			  *counter, "Median: Tx/Tz ") ;
#endif
      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 'Avg Error:%2.3f  StdDev:%1.4f  Density:%2.3f  Outlier:%2.3f '",average,std_dev,density,outlier);
      gnuplot_setstyle(foe_y_plot, "linespoints");
      gnuplot_plot1d_var1(foe_y_plot, foe_values_y, 
			  *counter, "Est. Ty/Tz ") ;
      gnuplot_plot1d_var1(foe_y_plot, 
			  foe_values_y_kal, 
			  *counter, "Kalman: Ty/Tz ") ;  
#if MEDIAN
      gnuplot_plot1d_var1(foe_y_plot, 
			  foe_values_y_median, 
			  *counter, "Median: Ty/Tz ") ;
#endif
      save_plot(foe_y_plot,"FOE_y.ps");	

#if ROTATION      
      //Omx
      gnuplot_ctrl  *Omx_plot;
      Omx_plot   = gnuplot_init(); 
      //gnuplot_cmd(Omx_plot,"set title 'Avg Error:%2.3f  StdDev:%1.4f  Density:%2.3f  Outlier:%2.3f '",average,std_dev,density,outlier);
      gnuplot_setstyle(Omx_plot, "linespoints");
      gnuplot_plot1d_var1(Omx_plot, Omx_values, 
			  *counter, "Est. Omega x ") ;
      gnuplot_plot1d_var1(Omx_plot, Omx_values_kal, 
			  *counter, "Kalman: Omega x ");
      gnuplot_plot1d_var1(Omx_plot, 
			  Omx_values_median, 
			  *counter, "Median: Omega x ") ;
      save_plot(Omx_plot,"Omega_x.ps");	
      
      //Omy
      gnuplot_ctrl  *Omy_plot;
      Omy_plot   = gnuplot_init();
      //gnuplot_cmd(Omy_plot,"set title 'Avg Error:%2.3f  StdDev:%1.4f  Density:%2.3f  Outlier:%2.3f '",average,std_dev,density,outlier);
      gnuplot_setstyle(Omy_plot, "linespoints");
      gnuplot_plot1d_var1(Omy_plot, Omy_values, 
			  *counter, "Est. Omega y ") ;
      gnuplot_plot1d_var1(Omy_plot, Omy_values_kal, 
			  *counter, "Kalman: Omega y "); 
      gnuplot_plot1d_var1(Omy_plot, 
			  Omy_values_median, 
			  *counter, "Median: Omega y ") ;
      save_plot(Omy_plot,"Omega_y.ps");	
      
      //Omz 
      gnuplot_ctrl  *Omz_plot;
      Omz_plot   = gnuplot_init();
      //gnuplot_cmd(Omz_plot,"set title 'Avg Error:%2.3f  StdDev:%1.4f  Density:%2.3f  Outlier:%2.3f '",average,std_dev,density,outlier);
      gnuplot_setstyle(Omz_plot, "linespoints");
      gnuplot_plot1d_var1(Omz_plot, Omz_values, 
			  *counter, "Est. Omega z "); 
      gnuplot_plot1d_var1(Omz_plot, Omz_values_kal, 
			  *counter, "Kalman: Omega z ");
      gnuplot_plot1d_var1(Omz_plot, 
			  Omz_values_median, 
			  *counter, "Median: Omega z ") ;
      save_plot(Omz_plot,"Omega_z.ps");	
#endif
      
      //Wait a sec
      sleep(5);
      gnuplot_close(foe_x_plot);
      gnuplot_close(foe_y_plot);
#if ROTATION
      sleep(3);
      gnuplot_close(Omx_plot); 
      gnuplot_close(Omy_plot); 
      gnuplot_close(Omz_plot);
#endif
      
      return 1;
    } 

  return 0;
}

//Draws the rotation parameters
int draw_rotation(Picture *p_output,
		  float Omx_est, float Omy_est, float Omz_est, 
		  int factor, float thresh, int jiz)
{
  int number,side;
   
  //Calculate the image center
  int xCenter=p_output->width>>1;
  int yCenter=p_output->height>>1;
  
  //Visualize Omega y
  if(FABS(Omy_est) > thresh )
    {
      number=((int) FABS(Omy_est)*factor);
      
      if(Omy_est>0.0)
	side=0;
      else
	side=p_output->width-jiz;
      
      for(int i=0; i< (number>>1);i++)
	{
	  setDrawColorImageG((unsigned char) 255-255/(number>>1)*i);
	  setDrawColorImageB((unsigned char) 0);
	  setDrawColorImageR((unsigned char) 0);
	  
	  fillRectInImage(p_output, side, 
			  yCenter-i*jiz, 
			  jiz, jiz);
	}
      
      for(int i=0; i< (number>>1);i++)
	{
	  setDrawColorImageG((unsigned char) 255-255/(number>>1)*i);
	  setDrawColorImageB((unsigned char) 0);
	  setDrawColorImageR((unsigned char) 0);
	  
	  fillRectInImage(p_output, side, 
			  yCenter+i*jiz, 
			  jiz, jiz);
	  //fillRectInImage(p_output, side, 
	  //	  p_output->height-(jiz<<1)-i*jiz, 
	  //		  jiz, jiz);
	}
      
    }

  //Visualize Omega x
  if(FABS(Omx_est) > thresh )
    {
      number=((int) FABS(Omx_est)*factor);
      
      if(Omx_est>0.0)
	side=0;
      else
	side=p_output->height-jiz;
      
      for(int i=0; i< (number>>1);i++)
	{
	  setDrawColorImageG((unsigned char) 255-255/(number>>1)*i);
	  setDrawColorImageB((unsigned char) 0);
	  setDrawColorImageR((unsigned char) 0);
		  
	  fillRectInImage(p_output, xCenter-i*jiz, 
			  side, 
			  jiz, jiz);
	}
      
      for(int i=0; i< (number>>1);i++)
	{
	  setDrawColorImageG((unsigned char) 255-255/(number>>1)*i);
	  setDrawColorImageB((unsigned char) 0);
	  setDrawColorImageR((unsigned char) 0);
	  
	  fillRectInImage(p_output, xCenter+i*jiz, 
			  side, 
			  jiz, jiz);
	}
      
    }
  
  //Visualize Omega z
  if(FABS(Omz_est) > thresh )
    {
      number=((int) FABS(Omz_est)*factor);
      
      if(Omx_est>0.0)
	side=0;
      else
	side=1;
      
      for(int i=0; i< (number>>1);i++)
	{
	  setDrawColorImageG((unsigned char) 255-255/(number>>1)*i);
	  setDrawColorImageB((unsigned char) 0);
	  setDrawColorImageR((unsigned char) 0);
	  
	  if(side==1)
	    fillRectInImage(p_output, p_output->width-i*jiz-jiz, 
			    jiz, 
			    jiz, jiz);
	  else
	    fillRectInImage(p_output, jiz, 
			    p_output->height+i*jiz, 
			    jiz, jiz);
	
	  if(side==1)
	    fillRectInImage(p_output, p_output->width+i*jiz, 
			    p_output->height-jiz, 
			    jiz, jiz);
	  else
	    fillRectInImage(p_output,p_output->width-jiz, 
			    p_output->height-i*jiz-jiz, 
			    jiz, jiz);
	  
	}
      
    }
  
  return 1;  
}

#define TRACE 100
#define TOOBIIG 200
static int x_foe_liu[TRACE]={0}, y_foe_liu[TRACE]={0};
static int N_liu=0;
int draw_foe(Picture *p_output, float foe_x_kal, float foe_y_kal)
{
  //Calculate the image center
  int xCenter=p_output->width>>1;
  int yCenter=p_output->height>>1;
  
  if(FABS(foe_x_kal)<TOOBIIG && FABS(foe_y_kal)<TOOBIIG)
    {
      /////////////////////////
      //Draw the FOE estimates
      /////////////////////////
      x_foe_liu[N_liu]=(int)(foe_x_kal+xCenter);
      y_foe_liu[N_liu]=(int)(-foe_y_kal+yCenter);
      
      //Draw estimated FOE
      int factor_N= (int) 240 / TRACE; 
      int foe_entry=0;
      for(int ii = 0; ii < (TRACE-1); ii++)
	{
	  setDrawColorImageR((unsigned char) (factor_N * ii+15) );
	  setDrawColorImageB((unsigned char) 0);
	  setDrawColorImageG((unsigned char) (factor_N * ii+15));
	  
	  foe_entry=(TRACE+ii+N_liu+1)%TRACE;
	  // Draw the trace
	  drawLineInImage(p_output, x_foe_liu[foe_entry], 
			  y_foe_liu[foe_entry], 
			  x_foe_liu[(foe_entry+1)%TRACE], 
			  y_foe_liu[(foe_entry+1)%TRACE],1);
	}
      
      PutBigPixelInImage(p_output, x_foe_liu[(foe_entry+1)%TRACE], 
			 y_foe_liu[(foe_entry+1)%TRACE], 3);       
      
      //Increase FOE ring buffer counter
      N_liu=(N_liu+1)%TRACE;
      
    }
}

int draw_foe_bw(Picture *p_output, float foe_x_kal, float foe_y_kal)
{
  //Calculate the image center
  int xCenter=p_output->width>>1;
  int yCenter=p_output->height>>1;
  
  if(N_liu==0)
    {
      for(int ii = 0; ii < (TRACE); ii++)
	{
	  x_foe_liu[ii]=xCenter;
	  y_foe_liu[ii]=yCenter; 
	}
    }
  /////////////////////////
  //Draw the FOE estimates
  /////////////////////////
      x_foe_liu[N_liu]=(int)(foe_x_kal+xCenter);
      y_foe_liu[N_liu]=(int)(-foe_y_kal+yCenter);
      
      //Draw estimated FOE
      int factor_N= (int) 240 / TRACE; 
      int foe_entry=0;
      for(int ii = 0; ii < (TRACE-1); ii++)
	{
	  //setDrawGValImage((unsigned char) (255-factor_N * ii-15) );
	  setDrawGValImage(0);

	  foe_entry=(TRACE+ii+N_liu+1)%TRACE;
	  // Draw the trace
	  drawLineInImage(p_output, x_foe_liu[foe_entry], 
			  y_foe_liu[foe_entry], 
			  x_foe_liu[(foe_entry+1)%TRACE], 
			  y_foe_liu[(foe_entry+1)%TRACE],1);
	}
      
      PutBigPixelInImage(p_output, x_foe_liu[(foe_entry+1)%TRACE], 
			 y_foe_liu[(foe_entry+1)%TRACE], 4);       
          
  //Increase FOE ring buffer counter
  N_liu=(N_liu+1)%TRACE;
  
}

#define MAX_CALL 6
static Picture *p_tmp[MAX_CALL];
Picture *subsample_pic(Picture *p_input, Picture *p_output,
		       int factor, int nr)
{ 
  int orig_width=p_input->width;
  int orig_height=p_input->height;
  int ii,jj;
  unsigned char *ptr1, *ptr2;
  
  if(nr>=MAX_CALL)
    {
      fprintf(stdout,"Too biggie\n");
      nr=0;
    }
  
  if(!p_tmp[nr] || p_tmp[nr]->width  != (orig_width>>1) || 
     p_tmp[nr]->height != (orig_height>>1))
    {
      p_tmp[nr]=newImage();
      setImageType(p_tmp[nr], pix_grey);
      
      if(factor==2)
	resizeImage(p_tmp[nr], orig_width>>1, orig_height>>1);
      else    
	resizeImage(p_tmp[nr], orig_width>>2, orig_height>>2); 
    }
  
  if(p_input->width%2!=0)
    orig_width+=1;
  
  ptr1=p_tmp[nr]->data;
  ptr2=p_input->data;
  
  if(p_input->format==pix_rgb24)
    {
      if(factor==2) 
	{	
	  for(ii=0; ii< p_tmp[nr]->height; ii++ )
	    {
	      for(jj=0; jj < p_tmp[nr]->width; jj++ )
		{
		  *ptr1=(*(ptr2)   + *(ptr2+1) + *(ptr2+2)     +
			 *(ptr2+3) + *(ptr2+4) + *(ptr2+5)     + 
			 *(ptr2+    p_input->bytes_per_line)   + 
			 *(ptr2+ 1+ p_input->bytes_per_line)   + 
			 *(ptr2+ 2+ p_input->bytes_per_line)   +
			 *(ptr2+ 3+ p_input->bytes_per_line)   + 
			 *(ptr2+ 4+ p_input->bytes_per_line)   + 
			 *(ptr2+ 5+ p_input->bytes_per_line) )>>4; 
		  
		  ptr2+=6; ptr1++;
		} 
	      ptr2+=p_input->bytes_per_line;
	    } 
	}
      else
	{  
	  for(ii=0; ii< p_tmp[nr]->height; ii++ )
	    {
	      for(jj=0; jj < p_tmp[nr]->width; jj++ )
		{
		  *ptr1=(*(ptr2)     + *(ptr2+1)  + *(ptr2+2) +
			 *(ptr2+3)   + *(ptr2+4)  + *(ptr2+5) +
			 *(ptr2+6)   + *(ptr2+7)  + *(ptr2+8) +
			 *(ptr2+9)   + *(ptr2+10) + *(ptr2+11) +
			 *(ptr2+    p_input->bytes_per_line)   + 
			 *(ptr2+ 1+ p_input->bytes_per_line)   + 
			 *(ptr2+ 2+ p_input->bytes_per_line)   +
			 *(ptr2+ 3+ p_input->bytes_per_line)   + 
			 *(ptr2+ 4+ p_input->bytes_per_line)   + 
			 *(ptr2+ 5+ p_input->bytes_per_line)   +
			 
			 *(ptr2+ 6+ p_input->bytes_per_line)   + 
			 *(ptr2+ 7+ p_input->bytes_per_line)   +
			 *(ptr2+ 8+ p_input->bytes_per_line)   + 
			 *(ptr2+ 9+ p_input->bytes_per_line)   + 
			 *(ptr2+ 10+ p_input->bytes_per_line)  +
			 *(ptr2+ 11+ p_input->bytes_per_line))>>5; 
		  
		  ptr2+=12;  
		  ptr1++;
		}
	      ptr2+=(p_input->bytes_per_line<<2);
	    }
	}
    }
  else
    {
      for(ii=0; ii< p_tmp[nr]->height; ii++ )
	{
	  for(jj=0; jj < p_tmp[nr]->width; jj++ )
	    {
	      *ptr1=(*(ptr2) + *(ptr2+1) + 
		     *(ptr2 +     orig_width)+ 
		     *(ptr2 + 1 + orig_width))>>2;
	      ptr1++;
	      ptr2+=2;
	    }
	  ptr2+=orig_width;
	}
    }
  
  if(p_output->width!=p_tmp[nr]->width || p_output->height!=p_tmp[nr]->height)
    {
      freeImage(p_output);
      p_output=copyImage_cover(p_tmp[nr]);
    }
  
  return p_tmp[nr];
}  




