%================= Extract ridge curve from SWFT or SWT ===================
% Version 2.00 stable
%-------------------------------Copyright----------------------------------
%
% Author: Dmytro Iatsenko
% Information about these codes (e.g. links to the Video Instructions),
% as well as other MatLab programs and many more can be found at
% http://www.physics.lancs.ac.uk/research/nbmphysics/diats/tfr
% 
% Related articles:
% [1] D. Iatsenko, A. Stefanovska and P.V.E. McClintock,
% "Linear and synchrosqueezed time-frequency representations revisited.
%  Part I: Overview, standards of use, related issues and algorithms."
% {preprint - arXiv:1310.7215}
% [2] D. Iatsenko, A. Stefanovska and P.V.E. McClintock,
% "Linear and synchrosqueezed time-frequency representations revisited.
%  Part II: Resolution, reconstruction and concentration."
% {preprint - arXiv:1310.7274}
% [3] D. Iatsenko, A. Stefanovska and P.V.E. McClintock,
% "On the extraction of instantaneous frequencies from ridges in
%  time-frequency representations of signals."
% {preprint - arXiv:1310.7276}
%
%------------------------------Documentation-------------------------------
%
% [tfsupp,Optional:ecinfo,Skel] = ssecurve(TFR,freq,wopt,'PropertyName',PropertyValue)
% - extracts the curve (i.e. the sequence of the amplitude ridge points)
%   and its full support (the widest region of unimodal TFR amplitude
%   around them) from the given time-frequency representation [TFR] of the
%   signal. TFR can be either SWFT or SWT.
%
% OUTPUT:
% tfsupp: 3xL matrix
%    - extracted time-frequency support of the component, containing
%      frequencies of the TFR amplitude peaks (ridge points) in the
%      first row (referred as \omega_p(t)/2\pi in [3]), support lower
%      bounds (referred as \omega_-(t)/2/pi in [1]) - in the second row,
%      and the upper bounds (referred as \omega_+(t)/2/pi in [1]) - in
%      the third row.
% ecinfo: structure
%    - contains all the relevant information about the process of curve
%      extraction.
% Skel: 4x1 cell (returns empty matrix [] if 'Method' property is not 1,2,3 or 'nearest')
%    - contains the number of peaks N_p(t) in [Skel{1}], their frequency
%      indices m(t) in [Skel{2}], the corresponding frequencies
%      \nu_m(t)/2\pi in [Skel{3}], and the respective amplitudes Q_m(t)
%      in [Skel{4}] (in notations of [3]).
% 
% INPUT:
% TFR: NFxL matrix (rows correspond to frequencies, columns - to time)
%    - SWFT or SWT from which to extract [tfsupp]
% freq: NFx1 vector
%    - the frequencies corresponding to the rows of [TFR]
% wopt: structure | value (except if 'Method' property is 1 or 3) | 1x2 vector
%    - structure with parameters of the window/wavelet and the simulation,
%      returned as a third output by functions sswft.m and sswt.m;
%      alternatively, one can set wopt=[fs], where [fs] is the signal
%      sampling frequency (except methods 1 and 3); for methods 1 and 3
%      one can set wopt=[fs,D], where [D] is particular parameter of the
%      method (see [3]): (method 1) the characteristic growth rate of the
%      frequency - df/dt (in Hz/s) - for the SWFT, or of the log-frequency
%      - d\log(f)/dt - for the SWT; (method 2) the minimal distinguishable
%      frequency difference (in Hz) for SWFT, or log-difference for SWT.
%
% PROPERTIES: ({{...}} denotes default)
% BASIC####################################
% 'RidgeEst':'ridge'|{{'direct'}}|'maxpeak'
%    - determines the way how to estimate ridge point amplitudes and
%      frequencies: 'direct' - from the overall region of nonzero
%      SWFT/SWT amplitude (direct reconstruction [1]); 'ridge' - from the
%      individual peaks (ridge reconstruction [1]); 'maxpeak' - from the
%      highest peaks in each region of non-zero TFR ampltiude. As
%      discussed in [3], the options 'ridge' and 'maxpeak' are generally
%      inappropriate, although can work well in particular cases. Note,
%      that there is no such property 'RidgeEst' for the ecurve.m (curve
%      extraction from usual WFT/WT), as there one can always use peak
%      values which are well defined, noise-robust and fast to find.
% 'Method': 1|{{2}}|3|'nearest'|'max'|efreq|1i*efreq
%    - method by which to extract the curve, as described in [3]; to use
%      frequency-based extraction, specify an L-length vector [efreq] of
%      the frequencies (in Hz), in which case the program will form the
%      ridge curve from the peaks lying at each time in the same support
%      (i.e. region of unimodal TFR amplitude) as the specified frequency
%      profile [efreq]; to select just ridge points nearest to [efreq]
%      (on a logarithmic scale for SWT), use imaginary profile 1i*[efreq],
%      but this approach is more susceptible to noise and other effects.
% 'Param':[alpha,beta] - for methods 2 and 3 (default = [1,1])
%                alpha - for method 1 (default = 1)
%         [] (no parameters) - for all other methods
%    - parameters for each method, as described in [3].
% 'Display': {{'on'}}|'off'|'plot'|'plot+'
%    - to display the progress information or not; if set to 'plot',
%      additionally plots all the obtained curves and characteristics
%      (such as the averages and standard deviations of ridge frequencies
%      and their differences at each iteration for scheme II).
% 'Plot': 'on'|'on-wr'{{'off'}}
%    - if set to 'on', plots additionally the full [TFR] and shows on it
%      the extracted frequency profile and the boundaries of its support at
%      each time (do not confuse with 'Display'='plot', which plots the
%      progress information and not only the final result). To avoid
%      unnecessary plotting of the huge data, by default the TFR plot is
%      resampled to have no more than few data points displayed per pixel;
%      to turn this option off, use addition '-wr' (i.e.\ 'Plot'='on-wr').
% ADVANCED#################################
% 'AmpFunc':function (default AmpFunc=@(x)log(x))
%    - the functional of the ridge amplitudes which to use in the
%      optimization, so that one maximizes (over all ridge sequences) the
%      path functional $\sum_n[AmpFunc(Q_p(t_n))+(penalization terms)]$,
%      where Q_p(t_n) denotes the ridge amplitudes at time t_n.
% 'PenalFunc': 1x2 cell with two functions {func1,func2}
%    - penalization functions used for curve extraction. For the SWFT:
%      method 1 - {@(drf),@(rf)}, where [drf] denote difference between
%      frequencies of the consecutive ridges, and [rf] denote frequency
%      of the ridge, all in Hz (default = {@(drf)-abs(drf)/cv,[]}, where
%      [cv] is the sampling frequency multiplied on the typical frequency
%      growth rate for the current window parameters; by default there is
%      no discrimination over frequency, so second function is the empty
%      matrix []); method 2 - {@(drf,m,s),@(rf,m,s)}, where [m] and [s] are
%      the corresponding means and standard deviations of [drf] and [rf]
%      (default = {@(drf,m,s)-abs(drf-m)/s,@(rf,m,s)-0.5*((rf-m)./s).^2});
%      method 3 - {@(log(P1)),@(log(P2))}, where [P1] and [P2] are the
%      distributions of the [drf] and [rf], respectively. For the SWT all
%      is the same but all frequency variables are taken on a logarithmic
%      scale, so that e.g. [rf] and [drf] now denote the logarithms of the
%      ridge frequencies and their differences between the consecutive
%      ridges. Note, that 'PenalFunc' property overrides the 'Param'
%      property, so the functions should be specified with all the
%      parameters inside.
% 'PathOpt':{{'on'}}|'off'
%    - optimize the ridge curve over all possible trajectories ('on') or
%      use the one-step approach ('off'), see [3]; the path optimization
%      GREATLY improves the performance of all methods and is not
%      computationally expensive (is performed in O(N) operations using
%      fast algorithm of [3]), so DO NOT CHANGE THIS PROPERTY unless you
%      want to just play and see the advantages of the path optimization
%      over the one-step approach; note, that this property applies only
%      to methods 1,2,3, as the others are simple one-step approaches.
% 'Skel': {{[]}} | 4x1 cell returned as the third output of ecurve.m
%    - can be specified to avoid performing search of the peaks, their
%      numbers and corresponding amplitudes each time if the procedures
%      are applied to the same TFR (e.g. one compares the performance of
%      the different schemes).
% 'MaxIter': value (default = 20) /methods 2 and 3 only/
%    - maximum number of iterations allowed for methods 2 and 3 to converge
%
%----------------------- Additional possibilities -------------------------
%
% NOTE: One can alternatively pass the structure with the properties as the
% 4th argument, e.g. /opt.Method=1; ssecurve(TFR,freq,wopt,opt);/.
% If the other properties are specified next, they override those in the
% structure, e.g. /ssecurve(TFR,freq,wopt,opt,'Method',2);/ will
% always use 2nd method, irrespectively to what is specified in [opt].
%
%-------------------------------Examples-----------------------------------
%
% [SWFT,freq,wopt]=sswft(sig,fs); tfsupp=ssecurve(SWFT,freq,wopt);
% - extracts the ridge curve from the synchrosqueezed windowed Fourier
%   transform [SWFT] of the signal [sig] sampled at [fs] Hz.
%
%------------------------------Changelog-----------------------------------
%
% v2.00:
% - the algorithms are reprogrammed in accordance with the second version of [3]
% - some additional minor changes (Optimization/Display/Plotting)
% - added new possibilities (see PROPERTIES documentation)
% - added corrections for discretization effects, e.g. cubic
%   interpolation to better locate the peaks etc.
%
%--------------------------------------------------------------------------

function [tfsupp,varargout] = ssecurve(TFR,freq,wopt,varargin)

[NF,L]=size(TFR); freq=freq(:);
tfsupp=zeros(3,L)*NaN; pind=zeros(1,L)*NaN; pamp=zeros(1,L)*NaN; idr=zeros(1,L)*NaN;

%Default parameters
method=2; pars=[]; DispMode='on'; PlotMode='off';
Skel=[]; PathOpt='on'; AmpFunc=@(x)log(x); MaxIter=20;
iparm='median'; %method for estimation of initial parameters in II scheme
RidgeEst='direct'; %/////////////////////////////////
%Update if user-defined
vst=1;
if nargin>3 && isstruct(varargin{1})
    copt=varargin{1}; vst=2;
    if isfield(copt,'Display'), cvv=copt.Display; if ~isempty(cvv), DispMode=cvv; end, end
    if isfield(copt,'Method'), cvv=copt.Method; if ~isempty(cvv), method=cvv; end, end
    if isfield(copt,'Param'), cvv=copt.Param; if ~isempty(cvv), pars=cvv; end, end
    if isfield(copt,'Plot'), cvv=copt.Plot; if ~isempty(cvv), PlotMode=cvv; end, end
    if isfield(copt,'Skel'), cvv=copt.Skel; if ~isempty(cvv), Skel=cvv; end, end
    if isfield(copt,'PathOpt'), cvv=copt.PathOpt; if ~isempty(cvv), PathOpt=cvv; end, end
    if isfield(copt,'AmpFunc'), cvv=copt.AmpFunc; if ~isempty(cvv), AmpFunc=cvv; end, end
    if isfield(copt,'MaxIter'), cvv=copt.MaxIter; if ~isempty(cvv), MaxIter=cvv; end, end
    if isfield(copt,'PenalFunc'), cvv=copt.PenalFunc; if ~isempty(cvv), PenalFunc=cvv; end, end
    if isfield(copt,'RidgeEst'), cvv=copt.RidgeEst; if ~isempty(cvv), RidgeEst=cvv; end, end %/////////////////////////////////
end
for vn=1:2:nargin-3
    if strcmpi(varargin{vn},'Display'), if ~isempty(varargin{vn+1}), DispMode=varargin{vn+1}; end
    elseif strcmpi(varargin{vn},'Method'), if ~isempty(varargin{vn+1}), method=varargin{vn+1}; end
    elseif strcmpi(varargin{vn},'Param'), if ~isempty(varargin{vn+1}), pars=varargin{vn+1}; end
    elseif strcmpi(varargin{vn},'Plot'), if ~isempty(varargin{vn+1}), PlotMode=varargin{vn+1}; end
    elseif strcmpi(varargin{vn},'Skel'), if ~isempty(varargin{vn+1}), Skel=varargin{vn+1}; end
    elseif strcmpi(varargin{vn},'PathOpt'), if ~isempty(varargin{vn+1}), PathOpt=varargin{vn+1}; end
    elseif strcmpi(varargin{vn},'AmpFunc'), if ~isempty(varargin{vn+1}), AmpFunc=varargin{vn+1}; end
    elseif strcmpi(varargin{vn},'PenalFunc'), if ~isempty(varargin{vn+1}), PenalFunc=varargin{vn+1}; end
    elseif strcmpi(varargin{vn},'RidgeEst'), if ~isempty(varargin{vn+1}), RidgeEst=varargin{vn+1}; end %/////////////////////////////////
    else
        error(['There is no Property "',varargin{vn},'" (which is ',num2str(1+(vn-vst)/2,'%d'),...
            '''th out of ',num2str(ceil((nargin-1-vst)/2),'%d'),' specified)']);
    end
end
if isempty(pars) %if parameters are not specified, assign the defaults
    if ischar(method) || length(method)>1, pars=[];
    elseif method==1, pars=1;
    else pars=[1,1];
    end
else %if parameters are specified, check are they specified correctly
    if (ischar(method) || length(method)>1) && ~isempty(pars)
        error('Wrong number of parameters for the chosen method: there should be no parameters.');
    elseif method==1 && length(pars)~=1
        error('Wrong number of parameters for the chosen method: there should be one parameter.');
    elseif (method==2 || method==3) && length(pars)~=2
        error('Wrong number of parameters for the chosen method: there should be two parameters.');
    end
end
if nargout>1
    ec=struct;
    ec.Method=method; ec.Param=pars; ec.Display=DispMode; ec.Plot=PlotMode;
    ec.Skel=Skel; ec.PathOpt=PathOpt; ec.AmpFunc=AmpFunc;
    ec.RidgeEst=RidgeEst; %/////////////////////////////////
end
if RidgeEst~=2, iparm='mean'; end %/////////////////////////////////

%Determine the frequency resolution
if min(freq)<=0 || std(diff(freq))<std(diff(log(freq)))
    fres=1; fstep=mean(diff(freq));
    DF=(wopt.wp.xi2h-wopt.wp.xi1h)/2/pi; DT=(wopt.wp.t2h-wopt.wp.t1h);
    dfreq=[freq(1)-freq(end:-1:2);freq(end)-freq(end:-1:1)];
else
    fres=2; fstep=mean(diff(log(freq)));
    DF=log(wopt.wp.xi2h/wopt.wp.xi1h); DT=(wopt.wp.t2h-wopt.wp.t1h);
    dfreq=[log(freq(1))-log(freq(end:-1:2));log(freq(end))-log(freq(end:-1:1))];
end
%Assign numerical parameters
if isstruct(wopt)
    fs=wopt.fs; DT=(wopt.wp.t2h-wopt.wp.t1h);
    if fres==1, DF=(wopt.wp.xi2h-wopt.wp.xi1h)/2/pi;
    else DF=log(wopt.wp.xi2h/wopt.wp.xi1h); end
    if method==1, DD=DF/DT; end
    if method==3, DD=DF; end
else
    fs=wopt(1);
    if method==1 || method==3, DD=wopt(2); end
end

%//////////////////////////////////////////////////////////////////////////
if strcmpi(RidgeEst,'ridge'), RidgeEst=1; elseif strcmpi(RidgeEst,'direct'), RidgeEst=2; else RidgeEst=3; end
tn1=find(~isnan(TFR(end,:)),1,'first'); tn2=find(~isnan(TFR(end,:)),1,'last'); sflag=0;
if ischar(method) || length(method)==1 || max(abs(imag(method(:))))>0 %if not frequency-based extraction (or frequency-based with nearest selection)
    sflag=1;
    %----------------------------------------------------------------------
    %Construct matrices of ridge indices, frequencies and amplitudes:
    %[Ip],[Fp],[Qp], respectively; [Np] - number of peaks at each time.
    if ~isempty(Skel)
        Np=Skel{1}; Ip=Skel{2}; Fp=Skel{3}; Qp=Skel{4}; Mp=max(Np);
    else
        if RidgeEst==1
            RidgeEst=1; if ~strcmpi(DispMode,'off'), fprintf('Locating peaks in TFR... '); end
        elseif RidgeEst==2
            RidgeEst=2; if ~strcmpi(DispMode,'off'), fprintf('Locating supports in TFR and joining them into peaks... '); end
        else
            RidgeEst=3; if ~strcmpi(DispMode,'off'), fprintf('Locating supports in TFR and maximum peaks in them... '); end
        end
        
        if RidgeEst==1
            TFR=abs(TFR); %convert to absolute values, since we need only them; also improves computational speed as TFR is no more complex and is positive
            TFR=vertcat(zeros(1,L),TFR,zeros(1,L)); %pad TFR with zeros
            idft=1+find(TFR(2:end-1)>=TFR(1:end-2) & TFR(2:end-1)>TFR(3:end)); %find linear indices of the peaks
            [idf,idt]=ind2sub(size(TFR),idft); idf=idf-1; %find frequency and time indices of the peaks
            dind=[0;find(diff(idt(:))>0);length(idt)]; Mp=max(diff(dind));
            Np=zeros(1,L); idn=zeros(length(idt),1);
            for dn=1:length(dind)-1,
                ii=dind(dn)+1:dind(dn+1); idn(ii)=1:length(ii); Np(idt(ii(1)))=length(ii);
            end
            idnt=sub2ind([Mp,L],idn(:),idt(:));
            Ip=ones(Mp,L)*NaN; Fp=ones(Mp,L)*NaN; Qp=ones(Mp,L)*NaN;
            Ip(idnt)=idf; Fp(idnt)=freq(idf); Qp(idnt)=TFR(idft);
            clear idft idf idt idn dind idnt;
            TFR=TFR(2:end-1,:); %recover the original TFR
        else
            TFR=vertcat(zeros(1,L),TFR,zeros(1,L)); %pad TFR with zeros
            idft1=1+find(TFR(1:end-1)==0 & TFR(2:end)~=0); idft2=find(TFR(1:end-1)~=0 & TFR(2:end)==0);
            [idf1,idt1]=ind2sub(size(TFR),idft1); [idf2,idt2]=ind2sub(size(TFR),idft2);
            dind=[0;find(diff(idt1(:))>0);length(idt1)]; Mp=max(diff(dind));
            Np=zeros(1,L); for dn=1:length(dind)-1, Np(idt1(dind(dn)+1))=dind(dn+1)-dind(dn); end
            Ip=ones(Mp,L)*NaN; Fp=ones(Mp,L)*NaN; Qp=ones(Mp,L)*NaN; nid=0; tn=idt1(1);
            for kn=1:length(idft1)
                ii=(idft1(kn):idft2(kn))'; iif=-1+(idf1(kn):idf2(kn))';
                if RidgeEst==2
                    casig=sum(TFR(ii)); cfsig=real(sum(freq(iif).*TFR(ii))/casig);
                    if isnan(cfsig) || cfsig<freq(iif(1)) || cfsig>freq(iif(end))
                        cfsig=sum(freq(iif).*abs(TFR(ii)))/sum(abs(TFR(ii)));
                    end
                else
                    [casig,idm]=max(abs(TFR(ii))); cfsig=freq(iif(idm));
                end
                %Assign all
                if idt1(kn)~=tn, nid=1; else nid=nid+1; end
                tn=idt1(kn); Fp(nid,tn)=cfsig; Qp(nid,tn)=abs(casig);
                if fres==1, fid=1+(cfsig-freq(1))/fstep;
                else fid=1+(log(cfsig)-log(freq(1)))/fstep; end
                Ip(nid,tn)=min([max([fid,1]),NF]);
            end
            clear idft1 idft2 idf1 idt1 idf2 idt2 dind;
            TFR=TFR(2:end-1,:); %recover the original TFR
        end
        
        if ~strcmpi(DispMode,'off')
            fprintf('(number of ridges: %d+-%d, from %d to %d)\n',...
                round(mean(Np(tn1:tn2))),round(std(Np(tn1:tn2))),min(Np),max(Np));
        end
    end
    if nargout>2, varargout{2}={Np,Ip,Fp,Qp}; end
    Wp=AmpFunc(Qp); %apply the functional to amplitude peaks
    %----------------------------------------------------------------------
    
else %frequency-based extraction
    if length(method)~=L
        error('The specified frequency profile ("Method" property) should be of the same length as signal.');
    end
    
    efreq=method;
    if ~strcmpi(DispMode,'off')
        fprintf('Extracting the ridge curve lying in the same TFR supports as the specified frequency profile.\n');
    end
    
    tn1=max([tn1,find(~isnan(efreq),1,'first')]); tn2=min([tn2,find(~isnan(efreq),1,'last')]);
    if fres==1, eind=1+floor(0.5+(efreq-freq(1))/fstep);
    else eind=1+floor(0.5+log(efreq/freq(1))/fstep); end
    eind(eind<1)=1; eind(eind>NF)=NF;
    
    %Extract the indices of the nearest peaks and time-frequency support around them
    for tn=tn1:tn2
        cind=eind(tn); cs=abs(TFR(:,tn));
        
        %Assure that the point is in the support
        cpeak=cind;
        if cs(cpeak)==0
            cpeak1=cind-1+find(cs(cind:end)>0,1,'first');
            cpeak2=cind+1-find(cs(cind:-1:1)>0,1,'first');
            if ~isempty(cpeak1) && ~isempty(cpeak2)
                if cpeak1-cind==cind-cpeak2
                    if cs(cpeak1)>cs(cpeak2), cpeak=cpeak1;
                    else cpeak=cpeak2; end
                elseif cpeak1-cind<cind-cpeak2, cpeak=cpeak1;
                elseif cpeak1-cind>cind-cpeak2, cpeak=cpeak2;
                end
            elseif isempty(cpeak1), cpeak=cpeak2;
            elseif isempty(cpeak2), cpeak=cpeak1;
            end
        end
        
        %Boundaries of time-frequency support
        iup=[]; idown=[];
        if cpeak<NF-1, iup=cpeak+find(cs(cpeak+1:end)==0,1,'first'); end
        if cpeak>2, idown=cpeak-find(cs(cpeak-1:-1:1)==0,1,'first'); end
        iup=min([iup,NF]); idown=max([idown,1]);
        tfsupp(2,tn)=idown; tfsupp(3,tn)=iup;
        
        %Find ridge frequency
        ii=idown:iup;
        if RidgeEst==1
            cv=abs(cs(ii)); idp=1+find(cv(2:end-1)>=cv(1:end-2) & cv(2:end-1)>cv(3:end));
            idp=ii(idp); [~,idm]=min(abs(cind-idp)); tfsupp(1,tn)=freq(idp(idm));
            pamp(tn)=abs(cs(idp(idm)));
        elseif RidgeEst==2
            cv=TFR(ii,tn); cfsig=real(sum(freq(ii).*cv)/sum(cv));
            if cfsig>=freq(ii(1)) && cfsig<=freq(ii(end)), tfsupp(1,tn)=cfsig;
            else tfsupp(1,tn)=sum(freq(ii).*cs(ii))/sum(cs(ii)); end
            pamp(tn)=abs(sum(cv));
        else
            [pamp(tn),idm]=max(abs(cs(ii))); tfsupp(1,tn)=freq(ii(idm));
        end
    end
    %Transform to frequencies
    if fres==1, pind=1+floor(0.5+(tfsupp(1,:)-freq(1))/fstep);
    else pind=1+floor(0.5+log(tfsupp(1,:)/freq(1))/fstep); end
    tfsupp(2:3,tn1:tn2)=freq(tfsupp(2:3,tn1:tn2));
    
    %Optional output arguments
    if nargout>1
        ec.efreq=efreq; ec.eind=eind;
        ec.pfreq=tfsupp(1,:); ec.pind=pind; ec.pamp=pamp; ec.idr=idr;
        varargout{1}=ec;
    end
    if nargout>2, varargout{2}=[]; end
    
    %Plotting (if needed)
    if ~isempty(strfind(DispMode,'plot'))
        scrsz=get(0,'ScreenSize'); figure('Position',[scrsz(3)/4,scrsz(4)/8,2*scrsz(3)/3,2*scrsz(4)/3]);
        ax=axes('Position',[0.1,0.1,0.8,0.8],'FontSize',16); hold all;
        title(ax(1),'Ridge curve \omega_p(t)/2\pi'); ylabel(ax(1),'Frequency (Hz)'); xlabel(ax(1),'Time (s)');
        plot(ax(1),(0:L-1)/fs,efreq,'--','Color',[0.5,0.5,0.5],'LineWidth',2,'DisplayName','Specified frequency profile');
        plot(ax(1),(0:L-1)/fs,tfsupp(1,:),'-k','LineWidth',2,'DisplayName','Extracted frequency profile');
        legend(ax(1),'show');
    end
    if ~isempty(strfind(PlotMode,'on')), plotfinal(tfsupp,TFR,freq,fs,DispMode,PlotMode); end
    
    return;
end

%Frequency-based extraction with nearest ridges
if ~ischar(method) && length(method)>1 && max(abs(imag(method(:))))>0
    efreq=imag(method);
    if ~strcmpi(DispMode,'off')
        fprintf('Extracting the ridge curve lying nearest to the specified frequency profile.\n');
    end
    
    tn1=max([tn1,find(~isnan(efreq),1,'first')]); tn2=min([tn2,find(~isnan(efreq),1,'last')]);
    if fres==1
        for tn=tn1:tn2, [~,pind(tn)]=min(abs(efreq(tn)-Fp(1:Np(tn),tn))); end
    else
        for tn=tn1:tn2, [~,pind(tn)]=min(abs(log(efreq(tn))-log(Fp(1:Np(tn),tn)))); end
    end
    lid=sub2ind(size(Fp),pind(tn1:tn2),tn1:tn2);
    tfsupp(1,tn1:tn2)=Fp(lid); pind(tn1:tn2)=round(Ip(lid)); pamp(tn1:tn2)=Qp(lid);
    
    if nargout>1, ec.pfreq=tfsupp(1,:); ec.pind=pind; ec.pamp=pamp; end
    if ~isempty(strfind(DispMode,'plot'))
        scrsz=get(0,'ScreenSize'); figure('Position',[scrsz(3)/4,scrsz(4)/8,2*scrsz(3)/3,2*scrsz(4)/3]);
        ax=axes('Position',[0.1,0.1,0.8,0.8],'FontSize',16); hold all;
        title(ax(1),'Ridge curve \omega_p(t)/2\pi'); ylabel(ax(1),'Frequency (Hz)'); xlabel(ax(1),'Time (s)');
        plot(ax(1),(0:L-1)/fs,efreq,'--','Color',[0.5,0.5,0.5],'LineWidth',2,'DisplayName','Specified frequency profile');
        plot(ax(1),(0:L-1)/fs,tfsupp(1,:),'-k','LineWidth',2,'DisplayName','Extracted frequency profile');
        legend(ax(1),'show');
    end
    
end
%//////////////////////////////////////////////////////////////////////////


%--------------------------- Global Maximum -------------------------------
if strcmpi(method,'max') || length(pars)==2
    if ~strcmpi(DispMode,'off')
        if sflag==0, fprintf('Extracting the curve by Global Maximum scheme.\n');
        else fprintf('Extracting positions of global maximums (needed to estimate initial parameters).\n'); end
    end
    
    if sflag==0
        for tn=tn1:tn2, [pamp(tn),pind(tn)]=max(abs(TFR(:,tn))); end
        tfsupp(1,tn1:tn2)=freq(pind(tn1:tn2));
    else
        for tn=tn1:tn2, [pamp(tn),idr(tn)]=max(Qp(1:Np(tn),tn)); end
        lid=sub2ind(size(Fp),idr(tn1:tn2),tn1:tn2);
        tfsupp(1,tn1:tn2)=Fp(lid); pind(tn1:tn2)=round(Ip(lid));
    end
    idz=tn1-1+find(pamp(tn1:tn2)==0 | isnan(pamp(tn1:tn2)));
    if ~isempty(idz)
        idnz=tn1:tn2; idnz=idnz(~ismember(idnz,idz));
        pind(idz)=interp1(idnz,pind(idnz),idz,'linear','extrap');
        pind(idz)=round(pind(idz));
        tfsupp(1,idz)=interp1(idnz,tfsupp(1,idnz),idz,'linear','extrap');
    end
    
    if nargout>1, ec.pfreq=tfsupp(1,:); ec.pind=pind; ec.pamp=pamp; ec.idr=idr; end
    if ~isempty(strfind(DispMode,'plot')) && strcmpi(method,'max')
        scrsz=get(0,'ScreenSize'); figure('Position',[scrsz(3)/4,scrsz(4)/8,2*scrsz(3)/3,2*scrsz(4)/3]);
        ax=axes('Position',[0.1,0.1,0.8,0.8],'FontSize',16); hold all;
        title(ax(1),'Ridge curve \omega_p(t)/2\pi'); ylabel(ax(1),'Frequency (Hz)'); xlabel(ax(1),'Time (s)');
        plot(ax(1),(0:L-1)/fs,tfsupp(1,:),'-k','LineWidth',2,'DisplayName','Global Maximum curve');
        legend(ax(1),'show');
    end
end


%------------------------- Nearest neighbour ------------------------------
if strcmpi(method,'nearest')
    %Display, if needed
    [~,imax]=max(Qp(:)); [fimax,timax]=ind2sub([Mp,L],imax); idr(timax)=fimax;
    if ~strcmpi(DispMode,'off')
        fprintf('Extracting the curve by Nearest Neighbour scheme.\n');
        fprintf('The highest peak was found at time %0.3f s and frequency %0.3f Hz (indices %d and %d, respectively).\n',...
            (timax-1)/fs,Fp(fimax,timax),timax,Ip(fimax,timax));
        fprintf('Tracing the curve forward and backward from point of maximum.\n');
    end
    %Main part
    for tn=timax+1:tn2, [~,idr(tn)]=min(abs(Ip(1:Np(tn),tn)-idr(tn-1))); end
    for tn=timax-1:-1:tn1, [~,idr(tn)]=min(abs(Ip(1:Np(tn),tn)-idr(tn+1))); end
    lid=sub2ind(size(Fp),idr(tn1:tn2),tn1:tn2);
    tfsupp(1,tn1:tn2)=Fp(lid); pind(tn1:tn2)=round(Ip(lid)); pamp(tn1:tn2)=Qp(lid);
    %Assign the output structure and display, if needed
    if nargout>1, ec.pfreq=tfsupp(1,:); ec.pind=pind; ec.pamp=pamp; ec.idr=idr; end
    if ~isempty(strfind(DispMode,'plot'))
        scrsz=get(0,'ScreenSize'); figure('Position',[scrsz(3)/4,scrsz(4)/8,2*scrsz(3)/3,2*scrsz(4)/3]);
        ax=axes('Position',[0.1,0.1,0.8,0.8],'FontSize',16); hold all;
        title(ax(1),'Ridge curve \omega_p(t)/2\pi'); ylabel(ax(1),'Frequency (Hz)'); xlabel(ax(1),'Time (s)');
        plot(ax(1),(0:L-1)/fs,tfsupp(1,:),'-k','LineWidth',2,'DisplayName','Nearest Neighbour curve');
        plot(ax(1),(timax-1)/fs,Fp(fimax,timax),'ob','LineWidth',2,'MarkerSize',8,'MarkerFaceColor','r',...
            'DisplayName','Starting point (overall maximum of TFR amplitude)');
        legend(ax(1),'show');
    end
end


%----------------------------- Method I -----------------------------------
if length(pars)==1
    if ~strcmpi(DispMode,'off')
        fprintf('Extracting the curve by I scheme.\n');
    end
    %Define the functionals
    if isempty(PenalFunc{1}), logw1=@(x)-pars(1)*abs(fs*x/DD); else logw1=PenalFunc{1}; end
    if isempty(PenalFunc{2}), logw2=[]; else logw2=PenalFunc{2}; end
    %Main part
    if strcmpi(PathOpt,'on'), idr=pathopt(Np,Ip,Fp,Wp,logw1,logw2,freq,DispMode);
    else [idr,timax,fimax]=onestepopt(Np,Ip,Fp,Wp,logw1,logw2,freq,DispMode); end
    lid=sub2ind(size(Fp),idr(tn1:tn2),tn1:tn2);
    tfsupp(1,tn1:tn2)=Fp(lid); pind(tn1:tn2)=round(Ip(lid)); pamp(tn1:tn2)=Qp(lid);
    %Assign the output structure and display, if needed
    if nargout>1, ec.pfreq=tfsupp(1,:); ec.pind=pind; ec.pamp=pamp; ec.idr=idr; end
    if ~isempty(strfind(DispMode,'plot'))
        scrsz=get(0,'ScreenSize'); figure('Position',[scrsz(3)/4,scrsz(4)/8,2*scrsz(3)/3,2*scrsz(4)/3]);
        ax=axes('Position',[0.1,0.1,0.8,0.8],'FontSize',16); hold all;
        title(ax(1),'Ridge curve \omega_p(t)/2\pi'); ylabel(ax(1),'Frequency (Hz)'); xlabel(ax(1),'Time (s)');
        plot(ax(1),(0:L-1)/fs,tfsupp(1,:),'-k','LineWidth',2,'DisplayName','Extracted frequency profile');
        if ~strcmpi(PathOpt,'on')
            plot(ax(1),(timax-1)/fs,Fp(fimax,timax),'ob','LineWidth',2,'MarkerSize',8,'MarkerFaceColor','r',...
                'DisplayName','Starting point (overall maximum of local functional)');
        end
        legend(ax(1),'show');
    end
end


%----------------------------- Method II ----------------------------------
if length(pars)==2 && method==2
    %Initialize the parameters
    pf=tfsupp(1,:); if fres==2, pf=log(pf); end
    if strcmpi(iparm,'median')
        mv=[median(diff(pf)),0,median(pf),0];
        mv(2)=sqrt(median((diff(pf)-mv(1)).^2));
        mv(4)=sqrt(median((pf-mv(3)).^2));
    else
        mv=[mean(diff(pf)),std(diff(pf)),mean(pf),std(pf)];
    end
    
    %Display, if needed
    if ~strcmpi(DispMode,'off')
        if fres==1
            str0='median+-median std'; if ~strcmpi(iparm,'median'), str0='mean+-std'; end
            fprintf(['Maximums frequencies (',str0,'): ']);
            fprintf('%0.3f+-%0.3f Hz; frequency differences: %0.3f+-%0.3f Hz.\n',mv(3),mv(4),mv(1),mv(2));
        else
            str0='log-median*/median ratio'; if ~strcmpi(iparm,'median'), str0='log-mean*/mean ratio'; end
            fprintf(['Maximums frequencies (',str0,'): ']);
            fprintf('%0.3f*/%0.3f Hz; frequency ratios: %0.3f*/%0.3f.\n',exp(mv(3)),exp(mv(4)),exp(mv(1)),exp(mv(2)));
        end
        fprintf('Extracting the curve by II scheme: iteration discrepancy - ');
        if ~isempty(strfind(DispMode,'plot'))
            scrsz=get(0,'ScreenSize'); figure('Position',[scrsz(3)/4,scrsz(4)/8,2*scrsz(3)/3,2*scrsz(4)/3]);
            ax=zeros(3,1);
            ax(1)=axes('Position',[0.1,0.6,0.8,0.3],'FontSize',16); hold all;
            ax(2)=axes('Position',[0.1,0.1,0.35,0.35],'FontSize',16); hold all;
            ax(3)=axes('Position',[0.55,0.1,0.35,0.35],'FontSize',16); hold all;
            title(ax(1),'Ridge curve \omega_p(t)/2\pi'); ylabel(ax(1),'Frequency (Hz)'); xlabel(ax(1),'Time (s)');
            ylabel(ax(2),'Frequency (Hz)'); xlabel(ax(3),'Iteration number');
            title(ax(2),'\langle\Delta\omega_p\rangle/2\pi (solid), std[\Delta\omega_p]/2\pi (dashed)');
            ylabel(ax(3),'Frequency (Hz)'); xlabel(ax(2),'Iteration number');
            title(ax(3),'\langle\omega_p\rangle/2\pi (solid), std[\omega_p]/2\pi (dashed)');
            line0=plot(ax(1),(0:L-1)/fs,tfsupp(1,:),':','Color',[0.5,0.5,0.5],'DisplayName','Global Maximum ridges');
            line1=plot(ax(2),0,mv(1),'-sk','LineWidth',2,'MarkerSize',6,'MarkerFaceColor','k','DisplayName','\langle\Delta\omega_p\rangle/2\pi');
            line2=plot(ax(2),0,mv(2),'--ok','LineWidth',2,'MarkerSize',6,'MarkerFaceColor','k','DisplayName','std[\Delta\omega_p]/2\pi');
            line3=plot(ax(3),0,mv(3),'-sk','LineWidth',2,'MarkerSize',6,'MarkerFaceColor','k','DisplayName','\langle\omega_p\rangle/2\pi');
            line4=plot(ax(3),0,mv(4),'--ok','LineWidth',2,'MarkerSize',6,'MarkerFaceColor','k','DisplayName','std[\omega_p]/2\pi');
        end
    end
    
    %Iterate
    rdiff=NaN; itn=0; allpind=zeros(10,L); allpind(1,:)=pind;
    if nargout>1, ec.mv=mv; ec.rdiff=rdiff; end
    while rdiff~=0
        %Define the functionals
        smv=[mv(2),mv(4)]; %to avoid underflow
        if smv(1)<=0, smv(1)=10^(-32)/fs; end
        if smv(2)<=0, smv(2)=10^(-16); end
        if isempty(PenalFunc{1}), logw1=@(x)-pars(1)*abs((x-mv(1))/smv(1)); else logw1=@(x)PenalFunc{1}(x,mv(1),smv(1)); end
        if isempty(PenalFunc{2}), logw2=@(x)-(pars(2)/2)*((x-mv(3))/smv(2)).^2; else logw2=@(x)PenalFunc{2}(x,mv(3),smv(2)); end
        %Calculate all
        pind0=pind;
        if strcmpi(PathOpt,'on'), idr=pathopt(Np,Ip,Fp,Wp,logw1,logw2,freq,DispMode);
        else [idr,timax,fimax]=onestepopt(Np,Ip,Fp,Wp,logw1,logw2,freq,DispMode); end
        lid=sub2ind(size(Fp),idr(tn1:tn2),tn1:tn2);
        tfsupp(1,tn1:tn2)=Fp(lid); pind(tn1:tn2)=round(Ip(lid)); pamp(tn1:tn2)=Qp(lid);
        rdiff=length(find(pind(tn1:tn2)-pind0(tn1:tn2)~=0))/(tn2-tn1+1);
        itn=itn+1;
        %Update the averages
        pf=tfsupp(1,:); if fres==2, pf=log(pf); end
        mv=[mean(diff(pf)),0,mean(pf),0];
        mv(2)=sqrt(mean((diff(pf)-mv(1)).^2));
        mv(4)=sqrt(mean((pf-mv(3)).^2));
        %Update the information structure, if needed
        if nargout>1
            ec.pfreq=[ec.pfreq;tfsupp(1,:)]; ec.pind=[ec.pind;pind]; ec.pamp=[ec.pamp;pamp]; ec.idr=[ec.idr;idr];
            ec.mv=[ec.mv;mv]; ec.rdiff=[ec.rdiff,rdiff];
        end
        %Display, if needed
        if ~strcmpi(DispMode,'off')
            fprintf('%0.2f%%; ',100*rdiff);
            if ~isempty(strfind(DispMode,'plot'))
                line0=plot(ax(1),(0:L-1)/fs,tfsupp(1,:),'DisplayName',sprintf('Iteration %d (discrepancy %0.2f%%)',itn,100*rdiff));
                set(line1,'XData',0:itn,'YData',[get(line1,'YData'),mv(1)]);
                set(line2,'XData',0:itn,'YData',[get(line2,'YData'),mv(2)]);
                set(line3,'XData',0:itn,'YData',[get(line3,'YData'),mv(3)]);
                set(line4,'XData',0:itn,'YData',[get(line4,'YData'),mv(4)]);
                if ~strcmpi(PathOpt,'on'),
                    mpt=plot(ax(1),(timax-1)/fs,Fp(fimax,timax),'ok','LineWidth',2,'MarkerSize',8,'MarkerFaceColor','w',...
                        'DisplayName',['Starting point (iteration ',num2str(itn),')']);
                end
            end
        end
        %Check for "cycling" and maximum iterations
        allpind(itn+1,:)=pind; gg=Inf;
        if rdiff~=0 && itn>2
            for kn=2:itn-1, gg=min([gg,length(find(pind(tn1:tn2)-allpind(kn,tn1:tn2)~=0))]); end
        end
        if gg==0
            if ~strcmpi(DispMode,'off')
                fprintf('converged to a cycle, terminating iteration.');
            end
            break;
        end
        if itn>MaxIter, break; end
    end
    
    if ~strcmpi(DispMode,'off')
        fprintf('\n');
        if itn>MaxIter
            fprintf('Not converged within MaxIter=%d iterations; the latest curve estimate is used.',MaxIter);
        end
        if ~isempty(strfind(DispMode,'plot'))
            set(line0,'Color','k','LineWidth',2);
            if ~strcmpi(PathOpt,'on'), set(mpt,'Color',b,'MarkerFaceColor','k'); end
            if fres==2 %change plot if the resolution is logarithmic
                set(line3,'YData',exp(get(line3,'YData')),'DisplayName','exp(\langle\Deltalog\omega_p\rangle)');
                set(line4,'YData',exp(get(line4,'YData'))-1,'DisplayName','exp(std[\Deltalog\omega_p])-1');
                set(line3,'YData',exp(get(line1,'YData')),'DisplayName','exp(\langlelog\omega_p\rangle)/2\pi');
                set(line4,'YData',exp(get(line2,'YData'))-1,'DisplayName','exp(std[log\omega_p])-1');
                ylabel(ax(2),'Frequency Ratio'); ylabel(ax(3),'Frequency (Hz)');
                title(ax(2),'exp(\langle\Deltalog\omega_p\rangle) (solid), exp(std[\Deltalog\omega_p])-1 (dashed)');
                title(ax(3),'exp(\langlelog\omega_p\rangle)/2\pi (solid), exp(std[log\omega_p])-1 (dashed)');
            end
        end
    end
    
end

%----------------------------- Method III ---------------------------------
if length(pars)==2 && method==3
    %Initialize the distributions
    P1=zeros(1,2*NF+1); P2=zeros(1,NF+2);
    pf=tfsupp(1,:); gf=freq; if fres==2, pf=log(pf); gf=log(gf); end
    ci=1+(diff(pf)-dfreq(1))/fstep; cm=ci-floor(ci); ci=floor(ci);
    for tn=tn1:tn2-1, P1(ci(tn)+1)=P1(ci(tn)+1)+1-cm(tn); P1(ci(tn)+2)=P1(ci(tn)+2)+cm(tn); end
    ci=1+(Fp-gf(1))/fstep; cm=ci-floor(ci); ci=floor(ci);
    for tn=tn1:tn2
        for pn=1:Np(tn)
            cci=ci(pn,tn); ccm=cm(pn,tn);
            P2(cci+1)=P2(cci+1)+(1-ccm)*Qp(pn,tn); P2(cci+2)=P2(cci+2)+ccm*Qp(pn,tn);
        end
    end
    P1=P1(2:end-1); P2=P2(2:end-1);
    
    %Display, if needed
    if ~strcmpi(DispMode,'off')
        [~,idm1]=max(P1); [~,idm2]=max(P2);
        ss2=sort(diff(tfsupp(1,tn1:tn2))); CL2=length(ss2);
        if fres==1
            ss1=sort(diff(tfsupp(1,tn1:tn2))); CL1=length(ss1);
            fprintf('Maximums frequencies (mode and 75% range): %0.3f [%0.3f,%0.3f]; frequency differences: %0.3f [%0.3f,%0.3f].\n',...
                freq(idm2),ss2(round(0.125*CL2)),ss2(round(0.875*CL2)),dfreq(idm1),ss1(round(0.125*CL1)),ss1(round(0.875*CL1)));
        else
            ss1=sort(exp(diff(log(tfsupp(1,tn1:tn2))))); CL1=length(ss1);
            fprintf('Maximums frequencies (mode and 75% range): %0.3f [%0.3f,%0.3f]; frequency ratios: %0.3f [%0.3f,%0.3f].\n',...
                freq(idm2),ss2(round(0.125*CL2)),ss2(round(0.875*CL2)),exp(dfreq(idm1)),ss1(round(0.125*CL1)),ss1(round(0.875*CL1)));
        end
        fprintf('Extracting the curve by III scheme: iteration discrepancy - ');
        if ~isempty(strfind(DispMode,'plot'))
            cdfreq=dfreq; cxlim=[-DD,DD]; if fres==2, cdfreq=exp(cdfreq); cxlim=exp(cxlim); end
            scrsz=get(0,'ScreenSize'); figure('Position',[scrsz(3)/4,scrsz(4)/8,2*scrsz(3)/3,2*scrsz(4)/3]);
            ax=zeros(3,1);
            ax(1)=axes('Position',[0.1,0.6,0.8,0.3],'FontSize',16); hold all;
            ax(2)=axes('Position',[0.1,0.1,0.35,0.35],'FontSize',16,'XLim',cxlim); hold all;
            ax(3)=axes('Position',[0.55,0.1,0.35,0.35],'FontSize',16,'XLim',[freq(1),freq(end)]); hold all;
            xlabel(ax(2),'\Delta\nu/2\pi (Hz)'); title(ax(2),'P_1(\Delta\nu) (normalized on max)');
            xlabel(ax(3),'\nu/2\pi (Hz)'); title(ax(3),'P_2(\nu) (normalized on max)');
            title(ax(1),'Ridge curve \omega_p(t)/2\pi'); ylabel(ax(1),'Frequency (Hz)'); xlabel(ax(1),'Time (s)');
            line0=plot(ax(1),(0:L-1)/fs,tfsupp(1,:),':','Color',[0.5,0.5,0.5],'DisplayName','Global Maximum ridges');
            line1=plot(ax(2),cdfreq,P1/max(P1),':','Color',[0.5,0.5,0.5],'DisplayName','Initial (from global maximums)');
            line2=plot(ax(3),freq,P2/max(P2),':','Color',[0.5,0.5,0.5],'DisplayName','Initial (from global maximums)');
        end
    end
    
    %Iterate
    rdiff=NaN; itn=0; allpind=zeros(10,L); allpind(1,:)=pind;
    if nargout>1, ec.P1=P1; ec.P2=P2; ec.rdiff=rdiff; end
    while rdiff~=0
        %Construct the functionals
        logw1=pars(1)*log(P1); logw2=pars(2)*log(P2);
        ii=find(P1(1:end-1).*P1(2:end)>0 & abs(P1(1:end-1)+P1(2:end)-1)<=2*eps); logw1(ii)=-Inf; logw1(ii+1)=-Inf;
        if itn>0, ii=find(P2(1:end-1).*P2(2:end)>0 & abs(P2(1:end-1)+P2(2:end)-1)<=2*eps); logw2(ii)=-Inf; logw2(ii+1)=-Inf; end
        logw1(1:NF-ceil(DD/fstep))=-Inf; logw1(NF+ceil(DD/fstep):end)=-Inf;
        nnid=find(~isfinite(logw1)); dind=[0;find(diff(nnid(:))>1);length(nnid)];
        for dn=1:length(dind)-1
            cii=nnid(1+dind(dn)):nnid(dind(dn+1));
            if cii(1)>2 && logw1(cii(1)-2)>logw1(cii(1)-1)
                logw1(cii)=max(logw1(cii(1)-1)-(cii-cii(1)+1)*(logw1(cii(1)-2)-logw1(cii(1)-1)),logw1(cii));
            end
            if cii(end)<length(logw1)-2 && logw1(cii(end)+2)>logw1(cii(1)+1)
                logw1(cii)=max(logw1(cii(end)+1)-(cii(end)-cii+1)*(logw1(cii(end)+2)-logw1(cii(end)+1)),logw1(cii));
            end
        end
        nnid=find(~isfinite(logw2)); dind=unique([0;find(diff(nnid(:))>1);length(nnid)]);
        for dn=1:length(dind)-1
            cii=nnid(1+dind(dn)):nnid(dind(dn+1));
            if cii(1)>2 && logw2(cii(1)-2)>logw2(cii(1)-1)
                logw2(cii)=max(logw2(cii(1)-1)-(cii-cii(1)+1)*(logw2(cii(1)-2)-logw2(cii(1)-1)),logw2(cii));
            end
            if cii(end)<length(logw2)-2 && logw2(cii(end)+2)>logw2(cii(1)+1)
                logw2(cii)=max(logw2(cii(end)+1)-(cii(end)-cii+1)*(logw2(cii(1)+2)-logw2(cii(1)+1)),logw2(cii));
            end
        end
        logw1(~isfinite(logw1))=min(logw1)-10*(max(logw1)-min(logw1));
        logw2(~isfinite(logw2))=min(logw2)-10*(max(logw2)-min(logw2));
        if ~isempty(PenalFunc{1}), logw1=PenalFunc{1}(logw1/pars(1)); end
        if ~isempty(PenalFunc{2}), logw2=PenalFunc{2}(logw2/pars(2)); end
        %Calculate all
        pind0=pind;
        if strcmpi(PathOpt,'on'), idr=pathopt(Np,Ip,Fp,Wp,logw1,logw2,freq,DispMode);
        else [idr,timax,fimax]=onestepopt(Np,Ip,Fp,Wp,logw1,logw2,freq,DispMode); end
        lid=sub2ind(size(Fp),idr(tn1:tn2),tn1:tn2);
        tfsupp(1,tn1:tn2)=Fp(lid); pind(tn1:tn2)=round(Ip(lid)); pamp(tn1:tn2)=Qp(lid);
        rdiff=length(find(pind(tn1:tn2)-pind0(tn1:tn2)~=0))/(tn2-tn1+1);
        itn=itn+1;
        %Update the distributions
        P1=zeros(1,2*NF+1); P2=zeros(1,NF+2);
        pf=tfsupp(1,:); gf=freq; if fres==2, pf=log(pf); gf=log(gf); end
        ci=1+(diff(pf)-dfreq(1))/fstep; cm=ci-floor(ci); ci=floor(ci);
        for tn=tn1:tn2-1, P1(ci(tn)+1)=P1(ci(tn)+1)+1-cm(tn); P1(ci(tn)+2)=P1(ci(tn)+2)+cm(tn); end
        ci=1+(pf-gf(1))/fstep; cm=ci-floor(ci); ci=floor(ci);
        for tn=tn1:tn2, P2(ci(tn)+1)=P2(ci(tn)+1)+1-cm(tn); P2(ci(tn)+2)=P2(ci(tn)+2)+cm(tn); end
        P1=P1(2:end-1); P2=P2(2:end-1);
        %Update the information structure, if needed
        if nargout>1
            ec.pfreq=[ec.pfreq;tfsupp(1,:)]; ec.pind=[ec.pind;pind]; ec.pamp=[ec.pamp;pamp]; ec.idr=[ec.idr;idr];
            ec.P1=[ec.P1;P1]; ec.P2=[ec.P2;P2]; ec.rdiff=[ec.rdiff,rdiff];
        end
        %Display, if needed
        if ~strcmpi(DispMode,'off')
            fprintf('%0.2f%%; ',100*rdiff);
            if ~isempty(strfind(DispMode,'plot'))
                line0=plot(ax(1),(0:L-1)/fs,tfsupp(1,:),'DisplayName',sprintf('Iteration %d (discrepancy %0.2f%%)',itn,100*rdiff));
                line1=plot(ax(2),cdfreq,P1/max(P1),'DisplayName',sprintf('Iteration %d',itn));
                line2=plot(ax(3),freq,P2/max(P2),'DisplayName',sprintf('Iteration %d',itn));
                if ~strcmpi(PathOpt,'on'),
                    mpt=plot(ax(1),(timax-1)/fs,Fp(fimax,timax),'ok','LineWidth',2,'MarkerSize',8,'MarkerFaceColor','w',...
                        'DisplayName',['Starting point (iteration ',num2str(itn),')']);
                end
            end
        end
        %Check for "cycling" and maximum iterations
        allpind(itn+1,:)=pind; gg=Inf;
        if rdiff~=0 && itn>2
            for kn=2:itn-1, gg=min([gg,length(find(pind(tn1:tn2)-allpind(kn,tn1:tn2)~=0))]); end
        end
        if gg==0
            if ~strcmpi(DispMode,'off')
                fprintf('converged to a cycle, terminating iteration.');
            end
            break;
        end
        if itn>MaxIter, break; end
    end
    
    if ~strcmpi(DispMode,'off')
        fprintf('\n');
        if itn>MaxIter
            fprintf('Not converged within MaxIter=%d iterations; the latest curve estimate is used.',MaxIter);
        end
        if ~isempty(strfind(DispMode,'plot'))
            set([line0,line1,line2],'Color','k','LineWidth',2);
            if ~strcmpi(PathOpt,'on'), set(mpt,'Color',b,'MarkerFaceColor','k'); end
            if fres==2 %change plot if the resolution is logarithmic
                set(ax(2:3),'XScale','log');
                xlabel(ax(2),'1+\Delta\nu/\nu');
                title(ax(2),'P_1(1+\Delta\nu/\nu) (normalized on max)');
            end
        end
    end
    
end
if nargout>1, varargout{1}=ec; end

%//////////////////////////////////////////////////////////////////////////
%Extract the time-frequency support around the ridge points
TFR=abs(TFR);
for tn=tn1:tn2
    cs=abs(TFR(:,tn)); cpeak=pind(tn);
    iup=[]; idown=[];
    if cpeak<NF-1, iup=cpeak+find(cs(cpeak+1:end)==0,1,'first'); end
    if cpeak>2, idown=cpeak-find(cs(cpeak-1:-1:1)==0,1,'first'); end
    iup=min([iup,NF]); idown=max([idown,1]);
    tfsupp(2,tn)=idown; tfsupp(3,tn)=iup;
end
tfsupp(2:3,tn1:tn2)=freq(tfsupp(2:3,tn1:tn2));
%//////////////////////////////////////////////////////////////////////////

%Final display
if ~strcmpi(DispMode,'off')
    fprintf('Curve extracted: ridge frequency %0.2f+-%0.2f Hz, lower support %0.2f+-%0.2f Hz, upper support %0.2f+-%0.2f Hz\n',...
        mean(tfsupp(1,:)),std(tfsupp(1,:)),mean(tfsupp(2,:)),std(tfsupp(2,:)),mean(tfsupp(3,:)),std(tfsupp(3,:)));
end

%Plot (if needed)
if ~isempty(strfind(PlotMode,'on')), plotfinal(tfsupp,TFR,freq,fs,DispMode,PlotMode); end

end


%==========================================================================
%========================= Support functions ==============================
%==========================================================================

%==================== Path optimization algorithm =========================
function idid=pathopt(Np,Ip,Fp,Wp,logw1,logw2,freq,DispMode)

[Mp,L]=size(Fp); NF=length(freq);
tn1=find(Np>0,1,'first'); tn2=find(Np>0,1,'last');
if min(freq)>0 && std(diff(freq))>std(diff(log(freq))), Fp=log(Fp); end

%Weighting functions
if ~isa(logw1,'function_handle')
    if isempty(logw1)
        logw1=zeros(2*NF+1,L);
    else
        logw1=[2*logw1(1)-logw1(2);logw1(:);2*logw1(end)-logw1(end-1)];
    end
end
if ~isa(logw2,'function_handle')
    if isempty(logw2)
        W2=zeros(Mp,L);
    else
        logw2=[2*logw2(1)-logw2(2);logw2(:);2*logw2(end)-logw2(end-1);NaN;NaN];
        ci=Ip; ci(isnan(ci))=NF+2; cm=ci-floor(ci); ci=floor(ci);
        W2=(1-cm).*logw2(ci+1)+cm.*logw2(ci+2); clear ci cm;
    end
else
    W2=logw2(Fp);
end
W2=Wp+W2;

%The algorithm by itself
q=zeros(Mp,L)*NaN; U=zeros(Mp,L)*NaN;
q(1:Np(tn1),tn1)=0; U(1:Np(tn1),tn1)=W2(1:Np(tn1),tn1);
if isa(logw1,'function_handle')
    for tn=tn1+1:tn2
        cf=Fp(1:Np(tn),tn)*ones(1,Np(tn-1))-ones(Np(tn),1)*Fp(1:Np(tn-1),tn-1)'; CW1=logw1(cf);
        [U(1:Np(tn),tn),q(1:Np(tn),tn)]=max(W2(1:Np(tn),tn)*ones(1,Np(tn-1))+CW1+ones(Np(tn),1)*U(1:Np(tn-1),tn-1)',[],2);
    end
else
    for tn=tn1+1:tn2
        ci=Ip(1:Np(tn),tn)*ones(1,Np(tn-1))-ones(Np(tn),1)*Ip(1:Np(tn-1),tn-1)';
        ci=ci+NF; cm=ci-floor(ci); ci=floor(ci);
        if Np(tn)>1, CW1=(1-cm).*logw1(ci+1)+cm.*logw1(ci+2);
        else CW1=(1-cm).*logw1(ci+1)'+cm.*logw1(ci+2)'; end
        [U(1:Np(tn),tn),q(1:Np(tn),tn)]=max(W2(1:Np(tn),tn)*ones(1,Np(tn-1))+CW1+ones(Np(tn),1)*U(1:Np(tn-1),tn-1)',[],2);
    end
end

%Recover the indices
idid=zeros(1,L)*NaN; [~,idid(tn2)]=max(U(:,tn2));
for tn=tn2-1:-1:tn1, idid(tn)=q(idid(tn+1),tn+1); end

%Plot if needed
%{
if ~isempty(strfind(DispMode,'plot+'))
    figure; axes('FontSize',16,'Box','on'); hold all;
    rlines=zeros(Np(tn2),1);
    for pn=1:Np(tn2)
        cridges=zeros(L,1)*NaN; cridges(tn2)=pn;
        for tn=tn2-1:-1:tn1
            cridges(tn)=q(cridges(tn+1),tn+1);
        end
        lind=sub2ind(size(Ip),cridges(tn1:tn2),(tn1:tn2)');
        cridges=[ones(tn1-1,1)*NaN;Ip(lind);ones(L-tn2,1)*NaN];
        crfreq=[ones(tn1-1,1);freq(cridges(tn1:tn2));ones(L-tn2,1)];
        rlines(pn)=plot((0:L-1)/fs,crfreq,'DisplayName',['U=',num2str(U(pn,tn2))]);
    end
    [~,imax]=max(U(:,tn2)); uistack(rlines(imax),'top');
    set(rlines(imax),'Color','k','LineWidth',2);
    title('Path to each of the last peaks maximizing the given functional');
    ylabel('Frequency (Hz)'); xlabel('Time (s)');
end
%}

end

%================== One-step optimization algorithm =======================
function [idid,varargout]=onestepopt(Np,Ip,Fp,Wp,logw1,logw2,freq,DispMode)

[Mp,L]=size(Fp); NF=length(freq);
tn1=find(Np>0,1,'first'); tn2=find(Np>0,1,'last');
if min(freq)>0 && std(diff(freq))>std(diff(log(freq))), Fp=log(Fp); end

%Weighting functions
if ~isa(logw1,'function_handle')
    if isempty(logw1)
        logw1=zeros(2*NF+1,L);
    else
        logw1=[2*logw1(1)-logw1(2);logw1(:);2*logw1(end)-logw1(end-1)];
    end
end
if ~isa(logw2,'function_handle')
    if isempty(logw2)
        W2=zeros(Mp,L);
    else
        logw2=[2*logw2(1)-logw2(2);logw2(:);2*logw2(end)-logw2(end-1);NaN;NaN];
        ci=Ip; ci(isnan(ci))=NF+2; cm=ci-floor(ci); ci=floor(ci);
        W2=(1-cm).*logw2(ci+1)+cm.*logw2(ci+2); clear ci cm;
    end
else
    W2=logw2(Fp);
end
W2=Wp+W2;

%The algorithm by itself
[~,imax]=max(W2(:)); [fimax,timax]=ind2sub([Mp,L],imax); %determine the starting point
idid=zeros(1,L)*NaN; idid(timax)=fimax;
if isa(logw1,'function_handle')
    for tn=timax+1:tn2
        cf=Fp(1:Np(tn),tn)-Fp(idid(tn-1),tn-1);
        CW1=logw1(cf); [~,idid(tn)]=max(W2(1:Np(tn),tn)+CW1);
    end
    for tn=timax-1:-1:tn1
        cf=-Fp(1:Np(tn),tn)+Fp(idid(tn+1),tn+1);
        CW1=logw1(cf); [~,idid(tn)]=max(W2(1:Np(tn),tn)+CW1);
    end
else
    for tn=timax+1:tn2
        ci=NF+Ip(1:Np(tn),tn)-Ip(idid(tn-1),tn-1); cm=ci-floor(ci); ci=floor(ci);
        CW1=(1-cm).*logw1(ci+1)+cm.*logw1(ci+2); [~,idid(tn)]=max(W2(1:Np(tn),tn)+CW1);
    end
    for tn=timax-1:-1:tn1
        ci=NF-Ip(1:Np(tn),tn)+Ip(idid(tn+1),tn+1); cm=ci-floor(ci); ci=floor(ci);
        CW1=(1-cm).*logw1(ci+1)+cm.*logw1(ci+2); [~,idid(tn)]=max(W2(1:Np(tn),tn)+CW1);
    end
end

if nargout>1, varargout{1}=timax; end
if nargout>2, varargout{2}=fimax; end

end

%========================= Plotting function ==============================
function plotfinal(tfsupp,TFR,freq,fs,DispMode,PlotMode)

[NF,L]=size(TFR); fres=1; if min(freq)>0 && std(diff(freq))>std(diff(log(freq))), fres=2; end
XX=(0:(L-1))/fs; YY=freq; ZZ=abs(TFR); scrsz=get(0,'ScreenSize');

MYL=round(scrsz(3)); MXL=round(scrsz(4)); %maximum number of points seen in plots
if isempty(strfind(lower(PlotMode),'wr')) && (size(ZZ,1)>MYL || size(ZZ,2)>MXL)
    if ~strcmpi(DispMode,'off')
        fprintf('Warning: TFR contains more data points (%d x %d) than pixels in the plot, so for a\n',size(ZZ,1),size(ZZ,2));
        fprintf('         better performance its resampled version (%d x %d) will be displayed instead.\n',min([MYL,size(ZZ,1)]),min([MXL,size(ZZ,2)]));
    end
    XI=XX; YI=YY;
    if size(ZZ,2)>MXL, XI=linspace(XX(1),XX(end),MXL); end
    if fres==1
        if size(ZZ,1)>MYL, YI=linspace(YY(1),YY(end),MYL); end
        ZZ=aminterp(XX,YY,ZZ,XI,YI,'max');
    else
        if size(ZZ,1)>MYL, YI=exp(linspace(log(YY(1)),log(YY(end)),MYL)); end
        ZZ=aminterp(XX,log(YY),ZZ,XI,log(YI),'max');
    end
    XX=XI(:); YY=YI(:);
end

figure('Position',[scrsz(3)/4,scrsz(4)/4,scrsz(3)/2,2*scrsz(4)/3]);
axes('Position',[0.1,0.1,0.8,0.75],'NextPlot','Add','FontSize',18,...
    'XLim',[XX(1),XX(end)],'YLim',[YY(1),YY(end)]);
xlabel('Time (s)'); ylabel('Frequency (Hz)');
title({'TFR amplitude', '(black: extracted peaks; gray: their supports)'});
if fres==2, set(gca,'YScale','log'); end
pc=pcolor(XX,YY,ZZ); set(pc,'LineStyle','none');
plot((0:(L-1))/fs,tfsupp(1,:),'Color','k','LineWidth',2);
plot((0:(L-1))/fs,tfsupp(2,:),'Color',[0.5,0.5,0.5],'LineWidth',2);
plot((0:(L-1))/fs,tfsupp(3,:),'Color',[0.5,0.5,0.5],'LineWidth',2);

%{
%Previous version
[NF,L]=size(TFR); scrsz=get(0,'ScreenSize');
vmax=max(abs(TFR(:))); tt=(0:(L-1))/fs;
if min(freq)<=0 || std(diff(freq))<std(diff(log(freq)))
    fres=1; fstep=mean(diff(freq)); eind=1+floor(0.5+(tfsupp-freq(1))/fstep);
else
    fres=2; fstep=mean(diff(log(freq))); eind=1+floor(0.5+log(tfsupp/freq(1))/fstep);
end
eind(eind<1)=1; eind(eind>NF)=NF;

figure('Position',[scrsz(3)/4,scrsz(4)/4,scrsz(3)/2,2*scrsz(4)/3]);
axes('Position',[0.1,0.1,0.8,0.75],'NextPlot','Add','FontSize',18,...
    'XLim',[tt(1),tt(end)],'YLim',[freq(1),freq(end)],'ZLim',[0,vmax]);
xlabel('Time (s)'); ylabel('Frequency (Hz)');
title({'TFR amplitude', '(black: extracted peaks; gray: their supports)'});
if fres==2, set(gca,'YScale','log'); end
surf(tt,freq,abs(TFR),'LineStyle','none'); view(2);

cc={'k',[0.5,0.5,0.5],[0.5,0.5,0.5]};
for kn=1:3
    line(tt,tfsupp(kn,:),vmax*ones(1,L),'Color',cc{kn},'LineWidth',2);
    line([tt(1),tt(1)],[tfsupp(kn,1),tfsupp(kn,1)],[abs(TFR(eind(kn,1),1)),vmax],'Color',cc{kn},'LineWidth',2);
    line([tt(end),tt(end)],[tfsupp(kn,end),tfsupp(kn,end)],[abs(TFR(eind(kn,end),end)),vmax],'Color',cc{kn},'LineWidth',2);
    line(tt,tfsupp(kn,:),abs(TFR(sub2ind(size(TFR),eind(kn,:),1:L))),'Color',cc{kn},'LineWidth',2);
end
%}

end

%=============== Interpolation function for plotting ======================

% nZ=aminterp(X,Y,Z,XI,YI,method)
% - the same as interp2, but uses different interpolation types,
% maximum-based ([method]='max') or average-based ([method]='avg'), where
% [nZ] at each point [XI,YI] represents maximum or average among values of
% [Z] corresponding to the respective quadrant in [X,Y]-space which
% includes point [XI,YI];
% X,Y,XI,YI are all linearly spaced vectors, and the lengths of XI,YI
% should be the same or smaller than that of X,Y; Z should be real,
% ideally positive; X and Y correspond to the 2 and 1 dimension of Z, as
% always. 

function nZ = aminterp(X,Y,Z,XI,YI,method)

%Interpolation over X
nZ=zeros(size(Z,1),length(XI))*NaN;
xstep=mean(diff(XI));
xind=1+floor((1/2)+(X-XI(1))/xstep); xind=xind(:);
xpnt=[0;find(xind(2:end)>xind(1:end-1));length(xind)];
if strcmpi(method,'max')
    for xn=1:length(xpnt)-1
        xid1=xpnt(xn)+1; xid2=xpnt(xn+1);
        nZ(:,xind(xid1))=max(Z(:,xid1:xid2),[],2);
    end
else
    for xn=1:length(xpnt)-1
        xid1=xpnt(xn)+1; xid2=xpnt(xn+1);
        nZ(:,xind(xid1))=mean(Z(:,xid1:xid2),2);
    end
end

Z=nZ;

%Interpolation over Y
nZ=zeros(length(YI),size(Z,2))*NaN;
ystep=mean(diff(YI));
yind=1+floor((1/2)+(Y-YI(1))/ystep); yind=yind(:);
ypnt=[0;find(yind(2:end)>yind(1:end-1));length(yind)];
if strcmpi(method,'max')
    for yn=1:length(ypnt)-1
        yid1=ypnt(yn)+1; yid2=ypnt(yn+1);
        nZ(yind(yid1),:)=max(Z(yid1:yid2,:),[],1);
    end
else
    for yn=1:length(ypnt)-1
        yid1=ypnt(yn)+1; yid2=ypnt(yn+1);
        nZ(yind(yid1),:)=mean(Z(yid1:yid2,:),1);
    end
end


end