//
//	Creates filters to a spec
//
//        Copyright (c) 2002 Jim Peters <http://uazu.net/>.  Released
//        under the GNU GPL version 2 as published by the Free
//        Software Foundation.  See the file COPYING for details, or
//        visit <http://www.gnu.org/copyleft/gpl.html>.
//

typedef struct BandPass BandPass;
struct BandPass {
   double freq0, freq1;		// Range this filter covers (at 50% -3dB levels)
   int n_stage;			// Number of filter stages
   double fc0, fc1, adj;
   double *buf;
};

//typedef struct LowPass LowPass;
//struct LowPass {
//   double freq, Q;		// Filter 50% frequency and Q factor
//   int n_stage;			// Number of filter stages
//   double fc0, fc1, adj;
//   double *buf;
//};

void bandpass_init(BandPass *bp, double freq0, double freq1, int n_stage) ;
void bandpass_genfilt(BandPass *bp, double freq, double Q) ;
double bandpass_process(BandPass *bp, double in) ;
void bandpass_reinit(BandPass *bp) ;
double bandpass_response(BandPass *bp, double freq) ;
double biquad_response(double freq, double a0, double a1, double a2, double b0, double b1, double b2);


//
//	Calculate the response of a general biquad filter (just
//	magnitude, not phase) -- see Robert Bristow-Johnson's
//	"Cookbook Formulae":
//
//	  http://www.harmony-central.com/Computer/Programming/Audio-EQ-Cookbook.txt
//

double 
biquad_response(double freq, double a0, double a1, double a2, double b0, double b1, double b2) {
   double xx= cos(-freq * 2 * PI);
   double yy= sin(-freq * 2 * PI);
   double xx2= xx*xx - yy*yy;
   double yy2= xx*yy * 2;

   double top_xx= b0 + b1 * xx + b2 * xx2;
   double top_yy= b1 * yy + b2 * yy2;
   double bot_xx= a0 + a1 * xx + a2 * xx2;
   double bot_yy= a1 * yy + a2 * yy2;
   double mag_bot2= bot_xx * bot_xx + bot_yy * bot_yy;
   double prod_xx= (top_xx * bot_xx + top_yy * bot_yy) / mag_bot2;	// two minus signs cancel
   double prod_yy= (-top_xx * bot_yy + top_yy * bot_xx) / mag_bot2;
   double prod= hypot(prod_xx, prod_yy);
   
   return prod;
}
   

//
//	Make a band-pass filter using 'n_stage' IIR stages, putting
//	50% points at freq0 and freq1 (expressed as a proportion of
//	the sampling frequency).  Makes a guess and then adjusts the
//	parameters until the 50% points hit their targets.
//

static inline double 
bp_ave_resp(BandPass *bp, double freq, double Q) {
   bandpass_genfilt(bp, freq, Q);
   return 0.5 * (bandpass_response(bp, bp->freq0) + 
		 bandpass_response(bp, bp->freq1));
}   

static inline double 
bp_diff_resp(BandPass *bp, double freq, double Q) {
   bandpass_genfilt(bp, freq, Q);
   return (bandpass_response(bp, bp->freq1) -
	   bandpass_response(bp, bp->freq0));
}   

void 
bandpass_init(BandPass *bp, double freq0, double freq1, int n_stage) {
   double freq, f0, f1;
   double Q, Q0, Q1;
   double resp, resp0, resp1;
   double diff;

   //DEBUG printf("bandpass_init %g, %g, %d\n", freq0,  freq1, n_stage);

   if (bp->buf) free(bp->buf);
   bp->n_stage= n_stage;
   bp->buf= ALLOC_ARR(n_stage*2, double);
   bp->freq0= freq0;
   bp->freq1= freq1;

   freq= 0.5 * (freq0 + freq1);
   Q= 2.0;

   while (1) {
      // First adjust Q to get average of the two points == 50%
      
      // Look for Q so that correct value is between Q and 2Q
      while (1) {
	 resp0= bp_ave_resp(bp, freq, Q);
	 resp1= bp_ave_resp(bp, freq, Q*2);
	 if (resp0 >= 0.5 && resp1 <= 0.5) break;
	 if (resp0 < 0.5) { Q *= 0.5; continue; }
	 if (resp1 > 0.5) { Q *= 2.0; continue; }
	 error("Some problem with algorithm in bandpass_init");
      }

      // Do a binary search for the correct value within 0.1%
      Q0= Q; Q1= Q*2.0;
      while (1) {
	 Q= 0.5 * (Q0+Q1);
	 resp= bp_ave_resp(bp, freq, Q);
	 //DEBUG  printf("Q-search, %g, %g -> %g\n", freq, Q, resp);
	 if (fabs(resp - 0.5) < 0.0005) break;
	 if (resp < 0.5) Q1= Q;
	 else Q0=Q;
      }

      // Now adjust 'freq' so that the two points are at the same
      // height (within 0.1%), using a binary search.
      f0= freq0; f1= freq1;
      while (1) {
	 freq= 0.5 * (f0+f1);
	 diff= bp_diff_resp(bp, freq, Q);
	 //DEBUG  printf("freq-search, %g, %g -> %g\n", freq, Q, diff);
	 if (fabs(diff) < 0.0005) break;
	 if (diff < 0) f0= freq;
	 else f1= freq;
      }

      // If the average is still 50%, then we are done
      resp= bp_ave_resp(bp, freq, Q);
      if (fabs(resp - 0.5) < 0.0005) break;
   }
      
   bandpass_genfilt(bp, freq, Q);

   //DEBUG    {	// TESTING
   //DEBUG       double a;
   //DEBUG       for (a= 0; a <= 0.501; a += 0.02) 
   //DEBUG 	 printf("%g -> %g\n", a, bandpass_response(bp, a));
   //DEBUG    }
}

void 
bandpass_genfilt(BandPass *bp, double freq, double Q) {
   double omega= freq * 2 * PI;
   double sinv= sin(omega);
   double cosv= cos(omega);
   double alpha= sinv / (2 * Q);
   double a0= 1 + alpha;
   double a1= -2 * cosv;
   double a2= 1 - alpha;

   bp->fc0= -a1/a0;
   bp->fc1= -a2/a0;
   bp->adj= pow(1 / biquad_response(freq, -1, bp->fc0, bp->fc1, 1, 0, -1), bp->n_stage);
}

//
//	Dump the filter spec as a line to the given FILE*
//   

void 
bandpass_dump(BandPass *bp, FILE *out) {
   int a;
   for (a= 0; a<bp->n_stage; a++) {
      if (a != 0) fprintf(out, "x ");
      fprintf(out, "1 0 -1 / -1 %g %g ", bp->fc0, bp->fc1);
   }
   fprintf(out, "\n");
}


//
//	Process a sample through the bandpass filter
//

double 
bandpass_process(BandPass *bp, double in) {
   double *dp= bp->buf;
   double v0, v1, v2;
   int a= bp->n_stage;

   v0= in * bp->adj;
   while (a-- > 0) {
      v1= dp[0];
      v2= dp[1];
      v0 += v1 * bp->fc0;
      v0 += v2 * bp->fc1;
      *dp++= v0;
      *dp++= v1; 
      v0= v0 - v2;
   }
   
   return v0;
}

//
//	Reinitialise the bandpass filter
//

void 
bandpass_reinit(BandPass *bp) {
   int a;
   for (a= 0; a < bp->n_stage*2; a++) 
      bp->buf[a]= 0;
}

//
//	Test the filter response at a given frequency (expressed as a
//	proportion of the sampling frequency)
//
   
double 
bandpass_response(BandPass *bp, double freq) {
   double resp= biquad_response(freq, 
				-1, bp->fc0, bp->fc1,
				1, 0, -1);

   resp= pow(resp, bp->n_stage) * bp->adj;

   return resp;
}

// END //
