close all; clear; clc;

%%
f = @(x,y) x.^2 + y.^2 + x.*sin(x) - sin(2*y);
J = @(x,y) [2*x + y.*cos(x)
    2*y - 2*cos(2*y) + sin(x)];
H = @(x,y) [2-y.*sin(x) cos(x);
            cos(x) 2+4*sin(2*y)];

tol = 1e-3; alpha = 0.1;
x = -5:.1:5; y = -5:.1:5;
[xx,yy] = meshgrid(x,y);
ff = f(xx,yy);

figure(1); clf;
contourf(xx,yy,ff,20);
grid on; box on; axis square; hold on
colormap(viridis);
plot(-0.27,0.56,'yp','MarkerSize',10);
x = -2; y = -2;
[x,y] = ginput(1);
xold = x; yold = y;

clc
for k = 0:100
    fxy = f(x,y); Jxy = J(x,y); Hxy = H(x,y);
    plot([xold x],[yold y],'r.-');
    plot([x x-0.2*Jxy(1)],[y y-0.2*Jxy(2)],'c-');
    drawnow;
    
    Hxy = 1/2*(Hxy+Hxy'); % Diagonalise hessian
    
    % Add a regularisation term to make sure H is positive semi-definite
    Hxy = Hxy + 3*sqrt(mean(Hxy(:).^2))*eye(2);
    %Hxy = Hxy + 100*eye(2); % Too much reg
    Hxy = 1/alpha*eye(2); % <- Just do gradient descent
    
    xold = x; yold = y;
    s = -(Hxy\Jxy);
    x = x+s(1); y = y+s(2);
    if norm(s)<tol; break; end
end
fprintf('iterations = %d\n',k);