function [a,covar,chisq] = mrqmin(x, y, sig, a0, ia, funcs, itmax, lamfac)
% function [a,covar,chisq] = mrqmin(x, y, sig, a0, ia, funcs[, itmax, lamfac])
% Levenberg Marquadt method for nonlinear fit.
% Adapted from Numerical Recipes in C, section 15.5
% Jeff Kepert May 2002.
 
if (nargin <  7) itmax  = 100; end
if (nargin <  8) lamfac = 10;  end

if any(isnan(x)) | any(isnan(y))
   disp('mrqmin >> nans in data ... exitting');
   a = repmat(nan, size(a0)); covar = nan; chisq = nan;
   return
end

% Initialisation
a = a0;
atry = a;
xa = find(ia ~= 0);  % Indices of a to fit
ma = length(a);
nobs = length(x);
lambda = 0.001;
lamfaci = 1.0/lamfac;
stopcount = 2;
[alpha,beta,chisq] = mrqcof(x, y, sig, a, xa, funcs);

disp(sprintf('mrqmin: Initial chisq = %8.2f', chisq));

for it = 1 : itmax

   % Alter linearised fitting matrix by augmenting diagonal elements
   % (NRC eq 15.5.13)
   covar = alpha + lambda*diag(diag(alpha));
   rcon = rcond(covar);
   if rcon < nobs*eps
      if 0  % Inflate diagonal
         lambda = lambda*lamfac;
         covar = alpha + lambda*diag(diag(alpha));
         disp(sprintf('mrqmin: covar badly conditioned, diagonal inflated (%10.2e %10.2e)', ...
            rcon,rcond(covar)));
         % Matrix solution (NRC eq 15.5.14)
         da = covar\beta; 
      else  % Solve by SVD
         [u,s,v] = svd(covar);
         diags = diag(s); diagsi = zeros(size(diags));
         thresh = nobs*eps*max(diags);
         ixin  = find(abs(diags) >  thresh);
         ixout = find(abs(diags) <= thresh);
         diagsi(ixin) = 1./diags(ixin);
         da = v * diag(diagsi) * u' * beta;
         if isempty(ixout)
            disp('mrqmin: SVD keeps all indices');
         else
            [crap,badx] = max(abs(v(:,ixout(1))));
            disp(sprintf('mrqmin: covar badly conditioned, SVD keeps %d indices, drops a[%d]', ...
               length(ixin), badx )); 
         end
       % disp([ u diags  v ]);
       % disp(da);
      end
   else
      % Matrix solution (NRC eq 15.5.14)
      da = covar\beta; 
   end
   if any(isnan(da)) | any(isinf(da))
      disp(sprintf('mrqmin: solution failure for da, lambda=%.1e',lambda));
      disp([covar beta])
      a = repmat(nan, size(a0)); covar = nan; chisq = nan;
      return
   end
   
   atry = a; atry(xa) = atry(xa) + da;
   
   [alpha_try,beta_try,chisq_try] = mrqcof(x, y, sig, atry, xa, funcs);
   if isnan(chisq_try)
      disp('mrqmin: evaluation failure chisq=nan, atry=');
      disp(atry)
      a = repmat(nan, size(a0)); covar = nan; chisq = nan;
      return
   end
   if chisq_try < 0
      disp('mrqmin: evaluation failure chisq < 0, atry=');
      disp(atry)
      a = repmat(nan, size(a0)); covar = nan; chisq = nan;
      return
   end
   if (chisq_try < chisq) 
      if ( chisq - chisq_try < 1e-3 | abs( chisq/chisq_try - 1 ) < 1e-4 )
         stopcount = stopcount - 1;
      end
      lambda = lambda * lamfaci;
      disp(sprintf('New     chisq = %.6g, lambda=%.3e',chisq_try,lambda));
      alpha = alpha_try;
      beta = beta_try;
      chisq = chisq_try;
      a = atry;
      if (stopcount <= 0) break; end;
   else
      lambda = lambda * lamfac;
      disp(sprintf('Failed, chisq = %.6g, lambda=%.3e',chisq_try,lambda));
      if ( chisq_try - chisq > 1e-2 )
         stopcount = 2;
      end
   end
%  disp(a)
end

if (it >= itmax)
   disp(sprintf('mrqmin convergence failure %d iterations',it));
end

covar = zeros(ma,ma);
covar(xa,xa) = inv(alpha);
 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [alpha,beta,chisq] = mrqcof(x, y, sig, a, xa, funcs)

ndata = size(x,1);
mxa = length(xa);

alpha = zeros(mxa,mxa);
beta = zeros(mxa,1);
chisq = 0.0;

[ymod,dyda] = feval(funcs, x, a);
sig2i = 1.0 ./ sig.^2;

for ii = 1 : ndata
   dy = y(ii) - ymod(ii);
   for jj = 1 : mxa
      wt = dyda(ii,xa(jj))*sig2i(ii);
      for kk = 1 : jj
         alpha(jj,kk) = alpha(jj,kk) + wt*dyda(ii,xa(kk));
      end
      beta(jj) = beta(jj) + dy*wt;
   end
   chisq = chisq + sig2i(ii)*dy^2;
end

for jj = 2 : mxa 
   alpha(1:jj-1,jj) = alpha(jj,1:jj-1)';
end

if any(isnan(alpha))
   disp('mrqmin.mrqcof >> alpha has nans');
end
if any(isnan(beta))
   disp('mrqmin.mrqcof >> beta has nans');
end
if isnan(chisq)
   disp('mrqmin.mrqcof >> chisq is nan');
end

