[xm,ym,xx,yy]=Seekpeaks(fun,x,y,y_tol,Nsteps,varargin)

function [xm,ym,xx,yy]=Seekpeaks(fun,x,y,y_tol,Nsteps,varargin)
%     [xm,ym,xx,yy]=Seekpeaks(fun,x,y,y_tol,Nsteps,varargin)
% Searches the maxima of the function 'fun', which is an object such as
% produced by inline or by a m-function. y=fun(x,P1,P2,...), where P1, P2,...
% are additional parameters used in the function 'fun' and are given in
% 'varargin' in Seekpeaks as p1,p2,... (see example below)
% Note: use 'fun' for m-function and (just) fun for inline function
%     The INPUT x and y (=fun(x)) are the samples used to locate the maxima and the program 
% will refine iterativeley around the local maxima to find the exact
% maxima. y_tol gives the relative stop criteria for stopping the maxima
% search and Nsteps is a number to terminate the session in case it takes
% long time to find the maxima. Default are 1e-6 and 50 and will be used
% in case [],[] are used as arguments in the function.
%     The OUTPUT xm gives the positions of the maxima and ym the corresponding
% values (vectors). xx and yy gives the union of all values that are 
% found during the refinement including the input sample x and y (it is
% useful for plot-purposes).
%
% %%% Example:
%
% % define a function fun(x) with an additional parameter s
% fun1=inline('s^2*(1./(s^2+(x-1).^2)+1./(s^2+(x-2).^2))','x','s')
% % calculate a rough sample (x,y) of the function
% x=linspace(0,3,20);
% s=0.001;    %parameter
% y=fun1(x,s);
% % Seek the peaks of the function fun1
% [xm,ym,xx,yy]=Seekpeaks(fun1,x,y,[],[],s);
% xm
% plot(xx,yy,x,y,'.');


%START fix default values
if nargin <4
    y_tol=1e-6; %default
    Nsteps=50; %default
elseif nargin <5
    if isempty(y_tol)
        y_tol=1e-6; %default
    end
    Nsteps=50; %default
end
if and(nargin>=5,isempty(Nsteps))
    Nsteps=50; %default
end
if isempty(y_tol)
    y_tol=1e-6; %default
end
%END fix default

%sort values and make them rows
x=x(:)';y=y(:)';
[x,ix]=sort(x);y=y(ix);
%scale y-axis to avoid zero-division in error-estimation
%(scale back at the end of the program)
ymin=min(y);ymax=max(y);
y=(y-ymin)/(ymax-ymin);

%first find local maxima of (x,y)
nm=0;   %number of maxima (exluded end-points)
p1=y(2)<y(1);
pN=y(end-1)<y(end);
for j=2:length(x)-1
    if or(and(y(j)>=y(j-1),y(j)>y(j+1)),and(y(j)>=y(j+1),y(j)>y(j-1)))
        nm=nm+1;
        idx(nm)=j;  %index for maxima;
    end
end

if and(nm==0,not(or(p1,pN)))
    sprintf(['There are no local maxima.','\n',...
        'Empty xm and ym returned'])
    xx=x;yy=y;xm=[];ym=[];
    return
end
sprintf(['looking for ',int2str(nm),' maxima'])

%now check if maxima at ends
xm=[];ym=[];
cc=1;   %maxima counter
if p1
    xm(cc)=x(1);ym(cc)=y(1);        %max values
    cc=cc+1;
end
if pN
    xm(cc)=x(end);ym(cc)=y(end);    %max values
    cc=cc+1;
end

xs=[];ys=[];    %accumulated search values
pex=0;          %extra found maxima
warn=0;         %warning message
for j=1:nm
    x1=x(idx(j)-1);x2=x(idx(j));x3=x(idx(j)+1);%init values
    y1=y(idx(j)-1);y2=y(idx(j));y3=y(idx(j)+1);
    Noterminate=true;cp=1;
    while Noterminate
        %calculate intermediate values (scaled)
        x4=(x1+x2)/2;
        x5=(x2+x3)/2;
        y4=(feval(fun,x4,varargin{:})-ymin)/(ymax-ymin);
        y5=(feval(fun,x5,varargin{:})-ymin)/(ymax-ymin);
        %add new data to output
        xs=[xs,[x4 x5]];ys=[ys,[y4 y5]];
        %seek maxima
        xvec=[x1 x4 x2 x5 x3];
        yvec=[y1 y4 y2 y5 y3];
        if and(y4>y2,y5>y2)
            pex=pex+1;
            %2 maxima
            [junk,wh1]=sort([-1 y4 y2 y5 -2]);
            wh1=wh1(end);
            x1=xvec(wh1-1);y1=yvec(wh1-1);
            x2=xvec(wh1);y2=yvec(wh1);
            x3=xvec(wh1+1);y3=yvec(wh1+1);
            err=(max([y2 y4 y5])-min(abs([y2 y4 y5])))/min(abs([y2 y4 y5]));
        elseif and(y4~=y5,and(y4~=y2,y5~=y2))   %no equal
            %only 1 maximum
            [junk,wh1]=sort([-1 y4 y2 y5 -2]);
            wh1=wh1(end);
            x1=xvec(wh1-1);y1=yvec(wh1-1);
            x2=xvec(wh1);y2=yvec(wh1);
            x3=xvec(wh1+1);y3=yvec(wh1+1);
            err=(max([y2 y4 y5])-min(abs([y2 y4 y5])))/min(abs([y2 y4 y5]));
        elseif and(y4==y5,and(y4==y2,y5==y2))   %all equal
            wh1=3;
            err=0;
        else
            [junk,wh1]=sort([-1 y4 y2 y5 -2]);
            wh1=wh1(end);
            x1=xvec(wh1-1);y1=yvec(wh1-1);
            x2=xvec(wh1);y2=yvec(wh1);
            x3=xvec(wh1+1);y3=yvec(wh1+1);
            err=(max([y2 y4 y5])-min(abs([y2 y4 y5])))/min(abs([y2 y4 y5]));
        end
        cp=cp+1;    %increase counter for seekpeaks
        if cp>Nsteps
            warn=warn+1;
        end
        Noterminate=not(or(cp>Nsteps,err<=y_tol));
    end
    %errvec(j)=err
    xm(cc)=xvec(wh1);
    ym(cc)=yvec(wh1);cc=cc+1;
end

%sort output values and scale back
xx=[x,xs];
yy=[y,ys];
[xx,ix]=sort(xx);
yy=ymin+(ymax-ymin)*yy(ix);
[xm,ix]=sort(xm);
ym=ymin+(ymax-ymin)*ym(ix);

%write result text
sprintf([int2str(length(xm)),' maxima was found','\n',...
         int2str(warn),' peaks exceeded maximum iterations!','\n',...
         int2str(pex),' extra peaks found -> run again to find (if >0)','\n'])