% Illustration of Newton and Newton-CG method
% for Rosenbrock-like function
close all;
gamma = 100;
[xx,yy]=meshgrid(-.5:.01:1.5);
f_fun = @(x,y)gamma*(x.^2-y).^2 + (x-1).^2;
g_fun = @(x,y)[4*gamma*x.*(x.^2-y)+2*x-2; -2*gamma*(x.^2-y)];
H_fun = @(x,y)[4*gamma*(x.^2-y)+8*gamma*x.^2+2, -4*gamma*x;
	-4*gamma*x,                       2*gamma];
zz = f_fun(xx,yy);
figure(1);
contour(xx,yy,zz,30,'k','linewidth', 1);
axis equal tight;
pause;

figure(2);
surf(xx(1:5:end,1:5:end),yy(1:5:end,1:5:end),zz(1:5:end,1:5:end));
axis tight;

figure(1);
hold on;
plot(1,1,'rx','Linewidth',4);
title('minimizer');

X = -0.1;
Y = -0.2;
pause
for k=1:20
    figure(1);
    contour(xx,yy,zz,30,'k','linewidth', 1);
    hold on;
    axis equal tight;
    if k == 1
	pause;
    end
    plot(1,1,'rx','Linewidth',4);
    title('Solution point');
    plot(X,Y,'bx','Linewidth',5);
    title('Iterate x_k');

    g = g_fun(X,Y);
    H = H_fun(X,Y);
    xxd = xx - X;
    yyd = yy - Y;
    zzlin = g(1)*xxd + g(2)*yyd + ...
	0.5*(xxd.*xxd*H(1,1)+2*H(1,2)*xxd.*yyd+H(2,2)*yyd.*yyd);
    pause(.5);
    step = 1;
    plot([X,X-step*g(1)],[Y,Y-step*g(2)],'g','Linewidth',3);
    title('steepest descent direction -g');
    axis([-.5,1.5,-.5,1.5]);
    %pause;
    contour(xx,yy,zzlin,20,'r','linewidth', 1);
    title('quadratic approximation');
    pause;
    d = -H \ g;
    %d = - g;
    Xnew = X+d(1);
    Ynew = Y+d(2);
    alpha = 1; c = 1e-4;
    suff_descent = 0; i = 0;
    f = f_fun(X,Y);
    % line search
    while (suff_descent == 0 && i < 10)
        Xnew = X+alpha*d(1);
        Ynew = Y+alpha*d(2);
        f_new = f_fun(Xnew,Ynew);
        fprintf('        Backtrack: New cost: %f, Old cost: %f\n', f_new, f);
        if (f_new < f + c * alpha * g' * d)
            suff_descent = 1;
            fprintf('Iter %d: #backtracks: %d, f: %2.16f.\n',k,i,f_new);
        else
            alpha = alpha/2;
        end
        i = i+1;
    end
    if (suff_descent == 0)
        fprintf('Did not find sufficient descent!\n');
        return;
    end
    plot([X,Xnew],[Y,Ynew],'m','Linewidth',3);
    pause(1);
    X = Xnew; Y = Ynew;
    hold off;
end