function [r,w,a] = fit_gaussian(x,y,varargin)
%function [r,w,a] = fit_gaussian(x,y)
% fit gaussian of the form 
%      y = a * exp(-4*(x-r).^2/(w.^2)) 
% to input profile data y on spatial grid x
%
% r is x location of peak of gaussian
% w is full width
% a is amplitude (peak value)
%
%  optinal inputs
%     function [r,w,a] = fit_gaussian(x,y,tol,nmax,verbose)
%     tol: stopping tolerance for norm of parameter difference
%     nmax: maximum iterations
%     verbose: boolean to enable nice plots and convergence info


% F. Felici TU/e 2017, based on Guo's algorithm :
%   [IEEE SIGNAL PROCESSING MAGAZINE [134] SEPTEMBER 2011]


%% defaults
tol=1e-3; % stop when norm of parameter estimates changes by less than tol
nmax=30; % maximum iterations
verbose = false;

if numel(varargin)>=1 && ~isempty(varargin{1})
    tol = varargin{1};
end
if numel(varargin)>=2 && ~isempty(varargin{2})
    nmax = varargin{2};
end
if numel(varargin)>=3 && ~isempty(varargin{3})
    verbose = varargin{3};
end

if norm(y)==0
    warning('zero profile, return default fit')
    a = 0;
    r = 0;
    w = 1;
end

%% mask negative numbers
ineg = (y<=0);
assert(sum(~ineg)>=3,'need at least 3 valid points to fit gaussians') 
xx = x(~ineg);
yy = y(~ineg);
%%
xnorm = (x-x(1))/(x(end)-x(1)); %weight squared, first guess
w2 = ones(size(xx));
vprev = 0;

for ii=1:nmax;
    wa0 = sum(w2);
    wa1 = sum(w2.*xx);
    wa2 = sum(w2.*xx.^2);
    wa3 = sum(w2.*xx.^3);
    wa4 = sum(w2.*xx.^4);
    
    A = [wa0, wa1 , wa2 ;
        wa1, wa2 , wa3 ;
        wa2, wa3 , wa4];
    
    lny = log(yy);
    b = [sum(w2.*lny);sum(w2.*xx.*lny);sum(w2.*xx.^2.*lny)];
    
    v = A\b;
    
    assert(v(3)<=0,'could not fit gaussian to this profile')
    assert(~any(isnan(v)),'grave problems with gaussian fitting, NaNs found')

    r = -v(2)/(2*v(3));
    w = 2/sqrt(-v(3));
    a    = exp(v(1)-v(2).^2/(4*v(3)));
    
    nrm = norm(v-vprev);
    if nrm<tol 
        break
    end
    
    fit = exp(-4*(xx-r).^2/(w.^2));
    w2 = fit.^2; % new weight

    if verbose
        plot(xx,fit,'b.-',x,y,'x-r');
        drawnow
        fprintf('%4.4e\n',nrm)
    end
    vprev = v;
end




