% function m = hwpsDemo()
% You can obtain this Matlab script here
% Copyright Mathieu Lagrange (lagrange at uvic dot ca)
%
% Demonstration of the Harmonically Wrapped Peak Similarity measure.
% We consider a discrete set of frequency components and compute all
% similarity among them by considering the harmonic cue

% debug toggle set to one for step-by-step plot
%              set to two for similarity matrix visualization only
debug=1;
% type of estimation for the fundamental
fundamentalEstimateType = 'conservative';
% fundamentalEstimateType = 'groundThruth';
% fundamentalEstimateType = 'underestimate';

% number of bins of the histograms
l=10;

% first set with four harmonics and two noisy peaks
% f=[440, 880, 1320, 1760, 2173]; %, 511];
% a=[.8, .6, .4, .4];

% second set with two sets of harmonics and one shared harmonic
f=[440, 880, 1320, 1760, 2200, 550, 1100, 1650, 2750];
a=[.8, .8, .6, .4, .4, 1, .8, .6, .4];

% fundamental frequencies as ground truth
f0 = [440, 440, 440, 440, 550, 550, 550, 550, 550];

nbPeaks = length(f);
m=zeros(nbPeaks);
for i=1:nbPeaks
    for j=1:i
        %% computing HWPS
        %create spectral pattern
        fsp1 = f-f(i);
        fsp2 = f-(f(j));
        %
        switch(fundamentalEstimateType)
            case('groundTruth')
                hF = min([f0(i), f0(j)]);
            case('conservative')
                hF = min([f(i), f(j)]);
            case('underestimate')
                hF = min([f(i), f(j)]);
                if(abs(f(i)-f(j)) > 50)
                    hF = min(hF, abs(f(i)-f(j)));
                end
        end
        %% wrap frequencies harmonically
        fhw1=mod(fsp1/hF, 1);
        fhw2= mod(fsp2/hF, 1);
        % compute spectral patterns correlation
        h1=zeros(1,l);
        h2=zeros(1,l);
        % quantizing frequencies with special care of boundaries
        % for circular property preservation
        i1=mod(round(fhw1*l), l)+1;
        i2=mod(round(fhw2*l), l)+1;
        % amplitude weighted histograms
        for k=1:length(a)
            h1(i1(k)) = h1(i1(k))+a(k);
            h2(i2(k)) = h2(i2(k))+a(k);
        end
        % cosine similarity computation
        val = (h1*h2')/(sqrt(h1*h1')*sqrt(h1*h2'));

        if(debug==1)
            figure(1);
            % histograms
            t = (0:1/l:1-1/l);
            subplot(4, 1, 4);
            bar(t, [h1; h2]');
            xlabel('Harmonically Wrapped Frequency');
            ylabel('Amplitude');
        end

        % fill similarity matrix
        m(i,j)=val;
        m(j,i)=val;
        % plot for one particular couple of peaks
        if(debug==1)
            figure(1)
            % general spectral frame
            subplot(4, 1, 1)
            Title(['HWPS for peaks ' num2str(i) ' and ' num2str(j)]);
            hold on
            text(f+50, a, num2str((1:length(f))'-1));
            stem(f, a);
            plot(f(1:5), a(1:5), 'kd');
            stem(f(i), a(i), 'rd');
            stem(f(j), a(j), 'rd');
            hold off
            xlabel('Frequency (Hz)');
            ylabel('Amplitude');
            % two spectral pattern
            subplot(4, 1, 2);
            cla
            hold on
            stem(fsp1, a, 'r*');
            stem(fsp2, a, 'bd');
            xlabel('Frequency (Hz)');
            ylabel('Amplitude');
            hold off
            % spectral patterns harmonicaly wrapped
            subplot(4, 1, 3);
            cla
            hold on
            stem(fhw1, a, 'r*');
            stem(fhw2, a, 'bd');
            xlabel('Harmonically Wrapped Frequency');
            ylabel('Amplitude');
            axis([-0.2 1.2 0 1.5]);
            hold off

            % similarity matrix
            figure(2);
            colormap('gray');
            imagesc(flipud(m));
            set(gca,'XTick', (1:9));
            set(gca,'YTick', (1:9));
            set(gca, 'FontSize', 14);
            textAxis = {'A0';'A1';'A2';'A3';'B3, A4';'B0';'B1';'B2';'B4'};
            set(gca,'XTickLabel',textAxis);
            set(gca,'YTickLabel',flipud(textAxis));
            Title('Similarity Matrix');
            % pause for visualizing
            pause(.5);
        end

    end
end

if(debug>0)
     % similarity matrix
     figure(2);
     imagesc(flipud(m));
     set(gca,'XTick', (1:9));
     set(gca,'YTick', (1:9));
     set(gca, 'FontSize', 14);
     textAxis = {'A0';'A1';'A2';'A3';'B3, A4';'B0';'B1';'B2';'B4'};
     set(gca,'XTickLabel',textAxis);
     set(gca,'YTickLabel',flipud(textAxis));
     Title('Similarity Matrix');
end