close all;  clc;    clear all;

fs = 390.625;

% 390.625 Hz
B0 = 2.529504324943790e-04;
B1 = 2*B0;
B2 = B0;
A1 = -1.954512463723770;
A2 = 0.955524265453747;

% 1kHz
% B0 = 3.913020539914436e-05;
% B1 = 2*B0;
% B2 = B0;
% A1 = -1.982228929792529;
% A2 = 0.982385450614125;

% 6.25 kHz
% B0 = 0.000001008618523;
% B1 = 0.000002017237047;
% B2 = 0.000001008618523;
% A1 = -1.995982799933390;
% A2 = 0.995986834407488;

% 100 kHz
% B0 = 0.000000003947346 ;
% B1 = 0.000000007894691 ;
% B2 = 0.000000003947346 ;
% A1 = -1.999748688378090;
% A2 = 0.999748704167471 ;

len = 10000;
T = 1 / fs;
endtime = 100;
t = 0:endtime/len:endtime - T;

%- double precision ------------------------

U = ones(1, len);
Y = zeros(1, len);
Xn = 0; Yn = 0; Vn = 0;

X1 = zeros(1, len);
X2 = zeros(1, len);

for i=1:len
    Xn_1 = Xn;
    Yn_1 = Yn;
    Xn = U(i); %inpput
    Yn = (B0*Xn + Vn);
    Vn = B1*Xn + B2*Xn_1 - A1*Yn - A2*Yn_1; 
    Y(i) = Yn;
    
    X1(i) = Vn;
    X2(i) = B2*Xn_1 - A2*Yn_1;
end

figure
plot(t,Y)

%- single precision ------------------------

b0 = single(B0);
b1 = single(B1);
b2 = single(B2);
a1 = single(A1);
a2 = single(A2);

u = single(U);
y = zeros(1, len);
xn = 0; yn = 0; vn = 0;

for i=1:len
    xn_1 = xn;
    yn_1 = yn;
    xn = single(u(i)); %inpput
    yn = (b0*xn + vn);
    vn = b1*xn + b2*xn_1 - a1*yn - a2*yn_1; 
    y(i) = yn;
end

hold all
plot(t,y)

%- states ----------------------------------

figure
subplot(2,1,1)
plot(t,X1)
subplot(2,1,2)
plot(t,X2)

% end of file