/****************************************************************
*   Numerical Bandpass Filter (see demo program test_fil.cpp)   *
* ------------------------------------------------------------- *
* References:                                                   * 
*                                                               *
* http://en.wikipedia.org/wiki/Digital_biquad_filter            *  
* http://www.musicdsp.org/archive.php?classid=3#225             *  
* http://www.musicdsp.org/showone.php?id=197                    *
* http://www.musicdsp.org/files/Audio-EQ-Cookbook.txt           *
* http://www.musicdsp.org/archive.php?classid=3#225             *
* http://www.musicdsp.org/showArchiveComment.php?ArchiveID=225  *
*                                                               *
*                      Visual C++ Release By J-P Moreau, Paris. *
*                                  (www.jpmoreau.fr)            *
****************************************************************/ 
#include "filter_r.h"


float sinh(float x) {
  float expx;
  expx = (float) exp(x);
  return (float) (0.5*(expx-1.0/expx));
}

float Power(float y, float x) {
  if (x<0) 
    return 0;
  else
    return (float) exp(x*log(y));
}

void TRbjEqFilter::InitFilter(float SampleRate, int MaxBlockSize) {

  fMaxBlockSize = MaxBlockSize;
  fSampleRate = SampleRate;

  fFilterType=0;
  fFreq=500;
  fQ=(float) 0.3;
  fDBGain=0;
  fQIsBandWidth=true;

  in1=0;
  in2=0;
  ou1=0;
  ou2=0;

}

void TRbjEqFilter::SetQ(float NewQ) {
  fQ=(float) ((1-NewQ)*0.98);
}

void TRbjEqFilter::CalcFilterCoeffs(int pFilterType, float pFreq, float pQ, float pDBGain, bool pQIsBandWidth) {
  fFilterType=pFilterType;
  fFreq=pFreq;
  fQ=pQ;
  fDBGain=pDBGain;
  fQIsBandWidth=pQIsBandWidth;

  CalcFilterCoeff();
}

void TRbjEqFilter::CalcFilterCoeff() {
  float alpha,a0,a1,a2,b0,b1,b2;
  float A,beta,omega,pi,tsin,tcos;

  pi = (float) 3.1415926535;

  // peaking, LowShelf or HiShelf
  if (fFilterType>=6) {
    A=(float) pow(10.0,(fDBGain/40.0));
    omega=(float) (2*fFreq/fSampleRate);
    tsin=(float) sin(omega);
    tcos=(float) cos(omega);

    if (fQIsBandWidth)
      alpha=(float) (tsin*sinh(log(2.0)/2.0*fQ*omega/tsin));
    else
      alpha=(float) (tsin/(2.0*fQ));

    beta=(float) (sqrt(A)/fQ);

    // peaking
    if (fFilterType==6) {
      b0=1+alpha*A;
      b1=-2*tcos;
      b2=1-alpha*A;
      a0=1+alpha/A;
      a1=-2*tcos;
      a2=1-alpha/A;
    }
    else if (fFilterType==7) {  //lowshelf
      b0=(A*((A+1)-(A-1)*tcos+beta*tsin));
      b1=(2*A*((A-1)-(A+1)*tcos));
      b2=(A*((A+1)-(A-1)*tcos-beta*tsin));
      a0=((A+1)+(A-1)*tcos+beta*tsin);
      a1=(-2*((A-1)+(A+1)*tcos));
      a2=((A+1)+(A-1)*tcos-beta*tsin);
    }
    else if (fFilterType==8) { //hishelf
      b0=(A*((A+1)+(A-1)*tcos+beta*tsin));
      b1=(-2*A*((A-1)+(A+1)*tcos));
      b2=(A*((A+1)+(A-1)*tcos-beta*tsin));
      a0=((A+1)-(A-1)*tcos+beta*tsin);
      a1=(2*((A-1)-(A+1)*tcos));
      a2=((A+1)-(A-1)*tcos-beta*tsin);
    }
	else ;
  }
  else { //other filter types
    omega=2*pi*fFreq/fSampleRate;
    tsin=(float)sin(omega);
    tcos=(float)cos(omega);
    if (fQIsBandWidth)
      alpha=(float) (tsin*sinh(log(2)/2*fQ*omega/tsin));
    else
      alpha=(float) (tsin/(2*fQ));
    //lowpass
    if (fFilterType==0) {
      b0=(1-tcos)/2;
      b1=1-tcos;
      b2=(1-tcos)/2;
      a0=1+alpha;
      a1=-2*tcos;
      a2=1-alpha;
    }
	else if (fFilterType==1) { //highpass
      b0=(1+tcos)/2;
      b1=-(1+tcos);
      b2=(1+tcos)/2;
      a0=1+alpha;
      a1=-2*tcos;
      a2=1-alpha;
    } 
    else if (fFilterType==2) { //bandpass CSG
      b0=tsin/2;
      b1=0;
      b2=-tsin/2;
      a0=1+alpha;
      a1=-1*tcos;
      a2=1-alpha;
    }
    else if (fFilterType==3) { //bandpass CZPG
      b0=alpha;
      b1=0;
      b2=-alpha;
      a0=1+alpha;
      a1=-2*tcos;
      a2=1-alpha;
    }
	else if (fFilterType==4) { //notch
      b0=1;
      b1=-2*tcos;
      b2=1;
      a0=1+alpha;
      a1=-2*tcos;
      a2=1-alpha;
    }
	else if (fFilterType==5) { //allpass
      b0=1-alpha;
      b1=-2*tcos;
      b2=1+alpha;
      a0=1+alpha;
      a1=-2*tcos;
      a2=1-alpha;
    }
	else ;
  } //emse

  b0a0=b0/a0;
  b1a0=b1/a0;
  b2a0=b2/a0;
  a1a0=a1/a0;
  a2a0=a2/a0;

} //CalcFilterCoeff()


float TRbjEqFilter::Process1(float input) {
  float LastOut;
  //filter
  LastOut = b0a0*input + b1a0*in1 + b2a0*in2 - a1a0*ou1 - a2a0*ou2;

  //push in/out buffers
  in2=in1;
  in1=input;
  ou2=ou1;
  ou1=LastOut;

  return LastOut;

}

/* Note:
use Process1(input:single):single;
for per sample processing
use Process(Input:psingle;sampleframes:integer);
for block processing. The input is a pointer to
the start of an array of single which contains
the audio data.
i.e.
RBJFilter.Process(@WaveData[0],256);
*/

void TRbjEqFilter::Process(float *input, int Sampleframes) {
  int i;
  float LastOut;

  void *vmblock1;

// Allocate memory for vector Out1
  vmblock1 = vminit();
  out1 = (float *) vmalloc(vmblock1, VEKTOR, Sampleframes, 0);

  for (i=0; i<Sampleframes; i++) {
    // filter
    LastOut = b0a0*(input[i])+ b1a0*in1 + b2a0*in2 - a1a0*ou1 - a2a0*ou2;
//  LastOut=input[i];
//  push in/out buffers
    in2=in1;
    in1=input[i];
    ou2=ou1;
    ou1=LastOut;

    out1[i]=LastOut;

  }
  vmfree(vmblock1);

}

// end of file filter_r.cpp