function [Hnm,fc] = toSH_MagLS(H,N,az,el,fs,w,fc,frac,r)
% Transform HRTF to SH domain at order N with the MagLS method [1],
% following the simplified approach from [2] sec. 4.11.2 and 4.11.3. The
% main difference of this implementation vs [1] is that the phase is
% calculated iteratively in a simple way, instead of with a least squares
% approach.
%
% Important: the method works better if the HRIRs are aligned at t=0. This
% is taken care of in the wrapper function 'toSH'.
%
% SIMPLE USAGE EXAMPLE:
% Hnm = toSH_MagLS(H,15,az,el,48000);
%
% INPUT:
% H = HRTF up to Nyquist frequency (nfreqs x ndirs x 2 ears)
% N = target SH order
% az = HRIR azimuth (ndirs x 1) in rad
% el = HRIR elevation (ndirs x 1) in rad (0=top, pi/2=front)
% fs = sampling frequency in Hz
% w = quadrature weights (ndirs x 1); if empty, use pseudoinverse
% fc = cutoff frequency in Hz above which phase is "disregarded" in
% favour of magnitude; if empty (default), use aliasing frequency
% frac = half-length of the 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.
% r = head radius in m (def=0.085)
%
% OUTPUT:
% Hnm = HRTF's SH coefficients (nfreqs x (N+1)^2 x 2 ears)
% fc = see above
%
% REFERENCES:
% [1] 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.
% [2] 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.
%
% AUTHOR: Isaac Engel - isaac.engel(at)imperial.ac.uk
% February 2021

%% Some parameters
if ~exist('r','var') || isempty(r)
r = 0.085; % default head radius
end
if ~exist('fc','var') || isempty(fc)
c = 343; % speed of sound (m/s)
fc = N*c/(2*pi*r); % if fc not provided, use aliasing frequency
end
if ~exist('frac','var') || isempty(frac)
frac = 2; % default smoothing = half octave
end
nfreqs = size(H,1);
f = linspace(0,fs/2,nfreqs).'; % frequency vector

%% Get the order-truncated SH-HRTF
Y = AKsh(N, [], az*180/pi, el*180/pi, 'real').';
if ~exist('w','var') || ~isempty(w)
Y_inv = 4*pi*w.*Y'; % if integrations weights are provided, use them
else
Y_inv = pinv(Y); % if not, the pseudoinverse will do just fine
end
Hnm = mult3(H,Y_inv); % SH coeffs up to N

%% Apply simplified MagLS from [2] sec.4.11.2

% For frequencies above a given cutoff, target the magnitude of the
% actual HRTF and the phase of the previous frequency bin. This avoids
% rapid changes in phase and the result is good enough for our
% purposes.

% Smooth transition parameters
if frac ~= 0
% Option 1: fc is at the middle of the transition band
fc1 = fc*2^(-1/frac); % smoothing starts 1/frac octaves before fc
fc2 = fc*2^(1/frac); % smoothing ends 1/frac octaves after fc
% Option 2: fc is at the end of the transition band
% fc1 = fc*2^(-1/(2*frac));
% fc2 = fc;
% Look for nearest available frequencies
fc1_ind = find(f>fc1,1,'first');
fc2_ind = find(f<fc2,1,'last');
fc1 = f(fc1_ind);
fc2 = f(fc2_ind);
% Define frequency and weight vector
fcvec_log = logspace(log10(fc1),log10(fc2),100); % f vector (log scale)
sw_log = linspace(0,1,100); % transition weights (log scale)
fcvec = f(fc1_ind:fc2_ind); % f vector (lin scale)
sw = interp1(fcvec_log,sw_log,fcvec,'spline'); % weights (lin scale)
sw = [zeros(fc1_ind-1,1);sw(:);ones(nfreqs-fc2_ind,1)]; % for all freqs
else
sw = ones(nfreqs,1);
fc1_ind = find(f>fc,1,'first');
end

for fi=fc1_ind:nfreqs % for frequencies above the cutoff
prevH = mult3(Hnm(fi-1,:,:),Y); % previous fi estimated HRTF
mag = abs(H(fi,:,:)); % magnitude of actual HRTF
phase = angle(prevH); % phase of previous fi
currH = mag.*exp(1i*phase); % current fi estimated HRTF
newHnm = mult3(currH,Y_inv); % magLS estimation of the SH-HRTF
Hnm(fi,:,:) = sw(fi)*newHnm + (1-sw(fi))*Hnm(fi,:,:); % smoothing
end