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

fs = 48000;
blockSize = 4096*2;
freq = (0:blockSize-1)'/blockSize*fs;
omega = 2*pi*freq;
c = 343;
k = omega/c;
t = (0:blockSize-1)'/fs;

N = 3;

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

Nf = blockSize;
[~, Hrad] = get_inv_scat_filters( rmic, N, fs, Nf );
[~, Hreg] = get_regularization_filters( N, Nf, fs, [90,680,1650,2600]'*(0.042/rmic) );
H = Hrad;
h = ifft(H,[],1,'symmetric');

% semilogx(2*pi*(0:Nf-1)/Nf*fs/c*rmic, 20*log10(abs(Hrad)));
% grid on;
% xlim([0.03,32]);
% hold on
% semilogx(2*pi*(0:Nf-1)/Nf*fs/c*rmic, 20*log10(abs(H)));

%%

filter_inv_scat = radial_filter_bank(N, h, blockSize);

%%
fig =  figure('units','normalized','outerposition',[0 0 1 1]);
pos = [ -0.1 0.3 0.5 0.5];
p1 = axes('Units','normalized','Position',pos);
hp = micArray.plot_voronoi;
C = colormap('parula');
%
colors = [10,141,130;
    146,150,148]/255;
phi = linspace(0,2*pi,360/8);
theta = linspace(0,pi,180/8);
[Theta,Phi] = meshgrid(theta, phi);
[x,y,z] = sph2cart(Phi,Theta+pi/2,1);
for n = 0:N
    for m = -n:n
        Y_mn = getSpherHarm( Theta, Phi, n, m, 'real' );
        [x0,y0,z0] = sph2cart(Phi,Theta+pi/2,abs(Y_mn));
        
        l = n^2 + n + m + 1;
        pos = [ 0.24+l*0.042 0.75 0.075 0.075];
        p1 = axes('Units','normalized','Position',pos);
        s(l) = surf(x0,y0,z0, sign(Y_mn));
        axis equal tight
        clim([-1,1]*0.25)
        %shading flat
        set(gca,'Visible','off')
        lighting phong
    end
end

pos = [ 0.305 0.3 0.662 0.4];
p1 = axes('Units','normalized','Position',pos);
x = (0:15);
y = zeros(16,1);
b = bar(x,y);
ylim([0,1e-3])
axis tight
%%
outputDevice = audioDeviceWriter;
set(outputDevice,'SampleRate',fs);
set(outputDevice,'BufferSize',blockSize);
%%

%fName = 'fifi.dat';             % file name
%fid = fopen(fName, 'r');

%% Start acquisition
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);

        drms = 20*log10(rms(data, 1)/2e-5);
        lims = [10 70];
        cidx = (drms - lims(1)) / diff(lims);
        cidx = floor(max(min(cidx, 1), 0) * 255) + 1;
        for i = 1 : 24
            set(hp(i), 'FaceColor', C(cidx(i),:));
        end
        drawnow();
    %    outputDevice(sum(data,2));

        psi_total = micArray.SHT('pinv',data,N);
        psi_in = filter_inv_scat.filter(psi_total);
        psi_rms = rms(psi_in, 1);
        b.YData = 20*log10(psi_rms)+100;
        ylim([-60,0]+100)
    end
catch E
    disp(E.message)
end

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