% 
% 
% function [estimatedBR_WT, estimatedHR_WT]=Calculate_VSWT(phi_cut,peakAmp,peakFreq,param)
% 
%     
% 
%     peakFreq_level = maxFreq(peakAmp,peakFreq);
%     peakFreq_br = peakFreq_level(length(peakFreq_level));
%     estimatedBR_WT = peakFreq_br * 60;
% 
% 
% 
%     [Har2, Har3, Har4, Har5] = HarmoicFilterTrial(peakFreq_br,param);
% 
%     signal = filter(Har2,phi_cut);
%     signal = filter(Har3,signal);
%     signal = filter(Har4,signal);
%     signal = filter(Har5,signal);
% 
%     
%     [peakAmp, peakFreq]=waveletTrial(signal,param);
% 
% 
%     for i=1:length(peakAmp)
% 
%         for index=1:length(peakAmp{i})
% 
% 
%             if(peakFreq{i}(index) >= 1.9 || peakFreq{i}(index) <= 0.9)
% 
%                 peakAmp{i}(index)=0;
% 
%             end
% 
%         end
% 
%     end
% 
% 
%     for i=1:length(peakAmp)
% 
%         peakestAmps(i) = max(peakAmp{i});
%         index = peakAmp{i}==peakestAmps(i);
%         peakestFreqs(i) = peakFreq{i}(index);
% 
%     end
% 
%     peakestAmp = max(peakestAmps);
%     index = peakestAmps == peakestAmp;
%     peakestFreq = peakestFreqs(index);
% 
%     estimatedHR_WT = peakestFreq * 60;
%     
% 
% end
% 
% 
% 
% function [Hd2, Hd3, Hd4, Hd5] = HarmoicFilterTrial(brPeak,param)
%     %HARMOICFILTERTRIAL Returns a discrete-time filter object.
%     
%     % MATLAB Code
%     % Generated by MATLAB(R) 9.12 and DSP System Toolbox 9.14.
%     % Generated on: 05-Sep-2022 07:33:50
%     
%     % Elliptic Bandstop filter designed using FDESIGN.BANDSTOP.
%     
%     % All frequency values are in Hz.
%     Fs = param.Fs;  % Sampling Frequency
%     
%     N      = 10;    % Order
%     Apass  = 1;     % Passband Ripple (dB)
%     Astop  = 80;    % Stopband Attenuation (dB)
%     
%     % Construct an FDESIGN object and call its ELLIP method.
%     h2  = fdesign.bandstop('N,Fp1,Fp2,Ap,Ast', N, 2*brPeak-0.05, 2*brPeak+0.05, Apass, ...
%                           Astop, Fs);
%     h3  = fdesign.bandstop('N,Fp1,Fp2,Ap,Ast', N, 3*brPeak-0.05, 3*brPeak+0.05, Apass, ...
%                           Astop, Fs);
%     h4  = fdesign.bandstop('N,Fp1,Fp2,Ap,Ast', N, 4*brPeak-0.05, 4*brPeak+0.05, Apass, ...
%                           Astop, Fs);
%     h5  = fdesign.bandstop('N,Fp1,Fp2,Ap,Ast', N, 5*brPeak-0.05, 5*brPeak+0.05, Apass, ...
%                           Astop, Fs);
%     Hd2 = design(h2,'ellip');
%     Hd3 = design(h3, 'ellip');
%     Hd4 = design(h4,'ellip');
%     Hd5 = design(h5,'ellip');
%     
%     % [EOF]
% 
% end








function [estimatedBR_WT, estimatedHR_WT]=Calculate_VSWT(peakAmp,peakFreq)


    for i=1:length(peakAmp)

        peakestAmps(i) = max(peakAmp{i});
        index = peakAmp{i}==peakestAmps(i);
        peakestFreqs(i) = peakFreq{i}(index);

    end

    peakestAmp = max(peakestAmps);
    index = peakestAmps == peakestAmp;
    peakFreq_br = peakestFreqs(index);

    estimatedBR_WT = peakFreq_br * 60;




    for i=1:length(peakAmp)

        for index=1:length(peakAmp{i})

            modulus = mod(peakFreq{i}(index),peakFreq_br);

            if(modulus < 0.05)

                peakAmp{i}(index)=0;

            end

            if(peakFreq{i}(index) >= 1.9 || peakFreq{i}(index) <= 0.9)

                peakAmp{i}(index)=0;

            end

        end

    end

   
    for i=1:length(peakAmp)

        new_peakestAmps(i) = max(peakAmp{i});
        index = peakAmp{i} == new_peakestAmps(i);
        new_peakestFreqs(i) = peakFreq{i}(index);

    end

    peakestAmp = max(new_peakestAmps);
    index = new_peakestAmps == peakestAmp;
    peakestFreq = new_peakestFreqs(index);

    estimatedHR_WT = peakestFreq * 60;


end