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

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

fs = 48000;
blockSize = 4096;

N = 3;
rmic = 0.1;
micArray = spherical_design('File','firthenmike.dat',rmic);

[hrtf_out, hrtfArray] = extrapolate_hrtf_set('FABIAN_HRIR_measured_HATO_0', 98);

rls = mean(hrtfArray.Phi0(:,3));
binaural_filters = binauralRenderer(hrtf_out, blockSize);

Nf = blockSize;
[~, H_hoa] = get_hoa_filters( rmic, rls, N, fs, Nf );
[~, H_reg] = get_regularization_filters( N, Nf, fs, [90,680,1650,2600]'*(0.042/rmic) );
freq = (0:Nf-1)'/Nf*fs;

H_hoa = H_hoa.*H_reg;
h_hoa = ifft(H_hoa,[],1,'symmetric');
hoa_filters = radial_filter_bank(N,h_hoa, Nf);

virtualArray = spherical_design('Design',1302,'T',rls);
Ymx = virtualArray.get_SH_mx;

[ls_groups, ~] = findLsTriplets([hrtfArray.Phi0(:,1) hrtfArray.Phi0(:,2) ]*180/pi);
layoutInvMtx = invertLsMtx([hrtfArray.Phi0(:,1) hrtfArray.Phi0(:,2) ]*180/pi, ls_groups);
gains3D = vbap([virtualArray.Phi0(:,1) virtualArray.Phi0(:,2)]*180/pi, ls_groups, layoutInvMtx);

D_vbap =  4*pi/size(virtualArray.x0,1)*gains3D'*Ymx;
%%
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, 128);
        mcdmex('start');
    case 'file'
        fid = fopen(fName, 'r');
        numChannels = fread(fid, 1,'uint');
        inputIndices = fread(fid, numChannels,'uint');
        Nblock = fread(fid, 1,'uint');
end
n = 0;
try
    while true
        n = n+1;
        switch input
            case 'measurement'
                data = mcdmex('read', blockSize)';
                status = mcdmex('status');
                disp(status);
            case 'file'
                data = fread(fid, [numChannels, blockSize], 'float')';
        end

        psi = micArray.SHT('pinv',data,N);
        psi_ls = hoa_filters.filter(psi);
        y_ls = psi_ls*D_vbap';
        y_headphone = binaural_filters.render(y_ls);
        outputDevice(y_headphone/10);
    end
catch E
    disp(E.message)
end

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