close all; clear; clc; format short


%% Fit function and Jacobian
f = @(x,y) log(1 + (x.*sign(real(x))).^(2 + sin(x+y))) + y.^2/20;
h = 1e-4; % Finite difference step size
%J = @(x,y) [(f(x+h,y)-f(x-h,y)) (f(x,y+h)-f(x,y-h))]/(2*h);
J = @(x,y) [imag(f(x+1i*h,y))/h imag(f(x,y+1i*h))/h]; % Complex step

%% Plot
% Optimisation by physics-like update of the fit point.

tol = 1e-4; mass = 3; drag = 2;
cm = lines;
figure(1); clf;
a = axes; hold on;
x = -1:.1:10; y = -10:.1:1;
[xx,yy] = meshgrid(x,y);
ff = f(xx,yy);
imagesc(x,y,ff);
a.XDir = 'normal'; a.YDir = 'normal';
grid on; box on; axis tight square;
colormap(viridis);
%[x,y] = ginput(1); p = [x,y];
p = [8.3 -8.9]; % Starting point p0
v = [0 0]; dt = 1;
pold = p;
plot(p(1),p(2),'yo','MarkerSize',10);
plot(0,0,'yp','MarkerSize',10);

tic
for j = 1:1000 % Limit number of iterations to 1e3
    pold = p;
    
    % Do physics update
    F = - J(p(1),p(2)) - drag*v;
    a = F/mass;
    p = p + v*dt; % p = p+v*dt
    v = v + a*dt; % v = v+a*dt
    
    plot([p(1) p(1)+a(1)],[p(2) p(2)+a(2)],'c-','LineWidth',1);
    if norm(p)>2.5
        plot([pold(1) p(1)],[pold(2) p(2)],'.-','color',cm(2,:),'LineWidth',1,'MarkerSize',10);
    else
        plot([pold(1) p(1)],[pold(2) p(2)],'-','color',cm(2,:),'LineWidth',1);
    end
    if ~mod(j,10); drawnow; end
    
    if norm(v)<tol && j > 1; break; end
end
toc

%% Print info
fprintf('Found minimum at:\n');
fprintf('   pf = (% .2e,% .2e)\n',p);
fprintf(' fval = %.2e\n',f(p(1),p(2)));
fprintf('Actual minimum at:\n');
fprintf('   pf = (% .2e,% .2e)\n',[0,0]);
fprintf(' fval = %.2e\n',0);
