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

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

fs = 48000;
blockSize = 4096;


T = 1;
T = floor(T*fs/blockSize)/fs*blockSize;
Nt = T*fs;
t = (0:Nt-1)'/fs;
omega1 = 2*pi*50;
omega2 = 2*pi*20e3;
c = log(omega2/omega1);

s_in = sin( omega1*T/c*(exp(t*c/T-1)) );
Nmes = 5;
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',blockSize);

switch input
    case 'measurement'
        numChannels = 24;
        inputIndices = 1:numChannels;
        mcdmex('open', '10.0.0.4', inputIndices-1, 5*fs);
        mcdmex('start');
    case 'file'
        fid = fopen(fName, 'r');
        numChannels = fread(fid, 1,'uint');
        inputIndices = fread(fid, numChannels,'uint');
        Nblock = fread(fid, 1,'uint');
        dataTotal = zeros(blockSize,numChannels);
end
n = 0;
try
    while true
        n = n+1;
        exc = s_meas((n-1)*blockSize+1:n*blockSize);
        outputDevice(exc);
        switch input
            case 'measurement'
                data = mcdmex('read', blockSize)';
                status = mcdmex('status');
                disp(status);
            case 'file'
                data = fread(fid, [numChannels, blockSize], 'float')';
        end
        dataTotal((n-1)*blockSize+1:n*blockSize,:) = data;
        

    end
catch E
    disp(E.message)
end
release(outputDevice);

switch input
    case 'measurement'
        mcdmex('stop');
        mcdmex('close');
    case 'file'
        fclose(fid);
end
%%

%[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);
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)')