##clear all ;

                                %debug_on_warning(1);
                                %debug_on_error(1);


##########################################################################
function multivar_opt ()
  
  x0 = [ -2; 3 ] ;
  ##x0 = [-1.5; 1.5 ] ;
  g_tol = 1.e-5 ;
  maxEval = 5000 ;


  ##  s = [x0(1), x0(1)+.5, x0(1)] ;
  ##  t = [x0(2), x0(2), x0(2)+.5] ;
  ##  plot_r ( ) ;

  opt_alg = 3
  if ( opt_alg == 1 ) 
    ## steepest descent
    [x,f,g,stepx, stepy] = steepest_descent ( x0, g_tol, maxEval ) ;
    
  elseif ( opt_alg == 2 )
    ## Newton
    lambda =-1. ; ## Armijo line search.
    ##lambda = 0. ; ## Newton, no safeguard.
    ##lambda = 10.0 ; ## Newton, Trust region
    [x,f,g,stepx, stepy] = newton ( x0, g_tol, maxEval, lambda ) ;

  elseif ( opt_alg == 3 )
    ## Quasi-Newton
    [x,f,g,stepx, stepy] = quasi_newton ( x0, g_tol, maxEval ) ;

  elseif ( opt_alg == -1 )
    ## F-D test
    fd_test ( x0 ) ;

  elseif ( opt_alg == -2 )
    ## F-D test, 2nd deriv
    fd2nd_test ( x0 ) ;

  else
    disp ( "unknown optimisation algorithm" )
  endif 



  if ( opt_alg > 0 )
    ##stepx = [ 0., 0.5, 1., 0. ] ;
    ##stepy = [ 0., 1., 0., 0. ] ;
    N=20 ;
    plot_r ( N, stepx, stepy) ;
  endif

endfunction


% f = (1-x)^2 + 100*(y-x^2)^2
% fx = 2*(1-x)*(-1) + 2*100*(y-x^2)*(-2*x)
% fy =                2*100*(y-x^2)

function funVal = rosenbrock ( x, y )  
a = 1 - x ;
b = y - x^2 ;
funVal = a^2 + 100*b^2 ;
endfunction

function funVal_d = rosenbrock_d ( x, x_d, y, y_d )  
a = 1-x ;
a_d = -x_d ;

b = y - x^2 ;
b_d = y_d - 2*x*x_d ;
%funVal = a^2 +100*b^2 ;
funVal_d = 2*a*a_d +2*100*b*b_d ;
endfunction

function funVal_d_d = rosenbrock_d_d ( x, x_d2, x_d, y, y_d2, y_d )  
a = 1-x ;
a_d2 = -x_d2 ;
a_d = -x_d ;

b = y - x^2 ;
b_d2 = y_d2 - 2*x*x_d2 ;
b_d = y_d - 2*x*x_d ;
b_d_d = -2*x_d*x_d2 ;

%funVal = a^2 +100*b^2 ;
%funVal_d2 = 2*a*a_d2 +2*100*b*b_d2 ;
%funVal_d = 2*a*a_d +2*100*b*b_d ;
funVal_d_d = 2*a_d2*a_d + 2*100*( b_d2*b_d + b*b_d_d ) ;
endfunction


function  [g,mEval] = rosenbrock_grad( x0, y0 ) ;
  mEval = 0 ;
  x = [ x0 ; y0 ] ;

  g = zeros(2,1) ; % force g to be a column vector.
  for ( i=1:2 ) % Loop over all variables to compute the gradient vector.
    x_d = zeros(2,1) ;
    x_d(i) = 1 ;
    g(i) = rosenbrock_d ( x(1), x_d(1), x(2), x_d(2) ) ;
    mEval++ ;
  endfor
endfunction

function  [g,H,mEval] = rosenbrock_grad_hess( x0, y0 ) ;
  mEval = 0 ;
  x = [ x0 ; y0 ] ;

  g = zeros(2,1) ;
  H = zeros(2,2) ;
  for ( i=1:2 ) % Loop over all variables to compute the gradient vector.
    x_d = zeros(2,1) ;
    x_d(i) = 1 ;
    g(i) = rosenbrock_d ( x(1), x_d(1), x(2), x_d(2) ) ;
    mEval++ ;

    for ( j=1:2 ) % Second derivatives
      x_dj = zeros(2,1) ;
      x_dj(j) = 1 ;
      H(i,j) = rosenbrock_d_d ( x(1), x_dj(1), x_d(1), x(2), x_dj(2), x_d(2) ) ;
      mEval++ ;
    endfor
  endfor

endfunction

function fd_test ( x0 )
  eps = 1.e-10 ;

  f = rosenbrock( x0(1), x0(2) ) ;

  for ( k = 0:4 )
    [g, mEval] = rosenbrock_grad ( x0(1), x0(2) ) ;

    x = x0(1)+eps ;
    y = x0(2) ;
    f2 = rosenbrock(x,y) ;
    fx = (f2-f)/eps ;

    x = x0(1) ;
    y = x0(2)+eps ;
    f2 = rosenbrock(x,y) ;
    fy = (f2-f)/eps ;

    printf ( "eps: %9.5e; ", eps ) ;
    printf ( "fx: alg: %9.5e, f-d: %9.5e, err: %9.5e, ", g(1), fx, abs(fx-g(1)) ) ;
    printf ( "fy: alg: %9.5e, f-d: %9.5e, err: %9.5e\n", g(2), fy, abs(fy-g(2)) ) ;
    eps = eps*10 ;
  endfor

endfunction


function fd2nd_test ( x0 )
  eps = 1.e-10 ;

  for ( k = 0:7 )
%    printf ( "\n Gradient validation, eps = %g\n", eps ) ;

    x = x0 ;
    [g,H,mEval] = rosenbrock_grad_hess( x(1), x(2) ) ;


    f9pt = zeros(3,3) ; % Finite Differences, compute a 9 point stencil with h=eps
    for ( i = [-1,0,1] )
      for ( j = [-1,0, 1] )
        x(1) = x0(1) +i*eps ;
        x(2) = x0(2) +j*eps ;
        f9pt(2+i,2+j) = rosenbrock(x(1),x(2)) ;
      endfor
    endfor

    % Diagonal terms
    H_fd(1,1) = (f9pt(3,2) - 2*f9pt(2,2) + f9pt(1,2))/eps^2 ;
    H_fd(2,2) = (f9pt(2,3) - 2*f9pt(2,2) + f9pt(2,1))/eps^2 ;
    
    % Off diag,
    H_fd(1,2) = (f9pt(3,3) - f9pt(3,1) - f9pt(1,3) + f9pt(1,1))/4./eps^2 ;
    H_fd(2,1) = H_fd(1,2) ;

    printf ( "eps: %9.5e; ", eps ) ;
%    printf ( "fxx: alg: %9.5e, f-d: %9.5e, err: %9.5e, ", H(1,1), H_fd(1,1), abs(H(1,1)-H_fd(1,1)) ) ;
%    printf ( "fxy: alg: %9.5e, f-d: %9.5e, err: %9.5e, ", H(1,2), H_fd(1,2), abs(H(1,2)-H_fd(1,2)) ) ;
%    printf ( "fyx: alg: %9.5e, f-d: %9.5e, err: %9.5e, ", H(2,1), H_fd(2,1), abs(H(2,1)-H_fd(2,1)) ) ;
%    printf ( "fyy: alg: %9.5e, f-d: %9.5e, err: %9.5e, ", H(2,2), H_fd(2,2), abs(H(2,2)-H_fd(2,2)) ) ;
%    printf ("\n" ) ;

    printf ( " errors: fxx %12.5e,  fxy %12.5e,  fyx %12.5e,  fyy %12.5e\n", 
             abs(H(1,1)-H_fd(1,1)), abs(H(1,2)-H_fd(1,2)), abs(H(2,1)-H_fd(2,1)), abs(H(2,2)-H_fd(2,2)) ) ;

    eps = eps*10 ;
  endfor

endfunction


function plot_r ( N, stepx, stepy )

  x = linspace(-3,2,N) ;
  y = linspace(-1,4,N) ;
  [xx,yy] = meshgrid(x,y) ;

  for i = 1:N
    for j = 1:N
      f(i,j) = rosenbrock ( xx(i,j), yy(i,j) ) ;
    endfor
  endfor

  contour  ( xx, yy, f, 30 ) ;

  if ( nargin > 1 )
  hold on ;

  plot ( stepx, stepy, "-+24" ) ;
  set (findobj (gcf, "-property", "fontsize"), "fontsize", 20) ;  
  print ( '-dpng','-color', 'rosenbrock.png')
  print ()
  hold off ;
  pause
  endif
endfunction


function [s, x2, f2, mEval] = armijo_line_search ( x, f, g, p, s )

  eta0 = 0.01 ;
  eta1 = 0.1 ;
  eta2 = 0.1 ;
  pg = p'*g ;
  C_enlrg = 3.0 ;
  c_reduce = 0.3 ;

  % Function value at predicted step.k
  x2 = x + s*p ;
  f2 = rosenbrock( x2(1), x2(2) )  ;
  mEval = 1 ;

  D = ( f2-f )/s/pg ;


  nIter = 0 ;
  sMin = 0. ;
  while ( D > 1-eta2 ) % Third Wolfe cond: enlarge step.
    s = min( C_enlrg*s, 0.5*s/(1-D) ) ;
    sMin = s ;

    x2 = x + s*p ;
    f2 = rosenbrock( x2(1), x2(2) ) ;
    mEval++ ;
    D = ( f2-f )/s/pg ;

    nIter++ ;
%    printf ( "    enlarging step size: nIter: %d, s: %g, f: %g, f2: %g, D %g, 1-eta2: %g\n", 
%             nIter, s, f, f2, D, 1-eta2 )
  endwhile

                              
  while ( D < eta1 && s > sMin ) % Second Wolfe cond: reduce step.
    s = max( sMin + c_reduce*(s-sMin), 0.5*s/(1-D) ) ; 

    x2 = x + s*p ;
    f2 = rosenbrock( x2(1), x2(2) ) ;
    mEval++ ;
    D = ( f2-f )/s/pg ; 

    nIter++ ;
%    printf ( "    reducing step size: nIter: %d, s: %5.3g, x,y: %g,%g, f: %g, f2: %g, D %g, eta1: %g\n", 
%             nIter, s, x2(1), x2(2), f, f2, D, eta1 )

  endwhile

endfunction




function [x,f,g,stepx,stepy] = steepest_descent ( x0, g_tol, maxEval )
  % Find the minimum of Rosenbrock's function using steepest descent
  mTotEval = 0 ;

  x = x0 ; % Current iterate in vector form.
  f = rosenbrock( x(1), x(2) ) ;
  mTotEval++ ;

  [g,mEval] = rosenbrock_grad( x(1), x(2) ) ;
  mTotEval += mEval ;

  nIter = 1 ;
  g_norm = norm( g ) ;
  s = 1 ;

  printf ( "steep.d. iter: %3d, eval/tot: %2d/%5d s: %6.2e x;y:%12.4e;%12.4e, f:%11.4e, ||g||:%11.4e\n", 
           nIter, mTotEval, mTotEval, s, x(1), x(2), f, g_norm ) ;
  stepx(nIter) = x(1) ;
  stepy(nIter) = x(2) ;


  while ( g_norm > g_tol && mTotEval < maxEval ) % Iterate until close enough to minimum
    nIter++ ;

    s = 1. ;
    % Line search, computes only function values
    [s, x2, f2, mEval] = armijo_line_search ( x, f, g, -g, s ) ;
    mTotEval+= mEval ;
    
    % gradient at the new x
    x = x2 ; 
    f = f2 ;
    [g, mEval] = rosenbrock_grad ( x(1), x(2) ) ;
    mTotEval += mEval ;
    g_norm = norm( g ) ;

    printf ( "steep.d. iter: %3d, eval/tot: %2d/%5d s: %6.2e x;y:%12.4e;%12.4e, f:%11.4e, ||g||:%11.4e\n", 
             nIter, mEval, mTotEval, s, x(1), x(2), f, g_norm ) ;
    stepx(nIter) = x(1) ;
    stepy(nIter) = x(2) ;
  endwhile

endfunction




function [x,f,g,stepx,stepy] = newton ( x0, g_tol, maxEval, lambda )
  ## Find the minimum of Rosenbrock's function using Newton's method.

  ## count total number of evaluations:
  mTotEval = 0 ;

  ## force the gradient vector g to be a column vector. 
  g = zeros(2,1) ;
  H = zeros(2,2) ;


  x = x0 ; ## Current iterate and function value.
  f = rosenbrock( x(1), x(2) ) ;
  mTotEval++ ;

  nIter = 0 ;
  s = 1 ;
  while (  mTotEval < maxEval ) ## Iterate until close enough to minimum
    nIter++ ;

    ## Gradient and Hessian
    [g,H,mEval] = rosenbrock_grad_hess( x(1), x(2) ) ;
    g_norm = norm( g ) ;

    mTotEval += mEval ;
    printf ( "steep.d. iter: %3d, eval/tot: %2d/%5d s: %6.2e x;y:%12.4e;%12.4e, f:%11.4e, ||g||:%11.4e\n", 
             nIter, mEval, mTotEval, s, x(1), x(2), f, g_norm ) ;
    stepx(nIter) = x(1) ;
    stepy(nIter) = x(2) ;

    if ( g_norm < g_tol )
      break ;
    else
      mEval = 0 ;

      detH = det(H) ;
      if ( abs( detH ) < 1.e-10 )
        printf ( " singular H\n" ) ;
      endif

      if ( lambda < 0 )
        ## Armijo line search safeguard
        ## Search direction.
        p = -H\g ;

        s = 1;
        ## Full step.
        ##      x2 = x + s*p ;
        ##      f2 = rosenbrock( x2(1), x2(2) )  ;
        ##      mEval++ ;
        
        ## Line search
        [s, x2, f2, mLsEval] = armijo_line_search ( x, f, g, p, s ) ;
        mEval+= mLsEval ;

        ## New iterate, line search already evaluated the function value there.
        x = x2 ;
        f = f2 ;
      else
        ## Trust region, zero or positive lambda given.
        H = H+lambda*eye(2) ;
        p = -H\g ;
        s = 1;## Full step.
        
        x = x + s*p ;
        f = rosenbrock( x(1), x(2) )  ;
        mEval = mEval+1 ;
      endif
    endif
  endwhile

endfunction






function [x,f,g,stepx,stepy] = quasi_newton ( x0, g_tol, maxEval )
  % Find the minimum of Rosenbrock's function using Newton's method.

  eps_BFGS = 1e-15;

  % count total number of evaluations:
  mTotEval = 0 ;

  % force the gradient vector g to be a column vector. 
  x = x0 ;
  g = zeros(2,1) ;
  H = zeros(2,2) ;

  % Compute first function value, in the loop this is provided by the line search.
  f = rosenbrock( x(1), x(2) ) ;
  mTotEval++ ;


  nIter = 0 ;
  s = 1 ;
  while (  mTotEval < maxEval ) % Iterate until close enough to minimum
    nIter++ ;

    [g,mEval] = rosenbrock_grad( x(1), x(2) ) ;
    mTotEval += mEval ;
    g_norm = norm(g) ;

    mTotEval += mEval ;
    printf ( "steep.d. iter: %3d, eval/tot: %2d/%5d s: %6.2e x;y:%12.4e;%12.4e, f:%11.4e, ||g||:%11.4e\n", 
             nIter, mEval, mTotEval, s, x(1), x(2), f, g_norm ) ;
    mEval = 0 ;
    stepx(nIter) = x(1) ;
    stepy(nIter) = x(2) ;


    if ( g_norm < g_tol )
      break ;
    else

      % New inverse Hessian estimate
      if ( nIter ==1 )
        H=eye(size(x,1));
      else
        % BFGS update
        Dx=x-xpr;
        Dg=g-gpr;
        HDg=H*Dg;
        if ( (Dx'*Dg) < eps_BFGS || ( HDg'*Dg) < eps_BFGS || (HDg'*HDg) < eps_BFGS )
          H=eye(size(x,1));
        else
          u = Dx / (Dx'*Dg) - HDg / ( HDg' * Dg );
          H = H ...
              + ( Dx * Dx' ) / ( Dx' * Dg ) ...
              - ( HDg * HDg' ) / ( Dg' * HDg ) ...
              + ( HDg' * Dg ) * u * u';
        endif
      endif


      detH = det(H) ;
      if ( abs( detH ) < 1.e-10 )
        printf " singular H\n" ) ;
      endif

      % Search direction, note H is an approx to the inverse Hessian.
      p = -H*g ;

      % Remember current iterate before updating.
      xpr = x ;
      gpr = g ;
      fpr = f ;

      s = 1;
      % Full step.
%      x2 = x + s*p ;
%      f2 = rosenbrock( x2(1), x2(2) )  ;
%      mEval++ ;
      
      % Line search
      [s, x2, f2, mLsEval] = armijo_line_search ( x, f, g, p, s ) ;
      mEval+= mLsEval ;

      % New iterate, line search already evaluated the function value there.
      x = x2 ;
      f = f2 ;

    endif
  endwhile

endfunction



