% Comment
% 
% 
% 
% 
% 
% 
% 
% 

clc;
close all
clear

fc =  1000; wc=2*pi*fc;
fs = 10000; Ts=1/fs;
f  = 1:fs/2;

B=[0 0 1];         
A=[1/wc^2 sqrt(2)/wc 1];

disp('Continuous-time system')
disp('======================')
disp(' ')

SYS_c = tf(B,A)
[MAG0,PHASE0]=bode(SYS_c,2*pi*f);

semilogx(f, 20*log10(abs(squeeze(MAG0))+eps), 'b.-')

[Z_c,P_c,K_c]=tf2zp(B,A); % Attention to the vector sizes with Matlab


disp(' ')
disp('First discretization')
disp('====================')
disp(' ')

Infinite_zero_val = -1e50;  % Inf not ok for bilinear: 1e308 max
Z_c=[1; 1]*Infinite_zero_val;

P_d=exp(P_c*Ts);
Z_d=exp(Z_c*Ts);

[B_d,A_d]=zp2tf(Z_d, P_d, 1);


K_d = K_c*prod((1-exp(P_c*Ts))./ P_c);                                      % (Eq. 1) for LP (see comment above)
K_d = real(K_d); % K_d could be complex by arithmetic roundings
B_d_scaled = B_d * K_d;

hold on
SYS1_d = tf(B_d_scaled,A_d, Ts)
[MAG1,PHASE1]=bode(SYS1_d, 2*pi*f);
semilogx(f, 20*log10(abs(squeeze(MAG1))+eps),'r')
clear B_d A_d

P_d
Z_d


disp(' ')
disp('Bilinear discretization')
disp('=======================')
disp(' ')


Z_c  = Z_c(isfinite(Z_c));
Zf_c = Z_c;              
Z_c  =[Z_c; Infinite_zero_val*ones(length(P_c)-length(Z_c),1)]

P_d = (1+P_c*Ts/2)./(1-P_c*Ts/2);

Z_d = (1+Z_c*Ts/2)./(1-Z_c*Ts/2);


[B_d,A_d]=zp2tf(Z_d, P_d,1);



if min(abs(Zf_c)) >= 1e50                % Replacement of artificial
    Zf_c = [];
end
K_d=K_c*prod(2/Ts-Zf_c)/prod(2/Ts-P_c);  % (Eq. 5) for notch, HP, BP (and LP with empty zero matrix) (see comment above)
K_d = real(K_d); % K_d could be complex by arithmetic roundings

B_d = B_d * K_d;

hold on
SYS2_d = tf(B_d,A_d, Ts)
[MAG2,PHASE2]=bode(SYS2_d, 2*pi*f);
semilogx(f, 20*log10(abs(squeeze(MAG2))+eps),'g')
grid on
axis([100 fs/2 -100 0])
title('Butterworth filter amplitude response')
xlabel('[Hz]')
ylabel('[dB]')
legend('s-domain', 'First discretization', 'Bilinear discretization', 'Location','SouthWest')
hold off

P_d
Z_d



figure
step(SYS_c,SYS2_d,SYS1_d)
%legend('continuous','bilin','discrete')
grid on

figure;
t = 0:Ts:0.03;
f1=100;
s1 = sin(2*pi*f1*t);
lsim(tf(1,1),s1,t,'b');  % = plot(t,s1) except for trace color (twice same color with plot)
hold on;
lsim(SYS_c,s1,t,'g'); hold on; % Octave needs hold on after each call (and manual color increment)
lsim(SYS2_d,s1,t,'r'); hold on;
lsim(SYS1_d,s1,t,'c'); hold on;
%legend('input','continuous','bilin','discrete')
grid on

