clear all 
close all

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

% Initialize variables
x_old = 2*rand(2,1); % Random starting point
tol = 1E-10; % Tolerance for stopping criterion
change = inf; % Measure of change between iterations
iter = 0; % Iteration counter
maxit = 30; % Maximum number of iterations
rho = 1/2; % Step length decrease
c = 0.001; % Slope modifier for line of sufficient decrease

x_collection = x_old; % Keeps track of all points considered



while change > tol && iter <= maxit
    % Gradient and hessian
    grad = [-400*x_old(1)*(x_old(2)-x_old(1)^2) - 2*(1-x_old(1));
            200*(x_old(2)-x_old(1)^2)];
    hess = [2 - 400*x_old(2) + 1200*x_old(1)^2,  -400*x_old(1);
                       -400*x_old(1),                  200   ];
                   
    % Search direction
    p = -hess\grad;
    
    % Check whether p is a descent direction. If not, use steepest descent
    % direction.
    if p'*grad >= 0
        disp('Newtons did not give descent direction - using Steepest Descent')
        p = -grad;
    end
   
    % Find step length
    alpha = 1;
    while f(x_old + alpha*p) > f(x_old) + c*alpha*grad'*p
        alpha = rho*alpha;
    end
    
    % Update minimizer candidate
    x_new = x_old +alpha*p;
    
    % Measure change from previous iteration
    change = abs(f(x_new)-f(x_old));
    
    % Make ready for next iteration and store minimizer candidate
    x_old = x_new;
    x_collection(:,end+1) = x_old;
    iter = iter + 1;
end

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

% Plot path taken by Newton's 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 = 0:0.001:2;
C = 5:100:2000;
colors = cool(length(C));
for i = 1:length(C)
    yplus = x.^2 + sqrt(1/100*(C(i)-(1-x.^2)));
    yminus = x.^2 - sqrt(1/100*(C(i)-(1-x.^2)));
    plot(x,yplus,'color',colors(i,:));
    plot(x,yminus,'color',colors(i,:));
end
axis([0 2 0 4])