function [hnm,fs,varOut] = toSH(h,N,varargin)
% Transform HRTF to SH domain at order N. This is a wrapper for the other
% toSH_* functions and applies some additional pre- and post- processing:
% 1. Zero-pad the HRIRs if indicated
% 2. Circular shift so the average HRIR onset falls at t=0
% 3. Obtain HRTF's SH coefficients using one of the available methods:
% a. 'Trunc' = direct order truncation (default)
% b. 'SpSub' = spatial subsampling (toSH_SpSub) [1]
% c. 'MagLS' = magnitude least squares (toSH_MagLS) [2]
% d. 'FDTA' = frequency-dependent time alignment (toSH_FDTA) [3]
% e. 'TA' = phase correction by ear alignment (toSH_TA) [4]
% f. 'SpSubTA' = frequency-dependent time alignment + spSub
% (toSH_SpSubMod) [5]
% g. 'BiMagLS' = phase correction by ear alignment + magLS
% 4. Apply tapering weights (e.g. Hann [6] or Max-rE [7])
% 5. Apply diffuse field EQ [8] or covariance constraint [9, sec.4.11.3]
% 6. Undo circular shift from step 2
% 7. Crop HRIR and apply fade in/out if indicated
%
% SIMPLE USAGE EXAMPLES:
% hnm = toSH(h,15,'mode','MagLS','az',az,'el',el,'fs',48000);
% hnm = toSH(sofa_h,15,'mode','MagLS');
%
% INPUT:
% h = HRIRs in SOFA or matrix format (irlen x ndirs x 2 ears)
% N = target SH order
%
% Mandatory inputs (as argument pairs) if h is provided in matrix format:
% az = HRIR azimuth (ndirs x 1) in rad (0=front, pi/2=left)
% el = HRIR elevation (ndirs x 1) in rad (0=top, pi/2=front)
% fs = sampling frequency in Hz
%
% Optional inputs (provided as argument pairs):
% mode = any of 'Trunc' (def), 'SpSub', 'MagLS', 'FDTA', 'TA',
% 'SpSubMod', 'BiMagLS' (case insensitive; descriptions above)
% w = quadrature weights (ndirs x 1); if empty (def), use pseudoinverse
% EQ = one of the available equalisation options:
% 0 = no EQ (default)
% 1 = covariance constraint [9, sec.4.11.3]
% 2 = diffuse field EQ (HRF from [8])
% 3 = spherical head filters (SHF from [8]), adapted to tapering
% weights if appropriate [6]
% fc = cutoff frequency for MagLS, frequency-dependent TA and dual-band
% tapering, in Hz (def=aliasing frequency)
% nfftIn = if non-empty, zero-pad HRIRs to this length (samples)
% nfftOut = if non-empty, crop the output to this length (samples)
% r = head radius in m (def=0.085)
% c = speed of sound in m/s (def=343)
% earAz = left/right ear azimuth (1 x 2) in rad (def=[pi/2, 3*pi/2])
% earEl = left/right ear elevation (1 x 2) in rad (def = [pi/2, pi/2])
% Nmax = reference SH order for spSub and cov. constraint (def=35).
% covConstPlots = whether to draw plots when processing covariance
% constraint (def=false).
% fadeIn = fade-in length in samples to apply to the output (def=0)
% fadeOut = fade-out length in samples to apply to the output (def=0)
% frac = half-length of the MagLS transition band (def=2 -> 1/2 octave).
% E.g. if fc=623.89 Hz, smooth between 349.65 and 882.31 Hz. If
% frac==0, don't smooth.
% tapering = apply weights to high SH orders to reduce side lobes:
% 0 = off (default)
% 1 = original Hann weights from Hold et al (2019)
% 2 = improved (shorter) Hann window
% 3 = max-rE weights from Zotter and Frank (2012), non-normalized
% 4 = same as 3, but normalize weights (McKenzie et al, 2018)
% dualBand = whether to use dual-band tapering (def=false)
% Hnm_ref = precomputed high-order Hnm used to calculate diffuse field
% EQ. To speed things up if processing many HRTFs
%
% OUTPUTS:
% hnm = HRIRs' SH coefficients (nfftOut x (N+1)^2 x 2 ears)
% varOut = variable output, i.e. spherical head filter for 'Trunc', fc
% for 'MagLS'/'BiMagLS', EQ filters if applied, etc.
%
% EXTERNAL DEPENDENCIES:
% SOFA API for Matlab (github.com/sofacoustics/API_MO)
% (only if the input HRTF is provided in SOFA format)
%
% REFERENCES:
% [1] BernschÃ¼tz, Benjamin, et al. "Binaural reproduction of plane waves
% with reduced modal order." Acta Acustica united with Acustica 100.5
% (2014): 972-983.
% [2] SchÃ¶rkhuber, C., Zaunschirm, M., HÃ¶ldrich, R., 2018. Binaural
% Rendering of Ambisonic Signals via Magnitude Least Squares.
% Presented at the DAGA 2018, Munich, Germany, pp. 339â€“342.
% [3] Zaunschirm, M., SchÃ¶rkhuber, C., HÃ¶ldrich, R., 2018. Binaural
% rendering of Ambisonic signals by head-related impulse response
% time alignment and a diffuseness constraint. The Journal of the
% Acoustical Society of America 143, 3616â€“3627.
% https://doi.org/10.1121/1.5040489
% [4] Ben-Hur, Zamir, et al. "Efficient Representation and Sparse
% Sampling of Head-Related Transfer Functions Using Phase-Correction
% Based on Ear Alignment." IEEE/ACM Transactions on Audio, Speech,
% and Language Processing 27.12 (2019): 2249-2262.
% [5] McKenzie, T., Murphy, D., Kearney, G., 2019. An evaluation of
% pre-processing techniques for virtual loudspeaker binaural
% ambisonic rendering, in: EAA Spatial Audio Signal Processing
% Symposium. Paris, France, pp. 149â€“154.
% https://doi.org/10.25836/sasp.2019.09
% [6] Hold, C., Gamper, H., Pulkki, V., Raghuvanshi, N., Tashev, I.J.,
% 2019. Improving Binaural Ambisonics Decoding by Spherical Harmonics
% Domain Tapering and Coloration Compensation, in: ICASSP 2019 - 2019
% IEEE International Conference on Acoustics, Speech and Signal
% Processing (ICASSP). Presented at the ICASSP 2019 - 2019 IEEE
% International Conference on Acoustics, Speech and Signal Processing
% (ICASSP), pp. 261â€“265. https://doi.org/10.1109/ICASSP.2019.8683751
% [7] Zotter, Franz, and Matthias Frank. "All-round ambisonic panning and
% decoding." Journal of the audio engineering society 60.10 (2012):
% 807-820.
% [8] Ben-Hur, Zamir, et al. "Spectral equalization in binaural signals
% represented by order-truncated spherical harmonics." The Journal of
% the Acoustical Society of America 141.6 (2017): 4087-4096.
% [9] Zotter, F., Frank, M., 2019. Ambisonics: A Practical 3D Audio
% Theory for Recording, Studio Production, Sound Reinforcement, and
% Virtual Reality, Springer Topics in Signal Processing. Springer
% International Publishing, Cham.
% [10] McKenzie, Thomas, Damian T. Murphy, and Gavin Kearney.
% "Diffuse-field equalisation of binaural ambisonic rendering."
% Applied Sciences 8.10 (2018): 1956.
% [11] Archontis Politis, Microphone array processing for parametric
% spatial audio techniques, 2016 Doctoral Dissertation, Department of
% Signal Processing and Acoustics, Aalto University, Finland.
%
% AUTHOR: Isaac Engel - isaac.engel(at)imperial.ac.uk
% February 2021

% TODO: implement Tikhonov regularization for irregular grids
% According to Duraiswami 2004, it goes like this:
% reg_eps = 1e-6; % epsilon
% D = (1 + N*(N+1)) * eye((N+1)^2); % Tikhonov regularization matrix
% Y_inv = Y' * (Y * Y' + reg_eps*D)^(-1); % this instead of pseudoinverse

%% Parse inputs
p = inputParser;
addParameter(p,'mode','Trunc',@ischar)
addParameter(p,'az',[])
addParameter(p,'el',[])
addParameter(p,'fs',[],@isscalar)
addParameter(p,'w',[])
addParameter(p,'EQ',0)
addParameter(p,'fc',[],@isscalar)
addParameter(p,'nfftIn',[],@isscalar)
addParameter(p,'nfftOut',[],@isscalar)
addParameter(p,'r',0.0875,@isscalar)
addParameter(p,'c',343,@isscalar)
addParameter(p,'earAz',[pi/2, 3*pi/2])
addParameter(p,'earEl',[pi/2, pi/2])
addParameter(p,'Nmax',35,@isscalar)
addParameter(p,'covConstPlots',false,@isscalar)
addParameter(p,'fadeIn',0,@isscalar);
addParameter(p,'fadeOut',0,@isscalar);
addParameter(p,'frac',2,@isscalar);
addParameter(p,'dualBand',false,@isscalar)
addParameter(p,'tapering',0,@isscalar)
addParameter(p,'Hnm_ref',[])

parse(p,varargin{:})
mode = p.Results.mode;
az = p.Results.az;
el = p.Results.el;
fs = p.Results.fs;
w = p.Results.w;
EQ = p.Results.EQ;
fc = p.Results.fc;
nfftIn = p.Results.nfftIn;
nfftOut = p.Results.nfftOut;
r = p.Results.r;
c = p.Results.c;
earAz = p.Results.earAz;
earEl = p.Results.earEl;
Nmax = p.Results.Nmax;
covConstPlots = p.Results.covConstPlots;
fadeIn = p.Results.fadeIn;
fadeOut = p.Results.fadeOut;
frac = p.Results.frac;
dualBand = p.Results.dualBand;
tapering = p.Results.tapering;
Hnm_ref = p.Results.Hnm_ref;

if isempty(fc) && ~strcmpi(mode,'BiMagLS')
fc = N*c/(2*pi*r); % if fc not provided, use aliasing frequency
end

clear p
varOut = struct();

%% Process SOFA input
if isstruct(h) % if h is a struct, assume it is a SOFA object
if ~isempty(fs) || ~isempty(az) || ~isempty(el)
warning('Loading from SOFA overwrites parameters fs, az and el')
end
[h,fs,az,el] = sofa2hrtf(h); % overwrite az, el and fs
end

%% Preprocessing
% Zero-pad if indicated
if ~isempty(nfftIn)
% Window
win = hann(2*fadeIn-1);
win = [ones(size(h,1)-fadeIn,1);win(fadeIn:end)];
rms_before = rms(h);
h = h.*win; % rename to h later
rms_after = rms(h);
nrg_loss = 100*(1-sum(rms_after(:))/sum(rms_before(:)));
if nrg_loss > 1
warning('%0.2f%% of the HRIR energy was lost when windowing. Consider disabling zero-padding or circ-shifting the HRIR to the right',nrg_loss)
end
% Zero pad
h(end+1:nfftIn,:,:) = 0;
end

% Some parameters
irlen = size(h,1);
nfft = 2^nextpow2(irlen);
nfreqs = nfft/2+1;
f = linspace(0,fs/2,nfreqs).'; % frequency vector
az = az(:); % force column vector
el = el(:);
w = w(:);

% Calculate delay so the SH signal is centered at t=0
% We do this by calculating the phase delay of the omni channel
% This is recommended for magLS calculations and doesn't hurt the other
% methods

alignOption = 1; % 1=phase delay, 2=onset detection, 0=nothing
if alignOption == 1 % Option 1: phase delay
H = ffth(h,nfft,1); % to frequency domain
Y0 = AKsh(0, [], az*180/pi, el*180/pi, 'real').';
if ~isempty(w)
Y0_inv = 4*pi*w.*Y0'; % if integrations weights are provided, use them
else
Y0_inv = pinv(Y0); % if not, the pseudoinverse will do just fine
end
H0 = mult3(H,Y0_inv); % 0th order SH signal
for ii = 1:size(H0, 3)
pd(:,ii) = -unwrap(angle(H0(2:end,1,ii)))./(2*pi*f(2:end)); % phase delay in s
end
pd_mean = mean(pd(:)); % avg delay
for jj = 1:size(H, 3)
H(:,:,jj) = H(:,:,jj).*repmat(exp(1i*2*pi*f*pd_mean), 1, size(H,2)); % subtract delay
end
elseif alignOption == 2 % Option 2: onset detection
onsL = AKonsetDetect(h(:,:,1)); % left ear onsets
onsR = AKonsetDetect(h(:,:,2)); % right rear onsets
ons = min([onsL;onsR]); % ipsilateral ear onsets
% nshift = -floor(min(ons))); % detect earliest onset
nshift = -round(median(ons)); % median onset
h = circshift(h,nshift);
H = ffth(h,nfft,1);
else
H = ffth(h,nfft,1);
end

%% Preprocess using the indicated method

if strcmpi(mode,'Trunc') % just order truncation
% Order-truncated SH-HRTF (faster than order Nmax + truncation)
Y = AKsh(N,[],az*180/pi,el*180/pi,'real').';
if ~isempty(w)
Y_inv = 4*pi*w.*Y'; % use integrations weights if provided
else
Y_inv = pinv(Y); % if not, pseudoinverse will do just fine
end
Hnm = mult3(H,Y_inv);

elseif strcmpi(mode,'SpSub') % subsampling aka virtual loudspeaker
Hnm = toSH_SpSub(H,N,az,el,w,Nmax);

elseif strcmpi(mode,'FDTA') % frequency-dependent time-alignment
if fc>=fs/2
warning('fc (%0.2f Hz) >= fs/2 (%0.2f Hz). FDTA preprocessing has no effect...',fc,fs/2)
end
[Hnm,varOut.fc] = toSH_FDTA(H,N,az,el,fs,w,fc,r,earAz,earEl);

elseif strcmpi(mode,'MagLS') % magnitude least squares
if fc>=fs/2
warning('fc (%0.2f Hz) >= fs/2 (%0.2f Hz). MagLS preprocessing has no effect...',fc,fs/2)
end
[Hnm,varOut.fc] = toSH_MagLS(H,N,az,el,fs,w,fc,frac,r);

elseif strcmpi(mode,'TA') % time alignment (ear alignment)
Hnm = toSH_TA(H,N,az,el,fs,w,r,earAz,earEl);

elseif strcmpi(mode,'BiMagLS') % ear alignment + MagLS
if fc>=fs/2
warning('fc (%0.2f Hz) >= fs/2 (%0.2f Hz). BiMagLS preprocessing has no effect...',fc,fs/2)
end
[Hnm,varOut.fc] = toSH_BiMagLS(H,N,az,el,fs,w,fc,frac,r,earAz,earEl);

elseif strcmpi(mode,'SpSubMod') % FDTA + SpSub
if fc>=fs/2
warning('fc (%0.2f Hz) >= fs/2 (%0.2f Hz). SpSubMod preprocessing is the same as SpSub...',fc,fs/2)
end
[Hnm,varOut.fc] = toSH_SpSubMod(H,N,az,el,fs,w,fc,r,earAz,earEl,Nmax);

else
error('Unknown method %s. Use one of these: ''Trunc'', ''SpSub'', ''FDTA'', ''MagLS'', ''TA'', ''BiMagLS'', ''SpSubMod''',mode)
end

%% Tapering/max-rE weights

if tapering > 0

if tapering == 4 && ~dualBand && EQ == 0
warning('Normalized Max-rE weights (tapering=4) should be used with options dualBand=1 or EQ>0. Expect magnitude errors...')
end

[~,wnm] = getTaperWin(N,tapering);

if ~dualBand % apply tapering to whole signal [6]

Hnm = Hnm.*wnm.';

else % apply only to high frequencies [10]

if fc>=fs/2
warning('fc (%0.2f Hz) >= fs/2 (%0.2f Hz). Skipping dual band tapering...',fc,fs/2)
else
% Make crossover filters at fc (usually, f alias)
% Filters are phase-matched and their sum ~= dirac delta
lFilt = 100; % order of fir filters
lpf = fir1(lFilt, fc/(fs/2), 'low').';
hpf = fir1(lFilt, fc/(fs/2), 'high').';

% Apply filters to the SH-HRTF
nfreqs = size(H,1);
for ll = 1:size(Hnm, 3)
Hnm_lf(:,:,ll) = Hnm(:,:,ll).*repmat(ffth(lpf,2*(nfreqs-1)), 1, size(Hnm, 2)); % low-passed version
Hnm_hf(:,:,ll) = Hnm(:,:,ll).*repmat(ffth(hpf,2*(nfreqs-1)), 1, size(Hnm, 2)); % high-passed version
p = linspace(0,pi*(lFilt/2+1),nfreqs).'; % filter phase delay
Hnm_lf(:,:,ll) = Hnm_lf(:,:,ll).*repmat(exp(1i*p), 1, size(Hnm_lf, 2)); % undo filter delay
Hnm_hf(:,:,ll) = Hnm_hf(:,:,ll).*repmat(exp(1i*p), 1, size(Hnm_hf, 2)); % undo filter delay
end
for mm = 1:size(Hnm_hf, 3)
% Apply weights to high frequency version
Hnm_hf(:,:,mm) = Hnm_hf(:,:,mm) .* repmat(wnm.', size(Hnm_hf, 1), 1);
end
% Output is the sum of the two
Hnm = Hnm_lf + Hnm_hf;
end
end

end

%% Equalisation/covariance constraint

G1 = ones(nfreqs,2); % global EQ (left, right)
G2 = zeros(nfreqs,2); % left/right mix (for covariance constraint)

if EQ > 0

if EQ==1 || EQ==2 % these modes require a high-order reference
if isempty(Hnm_ref)
Y = AKsh(Nmax,[],az*180/pi,el*180/pi,'real').';
if contains(mode,'ear') % time-align reference if required
kr = 2*pi*f*r/c;
p = earAlign(kr,az,el,earAz,earEl);
Hear = H.*exp(-1i*p);
Hnm_ref = mult3(abs(Hear),pinv(Y));
else
Hnm_ref = mult3(abs(H),pinv(Y));
end
end
end

if EQ == 1 % covariance constraint [9, sec.4.11.3]

[~,G1,G2] = applyCovConst(Hnm,Hnm_ref,covConstPlots,fs);

elseif EQ == 2 % diffuse field EQ (HRF from [8])

DF_high = sqrt( sum(abs(Hnm_ref).^2,2) )/(4*pi); % [8] eq.13
DF_high = mean(DF_high,3); % average across ears
DF_N = sqrt( sum(abs(Hnm).^2,2) )/(4*pi);
DF_N = mean(DF_N,3); % average across ears
% Option 1: direct inverse (could produce ringing artifacts)
% G1 = h_high ./ h_N;
% Option 2: regularized inverse (better)
G1 = autoreg_minphase(DF_N,DF_high,fs);
G1 = repmat(G1,1,2); % same for both ears

elseif EQ == 3 % spherical head filters (SHF from [8])

if ~strcmpi(mode,'Trunc')
warning('SHF equalisation should only be used for non-preprocessed HRTFs; consider using EQ=3 (diffuse field EQ) instead')
end
if dualBand
warning('SHF equalisation not optimised for dual band processing; consider using EQ=3 (diffuse field EQ) instead')
end

kr = 2*pi*f*r/c;
G1 = getSHF(N,kr,tapering);
G1 = repmat(G1,1,2); % same for both ears

end

Hnm_EQ(:,:,1) = Hnm(:,:,1).*repmat(G1(:,1), 1, size(Hnm(:,:,1), 2)) + Hnm(:,:,2).*repmat(G2(:,1), 1, size(Hnm(:,:,1), 2));
Hnm_EQ(:,:,2) = Hnm(:,:,2).*repmat(G1(:,2), 1, size(Hnm(:,:,2), 2)) + Hnm(:,:,1).*repmat(G2(:,2), 1, size(Hnm(:,:,2), 2));
Hnm = Hnm_EQ;
clear Hnm_EQ

end

varOut.G1 = G1;
varOut.G2 = G2;

%% Postprocessing
% Undo alignment at t=0
if alignOption == 1 % Option 1: phase delay
%Hnm = Hnm.*exp(-1i*2*pi*f*pd_mean);
for jj = 1:size(H, 3)
Hnm(:,:,jj) = Hnm(:,:,jj).*repmat(exp(1i*2*pi*f*pd_mean), 1, size(Hnm,2)); % subtract delay
end
hnm = iffth(Hnm,[],1);
elseif alignOption == 2 % Option 2: onset detection + time shift
hnm = iffth(Hnm,[],1);
hnm = circshift(hnm,-nshift);
else
hnm = iffth(Hnm,[],1);
end

% Crop/window output
rms_before = rms(hnm);
if nfftOut < nfft
hnm(nfftOut+1:end,:,:) = []; % crop
end
fadeInVec = sin(linspace(0,pi/2,fadeIn).').^2; % fade in
fadeOutVec = cos(linspace(0,pi/2,fadeOut).').^2; % fade out
win = [fadeInVec;ones(size(hnm,1)-fadeIn-fadeOut,1);fadeOutVec]; % window
for kk=1:size(hnm, 3)
hnm(:,:,kk) = hnm(:,:,kk).*repmat(win, 1, size(hnm, 2)); % apply window
end
if nfftOut > nfft
hnm(end+1:nfftOut,:,:) = 0; % zero-pad
end
rms_after = rms(hnm);
nrg_loss = 100*(1-sum(rms_after(:))/sum(rms_before(:)));
if nrg_loss > 1
warning('%0.2f%% of the HRIR energy was lost when windowing. Consider changing fade in/out or output length.',nrg_loss)
end