% *******************************************************************
% *******************************************************************

resonatorNames = {'a', 'ae', 'e', 'schwa'};
numResonators = length(resonatorNames);

% *******************************************************************
% Initial estimates of the first 6 resonance frequencies to help the 
% peak-picking algorithm. Values of zero mean "don't use", for example,
% because they are above 5 kHz or because the peak is less than 3 dB
% above the left or right valley.
% *******************************************************************

freqEstimateBasic = [ ...
    697, 1128, 2610, 3025, 4519, 0; ...        % /a/
    570, 1650, 2124, 3093, 4337, 0; ...          % /ae/
    301, 1717, 2417, 3801, 4849, 5861; ...          % /e/
    462, 1395, 2365, 3339, 4319, 5341 ];            % schwa
    
freqEstimateNotch1 = [ ...
    710, 1139, 2617, 3107, 4551, 0; ...        % /a/
    582, 1691, 2137, 3175, 4440, 0; ...          % /ae/
    295, 1738, 2484, 3840, 4960, 5878; ...          % /e/
    477, 1441, 2427, 3427, 4417, 5432 ];            % schwa

freqEstimateNotch2 = [ ...
    750, 1215, 2682, 3233, 4604, 0; ...        % /a/
    623, 1736, 2206, 3292, 4535, 0; ...          % /ae/
    334, 1812, 2686, 3895, 5184, 5940; ...          % /e/
    496, 1495, 2513, 3502, 4497, 5492 ];            % schwa

freqEstimateNotch3 = [ ...
    773, 1293, 2713, 3460, 4611, 0; ...        % /a/
    657, 1788, 2280, 3402, 4626, 0; ...          % /ae/
    363, 1872, 3025, 3974, 5427, 6030; ...          % /e/
    518, 1567, 2597, 3594, 4565, 5500 ];            % schwa

freqEstimateNotch4 = [ ...
    798, 1424, 2741, 3571, 4600, 0; ...        % /a/
    700, 1838, 2354, 3516, 4720, 0; ...          % /ae/
    403, 1932, 3353, 4089, 5492, 6061; ...          % /e/
    551, 1665, 2739, 3719, 4656, 5571 ];            % schwa

disp(' ');

% *******************************************************************
% *******************************************************************

for n=1:1:numResonators
    
    figure;     % Open an individual figure for each resonator.

    % The first index in the function calls is the subplot index of a 3 x 2 
    % plot matrix.
    basicFileName = [resonatorNames{n} '-notch-0.txt'];
    plotSpectra(1, basicFileName, '');
    plotSpectra(2, [resonatorNames{n} '-notch-1.txt'], basicFileName);
    plotSpectra(3, [resonatorNames{n} '-notch-2.txt'], basicFileName);
    plotSpectra(4, [resonatorNames{n} '-notch-3.txt'], basicFileName);
    plotSpectra(5, [resonatorNames{n} '-notch-4.txt'], basicFileName);
    
    % -notch-0
    
    [peakFreq, bwLeftFreq, bwRightFreq] = getResonanceFreqAndBw([resonatorNames{n} '-notch-0.txt'], freqEstimateBasic(n, :));
    plotFoundResonances(1, peakFreq, bwLeftFreq, bwRightFreq);
    notch0Freq_Hz = peakFreq;
    notch0Bw_Hz = bwRightFreq - bwLeftFreq;
    
    % -notch-1

    [peakFreq, bwLeftFreq, bwRightFreq] = getResonanceFreqAndBw([resonatorNames{n} '-notch-1.txt'], freqEstimateNotch1(n, :));
    plotFoundResonances(2, peakFreq, bwLeftFreq, bwRightFreq);
    notch1Freq_Hz = peakFreq;
    notch1Bw_Hz = bwRightFreq - bwLeftFreq;
    
    % -notch-2

    [peakFreq, bwLeftFreq, bwRightFreq] = getResonanceFreqAndBw([resonatorNames{n} '-notch-2.txt'], freqEstimateNotch2(n, :));
    plotFoundResonances(3, peakFreq, bwLeftFreq, bwRightFreq);
    notch2Freq_Hz = peakFreq;
    notch2Bw_Hz = bwRightFreq - bwLeftFreq;
    
    % -notch-3
    
    [peakFreq, bwLeftFreq, bwRightFreq] = getResonanceFreqAndBw([resonatorNames{n} '-notch-3.txt'], freqEstimateNotch3(n, :));
    plotFoundResonances(4, peakFreq, bwLeftFreq, bwRightFreq);
    notch3Freq_Hz = peakFreq;
    notch3Bw_Hz = bwRightFreq - bwLeftFreq;

    % -notch-4

    [peakFreq, bwLeftFreq, bwRightFreq] = getResonanceFreqAndBw([resonatorNames{n} '-notch-4.txt'], freqEstimateNotch4(n, :));
    plotFoundResonances(5, peakFreq, bwLeftFreq, bwRightFreq);
    notch4Freq_Hz = peakFreq;
    notch4Bw_Hz = bwRightFreq - bwLeftFreq;
    
    % Final plot with all data:

    subplot(3, 2, 6);
   
    hold on;
    plot(notch0Freq_Hz, notch0Bw_Hz, '-ko','MarkerFaceColor','black');
    plot(notch1Freq_Hz, notch1Bw_Hz, '-rs','MarkerFaceColor','red');
    plot(notch2Freq_Hz, notch2Bw_Hz, '-go','MarkerFaceColor','green');
    plot(notch3Freq_Hz, notch3Bw_Hz, '-bs','MarkerFaceColor','blue');
    plot(notch4Freq_Hz, notch4Bw_Hz, '-mo','MarkerFaceColor','magenta');
    hold off;

    grid on;
    xlim([0, 6500]);
    ylim([0, 500]);
    xlabel('Frequency in Hz');
    ylabel('Bandwidth in Hz');
end


% *******************************************************************
% *******************************************************************

function plotSpectra(subplotIndex, primaryFileName, secondaryFileName)

    subplot(3, 2, subplotIndex);
    
    % Plot the primary spectrum.
    if (exist(primaryFileName, 'file'))
        data = dlmread(primaryFileName, ' ', 1, 0);    % Skip the first row.
        frequencies_Hz = data(:, 1);
        magnitudes_dB = 20.0*log10(data(:, 2));
        plot(frequencies_Hz, magnitudes_dB, 'k');
    end

    % Plot the secondary spectrum.
    if (exist(secondaryFileName, 'file'))
        data = dlmread(secondaryFileName, ' ', 1, 0);    % Skip the first row.
        frequencies_Hz = data(:, 1);
        magnitudes_dB = 20.0*log10(data(:, 2));
        hold on;
        plot(frequencies_Hz, magnitudes_dB, 'Color', [0.7, 0.7, 0.7]);
        hold off;
    end

    xlim([0, 8000]);
    ylim([-10, 40]);
    title(primaryFileName);
    xlabel('Frequency in Hz');
    ylabel('Magnitude in dB');
    grid on;
end


% *******************************************************************
% Mark the resonances and bandwiths in the given subplot.
% *******************************************************************

function plotFoundResonances(subplotIndex, peakFreq, bwLeftFreq, bwRightFreq)

    subplot(3, 2, subplotIndex);
    hold on;
    
    ymin = -10;
    ymax = 40;
    
    for n = 1:1:length(peakFreq)
        plot([peakFreq(n) peakFreq(n)], [ymin, ymax], 'k', ...
            [bwLeftFreq(n) bwLeftFreq(n)], [ymin, ymax], 'r', ...
            [bwRightFreq(n) bwRightFreq(n)], [ymin, ymax], 'r');
    end
    hold off;
end

    
% *******************************************************************
% Determine the exact frequencies and bandwidths of the resonances of the
% given spectrum (fileName) based on the given rough frequency estimates.
% *******************************************************************

function [peakFreq, bwLeftFreq, bwRightFreq] = getResonanceFreqAndBw(fileName, freqEstimates)
    peakFreq = [];
    bwLeftFreq = [];
    bwRightFreq = [];

    if (~exist(fileName, 'file'))
        return;
    end

    data = dlmread(fileName, ' ', 1, 0);    % Skip the first row.
    frequencies_Hz = data(:, 1);
    magnitudes_dB = 20.0*log10(data(:, 2));
    
    for n = 1:1:length(freqEstimates)
        % When a frequency estimate = 0 this means that we do not use it.
        if (freqEstimates(n) ~= 0)
            
            % Get the indices of the search range.
            deltaFreq_Hz = 100;  % Search between 100 Hz left and right from the initial estimate.
            minSearchIndex = interp1(frequencies_Hz, 1:length(frequencies_Hz), freqEstimates(n) - deltaFreq_Hz, 'nearest');
            maxSearchIndex = interp1(frequencies_Hz, 1:length(frequencies_Hz), freqEstimates(n) + deltaFreq_Hz, 'nearest');

            [dummy, peakIndex] = max(magnitudes_dB(minSearchIndex:maxSearchIndex));

            peakIndex = peakIndex + minSearchIndex - 1;

            thisPeakFreq = frequencies_Hz(peakIndex);
            peakAmp_dB = magnitudes_dB(peakIndex);

            % Go left and right from the peak frequency until the magnitude dropped
            % by 3 dB.
            index = peakIndex;
            while ((index > 1) && (magnitudes_dB(index) > peakAmp_dB - 3.0))
                index = index - 1;
            end
            thisBwLeftFreq = frequencies_Hz(index);

            index = peakIndex;
            while ((index < length(frequencies_Hz)) && (magnitudes_dB(index) > peakAmp_dB - 3.0))
                index = index + 1;
            end
            thisBwRightFreq = frequencies_Hz(index);

            % Append the data of the new resonance to the lists of return
            % values.
            peakFreq = [peakFreq thisPeakFreq];
            bwLeftFreq = [bwLeftFreq thisBwLeftFreq];
            bwRightFreq = [bwRightFreq thisBwRightFreq];
        end
    end
end

