% Wprowadzenie do filtracji cyfrowej - filtry rekurencyjne
% filtry o nieskończonej odpowiedzi impulsowej

clear all; close all;

% Sygnał #################################################################
if(0)
   Nx = 1000; fpr1=8000; 
   x = zeros(1,Nx);
   x(1) = 1;
end    
if(0)
   fpr1 = 8000;
   Nx = 3*fpr1;
   dt = 1/fpr1; t=dt*(0:Nx-1);
   x = sin( 2*pi*200*t) + sin( 2*pi*2000*t);
end   
if(1)
   [ x, fpr1 ] = audioread('mowa.wav');
 % [ x, fpr1 ] = audioread('track1.wav');
   disp('Rozmiar audio'); size(x),
   x = x(:,1);
   Nx = length(x);
   pause
end
%figure; plot(x); pause
figure; stem(x); pause
figure;
f = fpr1/Nx * [ -Nx/2 : Nx/2-1 ];
%plot( f,20*log10( fftshift(abs(fft(x)/Nx)) )); pause
soundsc( x, fpr1 );

% Wagi filtra #############################################################

%b = [ 2 3 4 ];
%a = [ 1 2 ];

if(0)
  z = [ 1 1 ] .* exp( j*2*pi*[ 2000 3000 ]/fpr1 );
  p = [ 0.99 0.99 ] .* exp( j*2*pi*[ 100 200 ]/fpr1 );
  z = [ z conj(z) ];
  p = [ p conj(p) ];
  
  b = poly( z );
  a = poly( p );
end
if(1)
   N = 8;
   f0 = 1000;
   
   pasmo = [1000 2000]
   
  [b,a] = butter( N, pasmo/(fpr1/2) );
  [b1,a1] = cheby1( N, 3, pasmo/(fpr1/2) );
  [b2,a2] = cheby2( N, 100, pasmo/(fpr1/2) );
  [b3,a3] = ellip( N, 3, 100, pasmo/(fpr1/2) );
  
  z = roots( b );
  p = roots( a );
  z1 = roots( b1 );
  p1 = roots( a1 );
  z2 = roots( b2 );
  p2 = roots( a2 );
  z3 = roots( b3 );
  p3 = roots( a3 );
  
end

figure;
alfa = 0 : 0.01 : 2*pi; c=cos(alfa); s=sin(alfa);
%plot(c,s,'k-',real(z),imag(z),'bo',real(p),imag(p),'r*'); grid; title('Z & P'); pause



f = 0 : 1 : fpr1/2; 
zz = exp( -j*2*pi*f/fpr1);
H = polyval( b, zz ) ./polyval( a, zz );
figure; 
plot( f, 20*log10( abs( H ) ),'r' ); hold on;
H1 = polyval( b1, zz ) ./polyval( a1, zz );
plot( f, 20*log10( abs( H1 ) ),'g' ); hold on;
H2 = polyval( b2, zz ) ./polyval( a2, zz );
plot( f, 20*log10( abs( H2 ) ),'b' ); hold on;
H3 = polyval( b3, zz ) ./polyval( a3, zz );
plot( f, 20*log10( abs( H3 ) ),'y' ); xlabel('f [Hz]'); title('|H(f)| [dB]'); grid;
legend('butter','cheby1','cheby2','ellip'); hold on; pause




figure;
bode(a,b,'r'); hold on;
bode(a1,b1,'g'); hold on;
bode(a2,b2,'b'); hold on;
bode(a3,b3,'y'); 
grid; legend('butter','cheby1','cheby2','ellip'); hold on; pause


figure;
plot(f,unwrap(angle(H)),'r'); hold on;
plot(f,unwrap(angle(H1)),'g'); hold on;
plot(f,unwrap(angle(H2)),'b'); hold on;
plot(f,unwrap(angle(H3)),'y'); hold on;
xlabel('[Hz]'),ylabel('[rad/s]')
grid; legend('butter','cheby1','cheby2','ellip'); hold on; pause



% Filtracja ###############################################################
if(1)
  M = length(b);
  bx = zeros(1,M);
  N = length(a);
  a = a(2:N); 
  N = N-1;
  by = zeros(1,N);
  
  M1 = length(b1);
  bx1 = zeros(1,M1);
  N1 = length(a1);
  a1 = a1(2:N1); 
  N1 = N1-1;
  by1 = zeros(1,N1);
  
  M2 = length(b2);
  bx2 = zeros(1,M2);
  N2 = length(a2);
  a2 = a2(2:N2); 
  N2 = N2-1;
  by2 = zeros(1,N2);
  
  M3 = length(b3);
  bx3 = zeros(1,M3);
  N3 = length(a3);
  a3 = a3(2:N3); 
  N3 = N3-1;
  by3 = zeros(1,N3);
  
  
  for n = 1 : Nx
      bx = [ x(n) bx(1:M-1) ];
      y(n) = sum( bx .* b ) - sum( by .* a );
      by = [ y(n) by(1:N-1) ];
      
      bx1 = [ x(n) bx1(1:M1-1) ];
      y1(n) = sum( bx1 .* b1 ) - sum( by1 .* a1 );
      by1 = [ y1(n) by1(1:N1-1) ];
      
      bx2 = [ x(n) bx2(1:M2-1) ];
      y2(n) = sum( bx2 .* b2 ) - sum( by2 .* a2 );
      by2 = [ y2(n) by2(1:N2-1) ];
      
      bx3 = [ x(n) bx3(1:M3-1) ];
      y3(n) = sum( bx3 .* b3 ) - sum( by3 .* a3 );
      by3 = [ y3(n) by3(1:N3-1) ];
  end
else
    y = filter(b,a,x);
    y1 = filter(b1,a1,x);
    y2 = filter(b2,a2,x);
    y3 = filter(b3,a3,x);
  
end

figure
subplot(211); plot(x(1:400)); title('WE');
subplot(212); 
plot(y(1:400),'r'); hold on;
plot(y1(1:400),'g'); hold on;
plot(y2(1:400),'b'); hold on;
plot(y3(1:400),'y'); hold on;
title('WY');legend('butter','cheby1','cheby2','ellip');
pause


figure
subplot(211); plot(f,20*log10(fftshift(abs(fft(x)/length(x))))); title('FFT WE');
subplot(212); plot(f,20*log10(fftshift(abs(fft(y)/length(y))))); title('FFT WY'); pause
soundsc( y, fpr1 );