function  [X, info, perf] = KMhybrid(fun,par, x0, opts)
%KMhybrid  Marquardt - Qausi-Newton hybrid method for least squares.
%  Find  xm = argmin{F(x)} , where  x = [x_1, ..., x_n]  and
%  F(x) = .5 * sum(f_i(x)^2) .
%  The functions  f_i(x) (i=1,...,m) and the Jacobian matrix  J(x)  
%  (with elements  J(i,j) = Df_i/Dx_j ) must be given by a MATLAB
%  function with declaration
%            function  [f, J] = fun(x, par)
%  par  can e.g. be an array with coordinates of data points,
%  or it may be dummy.
%  
%  Call
%      [X, info {, perf}] = KMhybrid(fun,par, x0, opts)
%
%  Input parameters
%  fun  :  String with the name of the function.
%  par  :  Parameters of the function.  May be empty.
%  x0   :  Starting guess for  x .
%  opts :  Vector with four elements:
%          opts(1)  used in starting value for Marquardt parameter: 
%              mu = opts(1) * max(A0(i,i))  with  A0 = J(x0)' J(x0)
%          opts(2:4)  used in stopping criteria:
%              ||F'||inf <= opts(2)                     or 
%              ||dx||2 <= opts(3)*(opts(3) + ||x||2)    or
%              no. of iteration steps exceeds  opts(4) .
%
%  Output parameters
%  X    :  If  perf  is present, then array, holding the iterates
%          columnwise.  Otherwise, computed solution vector.
%  info :  Performance information, vector with 6 elements:
%          info(1:4) = final values of 
%              [F(x)  ||F'||inf  ||dx||2  mu/max(A(i,i))] ,
%            where  A = J(x)' J(x) .
%          info(5) = no. of iteration steps
%          info(6) = 1 : Stopped by small gradient
%                    2 :  Stopped by small x-step
%                    3 :  Stopped by  kmax
%                    4 :  Singular matrix.  Restart from current 
%                         x  with increased value for  mu .  
%  perf :  (optional). If present, then array, holding 
%            perf(1,:) = values of  F(x)
%            perf(2,:) = values of  || F'(x) ||inf
%            perf(3,:) = mu-values
%            perf(4,:) = method

%  Hans Bruun Nielsen,  IMM, DTU.  99.06.10
  
   %  Check and initialize
   %  pt = struct('x', 'f', 'J', 'g', 'iaux', 'raux'),
   %       iaux = [m  n  count  method]   
   %       raux = [F  ||g||inf  mu  nu]
   pt = check(fun,par,x0,opts); 
   B = eye(pt.iaux(2));   ptb = pt;   kmax = opts(4);
   Trace = nargout > 2;
   if  Trace
         X = pt.x*ones(1,kmax+1);
         perf = [pt.raux(1:3) 1]'*ones(1,kmax+1);
       end 
   k = 1;   stop = 0;

   while  ~stop
     if  pt.raux(2) <= opts(2),  stop = 1;
     else 
       switch  pt.iaux(4)
         case 1
           [ptn  stop  nh] = MarqStep(fun,par,pt,opts);
           if  ptn.iaux(4) > 1,  ptb = ptn; end
         case 2,  [ptn stop  nh] = QuasiStep(fun,par,pt,ptb,opts,B);  
       end
       if  stop
         pt = ptn;   stop = max(0,stop);  % 'Restart'
       else     % Continue
         F = pt.raux(1);   Fn = ptn.raux(1);
         if  Fn < 1.5*F    %  Update  B
           h = ptn.x - pt.x;
           if  norm(h)
             y = ptn.J'*(ptn.J*h + ptn.f) - pt.J'*ptn.f;   hy = dot(h,y);
             if  hy > 0
               v = B*h;
               B = B + y*(y/hy)' - v*(v/dot(h,v))';
             end
           end
         end  % update B
         if  Fn < F | (Fn == F & ptn.raux(2) < pt.raux(2))
           pt = ptn;   % update x
         else,  pt.raux(3:4) = ptn.raux(3:4);  end
       end 
       k = k + 1;
       if  Trace 
             X(:,k) = pt.x;
             perf(:,k) = [pt.raux(1:3) pt.iaux(4)]';
           end
     end
     if  k > kmax,  stop = 3; end 
   end
   %  Set return values
   if  Trace
     X = X(:,1:k);   perf = perf(:,1:k);
   else,  X = pt.x;  end 
   newopts1 = pt.raux(3)/max(sum(pt.J .* pt.J));
   info = [pt.raux(1:2)  nh  newopts1  k-1  stop];

% ==========  auxiliary functions  =================================

function  pt = check(fun,par,x0,opts)
%  Check function call
   sx = size(x0);   n = max(sx);
   if  (min(sx) > 1)
       error('x0  should be a vector'), end
   x = x0(:);   [f J] = feval(fun,x,par);
   sf = size(f);   sJ = size(J);
   if  sf(2) ~= 1
       error('f  must be a column vector'), end
   if  sJ(1) ~= sf(1)
       error('row numbers in  f  and  J  do not match'), end
   if  sJ(2) ~= n
       error('number of columns in  J  does not match  x'), end
%  Thresholds
   if  length(opts) < 4
       error('opts  must have 4 elements'), end
   if  length(find(opts(1:4) <= 0))
       error('The elements in  opts  must be strictly positive'), end
%  Return point as struct
   g = J'*f;   ng = norm(g,inf);
   mu = opts(1) * max(sum(J .* J));
   pt = struct('x',x, 'f',f, 'J',J, 'g',g, ...
               'iaux',[sf(1)  n  0  1], 'raux',[(f'*f)/2  ng  mu  2]);   

function  [ptn, stop, nh] = MarqStep(fun,par,pt,opts)
%  Marquardt step

   ptn = pt;   stop = 0;   
   mu = pt.raux(3);  method = 1;
   h = (pt.J'*pt.J + mu*eye(pt.iaux(2)))\(-pt.g);
   nh = norm(h);   nx = opts(3) + norm(pt.x); 
   if      nh <= opts(3)*nx,  stop = 2;
   elseif  nh >= nx/eps,   stop = 4;    % Almost singular ?
   else
     ptn.x = pt.x + h;   F = pt.raux(1);   nu = pt.raux(4);   
     h = ptn.x - pt.x;   dL = (h'*(mu*h - pt.g))/2;
     [ptn.f ptn.J] = feval(fun, ptn.x,par);
     Fn = .5*norm(ptn.f)^2;   dF = F - Fn;
     ptn.g =  ptn.J'*ptn.f;   ng = norm(ptn.g,inf);
     count = pt.iaux(3);
     if  ng <= opts(2),  stop = 1;
     else 
       if  (dL > 0) & (dF > 0)
         if  ng < .02*Fn,  count = count + 1;
         else,             count = 0;  end
         if  count >= 3,  method = 2; end
         mu = mu * max(1/3, (1 - (2*dF/dL - 1)^3));   nu = 2;
       else
         mu = mu*nu;  nu = 2*nu;   count = 0;
       end
     end
     ptn.raux = [Fn ng mu nu];
     ptn.iaux(3:4) = [count method];   
   end

function  [ptn, stop, nh] = QuasiStep(fun,par,pt,ptb,opts,B)
%  Quasi-Newton step

   ptn = pt;   stop = 0;   method = 2;
   h = B\(-pt.g);   nh = norm(h);   nx = opts(3) + norm(pt.x); 
   if      nh <= opts(3)*nx,  stop = 2;
   elseif  nh >= nx/eps,   stop = 4;    % Almost singular ? 
   else
     ptn.x = pt.x + h;
     [ptn.f,ptn.J] = feval(fun, ptn.x,par); 
     ptn.g = ptn.J'*ptn.f;
     ng = norm(ptn.g,inf);  Fn = (ptn.f'*ptn.f)/2;
     ptn.raux(1:2) = [Fn ng];
     if  ng <= opts(2),  stop = 1;
     elseif  ng > .99*pt.raux(2)    % reject
       method = 1;    stop = -1;
       if  Fn > ptb.raux(1),  ptn = ptb; end
     end
     ptn.iaux(3:4) = [0  method];
   end