function[ xstar, info] = NLSQ_RES( fcn, fo, fw, x0, uds, options)
%
%  NLSQ_RES solves nonlinear least squares problems of the form
%
%                norm( ((fobs-fmodel(x)) ./ fweight) = MIN, 
%                     length(x) = n, length(f...) = m
%
%           with a global unconstained Gauss-Newton method with
%           projected residual based convergence criterion and 
%           adaptive trust region strategy.
%
%           See Section 4.2.3 of the monograph
%           P. Deuflhard: Newton Methods for Nonlinear Problems.
%           Affine Invariance and Adaptive Algorithms.
%           Series Computational Mathematics 35 
%           Springer (2004)
%            
%
%  The input and output arguments of the M-function 
%
%    function[ xstar, info] = NLSQ_RES( fcn, fo, fw, x0, uds, options)
%
%  are:
%
%  IN:
%
%    fcn     - name of a M-file, where the model function is defined
%    fo      - column array of length m holding the observed data
%    fw      - column array of length m holding the weights for the
%              mesurement data in fo
%    x0      - column array of length n holding the initial estimate 
%              for the unknown parameter vector
%    uds     - user defined structure, passed to the model function
%              use uds = [] as a place holder if not needed
%    options - structure with input options, see below
%              use options = [] to use all default options
%
%  OUT:
%
%    xstar   - last (accepted) iterate
%    info    - structure with output information, see below
%
%
%  The problem to be solved must be coded as a M-file with arbirary name.
%  The function must be of the form
%
%    function[ varargout] = problem_name( x, flag, uds)
%
%  where 
%
%    x    - the column vector of length n of parameters
%    flag - lower case string indicating the type of information 
%           that the function should return
%    uds  - a user defined structure
% 
%  According to the value of flag, the function must return
%    flag         varargout
%    ''         - [f]
%                 column vector of lentgth m with the values of the
%                 model function fmodel(x)
%    'init'     - [fobs, fwght, x0, options, uds] 
%                 column vector of lentgth m with observations
%                 column vector of lentgth m with weights
%                 column vector of lentgth n with an initial guess for
%                 the model parameters to be estimated
%                 structure with input options for NLSQ solver, may be []
%                 user defined structue
%    'jacobian' - J
%                 Jacobian (m,n)-matrix J(x) = dF/dx
%  
%
%  The fields of structure options used by NLSQ_RES (missing fields
%  are created and set with default values) are:
%
%    options.ftol      - required accuracy for projected residual level
%           .kmax      - maximum iteration number
%           .lambdamin - minimal allowed damping factor
%           .lambda0   - initial damping factor
%           .numdif    - type of jacobian generation 
%                        0 - call of user function
%                        1 - internal numerical differentiation
%           .printmon  - level of monitor print information 
%                        0 - no print
%                        1 - Basic Info
%                        2 - Convergence monitor additionally
%                        3 - Further messages
%           .printsol  - level of solution print information 
%                        0 - no print
%                        1 - initial and final iterate
%                        2 - all iterates
%           .fid       - file identifier for print information
%           .iniscalx  - type of initial scaling for iterates 
%                        0 - xwght = max( abs(x0), xthrsh)
%                        1 - xwght = xthrsh
%                        2 - xwght = ones(n,1)
%           .rescalx   - type of rescaling for iterates
%                        0 - xwght = max( abs(xi), xthrsh);
%                        1 - xwght = xthrsh
%                        2 - xwght = ones(n,1)
%
%           .xthrsh    - threshold values for weighting in x
%
%
%  Information given
%
%    info.niter       - number of iterations
%        .convergence - convergence state ('false', 'true')
%        .ifail       - return code
%                       0 - NLSQ_RES converged
%                      <0 - NLSQ_RES failed to converge
%                      -1 - maximum iteration limit achieved
%                      -2 - maximum trial step limit achieved
%                     -11 - lambda < lambdamin in a posteriory estimate
%                     -12 - lambda < lambdamin in a priory estimate 
%        .nfcn        - number of function evaluation (numdif not counted)
%        .njac        - number of jacobian evaluations (user or numdif)
%        .ndec        - number of matrix decompositions (OR)
%        .nsol        - number of LSQ solves
%        .xiter(:,1:niter+1)  - Gauss-Newton iterates
%        .flevel(1:niter+1)   - residual level functions F'F
%        .clevel(1:niter+1)   - projected residual levels
%        .chatlevel(1:niter)  - modified projected residual levels
%        .theta(1:niter-1)    - contraction factors
%        .thetahat(1:niter-1) - modified contraction factors
%        .lambda(1:niter)     - damping factors
%
%  See also
%    other NLSQ solver: NLEQ_ERR (M-file), NLSCON (FORTRAN)
%    NLSQ examples:     Feulgen, Enyzme  
%
% 
% 
%  Contact   - Bodo Erdmann, erdmann@zib.de
%  Version   - 0.95
%  Date      - 2004/04/16
%  Copyright - Zuse Institute Berlin 2003-2004
%
%

%  Set internal parameters
  imax = 5;
  format short e;
   
%  check input options
  [ options, iniinfo] = zgn_inicheck( fcn, x0, uds, options, 'NLSQ_RES');

%  Set local copies of input parameters passed in structure options

  ftol = options.ftol;
  kmax = options.kmax;
  pmon = options.printmon;
  psol = options.printsol;
  luout = options.fid;
  numdif = options.numdif;
  lambda0 = options.lambda0;
  lambdamin = options.lambdamin;
  xthrsh = options.xthrsh;
  iniscalx = options.iniscalx;
  rescalx = options.rescalx;

%  Dimension arrays and matrices

  n = length( x0);
  m = length( fo);
  xk = zeros (n, 1);
  xkp1 = zeros (n, 1);
  dxk = zeros (n, 1);
  dxkp1 = zeros (n, 1);
  xwght = zeros(n,1);
  fm = zeros (m, 1);
  Fk = zeros (m, 1);
  Fkp1 = zeros (m, 1);
  Fk = zeros (m, 1);
  bk = zeros(m, 1);
  ck = zeros(n, 1);
  dk = zeros(m-n, 1);
  bkp1 = zeros (m, 1);
  ckp1 = zeros(n, 1);
  dkp1 = zeros(m-n, 1);
  chatkp1 = zeros(n, 1);
  dhatkp1 = zeros(m-n, 1);

  Jk = [];
  Jkp1 = [];
  Qk =[];
  Rk = [];
 
  flevel = zeros (1,kmax+1);
  clevel = zeros (1, kmax+1);
  chatlevel = zeros (1, kmax+1);
  theta = zeros (1, kmax-1);
  thetahat = zeros (1, kmax);
  lambda = zeros(1,kmax);

  info = [];

%  Initialize method

  ifail = 0;
  convergence = 'false';
  lambdak = lambda0;
  rhok = 0.25;

  k = 0;
  xk = x0(:);
  xiter(:,k+1) = xk(:);
  xwght = zgn_iniscalx (x0, xthrsh, iniscalx);
  info.nfcn = 0;
  info.njac = 0;
  info.ndec = 0;
  info.nsol = 0;

  if pmon > 0 
    fprintf(luout,' \n');
    fprintf(luout,' Global Gauss Newton Method, Residual Oriented Version \n');
    fprintf(luout,'  - full rank assumed    \n');
    fprintf(luout,'  - unscaled linear systems    \n');
    fprintf(luout,'  - initial damping factor lambda0 = %8.2E \n', lambda0);
    if numdif == 0
      fprintf(luout,'  - analytically given Jacobian used \n');
    else
      fprintf(luout,'  - numerical differentiation for âJacobian \n');
    end
    fprintf(luout,' \n');
    fprintf(luout,' Number of observations  m = %4.0f \n', m);
    fprintf(luout,' Number of parameters    n = %4.0f \n', n);
    fprintf(luout,' \n');
  end
  if pmon > 1
    fprintf(luout, ...
     '  Iter    ResLvl    MPRLvl ThetaHat     PRLvl    Theta ')
    fprintf(luout, ...
     '  Lambda    RhoEst\n');
  end

%  Initial function and Jacobian evaluation, initial GN correction

  [fm] = feval (fcn, xk, '', uds);
  info.nfcn = info.nfcn + 1;

  if numdif == 0
    [Jk] = feval (fcn, xk, 'jacobian', uds);
  else
    [Jk] = zgn_numdif0 (fcn, xk, xwght, uds, fm);
  end
  info.njac = info.njac + 1;

%  Set weighted residual and Jacobian
  Fk = -(fo-fm) ./ fw;
  Jk = diag( fw.^(-1)) * Jk;

% Full QR decomposition
  [ Qk, Rk] = qr(Jk);
  info.ndec = info.ndec + 1;
   
%  Solve linear least squares problem, partition transformed residual
  bk = Qk'*Fk;   
  dxk = -Rk \ bk;
  ck = bk(1:n); 
  dk = bk(n+1:m);
  info.nsol = info.nsol + 1;
  
  Fk_norm = norm(Fk)/sqrt(1);
  Fk_level = Fk_norm^2;
  ck_norm = norm(ck)/sqrt(1);
  ck_level = ck_norm^2;
   
  flevel(k+1) = Fk_level;
  clevel(k+1) = ck_level;

  if pmon > 1
    fprintf(luout,' %5.0f %9.2E %28.2E %27.1E \n', ...
     k, Fk_level, ck_level, rhok);
  end

%  Top of Gauss Newton iteration loop
%===================================

  while k < kmax;

    i = 0;
      
%  Top of trial loop
    while i < imax;

%  Compute new iterate      
      xkp1 = xk + lambdak*dxk;

      [fm] = feval (fcn, xkp1, '', uds);
      info.nfcn = info.nfcn + 1;
      Fkp1 = -(fo-fm) ./ fw;

      [bk] = Qk'*Fkp1;
      chatkp1 = bk(1:n); 
      dhatkp1 = bk(n+1:m);
      cdel = chatkp1 - (1-lambdak)*ck;
      ddel = dhatkp1 - dk;
      
      Fkp1_norm = norm(Fkp1)/sqrt(1);
      Fkp1_level = Fkp1_norm^2;
      chatkp1_norm = norm(chatkp1)/sqrt(1);
      chatkp1_level = chatkp1_norm^2;
      thetahatk = chatkp1_norm/ck_norm;
      cdel_norm =  norm(cdel)/sqrt(1);
      cdel_level = cdel_norm^2;
      ddel_norm =  norm(ddel)/sqrt(1);
      ddel_level = ddel_norm^2;
      
      flevel(k+2) = Fkp1_level;
      chatlevel(k+1) = chatkp1_level;
      thetahat(k+1) = thetahatk;

%  A priory monotonicity test

      if thetahatk >= 1-lambdak*rhok
        if pmon > 2 
          fprintf(luout,' a priory monotonicity test failed \n');
        end 
        if pmon > 1
          fprintf(luout,' %5.0f %9.2E %9.2E %8.1E %27.1E %9.1E \n',...
           k, Fkp1_level, chatkp1_level, thetahatk, lambdak, rhok);
        end

%  A priory correcter strategy activated

        hnom = 2*sqrt(cdel_level+ddel_level);
        hdenom = lambdak^2*ck_norm;
        hpost = hnom/hdenom;
        lambdak = min( lambdak/2, (1-rhok)/hpost);
        if lambdak < lambdamin 
          ifail = -11;
          break;
        end
        i = i + 1;
        continue
      end

%  Update x-weights
      xwght = zgn_rescalx (xwght, xkp1, xthrsh, rescalx);
      
      if numdif == 0
        [Jkp1] = feval (fcn, xkp1, 'jacobian', uds);
      else
        [Jkp1] = zgn_numdif (fcn, xkp1, xwght, uds, fm);
      end
      info.njac = info.njac + 1;  
      
      Jkp1 = diag( fw.^(-1)) * Jkp1;

      [Qkp1,Rkp1] = qr(Jkp1);

      info.ndec = info.ndec + 1;

%  Solve system

      [bkp1] = Qkp1'*Fkp1; 
      dxkp1 = -Rkp1\bkp1;
      ckp1 = bkp1(1:n);
      dkp1 = bkp1(n+1:m);
      info.nsol = info.nsol + 1;
 
      ckp1_norm = norm(ckp1)/sqrt(1);
      ckp1_level = ckp1_norm^2;
      thetak = ckp1_norm/ck_norm;
      
      clevel(k+2) = ckp1_level;
      theta(k+1) = thetak;

%  Estimate small residual factor rhok
      btildek = Qkp1'*Qk*[zeros(n,1);dk];
      ctildek = btildek(1:n);
      dtildek = btildek(n+1:m);
      ctildek_norm = norm(ctildek);
      ddel = dkp1-dtildek;
      deltilde_norm = sqrt(norm(ddel)^2 + ctildek_norm^2);
      deltilde_level = deltilde_norm^2;
      rhok = deltilde_norm/(lambdak*ck_norm);

%  Monitor print

      if pmon > 1
        fprintf(luout,' %5.0f %9.2E %9.2E %8.1E %9.2E %8.1E %8.1E  %8.1E \n',...
         k+1, Fkp1_level, chatkp1_level, thetahatk, ...
         ckp1_level, thetak, lambdak, rhok);
      end

%  A posteriory lambda estimate  
      hnom = 2*sqrt(cdel_level+ddel_level);
      hdenom = lambdak^2*ck_norm;
      hpost = hnom / hdenom;

%  A posteriory monotonicity test
      if thetak >= 1-lambdak*rhok
        if pmon > 2 
           fprintf(luout,' a posteriory monotonicity test failed \n');
        end 

% A posteriory correcter strategy activated
        lambdak = min( lambdak/2, (1-rhok)/hpost);
        if lambdak < lambdamin 
          ifail = -12;
          break;
        end
        i = i + 1;
        continue
      else

        lambda(k+1) = lambdak;
    
%  A priory lambda estimate for next step
        hprio = hpost*thetak;
        lambdak = min( 1, (1-rhok)/hprio);
        break
      end
   
%  End of trial loop over i
      end
    if i == imax
      ifail = -2;
    end
    if ifail < 0
      break
    end

%  Step is accepted

    k = k + 1;
    xk = xkp1;
    Fk = Fkp1;
    Qk = Qkp1;
    dxk = dxkp1;
    ck = ckp1;
    ck_norm = ckp1_norm;
    ck_level = ckp1_level;
    dk = dkp1;
    delres = abs(flevel(k+1)-flevel(k));
      
    xiter(:,k+1) = xk(:);
 
    if psol > 1
      fprintf(luout,' \n');
      fprintf(luout,' Iterate is \n');
      for i = 1 : n
        fprintf(luout,' %5.0f  -  %20.13E \n', i, xk(i));
      end
      fprintf(luout,' \n');
    end    
    
%  Convergence check

    err = ck_level;          % err = delres, alternatively
    if err <= ftol
      convergence = 'true';
      break;
    end

%  Check iteration count

    if k == kmax
      ifail = -1;
      break
    end

%  Bottom of main Gauss Newton iteration loop
  end
   
  xstar = xk;
   
  info.convergence = convergence;
  info.niter = k;
  info.ifail = ifail;
  info.xiter = xiter;
  info.flevel = flevel(1:k+1);
  info.clevel = clevel(1:k+1);
  info.chatlevel = chatlevel(1:k);
  info.theta = theta(1:k-1);
  info.thetahat = thetahat(1:k);
  info.lambda = lambda(1:k);

  if strcmp (convergence, 'true') == 1
    if pmon > 0 
      fprintf(luout,' \n');
      fprintf(luout,' Gauss-Newton Method successfully completed \n');
    end
    if psol > 0
      fprintf(luout,' \n');
      fprintf(luout,' Solution is \n');
      for i = 1 : n
        fprintf(luout,' %5.0f  -  %20.13E \n', i, xkp1(i));
      end
    end
  else
    if pmon > 0 
      fprintf(luout,' \n');
      fprintf(luout,' Gauss-Newton Method failed to converge \n');
    end 
    if pmon > 1
      if ifail == -1
        fprintf(luout,'  - more than kmax = %5.0f iterations \n', ...
         kmax);
      elseif ifail == -2
        fprintf(luout,'  - more than imax = %5.0f trial steps \n', ...
         imax);
      elseif ifail == -11| ifail ==-12
        fprintf(luout,'  - lambda =%9.2E < lambdamin =%9.2E  \n', ...
         lambdak, lambdamin);
      end  
    end      
  end
  if pmon > 0 
    fprintf(luout,'  \n');
    fprintf(luout,' Amount of work: \n');
    fprintf(luout,'  nIter =%5.0f \n  nJac =%5.0f,   nfcn =%5.0f,',... ;
     info.niter, info.njac, info.nfcn);
    fprintf(luout,'   nDec =%5.0f,   nSol =%5.0f \n', ... 
     info.ndec, info.nsol);
  end

% end of function NLSQ_RES
