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);

[ x_ls, theta_ls, N_dummy ] = get_loudspeaker_layout( 1, [0 0 0], '5.1');
%[hrtfData, hrtfArray] = extrapolate_hrtf_set('subject_003', x_ls);

load hrtfSet.mat
%%
rls = mean(hrtfArray.Phi0(:,3));
binaural_filters = binauralRenderer(hrtfData, 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);


numChannels = 24;
inputIndices = 1:numChannels;
mcdmex('open', '10.0.0.4', inputIndices-1, 5*fs, 128);
mcdmex('start');

n = 0;
try
    while true
        n = n+1;
        data = mcdmex('read', blockSize)';
        status = mcdmex('status');
        disp(status);

        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);
        y_direct = y_ls(:,1:size(x_ls,1)-N_dummy) + 0*mean(y_ls(:,end-N_dummy+1:end),2);

       % outputDevice(y_direct(:,1:5));
        outputDevice(y_headphone/5);
    end
catch E
    disp(E.message)
end

release(outputDevice);
mcdmex('stop');
mcdmex('close');