% **********************************************
% numerical optimization of analog filter
% Markus Nentwig, 18.8.2012
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
% **********************************************
function analogFilterDesign()
    if false
        % **********************************************
        % build list of equalizers to try
        % **********************************************
        caseList = {struct()};
        caseList = iterateEq(caseList);
    else
        % **********************************************
        % design plain filter (non-equalized)
        % **********************************************
        %caseList = {struct('h', [-1, 2, 2, -2])}; % e = -45 dB with Q = 0.73
        caseList = {struct('h', [1])}; % e = -39.5 dB with Q = 1.63
    end
    resultList = {};    
    
    % **********************************************
    % design filter for each EQ and keep the best one
    % **********************************************
    bestErr = 9e9; bestEq = [];
    for ix = 1 : numel(caseList)
        sprintf('%i out of %i\n', ix, numel(caseList))
        c = caseList{ix};
        [err, res] = optimizeOneDesign(c);
        
        if err < bestErr
            bestErr = err;
            bestEq = c.h;
            
            doPlot(res);
            pause(0.01);
        end
    end
end

% **********************************************
% exhaustive search over a range of
% multiplierless FIR filters
% **********************************************
function caseListOut = iterateEq(caseListIn)
    caseListOut= {};
    done = struct();
    
    % consider those FIR tap values (powers of 2 => bit shift)
    % eqList = [-1/16, -1/8, -1/4, -1/2, 0, 1/2, 1/4, 1/8, 1/16];
    eqList = [-1/4, -1/2, 0, 1/2, 1/4];

    % replicate for each entry in the input list
    for ix = 1 : numel(caseListIn);
        
        % iterate through all combinations
        for tap1 = eqList
            for tap2 = eqList
                for tap3 = eqList
                    for tap4 = eqList
                        % EQ at DAC rate
                        h = [tap1, tap2, tap3, tap4]; 
                        
                        %h = [tap1 0 0 0 tap2 0 0 0 tap3 0 0 0 tap4 0 0 0];
                        if sum(h) > 0
                            % normalize to unity DC gain
                            h = h / sum(h);
                            
                            % discard shifted or scaled replicas
                            ID = canonical_FIR_ID(h);
                            if ~isfield(done, ID)
                                done.(ID) = true;
                                c = caseListIn{ix};
                                c.h = h;
                                caseListOut{end+1} = c;
                            end % if ID not found
                        end % if not completely zero
                    end % for tap
                end % for tap
            end % for tap
        end % for tap
    end % for entries in input list 
end

% returns a key for a FIR impulse response, 
% disregards time shift (i.e. [0 1 0 0] and [1 0 0 0] result
% in the same key)
function key = canonical_FIR_ID(h)
    
    while h(1) == 0
        h(1) = [];
    end
    while h(end) == 0
        h(end) = [];
    end
    
    key = ['a', sprintf('%f_', h)];
    key = strrep(key, '-', 'm');
    key = strrep(key, '.', 'd');
end    

function [err, res] = optimizeOneDesign(c)
    
% data structure with information that is needed
% inside the optimization. 
% To avoid the clumsiness of passing auxillary variables
% to a generic optimizer, a global variable is used here.
    global q;
    q = struct();
    
    q.H_plot = [];
    q.legend = {};
    q.plotStyle = {};
    
    % evaluation frequencies

    % small frequency span for group delay calculation
    q.deltaF_Hz = 123.45;
    
    % **********************************************
    % Target response and weighting factors
    % **********************************************
    q = setup_filterDesignProblem(q, 30);
    q.H_plot(end+1, :) = q.w; q.legend{end+1} = 'weight'; q.plotStyle{end+1} = 'y';

    q.fMinusDelta = q.f_Hz - q.deltaF_Hz / 2;
    q.fPlusDelta = q.f_Hz + q.deltaF_Hz / 2;

    % **********************************************
    % Group delay weighting vector
    % **********************************************    
    % the linear error at any frequency is proportional to
    % - the magnitude of the test signal (weight)
    % - the phase error
    % and the phase error is proportional to
    % - group delay at that frequency
    % - the frequency
    q.wGrpDelay = q.wPass .* q.f_Hz;
    
    % alternatively, we could use the power or magnitude at each frequency
    % for weighting (ad-hoc methods, should give inferior results
    % to above)
    %q.wGrpDelay = q.wPass .^ 2;
    %q.wGrpDelay = q.wPass;
    
    % normalize
    q.wGrpDelay = q.wGrpDelay / sum(q.wGrpDelay);
    
    % **********************************************
    % filter stages that are to be optimized
    % initial value vector for optimizer
    % **********************************************
    [q, vec] = setup_optimizedStages(q);
    
    % **********************************************
    % Other stages (not optimized in this function)
    % taken into account via group delay compensated
    % frequency response
    % **********************************************
    q = setup_constantStages(q, c);
    
    % **********************************************
    % run optimization
    % **********************************************    
    vec = fminsearch(@objectiveFunction, vec, [0, 1e-6, 0, 0, 0, 0, 0, 0, 0, 1e9]);
    % calculate final error
    err = objectiveFunction(vec)
    
    % **********************************************
    % copy elements of result vec back into data structure q
    % **********************************************        
    res = restoreOptimVar(vec, q);

    % **********************************************
    % print optimized variables
    % **********************************************            
    dumpOptimVar(res);

    % **********************************************
    % plot (evaluate once again)
    % **********************************************            
    [H, HStage1, HStage2] = H_optim(res, res.f_Hz);
    res.H_plot(end+1, :) = HStage1; 
    res.legend{end+1} = 'p1'; 
    res.plotStyle{end+1} = 'c';
    
    res.H_plot(end+1, :) = HStage2; 
    res.legend{end+1} = 'p2'; 
    res.plotStyle{end+1} = 'b';
    
    res.H_plot(end+1, :) = H .* q.H_other; 
    res.legend{end+1} = 'combined'; 
    res.plotStyle{end+1} = 'r';

    res.err = err;
    res.h = c.h;
end

function q = setup_filterDesignProblem(q, fStop_dB)
    1;
    % sampling rate (DAC)
    q.rate_Hz = 38.4e6;
    
    % signal bandwidth (two-sided)
    BW = 3.84e6;
    
    % evaluation frequencies. Note that both the relative density of
    % frequency samples and the weighting function determine, 
    % how the error is seen by the objective function.
    % For the optimization, frequencies with weight = 0 could
    % be omitted, but they are kept for plotting.
    nFreq = 300;
    q.f_Hz = linspace(0.1e6, (q.rate_Hz + BW/2) * 1.1, nFreq);
    
    % wanted signal spectrum
    Hsig = rootRaisedCosineSpectrum(q.f_Hz, BW, 0.22);
    
    % alias spectrum (shifted to sampling rate)
    Halias = rootRaisedCosineSpectrum(q.f_Hz - q.rate_Hz, BW, 0.22);
    
    maskSig = abs(Hsig) > eps;
    maskAlias = abs(Halias) > eps;
    Hnom = zeros(size(q.f_Hz));
    wPass = zeros(size(q.f_Hz));
    
    % targeted frequency response is 1 where signal spectrum is nonzero
    % zero elsewhere
    % zero weights in-between make it a "don't care" region.
    Hnom(maskSig) = 1;
    
    % accuracy target weighted with signal spectrum
    wPass(maskSig) = Hsig(maskSig);
    
    % mask including stopband
    w = wPass;
    % weighting with known alias spectrum 
    w(maskAlias) = Halias(maskAlias) * 10 .^ (30/20);
    
    q.Hnom = Hnom; % desired frequency response
    q.w = w; % weights over all frequencies
    q.wPass = wPass; % passband-only weights
end

function H = rootRaisedCosineSpectrum(f_Hz, BW_Hz, alpha)
    c = abs(f_Hz * 2 / BW_Hz);        
    c = (c-1) / alpha;
    c = min(c, 1);
    c = max(c, -1);
    H = sqrt(1/2+cos(pi/2*(c+1))/2);
end

function q = setup_constantStages(q, c)
    H_other = ones(size(q.f_Hz));
    H_otherGdCompensated = ones(size(q.f_Hz));    
    
    % **********************************************
    % sample-and-hold stage (constant, precomputation)
    % **********************************************
    % centered rectangular pulse (non-causal but zero group delay)
    H = sinc(q.f_Hz / q.rate_Hz);    
    
    q.H_plot(end+1, :) = H; q.legend{end+1} = 'sinc'; q.plotStyle{end+1} = 'g';
    
    H_other = H_other .* H;

    if false
        % determine group delay of the S&H stage
        H1 = sinc(q.fMinusDelta / q.rate_Hz);    
        H2 = sinc(q.fPlusDelta / q.rate_Hz);    
        gd_s = calcEffGroupDelay_s(H1, H2, q.deltaF_Hz, q.wGrpDelay);
        % and remove it
        H = compensateGroupDelay(H, qq.f_Hz, gd_s);
        H_otherGdCompensated = H_otherGdCompensated .* H;;
    else
        % but it is known beforehand that the sinc() spectrum
        % is Hermitian symmetric
        % => there is no group delay to compensate
        H_otherGdCompensated = H_otherGdCompensated .* H;;
    end
        
    % **********************************************
    % FIR stage (constant, pre-computation)
    % **********************************************
    % calculate effective group delay
    H1 = H_FIR(c.h, q.fMinusDelta / q.rate_Hz); % at minus deltaF
    H2 = H_FIR(c.h, q.fPlusDelta / q.rate_Hz); % at plus deltaF
    gd_s = calcEffGroupDelay_s(H1, H2, q.deltaF_Hz, q.wGrpDelay);
    
    % calculate frequency response
    H = H_FIR(c.h, q.f_Hz / q.rate_Hz); % at nominal f
    H_other = H_other .* H;

    % calculate frequency response with effective 
    % group delay removed
    H = compensateGroupDelay(H, q.f_Hz, gd_s);
    H_otherGdCompensated = H_otherGdCompensated .* H;

    q.H_plot(end+1, :) = H; q.legend{end+1} = 'FIR'; q.plotStyle{end+1} = 'k';

    q.H_other = H_other;
    q.H_otherGdCompensated = H_otherGdCompensated;
end        

function [q, vec] = setup_optimizedStages(q)
    1;
    % **********************************************
    % optimization variables
    % **********************************************
    % constant gain
    q.H0 = 1;
    
    q.fNorm_Hz = 20e6;
    q.Q1 = 2;
    q.f1_Hz = 5e6;
    q.f2_Hz = 5e6;
    
    % **********************************************
    % manage parameters that are given to the optimizer.
    % if any SetOptimVar() line is commented out, the 
    % respective variable will not be optimized and its
    % initial value used. 
    % **********************************************    
    vec = []; % initial value for optimization variables
    [vec, q] = setOptimVar(vec, q, 'f1_Hz', 10e6);
    [vec, q] = setOptimVar(vec, q, 'f2_Hz', 10e6);
    [vec, q] = setOptimVar(vec, q, 'Q1', 1);
    [vec, q] = setOptimVar(vec, q, 'H0', 1);
end

function err = objectiveFunction(vec)
    global q; % auxillary data passed here
    
    % **********************************************
    % copy optimizer variables to their place in the
    % general data structure
    % **********************************************
    qq = restoreOptimVar(vec, q);
    
    % **********************************************
    % frequency response
    % **********************************************
    H = calcH_grpDelayComp(qq);
    
    % **********************************************
    % objective function: Weighted vector error
    % **********************************************
    % weight can be interpreted as magnitude of a test signal
    % at each frequency.
    % weighted error is product of input magnitude with 
    % error in H
    % => do not square w
    err = norm((H - q.Hnom) .* qq.w);
end

function H = H_FIR(h, fnorm)
    zInv = exp(-2i * pi * fnorm);
    H = polyval(fliplr(h), zInv);    
end

% **********************************************
% calculate the frequency response of the analog
% filter that is to be optimized.
% fNorm_Hz is redundant, but needed for numerical
% reasons
% **********************************************
function [H, HStage1, HStage2] = H_optim(qq, f_Hz)
    1;    
    % **********************************************
    % constant gain
    % **********************************************
    H = qq.H0;
    
    % **********************************************
    % passive pole
    % fNorm_Hz is redundant but needed for numerical reasons
    % **********************************************
    s = 1i * f_Hz / qq.fNorm_Hz;
    a2 = [qq.fNorm_Hz / qq.f2_Hz, 1];
    HStage1 = 1 ./ polyval(a2, s);
    H = H .* HStage1;

    % **********************************************
    % resonant pole
    % **********************************************
    % reflect negative values
    qq.f1_Hz = abs(qq.f1_Hz);
    qq.Q1 = abs(qq.Q1); 
    
    % [s^2, s, 1] coefficients
    a = [1 / ((qq.f1_Hz / qq.fNorm_Hz) ^ 2), ...
         1/(qq.Q1 * qq.f1_Hz / qq.fNorm_Hz), ...
         1];
    
    HStage2 = 1 ./ polyval(a, s);
    H = H .* HStage2;
end

% **********************************************
% calculates the frequency response from cascading
% the optimized stages and other stages, and compensate
% the group delay based on the same frequency 
% weighting function that is used for the
% filter optimization.
% **********************************************
function H = calcH_grpDelayComp(qq)
    1;
    % **********************************************
    % determine group delay of the stages that are 
    % being optimized
    % **********************************************
    HMinusDelta = H_optim(qq, qq.fMinusDelta);
    HPlusDelta = H_optim(qq, qq.fPlusDelta);
    gd_s = calcEffGroupDelay_s(HMinusDelta, HPlusDelta, qq.deltaF_Hz, qq.wGrpDelay);

    if true
        % get frequency response at center (simply average, deltaF is very small)
        H = (HMinusDelta + HPlusDelta) / 2;
    else
        % alternatively, re-evaluate (slower)
        H = H_optim(qq, qq.f_Hz, false);
    end
    
    % **********************************************
    % remove the effective group delay
    % **********************************************    
    H = compensateGroupDelay(H, qq.f_Hz, gd_s);
    
    % **********************************************
    % cascade corrected frequency response with 
    % other stages (corrected in a similar manner)
    % **********************************************    
    H = H .* qq.H_otherGdCompensated;
end

% **********************************************
% calculates the effective group delay for a signal
% with a magnitude spectrum given by weight.
% **********************************************    
function grpDelay_s = calcEffGroupDelay_s(H1, H2, deltaF_Hz, weight)
    deltaOmega_rad = deltaF_Hz * 2 * pi;
    
    % determine angle delta between frequency responses at each freq. point
    deltaAngle_rad = unwrap(angle(H2) - angle(H1));
    
    % calculate group delay at each freq. point
    grpDelay_s = -deltaAngle_rad / deltaOmega_rad;

    % weighted group delay (implicitly suppresses frequencies without
    % signal energy)
    grpDelay_s = sum(grpDelay_s .* weight);
end

% **********************************************
% applies a negative delay to the frequency response
% **********************************************    
function H = compensateGroupDelay(H, f_Hz, grpDelay_s)
    H = H .* exp(2i * pi * f_Hz * grpDelay_s);    
end

% **********************************************
% helper functions for optimization variable 
% management
% **********************************************

% 'moves' variable vName into the optimization. 
% copies value to initial optim. vector vec.
% vRef: optional redundant scaling factor to bring all
% optimizer variables into the same numerical range.
% Mirror image of "restoreOptimVar"
function [vec, q] = setOptimVar(vec, q, vName, vRef)
    if nargin < 4
        % normalize initial value to 1.
        % this won't work if it is too close to 0.
        vRef = q.(vName);
    end
    vec(end+1) = q.(vName) / vRef;
    f = sprintf('optVar%i', numel(vec));
    q.(f) = vName;    
    f = sprintf('optVarRef%i', numel(vec));
    q.(f) = vRef;    
end

% optimized variables are passed in vec, both to the 
% objective function and as optimizer result.
% Restore them into data structure q.
% Mirror image of "setOptimVar"
function q = restoreOptimVar(vec, q)    
    for ix = 1 : numel(vec)
        f1 = sprintf('optVar%i', ix);
        f2 = sprintf('optVarRef%i', ix);
        
        vName = q.(f1);
        % - look up value from vec
        % - denormalize
        % - set in q
        v = vec(ix) * q.(f2);
        q.(vName) = v;
    end
end

% prints out all variables that were used for optimization.
% useful, if q contains too much other data to be useful.
function q = dumpOptimVar(q)    
    ix = 1;
    while true
        f1 = sprintf('optVar%i', ix);
        if ~isfield(q, f1)
            break;
        end

        vName = q.(f1);
        sprintf('%s = %f\n', vName, q.(vName))
        ix = ix + 1;
    end
end

% plot collected frequency responses
function doPlot(q)
    figure(1); clf(); grid on; hold on;
    for ix2 = 1 : size(q.H_plot, 1)
        y = q.H_plot;
        y = y(ix2, :);
        y = 20*log10(abs(y) + eps);
        handle = plot(q.f_Hz, y, q.plotStyle{ix2});
        set(handle, 'lineWidth', 3);
    end
    xlabel('f / Hz');
    ylabel('|H|, dB');
    title([...
        sprintf('e = %f dB', 20*log10(q.err + eps)), ...
        ' h=', sprintf('%g;', q.h)]);
    legend(q.legend);    
end
