%Introduction
%This file include the implementation of ACS algorithem for PID tuning.
%This file only include pheromone intensity ,no visibility.

%Part 1: Initialization.
%Step 1: For a given control system with a PID controller, compute the PID 
%        parameters Kp, Ki, Kd using the Ziegler-Nichols tuning formula, and 
%        compute the system's ITAE performance index Itae.
%        In this case, The system model is 1/(s^2+1.6s+1). Using the Ziegler-Nichols
%        tuning formula reach the PID parameters as following: Opt_Kp=3.5 Opt_Ki=2.9167 
%        Opt_Kd=1.05   Opt_Itae=1.2204
warning off;
clear;
clc;
for ASCYCLES=1:1
    
Opt_Kp=3.5; Opt_Ki=2.9167; Opt_Kd=1.05; Opt_Itae=0.7266;

%Part 2: Define global variable

%Step 1: For implementation of ACS algorithem, define some global variable, such as:
%        ANTS(the number of ants)   Evaporation(coefficient which represents the evaporation
%        of pheromone substance     Cycles(the maximum number of iterated cycles)
ANTS=50; Evaporation=0.1; CYCLES=100;
%Step 2: Grid_X(the number of x coordinate)     Grid_Y(the number of y coordinate)
Grid_X=15;  Grid_Y=10;  First_Grid_Y=5; Sixth_Grid_Y=3; Eleventh_Grid_Y=3; 
Grid_Y_Array=[5,10,10,10,10,3,10,10,10,10,3,10,10,10,10];
%Step 3: FirstLine(the line which connect two spot between the original spot and Grid_X=1)
%        Lines(the lines which connect two spot in the Grid between Grid_X=1 and Grid_X=15)
%        Given initial pheromone intensity on lines.
FirstLine=ones(1,First_Grid_Y);  SecondLine=ones(First_Grid_Y,Grid_Y);    ThirdLines=ones(3,Grid_Y,Grid_Y);   
SixthLine=ones(Grid_Y,Sixth_Grid_Y);    SeventhLine=ones(Sixth_Grid_Y,Grid_Y);  EightLines=ones(3,Grid_Y,Grid_Y);
EleventhLine=ones(Grid_Y,Eleventh_Grid_Y);  TwelvethLine=ones(Eleventh_Grid_Y,Grid_Y);  ThirteenthLines=ones(3,Grid_Y,Grid_Y);
%Step 4: Paths(the paths which all ant passed)  Opt_Path(the optimal Path has been found)
Paths=zeros(ANTS,Grid_X);   Opt_Path=[3, 5, 0, 0, 0, 2, 9, 1, 6, 7, 1, 0, 5, 0, 0];

%Part 3: Beginning of ACS algorithem

%Step 1: Initialization of cycles. 
%        cycle(cycle counter)   Cof_Pheromone(the relative importance of pheromone intensity)
cycle=0; Cof_Pheromone=1;
%Step 2: Beginning of cycles
while cycle<CYCLES
%while Opt_Itae>0.119
    %Step 3: Compute the possibility from the original spot to spots on Grid_X=1.
    Total_Possi=0;
    for i=1:First_Grid_Y;
        Total_Possi=Total_Possi+FirstLine(1,i)^Cof_Pheromone;
    end;
    for i=1:First_Grid_Y;
        First_Possi(i)=FirstLine(1,i)^Cof_Pheromone/Total_Possi;
    end;
    %Step 4: The first step of ants from ariginal to Grid_X=1
    NextSpots=WheelSel(First_Possi,ANTS);  %The Roulette Wheel Selection method
    for i=1:ANTS;
         Paths(i,1)=NextSpots(i)-1;
    end;
    %Step 5: The next Grid_X-1 step of ants from Grid_X=1 ...Grid_X=15
    for i=1:Grid_X-1;%For every line from line 1 to line Grid_X-1
        for j=1:Grid_Y_Array(i);%For every point on start line 
            Total_Possi=0;
            for k=1:Grid_Y_Array(i+1);%Compute the possibility from this spot to spots on next line
                if i==1;
                    Total_Possi=Total_Possi+SecondLine(j,k)^Cof_Pheromone;
                elseif (i>1)&(i<5);
                    Total_Possi=Total_Possi+ThirdLines(i-1,j,k)^Cof_Pheromone;
                elseif i==5;
                    Total_Possi=Total_Possi+SixthLine(j,k)^Cof_Pheromone;
                elseif i==6;
                    Total_Possi=Total_Possi+SeventhLine(j,k)^Cof_Pheromone;
                elseif (i>6)&(i<10);
                    Total_Possi=Total_Possi+EightLines(i-6,j,k)^Cof_Pheromone;
                elseif i==10;
                    Total_Possi=Total_Possi+EleventhLine(j,k)^Cof_Pheromone;
                elseif i==11;
                    Total_Possi=Total_Possi+TwelvethLine(j,k)^Cof_Pheromone;
                elseif (i>11)&(i<15);
                    Total_Possi=Total_Possi+ThirteenthLines(i-11,j,k)^Cof_Pheromone;
                end;
            end;
            for k=1:Grid_Y_Array(i+1);
                if i==1;
                    Other_Possi(k)=SecondLine(j,k)^Cof_Pheromone/Total_Possi;
                elseif (i>1)&(i<5);
                    Other_Possi(k)=ThirdLines(i-1,j,k)^Cof_Pheromone/Total_Possi;
                elseif i==5;
                    Sixth_Possi(k)=SixthLine(j,k)^Cof_Pheromone/Total_Possi;
                elseif i==6;
                    Other_Possi(k)=SeventhLine(j,k)^Cof_Pheromone/Total_Possi;
                elseif (i>6)&(i<10);
                    Other_Possi(k)=EightLines(i-6,j,k)^Cof_Pheromone/Total_Possi;
                elseif i==10;
                    Eleventh_Possi(k)=EleventhLine(j,k)^Cof_Pheromone/Total_Possi;
                elseif i==11;
                    Other_Possi(k)=TwelvethLine(j,k)^Cof_Pheromone/Total_Possi;
                elseif (i>11)&(i<15);
                    Other_Possi(k)=ThirteenthLines(i-11,j,k)^Cof_Pheromone/Total_Possi;
                end;
            end;
            count=0;
            for k=1:ANTS;%Find ants which on this spot 
                if Paths(k,i)==(j-1);
                    count=count+1;
                    position(count)=k;
                end;
            end;
            %NextPosition=zeros(count); %Compute the next spot on next line the ants on this spot will be
            if i==5;
                NextPosition=WheelSel(Sixth_Possi,count);
            elseif i==10;
                NextPosition=WheelSel(Eleventh_Possi,count);    
            else                    
                NextPosition=WheelSel(Other_Possi,count);
            end;
            for k=1:count;
                Paths(position(k),(i+1))=NextPosition(k)-1;
            end;
        end;%end for j
    end;%end for i
    %Step 6: Change paths which ants passed into PID parameters Kp Ki Kd
    Kps=zeros(ANTS); Kis=zeros(ANTS); Kds=zeros(ANTS);
    [Kps,Kis,Kds]=PathToPID(Paths,ANTS);
    %Step 7: Execute control system simulation by the PID parameters and find optimal performance index
    for i=1:ANTS;
        PID_KP=Kps(i);  PID_KI=Kis(i);  PID_KD=Kds(i);    
        sim('case1_pid');
        k=length(PID_ITAE);
        ITAES(i)=PID_ITAE(k);
    end;
    [minn,OptAnt]=min(ITAES);%Find the optimal performance index and optimal path

    if Opt_Itae>ITAES(OptAnt);
        Opt_Itae=ITAES(OptAnt);  Opt_Kp=Kps(OptAnt);   Opt_Ki=Kis(OptAnt);   Opt_Kd=Kds(OptAnt);    
        for i=1:Grid_X;
            Opt_Path(i)=Paths(OptAnt,i);
        end;
    end;
    %Step 8: Update pheromone trails by applying the rule:
    %        Update FirstLine's pheromone trails
    Q_Constant=Opt_Itae;%1.2204;
    Min_Pheromone=0;
    Max_Pheromone=100;
    for i=1:First_Grid_Y;
        FirstLine(1,i)=(1-Evaporation)*FirstLine(1,i);
    end;
    for i=1:ANTS;
        k=Paths(i,1)+1;
        FirstLine(1,k)=FirstLine(1,k)+Q_Constant/ITAES(i);
    end;
    %       Limit the pheromone intensity on every line in a range
    for i=1:First_Grid_Y;
        if FirstLine(1,i)>Max_Pheromone;
            FirstLine(1,i)=Max_Pheromone;
        end;
        if FirstLine(1,i)<Min_Pheromone;
            FirstLine(1,i)=Min_Pheromone;
        end;
    end;
    %       Update other Line's pheromone trails
    for i=1:Grid_X-1;
        for j=1:Grid_Y_Array(i);
            for k=1:Grid_Y_Array(i+1);
                if i==1;
                    SecondLine(j,k)=(1-Evaporation)*SecondLine(j,k);
                elseif (i>1)&(i<5);
                    ThirdLines(i-1,j,k)=(1-Evaporation)*ThirdLines(i-1,j,k);
                elseif i==5;
                    SixthLine(j,k)=(1-Evaporation)*SixthLine(j,k);
                elseif i==6;
                    SeventhLine(j,k)=(1-Evaporation)*SeventhLine(j,k);
                elseif (i>6)&(i<10);
                    EightLines(i-6,j,k)=(1-Evaporation)*EightLines(i-6,j,k);
                elseif i==10;
                    EleventhLine(j,k)=(1-Evaporation)*EleventhLine(j,k);
                elseif i==11;
                    TwelvethLine(j,k)=(1-Evaporation)*TwelvethLine(j,k);
                elseif (i>11)&(i<15);
                    ThirteenthLines(i-11,j,k)=(1-Evaporation)*ThirteenthLines(i-11,j,k);
                end;
            end;
        end;
    end;
    for i=1:Grid_X-1;
        for m=1:ANTS;
            j=Paths(m,i)+1; k=Paths(m,i+1)+1;
            if i==1;
                SecondLine(j,k)=SecondLine(j,k)+Q_Constant/ITAES(m);
            elseif (i>1)&(i<5);
                ThirdLines(i-1,j,k)=ThirdLines(i-1,j,k)+Q_Constant/ITAES(m);
            elseif i==5;
                SixthLine(j,k)=SixthLine(j,k)+Q_Constant/ITAES(m);
            elseif i==6;
                SeventhLine(j,k)=SeventhLine(j,k)+Q_Constant/ITAES(m);
            elseif (i>6)&(i<10);
                EightLines(i-6,j,k)=EightLines(i-6,j,k)+Q_Constant/ITAES(m);
            elseif i==10;
                EleventhLine(j,k)=EleventhLine(j,k)+Q_Constant/ITAES(m);
            elseif i==11;
                TwelvethLine(j,k)=TwelvethLine(j,k)+Q_Constant/ITAES(m);
            elseif (i>11)&(i<15);
                ThirteenthLines(i-11,j,k)=ThirteenthLines(i-11,j,k)+Q_Constant/ITAES(m);
            end;
        end;
    end;
    %       Limit the pheromone intensity on every line in a range
    for i=1:Grid_X-1;
        for j=1:Grid_Y_Array(i);
            for k=1:Grid_Y_Array(i+1);
                if i==1;
                    if SecondLine(j,k)>Max_Pheromone
                        SecondLine(j,k)=Max_Pheromone;
                    end;
                    if SecondLine(j,k)<Min_Pheromone;
                        SecondLine(j,k)=Min_Pheromone;
                    end;
                elseif (i>1)&(i<5);
                    if ThirdLines(i-1,j,k)>Max_Pheromone
                        ThirdLines(i-1,j,k)=Max_Pheromone;
                    end;
                    if ThirdLines(i-1,j,k)<Min_Pheromone;
                        ThirdLines(i-1,j,k)=Min_Pheromone;
                    end;
                elseif i==5;
                    if SixthLine(j,k)>Max_Pheromone
                        SixthLine(j,k)=Max_Pheromone;
                    end;
                    if SixthLine(j,k)<Min_Pheromone;
                        SixthLine(j,k)=Min_Pheromone;
                    end;
                elseif i==6;
                    if SeventhLine(j,k)>Max_Pheromone
                        SeventhLine(j,k)=Max_Pheromone;
                    end;
                    if SeventhLine(j,k)<Min_Pheromone;
                        SeventhLine(j,k)=Min_Pheromone;
                    end;
                elseif (i>6)&(i<10);
                    if EightLines(i-6,j,k)>Max_Pheromone
                        EightLines(i-6,j,k)=Max_Pheromone;
                    end;
                    if EightLines(i-6,j,k)<Min_Pheromone;
                        EightLines(i-6,j,k)=Min_Pheromone;
                    end;
                elseif i==10;
                    if EleventhLine(j,k)>Max_Pheromone
                        EleventhLine(j,k)=Max_Pheromone;
                    end;
                    if EleventhLine(j,k)<Min_Pheromone;
                        EleventhLine(j,k)=Min_Pheromone;
                    end;
                elseif i==11;
                    if TwelvethLine(j,k)>Max_Pheromone
                        TwelvethLine(j,k)=Max_Pheromone;
                    end;
                    if TwelvethLine(j,k)<Min_Pheromone;
                        TwelvethLine(j,k)=Min_Pheromone;
                    end;
                elseif (i>11)&(i<15);
                    if ThirteenthLines(i-11,j,k)>Max_Pheromone
                        ThirteenthLines(i-11,j,k)=Max_Pheromone;
                    end;
                    if ThirteenthLines(i-11,j,k)<Min_Pheromone;
                        ThirteenthLines(i-11,j,k)=Min_Pheromone;
                    end;
                end;
            end;
        end;
    end;
    %Step 9: cycle plus 1 and go to next cycle
    cycle=cycle+1;
    %Step 10: Print the optimal performance index and parameters
    Opt_Itae;
    cycle
end;%end of while


%Part 4: Print the optimal result
 PID_KP=Opt_Kp;  PID_KI=Opt_Ki;  PID_KD=Opt_Kd;    
 sim('case1_pid');
 k=length(PID_ITAE);
 Opt_Itae=PID_ITAE(k);
 [Mo,tr,ts,ess]=performance(ScopeData.time,ScopeData.signals.values);
 Opt_Result=[Opt_Kp, Opt_Ki, Opt_Kd, Opt_Itae, Mo, tr, ts, ess];
 
 ASCYCLES
 ASPIDS(ASCYCLES,:)=[Opt_Kp,Opt_Ki,Opt_Kd];
 ASITAES(ASCYCLES)=Opt_Itae;
end;