// Filtry 2 rzędu - sekcje SOS

#include <math.h>

/* Numerator coefficients */
#define B0 0
#define B1 1
#define B2 2

/* Denominator coefficients */
#define A0 3
#define A1 4
#define A2 5

// projekt filtru pasmowoprzepustowego
void wsp_SOS_BP(int fo, int B3dB, unsigned int SAMPLES_PER_SECOND, signed int *wsp) 
{
    double b = cos(2.0*3.1415927*B3dB/SAMPLES_PER_SECOND);
    double delta = 4.0 - 4.0*b*b;
    double beta = cos(2.0*3.1415927*fo/SAMPLES_PER_SECOND);
    double alfa = (2.0 - sqrt(delta))/(2.0*b);
	int K = (int)((1.0 - alfa)*(1<<14));	
		
	wsp[B0] = K ;   // B0
    wsp[B1] = 0;    // B1/2
    wsp[B2] = -K;   // B2
    
    wsp[A0] = 32767;    								// A0 = 1.0
    wsp[A1] = (int)((-beta*(1.0 + alfa))*(1 << 14));    // A1/2
    wsp[A2] = (int)(alfa*(1L<<15));   					// A2	
} 
    
// projekt filtru pasmowozaporowego
void wsp_SOS_BS(int fo, int B3dB, unsigned int SAMPLES_PER_SECOND, signed int *wsp)
{	
	double b = cos(2.0*3.1415927*B3dB/SAMPLES_PER_SECOND);
    double delta = 4.0 - 4.0*b*b;
    double beta = cos(2.0*3.1415927*fo/SAMPLES_PER_SECOND);
    double alfa = (2.0 - sqrt(delta))/(2.0*b);
    int K = (int)((1.0 + alfa)*(1<<14));
	
	wsp[B0] = K ;   			// B0
    wsp[B1] = (int)(-beta* K);  // B1/2
    wsp[B2] = K ;				// B2
    
    wsp[A0] = 32767;    								// A0 = 1.0
    wsp[A1] = (int)((-beta*(1.0 + alfa))*(1 << 14));    // A1/2
    wsp[A2] = (int)(alfa*(1L<<15));   					// A2	
}

// sekcja SOS
signed int SOS( int *x, int *y, const signed int * coefficients, signed int input)
{
  long temp;
  int output;
      
     temp =  ( (long) coefficients[B0] * input) ;   /* B0 * x(n)     */
     temp += ( (long) coefficients[B1] * x[1]);    /* B1/2 * x(n-1) */
     temp += ( (long) coefficients[B1] * x[1]);    /* B1/2 * x(n-1) */
     temp += ( (long) coefficients[B2] * x[2]);    /* B2 * x(n-2)   */
     temp -= ( (long) coefficients[A1] * y[1]);    /* A1/2 * y(n-1) */
     temp -= ( (long) coefficients[A1] * y[1]);    /* A1/2 * y(n-1) */
     temp -= ( (long) coefficients[A2] * y[2]);    /* A2 * y(n-2)   */
 
     temp = _lsshl(temp, 1);			//Zamiana Q30 na Q31 z nasyceniem
     output = _rnd(temp) >> 16;			//Zamiana Q31 na Q15 z zaokrągleniem

     /* Shuffle values along one place for next time */
  
     y[2] = y[1];   /* y(n-2) = y(n-1) */
     y[1] = output;   /* y(n-1) = y(n)   */
    
     x[2] = x[1];   /* x(n-2) = x(n-1) */
     x[1] = input;   /* x(n-1) = x(n)   */

 return output; 
}

