function param = initSGEQ(gains, optimizeBands)
% initSGEQ.m
%
% Initialize the parameters for the sparse graphic equalizers presented in 
% "Sparse Graphic Equalizer Design" by M. Antonelli, J. Liski, and 
% V. Valimaki in IEEE SPL (2022).
%
% Input parameters: 
% gains = user set gains
% optimizeBands  = performs gain-dependent bandwidths and the gain at the
% Nyquist limit (true/false[default])
%
% Output:
% param  = struct containing the initialized parameters
%
% Uses sgeqParams.mat, interp1qr.m, peq.m
%
% Created by Mario Antonelli, Moricone, Roma, Italia, 28 March 2022
% for the
% Acoustics Laboratory
% Dept. of Signal Processing and Acoustics
% Aalto University


if nargin<2
    optimizeBands = false;
end
% Third-octave
param.fs = 44.1e3; % Sample rate
param.fc1 = 1000.*(2.^((1/3)*(-17:13))); % Exact log center frequencies for filters
param.fc2(1:2:61) = param.fc1;
param.fc2(2:2:61) = sqrt(param.fc1(1:end-1).*param.fc1(2:end)); % Extra design points between filters (Hz)
param.wg = 2*pi*param.fc1/param.fs; % Command gain frequencies in radians
param.wc = 2*pi*param.fc2/param.fs; % Center frequencies in radians for iterative design with extra points
param.c = 0.38; % Gain factor at bandwidth (parameter c)


%% Optimize Band Parameters
% 
% The Nyquist gain for each band filter has been calculated with the help 
% of a sufficiently highly oversampled parametric EQ filter emulating the 
% shape of an analog filter. The sample rate has been set to 10 MHz, and 
% the filter gain has been evaluated at 22.05 kHz, which is the Nyquist 
% limit fs/2 at the sample rate of 44.1 kHz.
% The bandwidth of each band filter has been optimized by using and
% optimization algorithm 'fminbnd2' (a quick implementation taken from
% "Algorithms for Minimization Without Derivatives",
% R. P. Brent, Prentice-Hall, 1973, Dover, 2002.).
Fs = param.fs;
% Center Frequencies
cf = param.fc1;
vgains = (1:33)';
if optimizeBands
    nt = 2^10;
    gw = param.c; % parameter for the dB gain at the bandwidth edges
    f = logspace(1, log10(Fs/2), nt)';
    K = ones(33,numel(cf));
    G1 = 0*K;
    for i = 1:numel(cf)
        for g = 1:33
            fun = @(x)fobjBands(x, cf(i), f, Fs, gw, g);
            [xmin2, ~] = fminbnd2(fun,0.1,1);
            K(g,i) = xmin2;
            Fst = 10*1e6;
            G = 10^(g/20);
            GB = 10^(gw*g/20);
            wt = pi*cf(i)/(Fst/2);
            B = 0.4662*wt;
            [num, den] = peq(1, G, GB, wt, B);
            H2 = freqz(num, den, f/(Fst/2)*pi);
            G1(g,i) = abs(H2(end));
            if g == 11 && i >= 31
                [num, den] = peq(1, G, GB, wt, B);
                H1 = freqz(num, den, f/(Fst/2)*pi);
                wt = pi*cf(i)/(Fs/2);
                B = K(g,i)*wt;
                [num, den] = peq(1, G, GB, wt, B, G1(g,i));
                H2 = freqz(num, den, f/(Fs/2)*pi);
                B = .4662*wt;
                [num, den] = peq(1, G, GB, wt, B, G1(g,i));
                H3 = freqz(num, den, f/(Fs/2)*pi);

                semilogx(f, 20*log10(abs(H1))/g,'g'), hold on
                semilogx(f, 20*log10(abs(H2))/g,'b')
                semilogx(f, 20*log10(abs(H3))/g,'r')
                
                xlim([1000, max(f+10000)])
                xline(max(f),'-',{'Nyquist Frequency'});
                drawnow
            end
        end
    end
    save('sgeqParams.mat', 'K', 'G1','vgains')
else
    load('sgeqParams.mat', 'K', 'G1','vgains')
end

param.K = K;
param.G1 = G1;
param.vgains = vgains;

%% Interaction matrix (Dictionary Building)
gw = param.c; 
wc = param.wc;
wg = param.wg;
Z = exp(-1i*wc); % Creating z^-1
Z2 = Z.^2;       % Creating z^-2
B = zeros(numel(wc), numel(wg));
g = 17;
G  = 10^(g/20);
GB = 10^(gw*g/20);
for i = 1:numel(wg)
    w0 = wg(i);
    k  = interp1qr(vgains, K(:,i),g);
    g1 = interp1qr(vgains, G1(:,i),g);
    Dw = k*w0;
    [b, a] = peq(1, G, GB, w0, Dw, g1);
    H = (b(1) + b(2)*Z + b(3)*Z2)./(a(1) + a(2)*Z + a(3)*Z2);
    B(:, i) = 20*log10(abs(H))/g;
end


param.B = B;
W = eye(numel(wc));
for i = 1:numel(gains)-1
    if gains(i) ~= gains(i+1)
        W(2*i,2*i) = 0.5;
    end
end
param.W = W;
param.vgains = vgains;


function et = fobjBands(K, cf, f, Fs, gw, g)

w0 = pi*cf/(Fs/2);
Fst = 10*1e6;

G = 10^(g/20);
GB = 10^(gw*g/20);
wt = pi*cf/(Fst/2);
B = 0.4662*wt;
[num, den] = peq(1, G, GB, wt, B);
H2 = freqz(num, den, f/(Fst/2)*pi);
B = K*w0;
G1 = abs(H2(end));
[num, den] = peq(1, G, GB, w0, B, G1);
H1 = freqz(num, den, f/(Fs/2)*pi);
et = norm(20*log10(abs(H2))-20*log10(abs(H1)));


function [xf,fval,exitflag] = fminbnd2(funfcn,ax,bx)

tol = 1e-4;

maxfun = 500;
maxiter = 500;
    
% Assume we'll converge
exitflag = 1;
funccount = 0;
iter = 0;
% checkbounds
if ax > bx
    exitflag = -2;
    xf=[]; fval = [];
    return
end





% Compute the start point
seps = sqrt(eps);
c = 0.5*(3.0 - sqrt(5.0));
a = ax; b = bx;
v = a + c*(b-a);
w = v; xf = v;
d = 0.0; e = 0.0;
x= xf; fx = funfcn(x);
funccount = funccount + 1;

fv = fx; fw = fx;
xm = 0.5*(a+b);
tol1 = seps*abs(xf) + tol/3.0;
tol2 = 2.0*tol1;

% Main loop
while ( abs(xf-xm) > (tol2 - 0.5*(b-a)) )
    gs = 1;
    % Is a parabolic fit possible
    if abs(e) > tol1
        % Yes, so fit parabola
        gs = 0;
        r = (xf-w)*(fx-fv);
        q = (xf-v)*(fx-fw);
        p = (xf-v)*q-(xf-w)*r;
        q = 2.0*(q-r);
        if q > 0.0,  p = -p; end
        q = abs(q);
        r = e;  e = d;

        % Is the parabola acceptable
        if ( (abs(p)<abs(0.5*q*r)) && (p>q*(a-xf)) && (p<q*(b-xf)) )

            % Yes, parabolic interpolation step
            d = p/q;
            x = xf+d;

            % f must not be evaluated too close to ax or bx
            if ((x-a) < tol2) || ((b-x) < tol2)
                si = sign(xm-xf) + ((xm-xf) == 0);
                d = tol1*si;
            end
        else
            % Not acceptable, must do a golden section step
            gs=1;
        end
    end
    if gs
        % A golden-section step is required
        if xf >= xm
            e = a-xf;
        else
            e = b-xf;
        end
        d = c*e;
    end

    % The function must not be evaluated too close to xf
    si = sign(d) + (d == 0);
    x = xf + si * max( abs(d), tol1 );
    fu = funfcn(x);
    funccount = funccount + 1;

    iter = iter + 1;
   
    

    % Update a, b, v, w, x, xm, tol1, tol2
    if fu <= fx
        if x >= xf
            a = xf;
        else
            b = xf;
        end
        v = w; fv = fw;
        w = xf; fw = fx;
        xf = x; fx = fu;
    else % fu > fx
        if x < xf
            a = x;
        else
            b = x;
        end
        if ( (fu <= fw) || (w == xf) )
            v = w; fv = fw;
            w = x; fw = fu;
        elseif ( (fu <= fv) || (v == xf) || (v == w) )
            v = x; fv = fu;
        end
    end
    xm = 0.5*(a+b);
    tol1 = seps*abs(xf) + tol/3.0; tol2 = 2.0*tol1;

    if funccount >= maxfun || iter >= maxiter
        exitflag = 0;
        
        fval = fx;
        
        return
    end
end % while


fval = fx;