%in+iir_out with 8 intp
clc;clear;close all
% addpath("/data/work/thfu/TailCorr/script_m");
in         = importdata("/home/thfu/work/TailCorr/sim/z_dsp_en/in.dat");       
wave_verdi = importdata("/home/thfu/work/TailCorr/sim/z_dsp_en/OrgOut.dat");   

dout0 = importdata("/home/thfu/work/TailCorr/sim/z_dsp_en/doutp0.dat");  
dout1 = importdata("/home/thfu/work/TailCorr/sim/z_dsp_en/doutp1.dat");
% dout2 = importdata("/home/thfu/work/TailCorr/sim/z_dsp_en/dout2.dat");
% dout3 = importdata("/home/thfu/work/TailCorr/sim/z_dsp_en/dout3.dat");

N = length(dout0);
cs_wave = zeros(2*N,1);
cs_wave(1:2:2*N) = dout0;
cs_wave(2:2:2*N) = dout1;
%cs_wave(3:4:4*N) = dout2;
%cs_wave(4:4:4*N) = dout3;


A   =  [0.025 0.015*1 0.0002*1 0];
tau = -[1/250 1/650 1/1600 0];
fs  = 2e9;

coef_len = length(A);
for i = 1:coef_len
    b(i) = exp(1e9/fs/(1-A(i))*tau(i));
    a(i) = A(i)/1/(1-A(i))*exp(1e9/fs/(1-A(i))/2*tau(i));  
    h_ideal(:,i) = filter(a(i),[1 -b(i)],diff(in));
end

wave_float = in(2:end)+ sum(h_ideal,2);

wave_float_len = length(wave_float);
wave_float_8 = interp1(1:wave_float_len,wave_float,1:1/8:(wave_float_len+1-1/8),'linear')';

[wave_float_A,cs_wave_A,Delay] = alignsignals(wave_float,cs_wave);
N = min(length(wave_float),length(cs_wave_A));
figure()
diff_plot(wave_float_A, cs_wave_A,'float','verdi',[0 N]);

%% Test of iir filter with no intp

[wave_float_A,wave_verdi_A,Delay] = alignsignals(wave_float,wave_verdi);
N = min(length(wave_float_A),length(wave_verdi_A));
figure()
diff_plot(wave_float_A, wave_verdi_A,'float','verdi',[0 N]);
%%
signalAnalyzer(wave_float,wave_verdi,'SampleRate',1);
%%

a_fix = round(a*2^31);
ab_fix = round(a.*b*2^31);
b2_fix = round(b.^2*2^31);

a_hex = dec2hex(a_fix,8);

a_bin = dec2bin(a_fix,32);

fprintf('a_fix is %d\n',a_fix);
fprintf('ab_fix is %d\n',ab_fix);
fprintf('b2_fix is %d\n',b2_fix);