/* 
 * Thomas Pantzer: filters.cc, v0.4.7  8.6.99 16:32 
 * 
 * 
 * Copyright 1999 by Thomas Pantzer
 * 
 * Permission to use, copy, modify, and distribute this software for noncommercial 
 * use and without fee is hereby granted, provided that the above copyright notice 
 * appear in all copies and that both that copyright notice and this permission 
 * notice appear in supporting documentation, and that the name of the Author not be 
 * used in advertising or publicity pertaining to distribution of the software 
 * without specific, written prior permission. The Author makes no representations about
 * the suitability of this software for any purpose.  It is provided "as is" 
 * without expressed or implied warranty. 
 * 
 * 
 * pantec@isys2.informatik.uni-leipzig.de   (Thomas Pantzer) 
 * 
 */




#include "copyright.h"

#include "khepera.h"
#include "filters.h"
#include "fft.h"
#include <math.h>
#include "forms.h"
#include "new_panel.h"
//#include "stuff.h"


#include "stuff.h"

void Clipping(short int *A, int len)
{
       int max=0;
       int min=16384;	   

#ifdef DEBUG
  fprintf(stderr,"Clipping()\n"); 
#endif    
       
       for (int j=0; j<len; j++)		// get max
         {
          if (max < A[j])
             max = A[j];

          if (min > A[j])
             min = A[j];
         } 
	
       int s = max - min;             

       if (s == 0)
	  return; 	

       for (int j=0; j<len; j++)		// clipping values lower than 10%
//         if (A[j] < (short int)((double)max * level) )
//            A[j] = 0;
//           else
//            A[j] = (short int) ( (double)A[j] * 255.0 / (double)max );
         A[j] = ((A[j] - min) * 255) / s;
}



void HighPass(short int *A, int len, int offs)
{
#define mask_size 5
//  short int mask[3] = {-1,2,-1};
  short int mask[mask_size] = {-1,-1,4,-1,-1};
  int S;
  
#ifdef DEBUG
  fprintf(stderr,"HighPass()\n"); 
#endif    
  int min = 16384;

  for (int j=0; j<len; j++)		// get max
     if (min > A[j])
        min = A[j];

  int range = (int)(mask_size / 2); 

  for (int k=0; k<range; k++)
    {
     A[0+offs+k] = 0;
     A[len-1+offs-k] = 0;
    }

  for (int i=range; i<len-range; i++)
    {
     S = 0;
     for (int j=0; j<mask_size; j++)
        S += (A[i+j-range] * mask[j]);

     if (S > 0)
        A[i+offs] = (short int) S;
      else
        A[i+offs] = 0;
    }

}

void LowPass(short int *A, int len, int offs)
{
#define mask_size 5
//  short int mask[3] = {-1,2,-1};
  short int mask[mask_size] = {1,2,4,2,1};
  int S;
  
#ifdef DEBUG
  fprintf(stderr,"LowPass()\n"); 
#endif    

  int range = (int)(mask_size / 2); 

  for (int k=0; k<range; k++)
    {
     A[0+offs+k] = 0;
     A[len-1+offs-k] = 0;
    }

  for (int i=range; i<len-range; i++)
    {
     S = 0;
     for (int j=0; j<mask_size; j++)
        S += (A[i+j-range] * mask[j]);

     A[i+offs] = (short int) S;
    }
  
//  fprintf(stderr,"%d\t",A[128+16]);	
}


double Median[4][64] = {
     {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,
      0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,
      0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,
      0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
     {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,
      0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,
      0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,
      0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
     {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,
      0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,
      0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,
      0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
     {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,
      0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,
      0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,
      0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0}};

void Average(short int *A, double scale, int stage)
{
	for (int i=0; i<64; i++)
          {
           Median[stage][i] += ((((double)A[i]) - Median[stage][i]) / scale);	
	   A[i] = (short int)Median[stage][i]; 
          }
}


void FilterPass(void)
{
	int start=0;


	for (int i=0; i<64; i++)
	  Robot_.Image[i+64] = Robot_.Image[i];

	for (int i=0; i<4; i++)
          {
	   int a = fl_get_choice(fd_panel->filter_ch[i]);
	   switch(a)
             {
	      case 0:
	      case 1:
                break;

	      case 2:
	        Clipping(&Robot_.Image[64],64);
//		for (int i=0; i<64; i++)
//		  Robot_.Image[i+64] = Robot_.Image[i+128];
	        break; 

	      case 3: // fft
		for (int j=0; j<64; j++)
        	  Robot_.Image[128+j] = (Robot_.Image[j+64] << 5);

		RealFFT(&Robot_.Image[128]);
		// 128..191 FFT        
		for (int j=0; j<32; j++)  
        	  {
        	   int k = (j * 2) + 64; 
        	   int n = BitReversed[j] + 128;

        	   double d = SQR(Robot_.Image[n]) + SQR(Robot_.Image[n+1]);
        	   if (d > 0.0)
        	      Robot_.Image[k] = (int)(sqrt(d));
        	    else
        	      Robot_.Image[k] = 0;   

        	   Robot_.Image[k+1] = Robot_.Image[k];      

// 		   for (int j=0; j<64; j++)
//		     Robot_.Image[j+64] = Robot_.Image[j+128];
        	  } 

		Robot_.Image[64] /= 32;  
		Robot_.Image[65] /= 32;  
		Robot_.Image[66] /= 8;  
		Robot_.Image[67] /= 8;  

		Clipping(&Robot_.Image[64],64);
	        break;
	
	      case 4: // low
//		fprintf(stderr,"lowpass\n");
	        LowPass(&Robot_.Image[64],64,64);
		start += 2;
//		fprintf(stderr,"%d\n",Robot_.Image[128+16]);

  		for (int j=0; j<64; j++)
		  Robot_.Image[j+64] = Robot_.Image[j+128];

  		Clipping(&Robot_.Image[64+2],64-4);
	        break;

	      case 5: // high
//		fprintf(stderr,"highpass\n");
	        HighPass(&Robot_.Image[64],64,64);
		start += 2;

		for (int j=0; j<64; j++)
		  Robot_.Image[j+64] = Robot_.Image[j+128];

  		Clipping(&Robot_.Image[64+2],64-4);
	        break;		
	      

	      case 6:
		fprintf(stderr,"average\n");
	        Average(&Robot_.Image[64],5.0,i);
  		Clipping(&Robot_.Image[64],64);
		
	        break;		
	     }
	  }
}









/**/
/**/
/**/
/**/
/**/
/**/
/**/
/**/
/*****  End Of File  ****************************/
