clear all
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)          ];
                     
% Hessian
hessianf = @(x) [2 - 400*x(2) + 1200*x(1)^2,  -400*x(1);
                    -400*x(1),                  200   ];
                
% Initialize variables                
x_k = -2 + 4*rand(2,1); % Random starting position
deltamax = 8;           % Maximum radius of trust region
delta = 2;              % Initial radius of trust region
eta = 1/8;              % Minimum reduction ratio allowed
maxit = 30;             % Maximum number of iterations allowed
iter = 0;               % Iteration counter
tol = 1E-10;            % Tolerance for stopping criterion
change = inf;           % Measure of change between iterations

x_collection = x_k; % Keeps track of all points considered

while change > tol && iter <= maxit
    %%%%%%%% Find p_k by dogleg method %%%%%%%
    B = hessianf(x_k);
    g = nablaf(x_k);
    f_k = f(x_k);
    p_B = -B\g; 
    
    % Model equation
    m = @(p) f_k + g'*p + 1/2*p'*B*p;
    
    if norm(p_B) <= delta
        % Use Newton step
        p_k = p_B;
    else
        p_U = -g'*g/(g'*B*g)*g;
        np_U = norm(p_U);
        if np_U >= delta
            % Use steepest descent step
            p_k = delta*p_U/np_U;
        else
            % Find tau as positive root of polynomial
            p_C = p_B - p_U;
            coeffs = [norm(p_C)^2, 2*p_C'*p_U, np_U^2-delta^2];
            tau = max(roots(coeffs));
            p_k = p_U + tau*p_C;
        end
    end
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
    %%%%% Check trust region assumptions %%%%%
    rho = (f_k - f(x_k + p_k))/(f_k - m(p_k));
    if rho < 1/4 % Too little reduction, shrink trust area
        delta = 1/4*delta;
    else % Good reduction, meets boundary, expand trust area
        if rho > 3/4 && norm(p_k) == delta
            delta = min(2*delta,deltamax);
        end
    end
    if rho > eta % Acceptable reduction
        x_new = x_k + p_k;
        change = abs(f(x_new)-f_k);
    else % Unacceptable reduction, try again
        x_new = x_k;
        change = inf;
    end
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  
    % Make ready for next iteration and store minimizer candidate
    x_k = x_new;
    x_collection(:,end+1) = x_k;
    iter = iter + 1;
end

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

% Plot path taken by Dogleg 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])