% Pure Newton, goes actually uphill
clear all; close all;
f = @(x) (x(1)+x(2)).^2+10.*cos(x(1).^2+x(2)).^2;
h = 10^-10;
h1 = [h 0]; h2 = [0 h];
hh = 10^-20;
hh1 = [hh 0]; hh2 = [0 hh];
path=[];
a = 0;
x0  = [-1.5 1.5];x=x0;
path=[path ;x0];
n = 0;

fx = f(x);f0=fx+10;
fun=fx;

fprintf('%5d %20.15f %20.15f %20g\n',n,x,fx)
n=n+1;
while abs(fx-f0)>10^-16
    f0 = fx;
    % compute derivatives using imaginary step
    % - numerically very accurate 
    % grad f
    gf = @(x) [f(x+i*hh1) f(x+i*hh2)]/hh;
    gg = imag(gf(x));
    % Hessian - not so accurate!
    g1 = imag(gf(x+h1)-gf(x-h1))/(2*h);
    g2 = imag(gf(x+h2)-gf(x-h2))/(2*h);
    H= [g1 ; g2 ];   
    %p  = linsolve(H,-gg');  % Matlab only   
    p = H\(-gg'); % Matlab and octave
    x  = x+p';    
    fp = f(x);
    path=[path;x];
    fx = fp;
    fun=[fun;fx];
    fprintf('%5d %20.15f %20.15f %20g %20g \n',n,x,fx,a)
    a = a/10;
    n = n+1;
    if n>100
        break;
    end
end
figure(1)
plot(fun,'b-o')
xlabel('iteration') 
ylabel('f(x)')
figure(2)
x1=min(path(:,1))-1;
x2=max(path(:,1))+1;
y1=min(path(:,2))-1;
y2=max(path(:,2))+1;
x=x1:0.1:x2;
y=y1:0.1:y2;
[X,Y]=meshgrid(x,y);
for i=1:size(X,1)
        for j=1:size(X,2)
            x=[X(i,j) Y(i,j)];
            Z(i,j)=f(x);
        end
end
contour(X,Y,Z,10) ;colorbar;hold on;
x=[path(:,1) path(:,2)];
plot(path(:,1),path(:,2),'mo-','linewidth',2);


