function [x, Info] = lyngby_opt_cg(func, x0, varargin) 

% lyngby_opt_cg        - Rasmussen/Goutte's conjugate gradient minimization
%
%       function [x, Info] = lyngby_opt_cg(func, x0)
% 
%       Input:    func       Function handle (@function)
%                 x0         Vector with parameters to be optimized
%
%       Property: OptimType  [ {PolakRibiere} | HestenesStiefel |
%                            FletcherReeves ] Optimization type.
%                 Parameters Additional parameters
%
%       Output:   x          Optimized parameters
%                 Info       Structure with additional information
%
%       Conjugate gradient algorithm for minimising a multidimensional
%       function. It uses the Polak-Ribiere update by default (but
%       Hestenes-Stiefel and Fletcher-Reeves are also coded) and an
%       approximate line search explained in Carl Rasmussen's thesis.
%
%       The input argument 'func' should contain a function handle
%       constructed with the at-operator '@'. The function should
%       return the function value and the gradient.
%
%       This function was original coded by Cyril Goutte in Matlab
%       based on Carl Edward Rasmussen's C-code and PhD-thesis.
%
%       Examples: 
%         function [f, df] = sin2(x)
%           f = sin(x);
%           df = cos(x);
%
%         function [f, df] = besseljn(x, n)
%           h = abs(x) * 10^6 * eps;
%           f = besselj(x, n);
%           df = mean(gradient(besselj([x-h x x+h], n), h));
%
%         lyngby_opt_cg(@sin2, 4)
%         lyngby_opt_cg(@besseljn, 2.8, 'parameters', {8}, ...
%            'optimtype', 'hestenesstiefel')
%
%       Ref: Press & al. (1992) Numerical Recipes in C, pp. 420-425,
%              Cambridge. 
%            Carl Edward Rasmussen (1996) Evaluation of Gaussian
%              processes and other methods for non-linear regression,
%              PhD thesis, University of Toronto, pp. 121-127. 
%            Gyril Goutte (1997) Statistical learning and
%              regularisation for regression, PhD thesis, Université
%              Paris 6, pp. 9-11. 
%
%       See also LYNGBY. 
% 
% $Id: lyngby_opt_cg.m,v 1.1 2002/10/18 16:17:24 fnielsen Exp $


    % Check arguments
    if nargin < 2
      error('Wrong number of arguments.');
    end
    

    % Default properties
    info = 0;
    maxIterations = 500;
    maxLineSearch = 20;
    minFuncChange = 1e-6;
    optimType = 'polakribiere';
    parameters = [];


    % Parse properties
    n = 1;
    while n < nargin-1
      arg = lower(varargin{n});

      if strcmp(arg, 'parameters')
	n = n + 1;
	parameters = varargin{n};
      
      elseif strcmp(arg, 'optimtype')
	n = n + 1;
	arg = lower(varargin{n});
	if isstr(arg)
	  if strcmp(arg, 'polakribiere') | strcmp(arg, ...
		'hestenesstiefel') | strcmp(arg, 'fletcherreeves')
	    optimType = arg;
	  else
	    error(['Argument to ''OptimType'' should be' ...
		  ' ''PolakRibiere'', ''HestenesStiefel'' or ' ...
		  '''FletcherReeves''']);
	  end
	else
	  error(sprintf(['Argument with ''OrthoType'' should be '...
		'string']));
	end

      else
	error(sprintf('Invalid property: %s', arg));
      end
      n = n + 1;
    end

    argstr = 'feval(func, X';
    for n = 1:length(parameters)
      argstr = [ argstr sprintf(', parameters{%d}', n) ];
    end
    argstr = [ argstr ');' ];

    
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % A bunch of constants used during the line search:
    % See Carl Edward's thesis for explanation.
    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.
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    nbeval = 0 ;              % Nb. of function evaluations.
    LSSUCCESS = 0 ;           % Success flag (Line search).
    PREVIOUS = 0 ;            % Flag for previous linesearch.
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
    
    
    fM = zeros(maxIterations,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
    
    % Congugate gradient iteration loop
    for iteration = 1:maxIterations
      % 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 + maxLineSearch ;     % 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       
	% Line search succeeded
	if info > 0
	  fprintf('Iteration %i  Value= %4.3e - Step=%4.3e ', ...
	      iteration, f2, step/sqrt(Sdir'*Sdir));
	end
	fM(iteration) = f2 ; %minimum found along the line
	if (((fX - f2) < minFuncChange*(abs(fX)+abs(f2)+3e-16)) | (df2'*df2 == 0))
	  break; %end if minimum is within tolerance bounds
	else
	  if strcmp(optimType, 'polakribiere')
	    gamma = (df2'*(df2 - GX)) / (GX'*GX + eps);     
	  elseif strcmp(optimType, 'hestenesstiefel')
	    gamma = -(df2'*(df2 - GX)) / (Sdir'*GX + eps);   
	  elseif strcmp(optimType, 'fletcherreeves')
	    gamma = (df2'*df2) / (GX'*GX + eps);
	  else
	    error('Internal error')
	  end
	  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 ;
	    if info > 0
	      fprintf(' SD\r');
	    end
	  else
	    if info > 0
	      fprintf(' CG\r');
	    end
	  end
	  if (slopeX / slope2 > 100)
	    step = 100 * step ;
	  else
	    step = slopeX / slope2 * step ;
	  end
	  slopeX = slope2 ;
	end
	PREVIOUS = 0 ;                % This iteration worked. 
      else
	% Line search failed 
	if info > 0 
	  fprintf('Tentative step: %5.3e - f(X) = %5.3e; f(x0) = %5.3e slope=%5.3e\n', ...
	      step, f2, fX, slopeX);
	end
	if PREVIOUS                   % Two line search failed in a row.
	  disp('Failure to converge (2).')
	  break;             % 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  
    
    if info > 0 
      fprintf('\n') ;
    end
    x = X ;
    fM = fM(find(fM ~= 0)) ;

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











