% NewtwonForRosenbrock.m
%
%------------------------------------------------------------------------
% Newton's Mehod for Minimization of the Rozebrock function
% f:[x1; x2] |---> 100(x2-x1^2)^2 + (1-x1)^2. 
%------------------------------------------------------------------------

cost_func='rosen';

% Choose initial guess and evaluate function and gradient. Store results. 
x=[-1.2,1]';
[f, g, H]=feval(cost_func,x);
x_hist = x;
num_hist = [f; norm(g); 0];

% Set stopping tolerances then implement Newton Iteration
iter_flag = 1;
iter = 1;
iter_max = 100;
grad_tol = 1e-10;
while iter_flag
  iter = iter+1;
  p = -H\g;           
  x = x + p;
  [f, g, H]=feval(cost_func,x);
  % Record iterates and numerical performance values
  x_hist = [x_hist x];
  num_hist = [num_hist [f; norm(g); norm(p)]];
  fprintf('iter=%d f=%5.5e |g|=%5.5e\tx=[%f %f]\n',iter,f,norm(g),x(1),x(2));
  % Check stopping criteria
  if norm(g) < grad_tol
      disp('Grandient tolerance met. Stop iterations')
      iter_flag = 0;
  elseif iter == iter_max
      disp('Maximum number of iterations reached.')
      iter_flag = 0;
  end
end


%------------------------------------------------------------------------
%  VISUALIZATION: To see how the iteration progresses. Plot cost function 
%  and convergence history. First generate array of function values.
%------------------------------------------------------------------------
x_soln = x_hist(1,:);
y_soln = x_hist(2,:);
x_min = min(x_soln) - .1;
x_max = max(x_soln) + .1;
nx = 80;
y_min = min(y_soln) - .1;
y_max = max(y_soln) + .1;
ny = 50;
%  Generate array of values of cost_fn evaluated on a square grid.
dx = (x_max - x_min) / (nx - 1);
x = [x_min:dx:x_max]';
dy = (y_max - y_min) / (ny - 1);
y = [y_min:dy:y_max]';
z = zeros(ny,nx);
for i = 1:nx
  for j = 1:ny
    xvec = [x(i);y(j)];
    z(j,i) = feval(cost_func, xvec );
  end
end

%  Gray-scale plot of cost function.
  
figure(1)
clf
  imagesc(x,y,flipud(z))  %  Put bottom rows of z array on top of plot.
  colorbar, 
  xlabel('x axis')
  ylabel('y axis')
  title('Intensity plot of cost function')
 
%  Contour plot of cost function.

figure(2)
clf
  nz = 20;
  contour(x,y,z,nz)
  %  Plot iteration path.
  hold on, plot(x_soln,y_soln,'-*'), hold off
  xlabel('x axis')
  ylabel('y axis')
  title('Contour plot of cost function, with iteration path.')

%  Plot numerical performance indicators.
costvec = num_hist(1,:);
gnormvec = num_hist(2,:);
snormvec = num_hist(3,:);
figure(3)
clf
  subplot(221)
    semilogy(costvec)
    title('Cost Function')
  subplot(222)
    semilogy(gnormvec)
    title('Gradient Norm')
  subplot(223)
    semilogy(snormvec)
    title('Step Norm')
    xlabel('Iteration')