close all; clear; clc; format short

%%
f = @(x,y) log(1 + (x.*sign(real(x))).^(2 + sin(x+y))) + y.^2/20;
tol = 1e-4; reflection = 1; contraction = 1/2;

% Plot
cm = lines;
cm2 = [cm(1,:); cm(6,:)];
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);
%}

% Starting point p0
p = 8.3+1i*(-8.9);
%[x,y] = ginput(1); p = x+1i*(y);

% Initialisation
P = p + exp(1i*[0 2*pi/3 4*pi/3]); %
fi = @(p) f(real(p),imag(p));
F = fi(P);

Pold = mean(P);
plot(p,'yo','MarkerSize',10);
plot(0,0,'yp','MarkerSize',10);
plot([P P(1)],'Linewidth',1,'color',cm2(1,:));
tic
nfv = numel(P);
for j = 1:1000 % Limit number of iterations to 1e3
    [~,l] = min(F); [~,h] = max(F); % Find highest and lowest points
    Pc = P; Pc(h) = []; Pc = mean(Pc); % Find center of the centroid
    
    % Calculate reflection
    Pn = (1+reflection)*Pc - reflection*P(h);
    Fn = fi(Pn);
    nfv = nfv + 1;
    
    if any(Fn < F) % Accept reflection if it is any better
        %disp('Reflection')
        P(h) = Pn; F(h) = Fn;
    else
        % Calculate contraction
        Pn = contraction*P(h) + (1-contraction)*Pc;
        Fn = fi(Pn);
        nfv = nfv + 1;
        
        if Fn < F(h) % Accept contraction if it is better
            %disp('Contraction')
            P(h) = Pn; F(h) = Fn;
        else % Perform an expansion step
            P = (P+P(l))/2;
            F = fi(P);
            nfv = nfv + numel(P);
        end
    end
    
    plot([P P(1)],'-','LineWidth',1,'color',cm2(mod(j,size(cm2,1))+1,:));
    %plot([Pold mean(P)],'-','LineWidth',1,'color',cm2(mod(j,size(cm2,1))+1,:));
    if norm(mean(P)) > 2
        plot([Pold mean(P)],'.-','LineWidth',1,'color',cm(2,:),'MarkerSize',10);
    else
        plot([Pold mean(P)],'-','LineWidth',1,'color',cm(2,:));
    end
    Pold = mean(P);
    drawnow;
    
    if max(abs(diff([P P(1)])))<tol; break; end
end
pf = mean(P); pf = [real(pf) imag(pf)];
toc

%% Print info
fprintf(' iter = %d\n',j);
fprintf('Minimum at:\n');
fprintf('   pf = (% .2e,% .2e)\n',pf);
fprintf(' fval = %.2e\n',f(pf(1),pf(2)));
fprintf('Actual minimum at:\n');
fprintf('   pf = (% .2e,% .2e)\n',[0,0]);
fprintf(' fval = %.2e\n',0);
fprintf(' loss fun evals = %d\n',nfv);
