% This script generates uniform grids for 5 test functions and calculates
% aBB and subenergy-aBB lower bounds based on the eigenvalues from the
% samples. All information is transformed to the f-range to make it easier
% to compare.
clc; clearvars -except cont; % "cont" is an INTLAB variable, it should never be cleared
syms x1 x2

x=[x1 x2];nuDims=2;

% Choose mode: calculations for optimal boxes, or sub-optimal ones?
calculateOptimal=true;
% calculateOptimal=false;

mu=10;
% Nu samples per axis, apart from the vertices.
nuSamplingPoints=[1 3 5 8 48];
% Total number of samples is (nuSamplingPoints+2)^2
nuGridPoints=(nuSamplingPoints+2).^2;

% Declare test functions
functions = {
      @(x) 20+x(1)^2+x(2)^2-10*(cos(2*pi*x(1))+cos(2*pi*x(2))); % Rastrigins function
      @(x) (1-x(1))^2+100*(x(2)-x(1)^2)^2;                      % Rosenbrock function
      @(x) -20*exp(-.2*sqrt(.5*(x(1)^2+x(2)^2)))-exp(.5*(cos(2*pi*x(1))+cos(2*pi*x(2))))+20+exp(1); % Ackley function
      @(x) (1.5-x(1)+x(1)*x(2))^2+(2.25-x(1)+x(1)*x(2)^2)^2+(2.625-x(1)+x(1)*x(2)^3)^2              % Beale's function
      @(x) (1+(x(1)+x(2)+1)^2*(19-14*x(1)+3*x(1)^2-14*x(2)+6*x(1)*x(2)+3*x(2)^2))...
      *(30+(2*x(1)-3*x(2))^2*(18-32*x(1)+12*x(1)^2+48*x(2)-36*x(1)*x(2)+27*x(2)^2));                % Goldstein-price function
    };

% Set up the boxes in which each function will be tested:
% - Optimal boxes
%   Format is :
%   x1L x1U x2L x2U
rangeOptimal = [
    -1 2 -2 1;              %Rastrigin
     0.5 1.5 0.5 1.5;       %Rosenbrock
     -1 1 -1 1 ;            %Ackley
      2 4  0 1;             %Beale
     -1 1 -1.5 -0.5;        %Goldstein
    ];
% - Sub-optimal boxes
rangeSuboptimal = [
    -2 -1 -2 -1;            %Rastrigin
    0.15 .25 0.15 .25;        %Rosenbrock
    -2 -1 -2 -1;            %Ackley
     1 2 1 2;               %Beale
     0.01 0.3  -0.9 -0.8;   %Goldstein
    ];

% Define f* (in this case the global minima) for each function
fs = [
     0      %Rastrigin
     0       %Rosenbrock
     0       %Ackley
     0       %Beale
     3       %Goldstein
    ]

if calculateOptimal==false
    range=rangeSuboptimal;
    % Define minimum value in each box
    minVals=[2       1.3282       3.6254       14.204        7.464];
    disp('Calculating bounds for suboptimal boxes:')
else
    range=rangeOptimal;
    % For optimal boxes, the minimum value is the one at the global minimum
    minVals=fs';
    disp('Calculating bounds for optimal boxes:')
end
range
numberOfSamplingConfigurations=size(nuSamplingPoints,2);

parfor j=1:size(functions,1) 
% for j=1:size(functions,1) % Use "parfor" instead, if parallel toolbox is available

    disp('=========NEW FUNCTION========')
    % Print some info on screen
    f = functions{j}
    fStarForThisFunction=fs(j)
    disp('=============================')
    for i=1:numberOfSamplingConfigurations % Loop through different sampling configurations
        
        % Set up box info
        samplingPoints=nuSamplingPoints(i)+2;
        xu = [range(j,2) range(j,4)];
        xl = [range(j,1) range(j,3)];
        
        % Find axes grid points
        xSample=zeros(nuDims,samplingPoints);
        samplingStep=zeros(nuDims,1);
        for k=1:nuDims
            samplingStep(k)=(xu(k)-xl(k))/(samplingPoints-1);
            xSample(k,:)=[xl(k):samplingStep(k):xu(k)];
        end

        % Populate sampling grid (easier than using meshgrid with intlab)
        x2DGrid=zeros(samplingPoints,samplingPoints,2);
        for k=1:samplingPoints 
            x2DGrid(:,k,1)=xSample(1,k);
            x2DGrid(k,:,2)=xSample(2,k);
        end

        % Get minimum eigenvalue
        minEigenvalue=inf;
        
        % Calculate the eigenvalues in all grid points
        for k=1:samplingPoints
            for t=1:samplingPoints
                xs=hessianinit([x2DGrid(k,t,1);x2DGrid(k,t,2)]);
                fsample=f(xs);
                hessianMatrix=fsample.hx;
                if any(isnan(hessianMatrix(:))) == 0
                    eigenvalues=eigs(hessianMatrix);
                    for kk=1:size(eigenvalues,1)
                        if eigenvalues(kk)<minEigenvalue
                            minEigenvalue=eigenvalues(kk);
                        end
                    end
                else
                    % Ignore this point - numeric error
                end
            end
        end

        if isnan(minEigenvalue)==0
            alphaF = max(0,-0.5*minEigenvalue);
            % aBB f(x)
            Lf= @(x) f(x)+alphaF*( ...
                (xl(1)-x(1)).*(xu(1)-x(1)) +...
                (xl(2)-x(2)).*(xu(2)-x(2)) );
            
            x0=0.5*[(xu(1)+xl(1)) (xu(2)+xl(2))];
            
            A=[-1 0;1 0;0 -1;0 1];
            B=[-xl(1);xu(1);-xl(2);xu(2)];
            options=optimset('Algorithm','active-set','Display', 'off','FinDiffType','central','TolFun',1.e-12);
            [xLB,fLBtemp]=fmincon(Lf,x0,A,B,[],[],[],[],[],options);
            fLB=fLBtemp;
            % Round values close to zero to a numerical tolerance to filter
            % numerical artifacts (i.e., "e-16" results)
            if abs(fLB)<1.e-6
               fLB=0 ;
            end
        else
            fLB=NaN;
        end
        
        fLBFromABB(j,i)=fLB;
                       
        % Subenergy
        s = @(x) -log(1+exp(-mu*(f(x)-fs(j))));
        % Get minimum eigenvalue
        minEigenvalueS=inf;
        for k=1:samplingPoints
            for t=1:samplingPoints
                xs=hessianinit([x2DGrid(k,t,1);x2DGrid(k,t,2)]);
                fsample=s(xs);
                hessianMatrix=fsample.hx;
                if any(isnan(hessianMatrix(:))) == 0
                    if any(isinf(hessianMatrix(:))) == 0
                        eigenvalues=eigs(hessianMatrix);
                        for kk=1:size(eigenvalues,1)
                            if eigenvalues(kk)<minEigenvalueS
                                minEigenvalueS=eigenvalues(kk);
                            end
                        end
                    end
                else
                    % Ignore this point
                end
            end
        end
        
        alphaS = max(0,-0.5*minEigenvalueS);
        % aBB f(x)
        Ls= @(x) s(x)+alphaS*( ...
            (xl(1)-x(1)).*(xu(1)-x(1)) +...
            (xl(2)-x(2)).*(xu(2)-x(2)) );
        
        x0=0.5*[(xu(1)+xl(1)) (xu(2)+xl(2))]; % It's important to start from the left side
        
        A=[-1 0;1 0;0 -1;0 1];
        B=[-xl(1);xu(1);-xl(2);xu(2)];
        options=optimset('Algorithm','active-set','Display', 'off','FinDiffType','central','TolFun',1.e-12);
        [xLB,sLBtemp]=fmincon(Ls,x0,A,B,[],[],[],[],[],options);
        sLB=sLBtemp;       
        S_to_F=-1/mu*log(exp(-sLB)-1)+fs(j);
        % Round values close to zero to a numerical tolerance to filter
        % numerical artifacts (i.e., "e-16" results)
        if abs(S_to_F)<1.e-6
            S_to_F=0;
        end
        % Display output
        fprintf('--> Number of grid points : %d\n',samplingPoints^2)
        fprintf('      Minimum eigenvalue for f is    : %.3f\n',minEigenvalue)
        fprintf('      Minimum eigenvalue for S is    : %.3f\n',minEigenvalueS)
        fprintf('      aBB lower bound for f is       : %.3f\n',fLB)        
        fprintf('      subenergy lower bound for f is : %.3f\n',S_to_F)
        fprintf('      subenergy lower bound in S is  : %.3f\n',sLB)

        fLBFromSubenergy(j,i)=S_to_F;       
        
    end
end

for i=1:size(functions,1)
    nodeWasFathomedABB(i)=any(fLBFromABB(i,:)>fs(i));
    nodeWasFathomedSub(i)=any(fLBFromSubenergy(i,:)>fs(i));
end

format shortG
fprintf('\n\n==================================== Results ======================================\n')

abbCol=[[inf inf nuGridPoints];[fs minVals' fLBFromABB]];
names={'Nu Samples/function', 'Rastrigin' ,'Rosenbrock' ,'Ackley' ,'Beale' ,'Goldstein'};
fLowerBoundsFromABB=array2table(abbCol,'RowNames',names)

subCol=[[inf inf nuGridPoints];[fs minVals' fLBFromSubenergy]];
names={'Nu Samples/function', 'Rastrigin' ,'Rosenbrock' ,'Ackley' ,'Beale' ,'Goldstein'};
fLowerBoundsFromSubenergy=array2table(subCol,'RowNames',names)

% Quick check to see which nodes were fathomed
nodeWasFathomedABB=nodeWasFathomedABB'
nodeWasFathomedSub=nodeWasFathomedSub'