function [Gbest,xGbest,convcurve,time_elap]=CALSAPSO_origfPB(lb,ub,max_FEs,fobj,initPos,init_f)
% -----------------------------------------------------------------
% Important Note: This code needs intralled SURROGATE TOOLBOX(https://sites.google.com/site/srgtstoolbox/)
Data=[initPos,init_f];
% -----------------------------------------------------------------
% Input:
% Data          - Offline (Initial) Data with c Decision Variables and Exact Objective Value
% BU            - Upper Boundary of c Decision Variables
% BD            - Lower Boundary of c Decision Variables
%
% Output: 
% time          - Execution Time
% Record        - Logbook of Evaluated Solutions
% gbest         - Predicted Optimum over Generations with c Decision Variables
%
    %%%%    Authors:    Handing Wang, Yaochu Jin, John Doherty
    %%%%    University of Surrey, UK
    %%%%    EMAIL:      wanghanding.patch@gmail.com
    %%%%    WEBSITE:    https://sites.google.com/site/handingwanghomepage
    %%%%    DATE:       May 2018
%------------------------------------------------------------------------
%This code is part of the program that produces the results in the following paper:
%Handing Wang, Yaochu Jin, John Doherty, Committee-based Active Learning for Surrogate-Assisted Particle Swarm Optimization of Expensive Problems, IEEE Transactions on Cybernetics, vol.47, no.9, pp.2664-2677, 2017.
%You are free to use it for non-commercial purposes. However, we do not offer any forms of guanrantee or warranty associated with the code. We would appreciate your acknowledgement.
% -Adapted by Pedro Bento November 2023 
%------------------------------------------------------------------------
warning off all;
time_begin=tic;
c=size(Data,2)-1;
n=size(Data,1);
Ub=ub.*ones(1,c);
Lb=lb.*ones(1,c);
Record=[];
n1=0;
n2=0;
n3=0;

%--------StepControl:1V 2D 3L-------------------

StepNext=1;
Tm=min(Data(:,c+1));
convcurve=Tm.*ones(1,n);

while size(Data,1)<max_FEs
    StepCur=StepNext;
    switch StepCur
        case 1
            P=sortrows(Data,c+1);
            DataT= selectData_PB(P,n+n1,c,2);
            DataT=sortrows(DataT,c+1);
            DataT(1,:)=P(1,:);
            [~,rawgbest,~]=PSO_DV(DataT,Ub,Lb);
        case 2
            P=sortrows(Data,c+1);
            DataT= selectData_PB(P,n+n1,c,2);
            DataT=sortrows(DataT,c+1);
            DataT(1,:)=P(1,:);
            [~,rawgbest,~]=PSO_D(DataT,Ub,Lb);
        case 3
            P=sortrows(Data,c+1);      
            I=find(P(:,c+1)<Tm);
            DataT=P(I,1:c+1);
            if size(DataT,1)<3
                DataT= selectData_PB(P,n+n1,c,2);
                DataT=sortrows(DataT,c+1);
                DataT(1,:)=P(1,:);
                [~,rawgbest,~] =PSO_DV(DataT,Ub,Lb);
            else
                bu=max(DataT(:,1:c),[],1);
                bd=min(DataT(:,1:c),[],1);
                [~,rawgbest,~] =PSO_DB(DataT,bu,bd);
            end
    end
    rawgbest(:,c+1)=fobj(rawgbest(:,1:c)); %REPLACES compute_objectives(gbest(:,1:c),c,problem_name);
    t=[rawgbest,StepCur];
    Record=[Record;t];
    switch StepCur
        case 1
            StepNext=2;
            n3=n3+1;
        case 2
            if min(Data(:,c+1))>=min(rawgbest(:,c+1))
                StepNext=1;
                n1=n1+1;
            else
                StepNext=3;  
            end
        case 3
            if min(Data(:,c+1))>=min(rawgbest(:,c+1))
                StepNext=3;
                n2=n2+1;
            else
                StepNext=1;
            end
    end
    Data=[Data;rawgbest(:,1:c+1)];
%     size(Data,1)
    StepPre=StepCur;
end

[A,I]=min(Data(:,c+1));
rawgbest=Data(I,:);
%return results
time_elap=toc(time_begin);
xGbest=rawgbest(1:c);
Gbest=rawgbest(c+1);
convcurve(n+1:n+size(Record,1))=Record(:,c+1)';
% transform the convergence curve into strictly decreasing
for i=2:length(convcurve)
    if (convcurve(i)>convcurve(i-1))
        convcurve(i)=convcurve(i-1);
    end
end

end
%% Secondary Functions: CALSAPSO
% -----------------------------------------------------------------
function [reorderedData]= selectData_PB(ordrdData,n,c,f)
% Input:
% n             -No. of Selected Data Points
% c             -No. of Decision Variables
% ordrdData     -Ordered Data with c Decision Variables based on the minimum Exact Objective Value (Population of Decision Variables)
% f             -Norm
%
% Output: 
% reorderedData          - Selected Data (also based on the distancecriteria)
%
    reorderedData=ordrdData(1,:);
    ordrdData(1,:)=[];
    if size(ordrdData,1)>0
        while size(reorderedData,1)<n
            d=zeros(size(ordrdData,1),1);
            for i=1:size(ordrdData,1)
                dis=sum((abs(reorderedData(:,1:c)-ones(size(reorderedData,1),1)*ordrdData(i,1:c))).^f,2).^(1/f);
                d(i)=min(dis);
            end
            [A,I]=max(d);
            reorderedData=[reorderedData;ordrdData(I,:)];
        end
    end
end
% -----------------------------------------------------------------
function [time,gbest,POP] =PSO_DV(Data,bu,bd )
% Input:
% Data          - Data with c Decision Variables and Exact Objective Value
% bd            - Upper Boundary of c Decision Variables
% bd            - Lower Boundary of c Decision Variables
%
% Output: 
% time          - Execution Time
% Record        - Logbook of Evaluated Solutions
% gbest         - Predicted Optimum over Generations with c Decision Variables
%
c=size(Data,2)-1;
x=Data(:,1:c);
y=Data(:,c+1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% fit surrogates

% kriging
srgtOPTKRG  = srgtsKRGSetOptions(x, y);
srgtSRGTKRG = srgtsKRGFit(srgtOPTKRG);

% polynomial response surface
srgtOPTPRS  = srgtsPRSSetOptions(x, y);
srgtSRGTPRS = srgtsPRSFit(srgtOPTPRS);


% radial basis function
srgtOPTRBF  = srgtsRBFSetOptions(x, y);
srgtSRGTRBF = srgtsRBFFit(srgtOPTRBF);



n=50;

POP = initialize_pop(n,c,bu,bd);

[f1 PredVar] = srgtsKRGPredictor(POP(:,1:c), srgtSRGTKRG);
[f2 PredVar] = srgtsPRSPredictor(POP(:,1:c), x, srgtSRGTPRS);
f3    = srgtsRBFEvaluate(POP(:,1:c), srgtSRGTRBF);
F=[f1,f2,f3];
obj=(min(F,[],2)-max(F,[],2));%./mean(F,2);%0-std(F,[],2)./obj;%./mean(F,2);

POP=[POP,obj];
lbest=POP;

[best,Ib]=min(POP(:,c+1));
gbest=POP(Ib,:);
v=(2*rand(n,c)-1).*(ones(n,1)*(bu-bd))*0.5;
g=0;
gmax=100;
B=gbest;
while g<gmax  
    [ POP,v ] = fly(POP,bu,bd,gbest,lbest,v,g/gmax);

    
    [f1 PredVar] = srgtsKRGPredictor(POP(:,1:c), srgtSRGTKRG);
    [f2 PredVar] = srgtsPRSPredictor(POP(:,1:c), x, srgtSRGTPRS);
    f3    = srgtsRBFEvaluate(POP(:,1:c), srgtSRGTRBF);
    F=[f1,f2,f3];
    POP(:,c+1)=(min(F,[],2)-max(F,[],2));%./mean(F,2);%0-std(F,[],2)./obj;%./mean(F,2);
    [ POP,gbest,lbest] = Update_best(POP,gbest,lbest);
    best=gbest(end);
    if best<B(end,end)
        B=[B;gbest];
    end
    g=g+1;
end


time=0;

end
% -----------------------------------------------------------------
function [time,gbest,POP] =PSO_D(Data,bu,bd )
% Input:
% Data          - Data with c Decision Variables and Exact Objective Value
% bu            - Upper Boundary of c Decision Variables
% bd            - Lower Boundary of c Decision Variables
%
% Output: 
% time          - Execution Time
% Record        - Logbook of Evaluated Solutions
% gbest         - Predicted Optimum over Generations with c Decision Variables
%
c=size(Data,2)-1;
x=Data(:,1:c);
y=Data(:,c+1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% fit surrogates

% kriging
srgtOPTKRG  = srgtsKRGSetOptions(x, y);
srgtSRGTKRG = srgtsKRGFit(srgtOPTKRG);
[PRESSRMS_KRG, eXV_KRG] = srgtsCrossValidation(srgtOPTKRG);

% polynomial response surface
srgtOPTPRS  = srgtsPRSSetOptions(x, y);
srgtSRGTPRS = srgtsPRSFit(srgtOPTPRS);
[PRESSRMS_PRS, eXV_PRS] = srgtsCrossValidation(srgtOPTPRS);
% [eXV_PRS PredVar] = srgtsPRSPredictor(x, x, srgtSRGTPRS);
% eXV_PRS=eXV_PRS-y;

% radial basis function
srgtOPTRBF  = srgtsRBFSetOptions(x, y);
srgtSRGTRBF = srgtsRBFFit(srgtOPTRBF);
[PRESSRMS_RBF, eXV_RBF] = srgtsCrossValidation(srgtOPTRBF);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% computing weights
eXVMatrix = [eXV_KRG eXV_RBF eXV_PRS];
CMatrix   = srgtsWASComputeCMatrix(x, eXVMatrix);

srgtsOPTs   = {srgtOPTKRG  srgtOPTRBF  srgtOPTPRS};
srgtsSRGTs  = {srgtSRGTKRG srgtSRGTRBF srgtSRGTPRS};
WAS_Model   = 'OWSdiag';
WAS_Options = CMatrix;

srgtOPTWAS  = srgtsWASSetOptions(srgtsOPTs, srgtsSRGTs, WAS_Model, WAS_Options);
srgtSRGTWAS = srgtsWASFit(srgtOPTWAS);
w=srgtSRGTWAS.WAS_Weights;

n=50;
POP = initialize_pop(n,c,bu,bd);
[obj PredVar] = srgtsWASPredictor(POP, srgtSRGTWAS);

POP=[POP,obj];

lbest=POP;

[best,Ib]=min(POP(:,c+1));
gbest=POP(Ib,:);
v=(2*rand(n,c)-1).*(ones(n,1)*(bu-bd))*0.5;
g=0;
gmax=100;
B=gbest;
while g<gmax  
    [ POP,v ] = fly(POP,bu,bd,gbest,lbest,v,g/gmax);
    [obj PredVar] = srgtsWASPredictor(POP(:,1:c), srgtSRGTWAS);

    POP(:,c+1)=obj;
    [ POP,gbest,lbest] = Update_best(POP,gbest,lbest);
    best=gbest(end);
    if best<B(end,end)
        B=[B;gbest];
    end
    g=g+1;
end


time=0;

end
% -----------------------------------------------------------------
function [time,gbest,POP] =PSO_DB(Data,bu,bd )
% Input:
% Data          - Data with c Decision Variables and Exact Objective Value
% bd            - Upper Boundary of c Decision Variables
% bd            - Lower Boundary of c Decision Variables
%
% Output: 
% time          - Execution Time
% Record        - Logbook of Evaluated Solutions
% gbest         - Predicted Optimum over Generations with c Decision Variables
%
c=size(Data,2)-1;
x=Data(:,1:c);
y=Data(:,c+1);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% fit surrogates

% kriging
srgtOPTKRG  = srgtsKRGSetOptions(x, y);
srgtSRGTKRG = srgtsKRGFit(srgtOPTKRG);
[PRESSRMS_KRG, eXV_KRG] = srgtsCrossValidation(srgtOPTKRG);

% polynomial response surface
srgtOPTPRS  = srgtsPRSSetOptions(x, y);
srgtSRGTPRS = srgtsPRSFit(srgtOPTPRS);
[PRESSRMS_PRS, eXV_PRS] = srgtsCrossValidation(srgtOPTPRS);
% [eXV_PRS PredVar] = srgtsPRSPredictor(x, x, srgtSRGTPRS);
% eXV_PRS=eXV_PRS-y;

% radial basis function
srgtOPTRBF  = srgtsRBFSetOptions(x, y);
srgtSRGTRBF = srgtsRBFFit(srgtOPTRBF);
[PRESSRMS_RBF, eXV_RBF] = srgtsCrossValidation(srgtOPTRBF);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% computing weights
eXVMatrix = [eXV_KRG eXV_RBF eXV_PRS];
CMatrix   = srgtsWASComputeCMatrix(x, eXVMatrix);

srgtsOPTs   = {srgtOPTKRG  srgtOPTRBF  srgtOPTPRS};
srgtsSRGTs  = {srgtSRGTKRG srgtSRGTRBF srgtSRGTPRS};
WAS_Model   = 'OWSdiag';
WAS_Options = CMatrix;

srgtOPTWAS  = srgtsWASSetOptions(srgtsOPTs, srgtsSRGTs, WAS_Model, WAS_Options);
srgtSRGTWAS = srgtsWASFit(srgtOPTWAS);
w=srgtSRGTWAS.WAS_Weights;

n=50;
POP = initialize_pop(n,c,bu,bd);
[obj PredVar] = srgtsWASPredictor(POP, srgtSRGTWAS);

POP=[POP,obj];
lbest=POP;

[best,Ib]=min(POP(:,c+1));
gbest=POP(Ib,:);
v=(2*rand(n,c)-1).*(ones(n,1)*(bu-bd))*0.5;
g=0;
gmax=100;
B=gbest;
while g<gmax  
    [ POP,v ] = fly(POP,bu,bd,gbest,lbest,v,g/gmax);
    [obj PredVar] = srgtsWASPredictor(POP(:,1:c), srgtSRGTWAS);

    POP(:,c+1)=obj;
    [ POP,gbest,lbest] = Update_best(POP,gbest,lbest);
    best=gbest(end);
    if best<B(end,end)
        B=[B;gbest];
    end
    g=g+1;
end



time=0;

end
% -----------------------------------------------------------------
function [POP] = initialize_pop(n,c,bu,bd)
% Input:
% bu            -Upper Bound
% bd            -Lower Bound
% c             -No. of Decision Variables
% n             -Population Scale
%
% Output: 
% POP           -Initial Population
%
POP=lhsdesign(n,c).*(ones(n,1)*(bu-bd))+ones(n,1)*bd;
end
% -----------------------------------------------------------------
function [POP,v] = fly(POP,bu,bd,gbest,lbest,v,theta)
% Input:
% POP           - Population of Decision Variables
% gbest         - Global Best
% lbest         - Local Best
% bu            - Upper Boundary of c Decision Variables
% bd            - Lower Boundary of c Decision Variables
% v             - Velocity
% theta         - Generation Parameter
%
% Output: 
% POP           - Updated Population 
% v             - Updated Velocity
%
c1=1.49445;
c2=1.49445;
N=size(POP,1);
n=size(bu,2);
w=0.9-theta*0.5;
v=w*v+c1*rand(N,n).*(lbest(:,1:n)-POP(:,1:n))+c2*rand(N,n).*(ones(N,1)*gbest(1:n)-POP(:,1:n));
tvmax=0.5*ones(N,1)*(bu-bd);
I=find(v>tvmax);
v(I)=tvmax(I);
I=find(v<(0-tvmax));
v(I)=0-tvmax(I);
tPOP=POP(:,1:n)+v;

Tbd=ones(N,1)*bd;
Tbu=ones(N,1)*bu;
I=find(tPOP>Tbu);
tPOP(I)=Tbu(I)-(tPOP(I)-Tbu(I));
I=find(tPOP<Tbd);
tPOP(I)=Tbd(I)+(Tbd(I)-tPOP(I));

POP(:,1:n)=tPOP;
end
% -----------------------------------------------------------------
function [POP,gbest,lbest] = Update_best(POP,gbest,lbest)
% Input:
% POP           - Population of Decision Variables
% gbest         - Global Best
% lbest         - Local Best
%
% Output: 
% POP           - Updated Population 
% gbest         - Updated Global Best
% lbest         - Updated Local Best
%
[best,Ib]=min(POP(:,end));
if best<=gbest(end)
    gbest=POP(Ib,:);
end
I=find(POP(:,end)<=lbest(:,end));
lbest(I,:)=POP(I,:);
end
% -----------------------------------------------------------------