%% Demonstration of Fourier self-deconvolution for a pair of fused peaks
% Tom O'Haver 3/23/2023
% With evaluation of the accuracy of peak area measurements by perpendicular drop.

clear
FrequencySpectra=false; % Check this box to display frequency spectrum 
PeakAreaMeasurements=true;  % Check this box to compute area measurement accuracy
PeakShape="Gaussian"; % Sets the peak shape for both the signal peaks and the deconvolution function
position=14;
% Interval between data points
increment=0.02;
% Separation between peaks
delta=0.7;
% Width of the original signal peaks.
width=1;
% Random white noise added to signal before deconvolution
Noise=0;
% dw = Deconvolution width (half-width of decnvolution function df)
dw=0.86 ;
% Percent denominator addition
DA=1.2;
% Percentage of entire spectrum included for Fourier filter
FrequencyCutoff=17;
% for Fourier filter
CutOffRate=2;

% Create simulated signal with random white noise to the signal for realism
x=5:increment:25; % Independent variable axis
NumPeaks=2;
if PeakShape=="Gaussian"
    % Create a function y of width w.
    y1=2.*gaussian(x,position-delta,width);
    y2=gaussian(x,position+delta,width);
    area1=trapz(x,y1);area2=trapz(x,y2);
    yo=y1+y2+Noise.*randn(size(x)); % yo=y observed, with noise added
    PercentNoise=100.*Noise./max(yo);
    DeconWidthRatio=dw/width;
    % Now attempt to sharpen the peaks by deconvolution
    % zero centered deconvolution function
    df=gaussian(x,min(x),dw)+gaussian(x,max(x),dw);
    ydc=ifft(fft(yo)./fft(df)).*sum(df);  % Deconvolution by fft/ifft
end

if PeakShape=="Lorentzian"
    % Create a function y of width w.
    y1=2.*lorentzian(x,position-delta,width); 
    y2=lorentzian(x,position+delta,width);
    area1=trapz(x,y1);area2=trapz(x,y2);
    yo=y1+y2+Noise.*randn(size(x)); % yo=y observed, with noise added
    PercentNoise=100.*Noise./max(yo);
    DeconWidthRatio=dw/width;
    % Now attempt to sharpen the peaks by deconvolution
    % zero centered deconvolution function
    df=lorentzian(x,min(x),dw)+lorentzian(x,max(x),dw);
    ydc=ifft(fft(yo)./fft(df)).*sum(df);  % Deconvolution by fft/ifft
end

if PeakShape=="Logistic"
    % Create a function y of width w.
    y1=2.*logistic(x,position-delta,width);
    y2=logistic(x,position+delta,width);
    area1=trapz(x,y1);area2=trapz(x,y2);
    yo=y1+y2+Noise.*randn(size(x)); % yo=y observed, with noise added
    PercentNoise=100.*Noise./max(yo);
    DeconWidthRatio=dw/width;
    % Now attempt to sharpen the peaks by deconvolution
    % zero centered deconvolution function
    df=logistic(x,min(x),dw)+logistic(x,max(x),dw);
    ydc=ifft(fft(yo)./fft(df)).*sum(df);  % Deconvolution by fft/ifft
end

% Apply only denominator addition to reduce noise and ringing
fftc=fft(df);
% Deconvolution by fft/ifft with Denominator addition
ydcDA=ifft(fft(yo)./(fftc+DA.*0.01.*max(fftc))).*sum(df);

% Apply only Fourier filtering to reduce noise and ringing
% Low-pass mode
sy=FourierFilter(x,ydc,0,FrequencyCutoff.*0.01.*length(x),CutOffRate,1);

% Apply both denominator addition and Fourier filtering to reduce noise
%  and ringing; Low-pass mode
syDA=FourierFilter(x,ydcDA,0,FrequencyCutoff.*0.01.*length(x),CutOffRate,1);


% Plot all the steps
figure(1)
clf
subplot(2,2,1);
plot(x,yo,'k'); title(['Original '+PeakShape+' peaks']);grid
xlabel(['Noise = ' num2str(PercentNoise) '%     ' ...
    'FWHM = ' num2str(width) ]);

subplot(2,2,3);
plot(x,ydcDA,'b');grid
title('FSD, with denominator addition only');
xlabel(['dw/width = ' num2str(DeconWidthRatio) '   ' ...
    'Denom. Addition = ' num2str(DA) '%']  );

subplot(2,2,2);
plot(x,sy,'g');grid
title('FSD with filtering only');
xlabel(['FrequencyCutoff = ' num2str(FrequencyCutoff) ...
    '%    FWHM = ' num2str(halfwidth(x,sy,position-delta)) ]);

subplot(2,2,4);
plot(x,syDA,'r');grid
title('Both filtering and denom. addition');
xlabel(['FWHM = ' num2str(halfwidth(x,syDA,position-delta)) ]);

% Optional peak area measurements if PeakAreaMeasurements 
% checkbox is checked
if PeakAreaMeasurements==true
    % Original signal
    PercentError1orig=inf;
    PercentError2orig=inf;
    PercentError1decon=inf;
    PercentError2decon=inf;
    p=peaksat(x,yo,1);
    sizep=size(p);
    pporig=p(:,2);
    if sizep(1)==NumPeaks
        [AreaVectorOrig]=PerpDropAreas(x,yo,min(x),max(x),pporig');
        PercentError1orig=100.*(area1-AreaVectorOrig(1))/area1;
        PercentError2orig=100.*(area2-AreaVectorOrig(2))/area2;
    else
        disp([num2str(sizep(1)) ' peaks detected in original signal.'])
    end
    % Deconvoluted signal
    p=peaksat(x,syDA,1);
    sizep=size(p);
    ppdecon=p(:,2);
    if sizep(1)==NumPeaks
        [AreaVectorDecon]=PerpDropAreas(x,syDA,min(x),max(x),ppdecon');
        PercentError1decon=100.*(area1-AreaVectorDecon(1))/area1;
        PercentError2decon=100.*(area2-AreaVectorDecon(2))/area2;
    else
        disp([num2str(sizep(1)) ' peaks detected in original signal.'])
    end

    disp('Peak area error %   Peak 1       Peak 2')
    disp(['Original            ' num2str([PercentError1orig  PercentError2orig])])
    disp(['Deconvoluted        ' num2str([PercentError1decon  PercentError2decon])])
end

% Optional Fourier spectrum plotted if FrequencySpectra 
% checkbox is checked
if FrequencySpectra==true
    figure(2)
    clf
    fyc=fft(ydcDA);
    syc=fyc .* conj(fyc);
    psrange=1:length(fyc)/2;
    psyc=real(syc(psrange));
    loglog(psrange,psyc,'b','linewidth',2)

    hold on
    fyc=fft(sy);
    syc=fyc .* conj(fyc);
    psrange=1:length(fyc)/2;
    psyc=real(syc(psrange));
    loglog(psrange,psyc,'g','linewidth',2)

    fyc=fft(yo);
    syc=fyc .* conj(fyc);
    psrange=1:length(fyc)/2;
    psyc=real(syc(psrange));
    loglog(psrange,psyc,'k','linewidth',2)

    fyc=fft(syDA);
    syc=fyc .* conj(fyc);
    psrange=1:length(fyc)/2;
    psyc=real(syc(psrange));
    loglog(psrange,psyc,'r','linewidth',2)

    hold off
    grid
    axis([1 200 1e-3 1e7])
    title('Frequency spectra')
    xlabel('Frequency')
    title('Frequency spectrum')
else
end

%

function g = gaussian(x,pos,wid)
%  gaussian(x,pos,wid) = gaussian peak centered on pos, half-width=wid
%  x may be scalar, vector, or matrix, pos and wid both scalar
%  T. C. O'Haver, 1988
% Examples: gaussian([0 1 2],1,2) gives result [0.5000    1.0000    0.5000]
% plot(gaussian([1:100],50,20)) displays gaussian band centered at 50 with width 20.
g = exp(-((x-pos)./(0.60056120439323.*wid)) .^2);
end

function g = lorentzian(x,position,width)
% lorentzian(x,position,width) Lorentzian function.
% where x may be scalar, vector, or matrix
% position and width scalar
% T. C. O'Haver, 1988
% Example: lorentzian([1 2 3],2,2) gives result [0.5 1 0.5]
g=ones(size(x))./(1+((x-position)./(0.5.*width)).^2);
end

function g = logistic(x,pos,wid)
% logistic function.  pos=position; wid=half-width (both scalar)
% logistic(x,pos,wid), where x may be scalar, vector, or matrix
% pos=position; wid=half-width (both scalar)
% T. C. O'Haver, 1991
n = exp(-((x-pos)/(.477.*wid)) .^2);
g = (2.*n)./(1+n);
end

function [FWHM,slope1,slope2,hwhm1,hwhm2]=halfwidth(x,y,xo)
% function [FWHM,slope1,slope2,x1,x2]=halfwidth(x,y,xo) computes the full
% width at half maximum of the maximum of any shape peak that peaks near xo
% and has a zero baseline. If xo is omitted, it computes the halfwidth from
% the maximum y value. Not highly accurate if the function is too sparsely
% sampled. Also optionally returns the leading and trailing edge slopes
% (slope1 and slope 2, respectively) and the leading and trailing edge
% half-width-at-half-maximum, if those output arguments are incuded.
% Tom O'Haver (toh@umd.edu) Version 2, July 2016
%
% Example 1:
% x=-5:.01:5;
% y=sinc(x).^2;
% FWHM=halfwidth(x,y,0) % FWHM of Central peak
% FWHM=halfwidth(x,y,1.5) % FWHM of Positive side peak
%
% Example 2:
% x=[0:.1:10];
% W=3; % W is the true half-width
% y=gaussian(x,5,W);
% FWHM=halfwidth(x,y,5)
% FWHM=halfwidth(x,y) % gives the same result since there is only one peak
%
% Example 3: % Returns NaN because signal does not contain the halfwidth
% x=[0:.1:1];
% y=exp(-(x).^2)
% halfwidth(x,y)
%
% Example 4: Also returns the leading and trailing edge slopes (+1 and -1)
% and the leading and trailing edge half-width-at-half-maximum
% x=[0:.1:10];
% y=triangle(x,5,1);
% [FWHM,slope1,slope2,x1,x2]=halfwidth(x,y)
%
% Example 5. Comparison of Gaussian (g) and BiGaussian peaks (bg)
% x=[1:100];
% g=gaussian(x,50,20);
% bg=bigaussian(x,50,20,3);
% plot(x,g,x,bg)
% [GaussianFWHM,slope1,slope2,hwhm1,hwhm2]=halfwidth(x,g)
% [BiGaussianFWHM,slope1,slope2,hwhm1,hwhm2]=halfwidth(x,bg)

if nargin==2,xo=x(val2ind(y,max(y)));end
try
    indmax=val2ind(x,xo);
    maxy=y(indmax);
    oy=y-maxy/2;

    n=indmax;
    while oy(n)>0
        n=n-1;
    end
    x1=interp1([y(n) y(n+1)],[x(n) x(n+1)],maxy/2);
    slope1=(y(n+1)-y(n))./(x(n+1)-x(n));
    % polyfit((x(n+1)-x(n)),(y(n+1)-y(n)),1)

    n=indmax;
    while oy(n)>0
        n=n+1;
    end
    x2= interp1([y(n-1) y(n)],[x(n-1) x(n)],maxy/2);
    slope2=(y(n-1)-y(n))./(x(n-1)-x(n));
    % polyfit((x(n-1)-x(n)),(y(n-1)-y(n)),1)
    FWHM=x2-x1;
    hwhm1=xo-x1;
    hwhm2=x2-xo;
catch
    FWHM=NaN;
end
end

function [index,closestval]=val2ind(x,val)
% Returns the index and the value of the element of vector x that is closest to val
% If more than one element is equally close, returns vectors of indicies and values
% Tom O'Haver (toh@umd.edu) October 2006
% Examples: If x=[1 2 4 3 5 9 6 4 5 3 1], then val2ind(x,6)=7 and val2ind(x,5.1)=[5 9]
% [indices values]=val2ind(x,3.3) returns indices = [4 10] and values = [3 3]
dif=abs(x-val);
index=find((dif-min(dif))==0);
closestval=x(index);

% Copyright (c) 2012, Thomas C. O'Haver
%
% Permission is hereby granted, free of charge, to any person obtaining a copy
% of this software and associated documentation files (the "Software"), to deal
% in the Software without restriction, including without limitation the rights
% to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
% copies of the Software, and to permit persons to whom the Software is
% furnished to do so, subject to the following conditions:
%
% The above copyright notice and this permission notice shall be included in
% all copies or substantial portions of the Software.
%
% 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
% AUTHORS OR COPYRIGHT HOLDERS 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.
end

function ry=FourierFilter(xvector,yvector,centerfrequency,filterwidth,filtershape,filtermode)
% function ry=FourierFilter(xvector,yvector,centerfrequency,filterwidth,filtershape,filtermode)
% Computes fourier filter for signal yvector. Centerfrequency and
% filterwidth are the center frequency and width of the pass band, in
% harmonics, input arguments set the intital values of center frequency,
% filter width, shape, and filtermode (1-6):
% 1 = 'Band-pass'
% 2 = 'Lowpass'
% 3 = 'Highpass',
% 4 = 'Band-reject (notch)',
% 5 = 'Comb pass')
% 6 = 'Comb notch'
% 'filtershape' determines the sharpness of the cut-off.
%
% Adjust x and y vector shape to 1 x n (rather than n x 1)
xvector=reshape(xvector,1,length(xvector));
yvector=reshape(yvector,1,length(yvector));
fy=fft(yvector);
lft1=[1:(length(fy)/2)];
lft2=[(length(fy)/2+1):length(fy)];
% Compute filter shape.
if filtermode==1 % 'Band-pass'
    ffilter1=shapefunction(lft1,centerfrequency+1,filterwidth,filtershape);
    ffilter2=shapefunction(lft2,length(fy)-centerfrequency+1,filterwidth,filtershape);
    ffilter=[ffilter1,ffilter2];
end
if filtermode==2 % 'High-pass'
    centerfrequency=length(xvector)/2;
    ffilter1=shapefunction(lft1,centerfrequency+1,filterwidth,filtershape);
    ffilter2=shapefunction(lft2,length(fy)-centerfrequency+1,filterwidth,filtershape);
    ffilter=[ffilter1,ffilter2];
end
if filtermode==3 % 'Low-pass'
    centerfrequency=0;
    ffilter1=shapefunction(lft1,centerfrequency+1,filterwidth,filtershape);
    ffilter2=shapefunction(lft2,length(fy)-centerfrequency+1,filterwidth,filtershape);
    ffilter=[ffilter1,ffilter2];
end
if filtermode==4 % 'Band-reject (notch)'
    ffilter1=shapefunction(lft1,centerfrequency+1,filterwidth,filtershape);
    ffilter2=shapefunction(lft2,length(fy)-centerfrequency+1,filterwidth,filtershape);
    ffilter=1-[ffilter1,ffilter2];
end
if filtermode==5 % 'Comb pass'
    n=2;
    ffilter1=shapefunction(lft1,centerfrequency+1,filterwidth,filtershape);
    ffilter2=shapefunction(lft2,length(fy)-centerfrequency+1,filterwidth,filtershape);
    while n<50
        ffilter1=ffilter1+shapefunction(lft1,n*(centerfrequency+1),filterwidth,filtershape);
        ffilter2=ffilter2+shapefunction(lft2,length(fy)-n*(centerfrequency+1),filterwidth,filtershape);
        n=n+1;
    end
    ffilter=[ffilter1,ffilter2];
end
if filtermode==6 % 'Comb notch'
    n=2;
    ffilter1=shapefunction(lft1,centerfrequency+1,filterwidth,filtershape);
    ffilter2=shapefunction(lft2,length(fy)-centerfrequency+1,filterwidth,filtershape);
    while n<50
        ffilter1=ffilter1+shapefunction(lft1,n*(centerfrequency+1),filterwidth,filtershape);
        ffilter2=ffilter2+shapefunction(lft2,length(fy)-n*(centerfrequency+1),filterwidth,filtershape);
        n=n+1;
    end
    ffilter=1-[ffilter1,ffilter2];
end
if length(fy)>length(ffilter), ffilter=[ffilter ffilter(1)];end
ffy=fy.*ffilter;  % Multiply filter by Fourier Transform of signal
ry=real(ifft(ffy));

    function g = shapefunction(x,pos,wid,n)
        %  shapefunction(x,pos,wid,n) = peak centered on x=pos, half-width=wid
        %  x may be scalar, vector, or matrix, pos and wid both scalar
        %  Shape is Lorentzian (1/x^2) when n=0, Gaussian (exp(-x^2))
        %  when n=1, and becomes more rectangular as n increases.
        %  Example: shapefunction([1 2 3],1,2,1) gives result [1.0000    0.5000    0.0625]
        if n==0
            g=ones(size(x))./(1+((x-pos)./(0.5.*wid)).^2);
        else
            g = exp(-((x-pos)./(0.6.*wid)) .^(2*round(n)));
        end
    end
end
