clear
close all

addpath(genpath('Data'));
addpath(genpath('Functions'));
addpath(genpath('Objects'));

load garazs_1.mat
fs = 1 / mean(diff(t));
h = h(1:3*fs,:);
%%
N = 3;
rmic = 0.1;
micArray = spherical_design('File','firthenmike.dat',rmic);

Psi_mic = micArray.SHT('pinv',h,N);

%% Beamformer:

Nf = 4096;
[~, Hbeam] = get_beamforming_filters( rmic, N, fs, Nf, 'modal' );
[~, Hreg2] = 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 );
Hbeam = Hbeam.*Hreg2.*Hlp;
hBeam = ifft(Hbeam,[],1,'symmetric');
Psi_beam  = zeros(size(Psi_mic));
for n = 0 : N
    for m = -n:n
        n
        Psi_beam(:,n^2+n+m+1) = conv( Psi_mic(:,n^2+n+m+1), hBeam(:,n+1),'same');
    end
end
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;
Nelev = 90;
azim = (-Nazim:Nazim)'/Nazim*pi;
elev = (0:Nelev)'/Nelev*pi;
[Az,El] = meshgrid(azim,elev);
beamformer_map = getSpherHarmMx( El(:), Az(:), 3, 'real').*a_n';

%
T = 0.0025;
Krms = T*fs;    
alpha = exp(-1/Krms);
RMS = sqrt(filtfilt(1/Krms, [1, -alpha], abs(Psi_mic(:,1)).^2));
[~, ix_onset] = max(abs(RMS));

winLength = 51;
ixs = ix_onset-floor(winLength/2) : ix_onset+floor(winLength/2);
s_polar = rms(beamformer_polar*Psi_beam(ixs,:)',2);
s_map = rms(beamformer_map*Psi_beam(ixs,:)',2);
s_polar = s_polar/max(abs(s_polar));
s_map = s_map/max(abs(s_map));


fig =  figure('units','normalized','outerposition',[0 0 1 1]);
t = (0:size(Psi_mic,1)-1)/fs;
pos = [ 0 0.07 1 0.4 ];
a1 = axes('Units','normalized','outerPosition',pos);
p1 = plot(t, Psi_mic(:,1),'PickableParts','visible','ButtonDownFcn',@myFunction,'LineWidth',1);
xlabel('t [s]')
ylabel('H_0(t) [Pa]')
grid on
yl = ylim;
hold on
bar = area([ixs(1),ixs(end)]/fs, [yl(2),yl(2)],yl(1),'facecolor',0.5*[1 1 1],'edgecolor','none','FaceAlpha',0.5);
xlim([ix_onset-256*4,ix_onset+4096*2]/fs)
p1.UserData = struct('fig', fig, 'beamPolar',beamformer_polar,'beam_map',beamformer_map,'hHOA',Psi_beam, 'wl',winLength , 'fs',fs, 'bar', bar, 'mapSize', [size(El,1),size(El,2)]);

pos = [ -0.01 0.55 0.4 0.35];
a2 = axes('Units','normalized','Position',pos);
Nfi = 360;
fi = (0:Nfi-1)/Nfi*2*pi;
p2 = polarplot(fi', abs(s_polar));
ax = gca;
d = ax.ThetaDir;
ax.ThetaZeroLocation = 'top';
rlim([0,1])

pos = [ 0.37 0.55 0.55 0.35 ];
a3 = axes('Units','normalized','Position',pos);
s_map2 = abs(reshape(s_map,size(El,1),size(El,2)));
p3 = pcolor(azim*180/pi,elev*180/pi,20*log10(s_map2));
set(gca, 'YDir','reverse')
set(gca, 'XDir','reverse')
xlabel('azimuth [deg]')
ylabel('zenith [deg]')
shading flat
clim([-30,0])
colorbar

%%



%%
function myFunction(src, event)
coords = event.IntersectionPoint;
fs = src.UserData.fs;
winLength = src.UserData.wl;
bar = src.UserData.bar;
ix0 = round( coords(1)*fs );
ixs = ix0-floor(winLength/2) : ix0+floor(winLength/2);
bar.XData = [ixs(1),ixs(end)]/fs;
beamformer_polar = src.UserData.beamPolar;
beamformer_map = src.UserData.beam_map;
Psi_beam = src.UserData.hHOA;
s_polar = rms(beamformer_polar*Psi_beam(ixs,:)',2);
s_map = rms(beamformer_map*Psi_beam(ixs,:)',2);
s_polar = s_polar/max(abs(s_polar));
s_map = s_map/max(abs(s_map));
mapSize = src.UserData.mapSize;
s_map2 = abs(reshape(s_map,mapSize(1),mapSize(2)));
src.UserData.fig.Children(3).Children.RData = s_polar;
src.UserData.fig.Children(2).Children.CData = 20*log10(s_map2);
end

%%

