clear
close all
addpath(genpath('Data'));
addpath(genpath('Functions'));
addpath(genpath('Objects'));

input = 'measurement';
fName = 'test_pw.dat';             % file name

fs = 48000;
blockSize = 4096*2;

Nmes = 3;
T = 5;
T = floor(T*fs/blockSize)/fs*blockSize;
Nt = T*fs;
t = (0:Nt-1)'/fs;
omega1 = 2*pi*1;
omega2 = 2*pi*fs/2;
c = log(omega2/omega1);
freq = (0:Nt-1)'/Nt*fs;

tau = T/c;
%s_in1 = sin( omega1*tau*(exp(t/tau-1)) );
s_in = sin( omega1*tau*(exp(t/tau)) );

% w = hann(size(s_in,1), "periodic");
% s_in = s_in .* w;

s_meas = repmat(s_in,[Nmes,1]);
%%
N = 3;
rmic = 0.1;
micArray = spherical_design('File','firthenmike.dat',rmic);
%%
outputDevice = audioDeviceWriter;
set(outputDevice,'SampleRate',fs);
set(outputDevice,'BufferSize',5*blockSize);
numChannels = 24;
inputIndices = 1:numChannels;
mcdmex('open', '10.0.0.4', inputIndices-1, 5*fs, 128);
mcdmex('start');

toThrow = mcdmex('read', blockSize)';

nSamples = size(s_meas,1);

dataTotal = nan(nSamples, numChannels);
nBlocks = nSamples / blockSize;
noUnderrun = nan(nBlocks,1);
shift = 3;
for iBlock = 1 : nBlocks + shift
    idx = (iBlock-1)*blockSize+1:iBlock*blockSize;
    if (iBlock <= nBlocks)
        exc = s_meas(idx);
        noUnderrun(iBlock) = outputDevice(exc);
        if noUnderrun(iBlock) > 0
            break;
        end
    end

    if iBlock > shift
        data = mcdmex('read', blockSize)';
        dataTotal(idx - shift*blockSize,:) = data;
    end
end
release(outputDevice);

mcdmex('stop');
mcdmex('close');
%%

%[b,a] = butter(3,[50,20e3]/(fs/2),'bandpass');
%dataTotal_flt = filtfilt(b,a,dataTotal);
y = dataTotal(Nt+1:end,:);
y = reshape(y, [Nt, size(y,1)/Nt , size(y,2) ]);

%Nfft = (size(y,1) + size(s_in,1) - 1);
Nfft = Nt;
F = fft(s_in,Nfft,1);
U = fft(y,Nfft,1);

freq = (0:Nfft-1)'/Nfft*fs;

H1 = squeeze( mean(conj(F).*U,2)./mean(conj(F).*F,2) );
H2 = squeeze( mean(conj(U).*U,2)./mean(conj(U).*F,2) );
Coh = H1./H2;

t = (0:Nt-1)'/fs;
h = ifft(H1,Nt,1,'symmetric');
%
figure
subplot(3,1,1)
semilogx(freq, 20*log10(abs(H1)));
grid on;
xlim([20,20e3])
title('Transfer')
xlabel('f [Hz]')
ylabel('|H(f)| [dB]')

subplot(3,1,2)
semilogx(freq, (abs(Coh)));
grid on;
xlim([20,20e3])
title('Coherence')
xlabel('f [Hz]')
ylabel('C(f)')

subplot(3,1,3)
plot(t,h)
grid on;
title('Impulse response')
xlabel('t [s]')
ylabel('h(t)')

save temp freq H1 t h