%in+iir_out with 8 intp
clc;clear;close all

in         = importdata("/home/thfu/work/TailCorr/sim/in.dat");       
wave_verdi = importdata("/home/thfu/work/TailCorr/sim/OrgOut.dat");   

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

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

A   =  [0.025 0.015 0.0002 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')';

[cs_wave_A,wave_float_8_A,Delay] = alignsignals(cs_wave,wave_float_8);
N = min(length(wave_float_8_A),length(cs_wave_A));
% figure()
% diff_plot(wave_float_8_A(90:end), cs_wave_A(154:end),'float','verdi',[0 N]);
%
%Test of iir filter
[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);
b_fix = round(b*2^31);

a_hex = dec2hex(a_fix,8);
b_hex = dec2hex(b_fix,8);

a_bin = dec2bin(a_fix,32);
b_bin = dec2bin(b_fix,32);

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