close all; clear; clc;

%% Evaluate loss function for a grid of possible values of the parameters
loss = @(p) -cos(2*pi*(p(2)-1)/2)+abs(p(2)-1) + (p(1)-2.5)^2;
options = optimset('MaxFunEvals',1e2,'MaxIter',1e6,'Display','none');
as = linspace(-4,4,100); bs = linspace(-4,4,100);
L = zeros(numel(as),numel(bs));
B1 = zeros(numel(as),numel(bs)); % Preallocate
B2 = zeros(numel(as),numel(bs)); % Preallocate
for ai = 1:numel(as)
    for bi = 1:numel(bs)
        L(ai,bi) = loss([as(ai),bs(bi)]);
        temp = fminsearch(loss,[as(ai),bs(bi)],options);
        B1(ai,bi) = temp(2);
        temp = random_search_function(loss,[as(ai),bs(bi)],0.1,0.2);
        B2(ai,bi) = temp(2);
    end
    if ~mod(ai,round(numel(as)/20)), fprintf('%.1f%% done\n',ai/numel(as)*100), end
end

%% Plot
figure(1); clf;

a = subplot(1,3,1); cla; hold on;
v = sort(unique([-10 0 2 4 linspace(0,100,10)]));
cm = viridis(15);
imagesc(as,bs,L');
caxis([0 50]);

colormap(a,cm);
grid on; box on; axis tight equal;
xlabel('p1'); ylabel('p2');
plot(2.5,1,'yp','MarkerSize',10);
plot(2.5,-0.8969,'y.','MarkerSize',10);
plot(2.5,-2.8969,'y.','MarkerSize',10);
plot(2.5,+2.8969,'y.','MarkerSize',10);

cm = inferno;
a = subplot(1,3,2); hold on;
colormap(cm);
imagesc(as,bs,round(B1'));
grid on; box on; axis tight equal;
xlabel('p1'); ylabel('p2');
a.YDir = 'normal'; a.XDir = 'normal';
colormap(a,cm);
plot(2.5,1,'yp','MarkerSize',10);
plot(2.5,-0.8969,'y.','MarkerSize',10);
plot(2.5,-2.8969,'y.','MarkerSize',10);
plot(2.5,+2.8969,'y.','MarkerSize',10);
caxis([min(round(B1(:))) max(round(B1(:)))]);

a = subplot(1,3,3); hold on;
colormap(cm);
imagesc(as,bs,round(B2'));
grid on; box on; axis tight equal;
xlabel('p1'); ylabel('p2');
a.YDir = 'normal'; a.XDir = 'normal';
colormap(a,cm);
plot(2.5,1,'yp','MarkerSize',10);
plot(2.5,-0.8969,'y.','MarkerSize',10);
plot(2.5,-2.8969,'y.','MarkerSize',10);
plot(2.5,+2.8969,'y.','MarkerSize',10);
caxis([min(round(B1(:))) max(round(B1(:)))]);

%% Plot detailed hypersurface for random search
load('B_1x1k.mat'); % Load pre-computed surface

figure(2); clf;
cm = inferno;
a = axes; hold on;
colormap(cm);
imagesc(as,bs,round(B)');
grid on; box on; axis tight equal;
xlabel('p1'); ylabel('p2');
a.YDir = 'normal'; a.XDir = 'normal';
colormap(a,cm);
plot(2.5,1,'yp','MarkerSize',10);
plot(2.5,-0.8969,'y.','MarkerSize',10);
plot(2.5,-2.8969,'y.','MarkerSize',10);
plot(2.5,+2.8969,'y.','MarkerSize',10);
caxis([min(round(B(:))) max(round(B(:)))]);

%% Save as hi-res image
im = flipud(round(B)');
im = im - min(im(:))+1;
cm = inferno(max(im(:)));
im = ind2rgb(im,cm);
imwrite(im,'B_1x1k.png')