%% Human Voice Directivity Postures - Demo Script and Plots
%
% This script reads a voice directivity in SOFA Format and makes basic plots
%
% Dependencies: SUpDEq Toolbox (GitHub, https://github.com/AudioGroupCologne/SUpDEq)
%
% References:   Pörschmann, C., Arend, J.M. (2022). "Effects of hand postures on voice directivity,"
%               published in JASA EL.
%   
% (C) 2022 by   Christoph Pörschmann
%               TH Köln - University of Applied Sciences
%               Institute of Communications Engineering
%               Department of Acoustics and Audio Signal Processing
%%
clear all
clc
%% Information on the chosen dataset
participant_id=3; % Values: (1...13)
repeats_id=1; % Values: (1...2)
phoneme_id=2; % Values: (1...3) corresponding to (REF,HFM,CAM)
upsampled=true; % Values: (true/false)
  

%% Load SOFA-File
if upsampled==true
    type=('_upsampled');
    Nmax=35;
else
    type=('');
    Nmax=4;
end
SOFAobj=SOFAload(['Voice_Directivities/Directivity_' num2str(participant_id) '_' num2str(repeats_id) type '.sofa']);
Nfft=1024;
phonemes=['REF';'HFM';'CAM'];
disp(['Actual dataset: [' phonemes(phoneme_id,:) '], Subject ' num2str(participant_id) ', Repetition ' num2str(repeats_id)]);
    
%% Transform to SH Domain
Directivity_TF=squeeze(SOFAobj.Data.Real(phoneme_id,:,:))+i*squeeze(SOFAobj.Data.Imag(phoneme_id,:,:));
sampling_grid=SOFAobj.ReceiverPosition(:,1:2);
r_opt=SOFAobj.SourcePosition(1,1);
sampling_grid(:,2)=90-sampling_grid(:,2);
Fs=48000;
Directivity_SH=supdeq_hrtf2sfd(Directivity_TF,Directivity_TF,Nmax,sampling_grid,Fs); 
Directivity_SH.FFToversize=4;

%% Perform SUpDEq Processing if sparse dataset is loaded
if upsampled==false
    % Get equalization dataset (SH-coefficients)
    Ndense = 35;
    eqDataset = supdeq_getEqDataset(Ndense,r_opt*2,Nfft,Fs,0,1,[0 115 0 115]);
    % Perform equalization
    Nmax;
    sparseHRTFdataset.samplingGrid=Nmax;
    sparseHRTFdataset.HRTF_L=Directivity_TF;
    sparseHRTFdataset.HRTF_R=Directivity_TF;
    sparseHRTFdataset.samplingGrid=sampling_grid;
    sparseHRTFdataset.f=SOFAobj.N';
    sparseHRTFdataset.FFToversize=Directivity_SH.FFToversize;
    eqHRTFdataset = supdeq_eq(sparseHRTFdataset,eqDataset,Nmax,sparseHRTFdataset.samplingGrid,0,0);
    % Perform de-equalization 
    denseSamplingGrid = supdeq_lebedev(2702);
    [~, ~, Directivity_SH] = supdeq_deq(eqHRTFdataset, eqDataset, Ndense, denseSamplingGrid,[8,32]);
end
 
%% Get IRs for Directivity Plots 
polarGrid_az(:,1)=0:359;
polarGrid_az(361,1)=360;
polarGrid_az(:,2)=90;    
[Directivity_az,~] = supdeq_getArbHRTF(Directivity_SH,polarGrid_az,'DEG',0,'ak');
Directivity_az=Directivity_az;
polarGrid_el(:,2)=0:179;
polarGrid_el(:,1)=0;
polarGrid_el(181:360,2)=180:-1:1;
polarGrid_el(181:360,1)=180;
polarGrid_el(361,1)=361;
polarGrid_el(361,2)=0;
%polarGrid_el(:,3)=0:359;      
[Directivity_el,~] = supdeq_getArbHRTF(Directivity_SH,polarGrid_el,'DEG',0,'ak');
Directivity_el=Directivity_el;

% Do Plots in Octave Bands
f_center=[ 125 250 500 1000 2000 4000 8000];
nOct=1;
f_high=f_center*sqrt(2^(1/nOct));
f_low=f_center/sqrt(2^(1/nOct));
for my_freqs_index=1:length(f_center)
    [~,fu]=min(abs(Directivity_SH.f-f_low(my_freqs_index)));
    [~,fo]=min(abs(Directivity_SH.f-f_high(my_freqs_index)));
    directivity_band_az(:,my_freqs_index)=sqrt(sum(abs(Directivity_az(:,fu:fo).^2),2));
    directivity_band_el(:,my_freqs_index)=sqrt(sum(abs(Directivity_el(:,fu:fo).^2),2));
end

%% Directivity plots 
%Define colors according to https://colorbrewer2.org/?type=qualitative&scheme=Set1&n=8
rgb(1,:) = [228/255, 26/255, 28/255];
rgb(2,:) = [55/255, 126/255, 184/255];
rgb(3,:) = [77/255, 175/255, 74/255];
rgb(4,:) = [152/255, 78/255, 163/255];
rgb(5,:) = [255/255, 127/255, 0/255];
rgb(6,:) = [255/255, 255/255, 51/255];
rgb(7,:) = [166/255, 84/255, 40/255];
rgb(8,:) = [247/255, 129/255, 191/255];
grey=[0.4 0.4 0.4];

my_azimuth_tick_label={'Front','30°','60°','90°','120°','150°','Back','-150°','-120°','-90°','-60°','-30°'};
my_elevation_tick_label={'90°','60°','30°','Front','-30°','-60°','-90°','-120°','-150°','Back','150°','120°'};

%Azimuth
AkfhFigureHandle = AKf(15, 10);
for my_freqs_index=1:length(f_center)
    plot_values=directivity_band_az(:,my_freqs_index);               
    AKp(20*log10(plot_values/plot_values(1)), 'x6', 'az', polarGrid_az(:,1),'dr',[6 -24],'c',rgb(my_freqs_index,:),'lw',2)
    set(gca,'ThetaTick',[0:30:330]);
    set(gca,'ThetaTickLabel',my_azimuth_tick_label,'FontName','Helvetica');
    set(gca,'RTick',[-24:6:6]);
    set(gca,'RTickLabel',{' ', '-18', '-12', '-6', '0', '+6 dB'},'FontName','Helvetica');
    set(gca,'GridAlpha',[1]);
    set(gca,'RColor',grey);
    set(gca,'ThetaColor',grey);
end
legend(num2str(f_center'))
title('Directivity Horizontal Plane')
    
% Elevation 
AkfhFigureHandle = AKf(15, 10);
for my_freqs_index=1:length(f_center)
    plot_values=directivity_band_el(:,my_freqs_index);   
    AKp(20*log10(plot_values/plot_values(91)), 'x6', 'az', polarGrid_az(:,1),'dr',[6 -24],'c',rgb(my_freqs_index,:),'lw',2)
    set(gca,'ThetaTick',[0:30:330]);
    set(gca,'ThetaTickLabel',my_elevation_tick_label,'FontName','Helvetica');
    set(gca,'RTick',[-24:6:6]);
    set(gca,'RTickLabel',{' ', '-18', '-12', '-6', '0', '+6 dB'},'FontName','Helvetica');
    set(gca,'GridAlpha',[1]);
    set(gca,'RColor',grey);
    set(gca,'ThetaColor',grey);
end
legend([num2str(f_center') repmat(' Hz',length(f_center),1)])
title('Directivity Vertical Plane');

%% Analysis - Calculate data for color-map surf plot 

% Define reference direction
%reference_dir= [0 -25]  % Main radiation
reference_dir= [0 0]  % Frontal
% initialize matrix
mtxData=zeros(361,181,Nfft/2+1);

for azimuth=-180:180
    clc
    azimuth
    pause(0.001)
    my_azimuth=mod(azimuth,360);
    angles=[ones(1,181)*azimuth; 0:180]';
    angles=mod(angles,360);  
    my_directivities=abs(supdeq_getArbHRTF(Directivity_SH,angles,'DEG',0,'ak'));     
    mtxData(azimuth+181,:,1:Nfft/2+1)=my_directivities;
end
mtxData=20*log10(mtxData);
mtxData=mtxData-mtxData(180+reference_dir(1),90-reference_dir(2),:);

%% Analysis - Do color-map surf plot I
f_low=0;
f_high=8000;
my_frequency_axis=Directivity_SH.f;
[~,fu]=min(abs(my_frequency_axis-f_low));
[~,fo]=min(abs(my_frequency_axis-f_high));

% Make surfplot
plotSpecDiffMap = figure;
set(plotSpecDiffMap, 'units', 'points','Position', [50 50 600 300])   
my_mtxData=mean(mtxData(361:-1:1,:,fu:fo),3)'; % Inver order in azimuth as in plot shown from 180 ... -180
imagesc(my_mtxData)
set(gcf,'color','w');
caxis([-10 4])
set(gca,'FontSize',10);
xlim([1 360]);
ylim([1 181]);
set(gca,'xTickLabel',{'180','90','0','-90','-180'});
set(gca,'yTickLabel',{'90','45','0','-45','-90'});
set(gca,'yTick',[1 46 91 136 181]);
set(gca,'xTick',[1 91 181 271 360]);
xlabel('Azimuth in degree');
ylabel('Elevation in degree');
hcb = colorbar;
set( hcb, 'YDir', 'reverse' );
hcb.Label.String = 'Directivity in dB';
%title(['Directivity [' phonemes(phoneme_id,:) '], Subj.' num2str(participant_id) ', Rep.' num2str(repeats_id)]);



%% Tube plots
% Azimuth
plotSpecDiffMap = figure;
set(plotSpecDiffMap, 'units', 'points','Position', [50 50 600 300])   
my_mtxData=squeeze(mtxData(:,91,:));
surf(my_mtxData)
shading interp
view(2)
set(gca,'XScale','log');
set(gcf,'color','w');
set(gca,'FontSize',10);
xlim([9 180]);
ylim([1 361]);
set(gca,'yTickLabel',{'-180','-90','0','90','180'});
set(gca,'xTickLabel',{'500','1k','2k','4k','8k'});
set(gca,'xTick',[12 22 44 86 172]);
set(gca,'yTick',[1 91 181 271 360]);
caxis([-10 10])
xlabel('Frequency in Hz');
ylabel('Azimuth in degree');
hcb = colorbar;
hcb.Label.String = 'Directivity in dB';
set( hcb, 'YDir', 'reverse' );
%title(['Directivity Horizontal Plane [' phonemes(phoneme_id,:) '], Subj.' num2str(participant_id) ', Rep.' num2str(repeats_id)]);

% Elevation
plotSpecDiffMap = figure;
set(plotSpecDiffMap, 'units', 'points','Position', [50 50 600 300])   
my_mtxData=squeeze(mtxData(1,91:180,:));
my_mtxData= [my_mtxData' squeeze(mtxData(181,180:-1:1,:))']';
my_mtxData= [my_mtxData' squeeze(mtxData(1,1:91,:))']';
surf(my_mtxData)
shading interp
view(2)
set(gca,'XScale','log');
set(gcf,'color','w');
%colormap(fliplr(hot));
set(gca,'FontSize',10);
xlim([9 180]);
ylim([1 361]);
set(gca,'yTickLabel',{'Back','-90','Front','90','Back'});
set(gca,'xTickLabel',{'500','1k','2k','4k','8k'});
set(gca,'xTick',[12 22 44 86 172]);
set(gca,'yTick',[1 91 181 271 360]);
caxis([-10 10])
xlabel('Frequency in Hz');
ylabel('Elevation in degree');
hcb = colorbar;
hcb.Label.String = 'Directivity in dB';
set( hcb, 'YDir', 'reverse' );
%title(['Directivity Vertical Plane [' phonemes(phoneme_id,:) '], Subj.' num2str(participant_id) ', Rep.' num2str(repeats_id)]);






