function [M, fM, iter, nbeval] = conjgrad(X0, cost, tol, P1, P2, P3, ...
    P4, P5)
%CONJGRAD Conjugate Gradient Minimisation.
%
%       CONJGRAD implements the conjugate gradient algorithm for minimising
%       a multidimensional function.
%       It uses the Hestenes-Stiefel update by default (but Polak-Ribiere
%       and Fletcher-Reeves are also coded) and an approximate line search
%       explained in Carl Rasmussen's thesis.
%
%       Use as [M, FM, IT, NB] = CONJGRAD(X, 'f', TOL, P1, ... P5)
%       where:
%       . X is the starting point (column vector),
%       . 'f' is the name of the function to minimise, and must be of the
%         type: [fX, dfX] = f(X), i.e. return value and derivatives,
%       . TOL: tolerance level/maximum iterations: if TOL < 1 it is taken as
%         the tolerance on the minimum, and the maximum nb. of iteration is
%         set to 500. If TOL > 1, it is the max. nb. iterations, and the
%         tolerance on the minimum is set to its default 1e-6.
%       . P1 to P5 are 5 optional arguments for function 'f'.
%       The function returns:
%       . M the position of the minimum (column vector),
%       . FM the value of the minimum,
%       . IT the actual number of iteration performed,
%       . NB the number of function evaluations, i.e. calls to 'f'.
%
%       !! No checks are made on the inputs, so use carefully !!
%
%       References:
%       Press & al. (1992) Numerical Recipes in C, pp. 420-5, Cambridge.
%       Rasmussen (1996) Evaluation of Gaussian processes and other methods
%         for non-linear regression, PhD thesis, U. of Toronto, pp. 121-7.
%       Goutte (1997) Statistical learning and regularisation for
%         regression, PhD thesis, U. Paris 6, pp. 9-11.
%
% See also: .
if (nargin < 3) | (nargin > 8)
  error('CONJGRAD: wrong number of arguments.') ;
end

if (tol >= 1)
  maxit = tol ;
  tol = 1e-6 ;
else
  maxit = 500 ;
end

argstr = [cost, '(X'] ;
for i = 1:(nargin - 3)
  argstr = [argstr, ',P', int2str(i)] ;
end
argstr = [argstr, ')'] ;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% A bunch of constants used during the line search:
% See Carl Edward's thesis for explanation.
MAXEVAL = 20 ;            % Nb. of function evaluation before we give up.
SIGMA = 0.5 ;             % Target attenuation of gradient. (def. .5)
RHO = 0.25 ;              % Max decrease from current slope. (def. .25)
INTLIM = 0.1 ;            % Interpolation limit 0.1=10% interval length.
EXTLIM = 3.0 ;            % Extrapolation limit 3.0=3 interval length.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
iter = 0 ;                % Nb. of conjugate gradient iterations.
nbeval = 0 ;              % Nb. of function evaluations.
CGSUCCESS = 0 ;           % Success flag (CG iteration).
LSSUCCESS = 0 ;           % Success flag (Line search).
PREVIOUS = 0 ;            % Flag for previous linesearch.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%
%
fM = zeros(maxit,1) ;              % Vector to put the values of f(X)
X = X0 ;                           % The current parameter vector
          % The current point is indexed '1': value df1 and gradient df1.
[fX GX] = eval(argstr) ;
nbeval = nbeval + 1 ;
Sdir = -GX ;                       % This is the search direction
slopeX = GX'*Sdir ;                % Slope at the initial point
step = 1/(1-slopeX) ;             % Initial guess for step size
while ((iter < maxit) & ~CGSUCCESS)
  iter = iter + 1;                 % Increase the iteration counter and ...
  %% ... line search algorithm from Carl's thesis.
  X0 = X ;
  X = X + step * Sdir ;            % Take one step, get value and gradient.
  [f2 df2] = eval(argstr) ;
  nbeval = nbeval + 1 ;
  slope2 = df2' * Sdir ;            % Slope at current guess.
  f3 = fX ; slope3 = slopeX ; step3 = -step ;       % Set point 3.
  limeval = nbeval + MAXEVAL ;     % Max 20 evaluations in 1 line search.
  LSSUCCESS = 0 ;
  maxstep = -1 ;
  while ((nbeval < limeval) & ~LSSUCCESS)
    while (((f2 > fX + step * RHO * slopeX) | ...   % Condition b(X).
            (slope2 > -SIGMA * slopeX)) & ...       % Condition a2(X).
           (nbeval < limeval))
      maxstep = step ;
      if (f2 > fX)                % Do quadratic interpolation.
        newstep = step3 - (slope3 * step3*step3 / 2) / ...
            (slope3 * step3 - f3 + f2) ;
      else                        % Do cubic interpolation.
        a = 6*(f2 - f3) / step3 + 3*(slope2 + slope3) ;
        b = 3*(f3 - f2) - step3 * (2*slope2 + slope3) ;
        newstep = (sqrt(b*b - a * slope2 * step3*step3) - b) / a ;
      end
      % We will now place the new current point (indexed 2).
      if ~isreal(newstep)                       % Numerical error.
        disp('CONJGRAD warning: numerical problem in line search.')
        newstep = step3 / 2 ;
      elseif (newstep > INTLIM * step3)       % too close to current.
        newstep = INTLIM * step3 ;
      elseif (newstep < (1-INTLIM) * step3)   % too close to x3.
        newstep = (1-INTLIM) * step3 ;
      else
        newstep = newstep ;
      end
      step = step + newstep ;     % Update global step value.
      X = X + newstep * Sdir ;    % Take another step, compute, etc.
      [f2 df2] = eval(argstr) ;
      nbeval = nbeval + 1 ;
      slope2 = df2' * Sdir ;       
      step3 = step3 - newstep ;           % Narrow the interpolation.
    end
    % Once we get here we know a2(X) and b(X) are fullfilled.
    if (slope2 > SIGMA * slopeX)              % Condition a1(X)
      LSSUCCESS = 1 ;             % Let's get outta here.
    else                          % Make cubic extrapolation.
      a = 6*(f2 - f3) / step3 + 3*(slope2 + slope3) ;
      b = 3*(f3 - f2) - step3 * (2*slope2 + slope3) ;
      % Carl I am not convinced about that, but if it works....
      newstep = -slope2 * step3*step3 / ...
          (sqrt(b*b - a * slope2 * step3*step3) + b) ;
      if ~isreal(newstep)                       % Numerical error.
%        disp('CONJGRAD warning: numerical problem in line search.')
        if (maxstep < 0)                          % No limit set ?
          newstep = step * (EXTLIM-1) ;
        else
          newstep = (maxstep - step) / 2 ;
        end
      elseif (newstep < 0)                    % Wrong side !
        if (maxstep < 0)
          newstep = step * (EXTLIM-1) ;
        else
          newstep = (maxstep - step) / 2 ;
        end
      elseif ((maxstep >= 0) & (step + newstep) > maxstep)
        newstep = (maxstep - step) / 2 ;      % If extrap. beyond max step.
      elseif ((maxstep < 0) & (step + newstep) > step*EXTLIM)
        newstep = step * (EXTLIM - 1) ;
      elseif (newstep < -step3 * INTLIM)      % Too close from current.
        newstep = -step3 * INTLIM ;
      elseif ((maxstep >= 0) & (newstep < (maxstep - step) * (1-INTLIM)))
        newstep = (maxstep - step) * (1-INTLIM) ; % Too close to max.
      end
      f3 = f2 ; slope3 = slope2 ;             % Point 3 <- Point 2.
      step3 = -newstep ;                      % Relative to current.
      step = step + newstep ;     % Update global step value.
      X = X + newstep * Sdir ;    % Take another step, compute, etc.
      [f2 df2] = eval(argstr) ;
      nbeval = nbeval + 1 ;
      slope2 = df2' * Sdir ;      
    end
  end                             % End of line search.
  if LSSUCCESS       % . . . . . . . If line search succeeded. . . . . . .
    fprintf('Iteration %i  Value= %4.3e - Step=%4.3e ', ...
        iter, f2, step/sqrt(Sdir'*Sdir));
    fM(iter) = f2 ;
    if (((fX - f2) < tol*(abs(fX)+abs(f2)+3e-16)) | (df2'*df2 == 0))
      CGSUCCESS = 1 ;
    else
      % gamma = -(df2'*(df2 - GX)) / (Sdir'*GX + eps) ;   % Hestenes-Stiefel
      gamma = (df2'*(df2 - GX)) / (GX'*GX + eps) ;      % Polak-Ribiere
      % gamma = (df2'*df2) / (GX'*GX + eps) ;             % Fletcher-Reeves
      Sdir = gamma * Sdir - df2 ;
      % Faut il echanger ou simplement affecter ?
      % Vtmp = df2 ; df2 = GX ;
      fX = f2 ;
      GX = df2 ;
      slope2 = GX' * Sdir ;
      if (slope2 > 0)             % Not going down? SD update.
        Sdir = - GX ;
        slope2 = GX' * Sdir ;
        fprintf(' SD\r') ;
      else
        fprintf(' CG\r') ;
      end
      if (slopeX / slope2 > 100)
        step = 100 * step ;
      else
        step = slopeX / slope2 * step ;
      end
      slopeX = slope2 ;
    end
    PREVIOUS = 0 ;                % This iteration worked. 
  else               % . . . . . . . If line search failed . . . . . . . .
    fprintf('Tentative step: %5.3e - f(X) = %5.3e; f(X0) = %5.3e slope=%5.3e\n', ...
        step, f2, fX, slopeX) ;
    if PREVIOUS                   % Two line search failed in a row.
      disp('Failure to converge (2).')
      CGSUCCESS = 1 ;             % Exit (even though it's not a success).
    else
      PREVIOUS = 1 ;              % Set the flag for next iteration.
      fX = f2 ;
      GX = df2 ;
      Sdir = -GX ;                % Try steepest descent.
      slopeX = GX' * Sdir ;
      step = 1 / (1 - slopeX) ;   % Reset step size guess.
    end
  end
end  

fprintf('\n') ;
M = X ;
fM = fM(find(fM ~= 0)) ;

% (c) 08/1997, 10/1997, 15/01/1999 C. Goutte.
% C code (c) C.E. Rasmussen.
