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

beamforming= 'MUSIC';       % MUSIC / conventional
input = 'measurement';
fName = 'test_4.dat';             % file name

mexfile = @mcdmex_msvc;

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

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');
%%
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;
        mexfile('open', '10.0.0.4', inputIndices-1, 5*fs, 128);
        mexfile('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
    cleanupObj = onCleanup(@() cleanup_mex(mexfile));

    while true
        n = n+1;
        switch input
            case 'measurement'
                data = mexfile('read', blockSize)';
                status = mexfile('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);

        switch beamforming
            case 'conventional'
                
        
                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);

                    
             case 'MUSIC'

                    % ===== FFT =====
                    psi_fft = fft(psi_rad, [], 1);
                    
                    % ===== SZŰK FREKVENCIASÁV =====
                    f_idx = 20:250;
                
                    % ===== FREKVENCIAÁTLAGOS KOVARIANCIA =====
                    R = zeros(16, 16);

                        for fi = f_idx
                            a = psi_fft(fi,:).';  
                            R = R + a*a';
                        end
                        R = R / length(f_idx(end));
                         

                    % ===== EIG =====
                    [V, D] = eig(R);
                    [~, idxEig] = sort(diag(D), 'descend');
                    V = V(:, idxEig);
                
                    En = V(:, 2:end);
                
                    % ===== MUSIC =====
                    P = 1 ./ sum(abs(En' * beamformer_map').^2,1);
                    s_map = reshape(P,size(El));
                    s_map = s_map / max(s_map(:) + 1e-12);
                
                    B = beamformer_polar;
                    s_polar = 1 ./ sum(abs(En' * B').^2,1);
                    s_polar = s_polar / max(s_polar + 1e-12);
                
                    % ===== PLOT =====
                    set(p1,'RData',abs(s_polar));
                    set(p2,'CData',20*log10(abs(s_map)+1e-12));
                
                    drawnow
                
                    % ===== BEAMFORMING OUTPUT =====
                    s_beam = psi_rad*(p2.UserData.*a_n')';
                    outputDevice(s_beam);

        end

                
    end

catch E
    disp(E.message)
end

release(outputDevice);
switch input
    case 'measurement'
        mexfile('stop');
        mexfile('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

function cleanup_mex(mexfile)
    try
        mexfile('stop');
    catch
    end
    try
        mexfile('close');
    catch
    end
    disp('MEX kapcsolat lezárva.');
end