// file: fbc_filter.C
//
// Apply filter to frame of data.
//

#include "fbc.h"

  int fbc_filter_C(float* buffer_out, float frame_dur_samples, float* filtered_frame,
		   float a1, float a2, float b1, float b2,
		   float& yn1, float& yn2, float& xn1, float& xn2,
		   float& a1_prev, float& a2_prev, float& b1_prev, float& b2_prev,
		   int frame_number) {
    
    // Main Filter
    //

    //  For the first frame, set the previous coef to the current one to
    //  prevent a spike in the first frame.
    //
    if (frame_number == 0){
       a1_prev = a1;
       a2_prev = a2;
       b1_prev = b1;
       b2_prev = b2;
    }
    
    // initialize temp variables for approximations
    //
    float a1_current = 0.0;
    float a2_current = 0.0;
    float b1_current = 0.0;
    float b2_current = 0.0;
    
    float gain;
    gain = (1+a1+a2)/(1+b1+b2);
    
    for (int n = 0; n<frame_dur_samples; n++){

        
      // inplement the linear smoothing
      //
      //  y = rise/run x + b
      a1_current = (a1 - a1_prev)/(frame_dur_samples - 1) * n + a1_prev; 
      a2_current = (a2 - a2_prev)/(frame_dur_samples - 1) * n + a2_prev; 
      b1_current = (b1 - b1_prev)/(frame_dur_samples - 1) * n + b1_prev; 
      b2_current = (b2 - b2_prev)/(frame_dur_samples - 1) * n + b2_prev; 
            
      float sum = -a1_current * yn1 - a2_current * yn2 +
	          gain * buffer_out[n] + gain * b1_current * xn1
	          + gain * b2_current * xn2;
      
      /*
      float sum = -a1 * yn1 - a2 * yn2 +
	          gain * buffer_out[n] + gain * b1 * xn1
	          + gain * b2 * xn2;
      */
      
      // push delays ahead for each sample
      //
      yn2 = yn1;
      yn1 = sum;
      xn2 = xn1;
      xn1 = buffer_out[n];
      filtered_frame[n] = sum;
      
    
      //    printf ("\n");
    //    printf ("a1 = %f\n",a1);
    //    printf ("a1_prev = %f\n",a1_prev);
      //      printf ("a1_current = %f\n",a1_current);
    
      
     }

   
    
      // push approximations ahead for each sample
      //
      a1_prev = a1;
      a2_prev = a2;
      b1_prev = b1;
      b2_prev = b2;

/*
printf("a1 = %f\n", a1);
printf("a2 = %f\n", a2);
printf("b1 = %f\n", b1);
printf("b2 = %f\n", b2);
*/



//////////////////////////////////////////////////////////////////
      
/*
    // Test with no filter...
    //
    
    for (int n = 0; n<frame_dur_samples; n++){
     float sum =  buffer_out[n];
     filtered_frame[n] = sum;
     }
*/
   

    /*
    //  Whistle filter
    //
    float center_freq = 1750;
    float bandwidth = 20;
    float sample_frequency = 8000;

    // compute normalized values of bandwidth and center frequency
    //
    float bw = 2 * 3.14 * bandwidth / sample_frequency;
    float cf = 2 * 3.14 * center_freq / sample_frequency;
    
    // compute coefiecients
    //
    a1 = -2.0 * bw * cos(cf);
    b1 = -2.0 * cos(cf);
        
printf("a1 = %f\n", a1);
printf("a2 = %f\n", a2);
printf("b1 = %f\n", b1);
printf("b2 = %f\n", b2);

    
    float gain = (1+a1+a2)/(1+b1+b2);

    for (int n = 0; n<frame_dur_samples; n++){
      float sum = -a1 * yn1 - a2 * yn2 +
	          gain * buffer_out[n] + gain * b1 * xn1 + gain * b2 * xn2;
      yn2 = yn1;
      yn1 = sum;
      xn2 = xn1;
      xn1 = buffer_out[n];
      filtered_frame[n] = sum;
    }
    */

    /*
    // Sine wave filter
    //
    float center_freq = 500;
    float bandwidth = 10;
    float sample_frequency = 44100;

    // compute normalized values of bandwidth and center frequency
    //
    float bw = 2 * 3.14 * bandwidth / sample_frequency;
    float cf = 2 * 3.14 * center_freq / sample_frequency;
    
    // compute coefiecients
    //
    a1 = -2.0* bw * cos(cf);
    b1 = -2.0 * cos(cf);
    a2 = bw * bw;
    b2 = 1.0;


    
    float gain = (1+a1+a2)/(1+b1+b2);

    for (int n = 0; n<frame_dur_samples; n++){
      float sum = -a1 * yn1 - a2 * yn2 +
	          gain * buffer_out[n] + gain * b1 * xn1 + gain * b2 * xn2;
      yn2 = yn1;
      yn1 = sum;
      xn2 = xn1;
      xn1 = buffer_out[n];
      filtered_frame[n] = sum;
    }

    */
    
    // Dr. Picone's test filter
    //
/*
    for (int n = 0; n<frame_dur_samples; n++){
      float sum = -0.5 * yn1 - 0.25 * yn2 + buffer_out[n];
      yn2 = yn1;
      yn1 = sum;
      filtered_frame[n] = sum;
    }
*/

    // Bobs test filter
    //
/*
    for (int n = 0; n<frame_dur_samples; n++){
      float sum = -.25 * yn1 - .5 * yn2 + buffer_out[n] + .25 * xn1 + .5 * xn2;
      yn2 = yn1;
      yn1 = sum;
      xn2 = xn1;
      xn1 = buffer_out[n];
      filtered_frame[n] = sum;
    }
*/


  // exit gracefully
  //
  return L_TRUE;

}