// Filtry 2 rzędu - sekcje SOS
// Współczynniki filtrów w formacie Q2.14

#include <math.h>

// Wsp. licznika
#define B0 0
#define B1 1
#define B2 2

// Wsp. mianownika
#define A0 3
#define A1 4
#define A2 5

// projekt filtru pasmowoprzepustowego
void wsp_SOS_BP_Q14(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<<13));	
		
	wsp[B0] = K ;   // B0
    wsp[B1] = 0;    // B1
    wsp[B2] = -K;   // B2
    
    wsp[A0] = 1<<15;    								// A0 = 1.0
    wsp[A1] = (int)((-beta*(1.0 + alfa))*(1 << 14));    // A1
    wsp[A2] = (int)(alfa*(1L<<14));   					// A2	
} 
    
// projekt filtru pasmowozaporowego
void wsp_SOS_BS_Q14(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<<13));
	
	wsp[B0] = K ;   				// B0
    wsp[B1] = (int)(-beta * 2 * K); // B1
    wsp[B2] = K ;					// B2
    
    wsp[A0] = 1<<15;    								// A0 = 1.0
    wsp[A1] = (int)((-beta*(1.0 + alfa))*(1 << 14));    // A1
    wsp[A2] = (int)(alfa*(1L<<14));   					// A2	
}

// sekcja SOS
signed int SOS_Q14( int *x, int *y, int * wsp, int input)
{
  long temp;
  int output;
      
     temp =  ( (long) wsp[B0] * input) ;  // B0 * x[n] , x[n] = input  */
     temp += ( (long) wsp[B1] * x[1]);    // B1 * x[n-1] */
     temp += ( (long) wsp[B2] * x[2]);    // B2 * x[n-2] */
     temp -= ( (long) wsp[A1] * y[1]);    // A1 * y[n-1] */
     temp -= ( (long) wsp[A2] * y[2]);    // A2 * y[n-2] */
 
     temp = _lsshl(temp, 2);			//Zamiana Q3.29 na Q1.31 z nasyceniem
     output = _rnd(temp) >> 16;			//Zamiana Q1.31 na Q1.15 z zaokrągleniem

     /* przesunięcie buforów */
  
     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; 
}

void wsp_LP(int f3dB, unsigned int SAMPLES_PER_SECOND, signed int *wsp)
{
	long alfa;		
    double omega3dB = 2*3.1415926*f3dB/SAMPLES_PER_SECOND;
    double ALFA = (1.0 - sin(omega3dB))/cos(omega3dB);

    alfa = (long)(ALFA * (1L << 15));
           
    wsp[B0] = wsp[B1] = (1 << 14) - (alfa >> 1);		// (1 - alfa) / 2
    wsp[A1] = alfa;                    
}

void wsp_HP(int f3dB, unsigned int SAMPLES_PER_SECOND, signed int *wsp)
{
	long alfa;		
    double omega3dB = 2*3.1415926*f3dB/SAMPLES_PER_SECOND;
    double ALFA = (1.0 - sin(omega3dB))/cos(omega3dB);

    alfa = (long)(ALFA * (1L << 15));
           
    wsp[B0] = (1 << 14) + (alfa >> 1);		// (1 + alfa) / 2
    wsp[B1] = -wsp[B0];
    wsp[A1] = alfa;                     
}

int IIR_I_order(int *buffer_X, int *buffer_Y, int *wsp, int input)
{ 
 long output;
 
 output =  (long)wsp[A1] * buffer_Y[0];  
 output += (long)wsp[B0] * input; 
 output += (long)wsp[B1] * buffer_X[0];

 output = _lsshl(output, 1);			//Zamiana Q2.30 na Q1.31 z nasyceniem
 output = _rnd(output) >> 16;			//Zamiana Q1.31 na Q1.15 z zaokrągleniem
          
 buffer_X[0] = input; 
 buffer_Y[0] = output; 

 return ((int) output);
}

int IIR_I_order______(int *buffer_X, int *buffer_Y, int *wsp, int input)
{ 
 long output;
 
// output =  (long)wsp[A1] * buffer_Y[0];  
// output += (long)wsp[B0] * input; 
// output += (long)wsp[B1] * buffer_X[0];
 
 
 output = _lsmpy(wsp[A1], buffer_Y[0]); 				// output = alfa * buffer_Y; 
 output = _smac(output, wsp[B0], input);				// output += wsp * input;
 output = _smacr(output, wsp[B1], buffer_X[0]) >> 16;	// output += _rnd(wsp * byffer_X) >> 16

 //output = _lsshl(output, 1);			//Zamiana Q2.30 na Q1.31 z nasyceniem
 //output = _rnd(output) >> 16;			//Zamiana Q1.31 na Q1.15 z zaokrągleniem
          
 buffer_X[0] = input; 
 buffer_Y[0] = output; 

 return ((int) output);
}


int IIR_I_orderLONG(int *buffer_X, int *buffer_Y, int *wsp, int input)
{ 
 long long temp;
 long outputINT;
 
 temp =  (long)wsp[A1] * buffer_Y[0];  
 temp += (long)wsp[B0] * input; 
 temp += (long)wsp[B1] * buffer_X[0];
 
 outputINT = _lsat(temp << 1);
 outputINT = _rnd(outputINT) >> 16;

 buffer_X[0] = input; 
 buffer_Y[0] = outputINT; 

 return ((int) outputINT);
}

long LP_VCO_PLL(int input, float ALFA)
{ 
 static int buffer_X;
 static int buffer_Y;
 long output;
 int alfa = (long)(ALFA * 32768L);
 int wsp =  ((1L << 15) - (long)alfa) << 4;	// wsp = (1 - alfa) * 2

 output = _lsmpy(alfa,  buffer_Y); 				// output = alfa * buffer_Y; 
 output = _smac(output, wsp, input);			// output += wsp * input;
 output = _smacr(output, wsp, buffer_X) >> 16;	// output += _rnd(wsp * byffer_X) >> 16
 //output = _rnd(output) >> 16;

 buffer_X = input; 
 buffer_Y = output; 

 return ((int) output);
}
