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

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

fs = 48000;
blockSize = 4096;
N = 3;

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

Nf = blockSize;
[~, Hrad] = get_beamforming_filters( rmic, N, fs, Nf, 'modal' );
[~, Hreg] = get_regularization_filters( N, Nf, fs, [90,680,1650,2600]'*(0.042/rmic) );
freq = (0:Nf-1)'/Nf*fs;
fc = 3e3;
Hlp = 1./sqrt( 1 + (freq/fc).^6 );


H = Hrad.*Hreg.*Hlp;
h = ifft(H,[],1,'symmetric');

beamforming_filters = radial_filter_bank(N,h,blockSize);
a_n = get_max_rE_weights( N )';

%%
Nfi = 360;
fi = (0:Nfi-1)'/Nfi*2*pi;
beamformer_polar = getSpherHarmMx( pi/2*ones(size(fi)), fi, 3, 'real').*a_n';

Nazim = 180/2;
Nelev = 90/2;
azim = (0:Nazim-1)'/Nazim*2*pi;
elev = (0:Nelev)'/Nelev*pi;
[Az,El] = meshgrid(azim,elev);
beamformer_map = getSpherHarmMx( El(:), Az(:), 3, 'real').*a_n';
%%
fig =  figure('units','normalized','outerposition',[0 0 1 1]);

pos = [ 0.05 0.3 0.4 0.4 ];
a1 = axes('Units','normalized','Position',pos);
s_polar = zeros(size(fi));
p1 = polarplot(fi, abs(s_polar));
rlim([0,1])

pos = [ 0.5 0.35 0.4 0.4 ];
a2 = axes('Units','normalized','Position',pos);
s_map = zeros(size(beamformer_map,1),1);
p2 = pcolor(azim*180/pi,elev*180/pi,20*log10(abs(reshape(s_map,size(El,1),size(El,2)))));
set(gca, 'YDir','reverse')
xlabel('azimuth [deg]')
ylabel('zenith [deg]')
shading flat
axis equal tight
%caxis([-10,0])
set(p2, 'ButtonDownFcn', @myFunction);
p0 = [0,90];
hold on
plot(p0(1),p0(2),'.r','MarkerSize',20)
p2.UserData = getSpherHarmMx( p0(2)*pi/180, p0(1)*pi/180, 3, 'real');

%%
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');
        dataTotal = zeros(blockSize,numChannels);
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
        %dataTotal((n-1)*blockSize+1:n*blockSize,:) = data;

        psi = micArray.SHT('pinv',data,N);
        psi_rad = beamforming_filters.filter(psi);

        s_polar = sum(abs(beamformer_polar*psi_rad').^2,2);
        s_polar = s_polar/max(abs(s_polar));

        s_map = sum(abs(beamformer_map*psi_rad').^2,2);
        s_map = s_map/max(abs(s_map));

        set(p1,'RData',abs(s_polar));
        set(p2,'CData',20*log10(abs(reshape(s_map,size(El,1),size(El,2)))));
        drawnow

        s_beam = psi_rad*(p2.UserData.*a_n')';
        outputDevice(s_beam);

    end
catch E
    disp(E.message)
end

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

function myFunction(src, event)
    coords = event.IntersectionPoint;
    [~, xidx] = min(abs(src.XData - coords(1)));
    [~, yidx] = min(abs(src.YData - coords(2)));
    fprintf('You have selected data point: %g, %g, %g\n',...
        src.XData(xidx), src.YData(yidx), src.CData(yidx, xidx));

    azim = src.XData(xidx)*pi/180;
    zenith = src.YData(yidx)*pi/180;
    src.UserData = getSpherHarmMx( zenith, azim, 3, 'real');
    set(src.Parent.Children(1),'XData',src.XData(xidx))
    set(src.Parent.Children(1),'YData',src.YData(yidx))
    
end