function RosenbrockExample
%% Optimal Control (EE-715)
% Course at EPFL
%  
%     Optimizing Rosenbrock's function
%
%
% Autor:    timm.faulwasser@kit.edu
% Date:     20-07-16
% Status:   final


global handle1 handle2

close all
clc

tol = 1E-10;

x0 = [5;5];
c  = 105;

%% visualisation

x1_span = [0:5e-3:6];
x2_span = [0:1e-2:18];
F = [];

for i = 1:length(x1_span)
    for j = 1:length(x2_span)        
        x_ij = [x1_span(i), x2_span(j)];
        F(i,j) = f(x_ij, c);        
    end
end

maxF = max(max(F));

delta = maxF/40; 

v = [0, 1, 10, 100, 500, 1000, 2000, 4000, 8000, 12000, 2.4E4];

handle1 = figure
%contour(x_span, x_span, 1*F')
contour(x1_span, x2_span, 1*F', v);
%hold on)
hold on

handle2 = figure

%% unconstrained optimization

% options = optimoptions('fminunc', 'Algorithm', 'quasi-newton', 'GradObj', ...
%           'on', 'Display', 'iter', 'DerivativeCheck', 'on', 'HessUpdate','bfgs',...
%            'Diagnostics', 'on', ...
%            'tolX', 1E-10, 'OutputFcn', @outfun, 'tolFun', 1e-10);
%        
options = optimoptions('fminunc', 'Algorithm', 'quasi-newton', 'GradObj', ...
          'on', 'Display', 'iter', 'DerivativeCheck', 'on', ...           
          'HessUpdate','bfgs',...
           'Diagnostics', 'on', ...
           'tolX', 1E-10, 'OutputFcn', @outfun, 'tolFun', 1e-10);
% options = optimset( 'Algorithm', 'sqp', 'GradObj', ...
%           'on', 'Display', 'iter', 'DerivativeCheck', 'off',...
%           'HessUpdate','bfgs',...
%            'Diagnostics', 'on', ...
%            'tolX', 1E-3, 'OutputFcn', @outfun, 'tolFun', 1e-10);       
          
[xopt, fopt, iout] = fminunc(@(x) f(x,c), x0, options);       


keyboard
     
end

%% ========================================================================
% subfunctions
% =========================================================================
function [f, grad_f] = f(x,c)
   
f = (1-x(1))^2 + c*(x(2)-(x(1))^2)^2;

    if nargout > 1
         grad_f = [-2*(1-x(1)) - 2*c*(x(2) - x(1)^2)*2*x(1); ...
                   2*c*(x(2) - x(1)^2)];
    end
   
end

function stop = outfun(x, optimValue, state)

   global handle1 handle2

   figure(handle1)
   plot(x(1), x(2), 'xr', 'Linewidth', 3)
   hold on
   if optimValue.iteration == 0
       grid on
       title('x_1-x_2 plane')
       xlabel('x_1')
       ylabel('x_2')
   end     
      
   figure(handle2)
   semilogy(optimValue.iteration, optimValue.fval, 'xr', 'Linewidth', 3)
   hold on
   
   if optimValue.iteration == 0
       grid on
       title('Objective function')
       xlabel('i')
       ylabel('f(x)')
   end       
   
   stop = false;
end




