function BFGS()
close all

% Objective function
f = @(x) 100*(x(2)-x(1)^2)^2 + (1-x(1))^2;

% Gradient
nablaf = @(x)  [-400*x(1)*(x(2)-x(1)^2) - 2*(1-x(1));
                         200*(x(2)-x(1)^2)          ];
                
% Initialize variables
x_k = -2 + 4*rand(2,1); % Random starting position
grad_k = nablaf(x_k);   % First gradient
I = eye(2);             % Identity matrix
H_k = I;                % Inverse Hessian starting approximation
maxit = 50;             % Maximum number of iterations allowed
iter = 0;               % Iteration counter
tol = 1E-8;            % Tolerance for stopping criterion
change = inf;           % Measure of change between iterations
c_1 = 0.1;              % 1st Wolfe constant
c_2 = 0.7;              % 2nd Wolfe constant

x_collection = x_k; % Keeps track of all points considered

while change > tol && iter <= maxit
    % Compute search direction and directional derivative
    p_k = -H_k*grad_k;
    
    % Find step length satisfying Wolfe conditions. Function further down.
    alpha_k = lineSearch(x_k,p_k,f,nablaf,c_1,c_2);
    
    % Update x_k and find new gradient
    x_new = x_k + alpha_k*p_k;
    grad_new = nablaf(x_new);
    
    % Update inverse Hessian approximation
    s_k = x_new - x_k;
    y_k = grad_new - grad_k;
    rho_k = 1/(y_k'*s_k);
    H_k = (I-rho_k*(s_k*y_k'))*H_k*(I-rho_k*(y_k*s_k')) + rho_k*(s_k*s_k');
  
    % Make ready for next iteration and store minimizer candidate
    x_k = x_new;
    grad_k = grad_new;
    x_collection(:,end+1) = x_k;
    iter = iter + 1;
    change = norm(grad_k);
end

if iter <= maxit
    disp(['BFGS method converged in ' int2str(iter) ' iterations.'])
else
    disp('BFGS method failed to converge in the maximum amount of iterations')
end

% Plot path taken by BFGS method

for i = 2:length(x_collection)
    plot(x_collection(1,i-1:i),x_collection(2,i-1:i),'linewidth',2)
    hold on
    plot(x_collection(1,i-1),x_collection(2,i-1),'r*','linewidth',2)
end

% Plot level curves of the function
x_k = -2:0.001:2;
C = 5:100:4000;
colors = cool(length(C));
for i = 1:length(C)
    yplus = x_k.^2 + sqrt(1/100*(C(i)-(1-x_k.^2)));
    yminus = x_k.^2 - sqrt(1/100*(C(i)-(1-x_k.^2)));
    plot(x_k,yplus,'color',colors(i,:));
    plot(x_k,yminus,'color',colors(i,:));
end
axis([-2 2 -2 4])
end


% Line search method satisfying Wolfe conditions
function alpha = lineSearch(x_k,p_k,f,nablaf,c_1,c_2)
rho = 2;     % Scaling factor
alpha = 1;   % Initial step size

% Function values at starting point
f_k = f(x_k);
grad_k = nablaf(x_k);
dir_k = grad_k'*p_k;

% Function values at proposed step
x_right = x_k + alpha*p_k;
f_right = f(x_right);
grad_right = nablaf(x_right);
dir_right = grad_right'*p_k;

% Boolean values for whether the Wolfe conditions are satisfied
wolfe1 = f_right <= f_k + c_1*alpha*dir_k;
wolfe2 = dir_right >= c_2*dir_k;

if wolfe1 && wolfe2 % Both conditions satisfied
    return  
elseif ~wolfe1      % 1st condition violated
    alpha_left = 0;
    alpha_right = 1;
else                % 1st condition satisfied, 2nd condition violated
    alpha_left = 1;
    alpha_right = 1;
    while wolfe1    % Keep looking for a right endpoint
         alpha_right = rho*alpha_right;
         
         x_right = x_k + alpha_right*p_k;
         f_right = f(x_right);
         grad_right = nablaf(x_right);
         dir_right = grad_right'*p_k;
         
         wolfe1 = f_right <= f_k + c_1*alpha*dir_k;
         wolfe2 = dir_right >= c_2*dir_k;
         
         if wolfe1 && wolfe2 % Both conditions satisfied
             alpha = alpha_right;
             return
         end
    end
end

while  ~(wolfe1 && wolfe2) % Find step length satisfying both conditions.
   alpha = (alpha_left + alpha_right)/2;
   
   x_new = x_k + alpha*p_k;
   f_new = f(x_new);
   grad_new = nablaf(x_new);
   dir_new = grad_new'*p_k;
   
   wolfe1 = f_new <= f_k + c_1*alpha*dir_k;
   wolfe2 = dir_new >= c_2*dir_k;
   
   if ~wolfe1
       alpha_right = alpha;
   elseif ~wolfe2
       alpha_left  = alpha;
   end
end
end