% %
%
% 
% Project Title: FAStep-Kinematic
% Revised and Merged by: Kaylash Chaudhary
% Project Code from: YOEA112 on Implementation of Firefly Algorithm (FA) in MATLAB
% 
%

clc;
clear;
%close all;

%% Problem Definition
%Obstacles position and size
delete('X.txt');
p1 = 45; p2 = 5; rt =0.2; 
nobs = 10;
o1 =[18, 6,	30, 39, 13, 10.5, 34, 4, 13, 22.5];
o2 = [16, 5, 35.5, 32, 31, 6, 5, 11, 25, 1.5];
ro = [0.4, 0.95, 0.9, 0.05,	1.8, 1,	0.8, 1,	2,	1];

x(1) = 5;  x(2) = 45;  x(3) = 0;  x(4) = 1; x(5) = 1;
source.position(1) = 5;
source.position(2) = 45;
%% Problem Definition

CostFunction=@(x,s, o1,o2,nobs) Objective(x,s,o1,o2,nobs);    

nVar=2;                 % Number of Decision Variables

VarSize=[1 nVar];       % Decision Variables Matrix Size

VarMin=5;             % Decision Variables Lower Bound
VarMax= 45;             % Decision Variables Upper Bound






%% Firefly Algorithm Parameters

MaxIt=100;         % Maximum Number of Iterations

nPop=50;            % Number of Fireflies (Swarm Size)

gamma=1;            % Light Absorption Coefficient

beta0=2;%2;            % Attraction Coefficient Base Value

alpha=0.2;%0.2;   %%%%%have changed       % Mutation Coefficient

alpha_damp=0.8;    % Mutation Coefficient Damping Ratio

delta=0.05*(VarMax-VarMin) ;    % Uniform Mutation Range

m=2;%%% major change.%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%ADDed

if isscalar(VarMin) && isscalar(VarMax)
    dmax = (VarMax-VarMin)*sqrt(nVar);
else
    dmax = norm(VarMax-VarMin);
end

    %Draw obstacle
 t = linspace(0,2*pi,1000);
for i = 1:nobs
ho(i) = fill(ro(i)*cos(t)+o1(i),ro(i)*sin(t)+o2(i),'.'); 
set(ho(i),'MarkerSize',1, 'FaceColor','r');
hold on 
end

hi = fill(0.2*cos(t)+x(1),0.2*sin(t)+x(2),'.'); 
set(hi,'MarkerSize',1, 'FaceColor','r');

ht = fill(0.2*cos(t)+p1,0.2*sin(t)+p2,'.'); 
set(ht,'MarkerSize',1, 'FaceColor','r');

hold on 

h = plot(x(1),x(2),'.','MarkerSize',18);
hf = plot(x(1),x(2),'.','MarkerSize',1); 
      % tolerance = 0.5;
       
       tolerance = 0.1;
 tic;      
distance =0;
 distTP = sqrt((source.position(1) -p1)^2 + ((source.position(2) - p2 )^2));
while distTP > tolerance 
   stepsize1=0.1;
   stepsize = 0.02;
   alpha1 = 0.1;
   alpha2= 0.1;
u1=alpha1*(p1-source.position(1));
u2=alpha2*(p2-source.position(2));

%Kinematic equations
Fx(1) = u1;
Fx(2) = u2;

%RK4 to solve for x(1),x(2):
for I = 1:2
kx1(I) = stepsize * Fx(I); x(I) = x(I) + kx1(I) / 2;
kx2(I) = stepsize * Fx(I); x(I) = x(I) + kx2(I) / 2;
kx3(I) = stepsize * Fx(I); x(I) = x(I) + kx3(I) / 2;
x(I) = x(I) + (kx1(I) + 2 * (kx2(I) + kx3(I)) + stepsize * Fx(I)) / 6;
end


  distance = distance + (sqrt(((source.position(1) - x(1) )^2)+((source.position(2) - x(2) )^2)));     

  source.position(1) = x(1);
  source.position(2) = x(2);
  drawnow;
   set(h,'XData',x(1),'YData',x(2))
%Write the data values to files
   dlmwrite('Y.txt',[x(1),x(2)], 'newline', 'pc', '-append') 
% Draw the path
  X = dlmread('Y.txt');   set(hf,'XData',X(:,1),'YData',X(:,2))

for f = 1:nobs
 cond1 =  ((((o1(f) - source.position(1))^2)+((o2(f) - source.position(2))^2) ));

 
if (cond1 <= (ro(f) + 2)^2)
%% Initialization
temp.position = source.position;
% Empty Firefly Structure
firefly.Step=[];
firefly.Position=[];
firefly.Cost=[];
% source.position(1) = 5;
% source.position(2) = 5;
% temp.position(1) = 5;
% temp.position(2) = 5;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%ADD



empty_individual.Step=[];
empty_individual.Position=[];
empty_individual.Cost=[];

M=repmat(empty_individual,MaxIt,1);

% Initialize Population Array
pop=repmat(firefly,nPop,1);
pop1=repmat(empty_individual,nPop,1);

% Initialize Best Solution Ever Found
BestSol.Cost=inf;
k = 1;
r = 10;
% Create Initial Fireflies
for i=1:nPop

   pop(i).Position=unifrnd(VarMin,VarMax,VarSize);
    pop(i).Cost=CostFunction(pop(i).Position, source.position, o1,o2,nobs);
 
   pop(i).Step=0;
   if pop(i).Cost<=BestSol.Cost
       BestSol=pop(i);
   end
   
end
 

%% Firefly Algorithm Main Loop

for it=1:r
    
    newpop=repmat(firefly,nPop,1);
%    g=it;
    
    for i=1:nPop
        newpop(i).Cost = inf;

        for j=1:nPop
            pop1(i).Step=0;
            if pop(j).Cost <= pop(i).Cost 
                rij=norm(pop(i).Position-pop(j).Position)/dmax;
                 
                beta=beta0*exp(-gamma*rij^m);
                 
                  e=1*randn(VarSize);
                newsol.Step=zeros(VarSize);
                newpop(i).Step=0;
                newsol.Position = pop(j).Position ...
                                + beta*rand(VarSize).*(pop(j).Position-pop(i).Position)...
                                  + alpha*e ; 
                            
                            
                newsol.Position=max(newsol.Position,VarMin);
                newsol.Position=min(newsol.Position,VarMax);
                
                

                newsol.Cost=CostFunction(newsol.Position, source.position, o1,o2, nobs);
                newsol1=newsol;
                 beta=beta0*exp(-gamma*rij^m);
                  e=delta.*unifrnd(-1,+1,VarSize);

                    newsol.Position = pop(i).Position ...
                                + beta*rand(VarSize).*(pop(j).Position-pop(i).Position) ...
                                + alpha*e ; 
                
                newsol.Position=max(newsol.Position,VarMin);
                newsol.Position=min(newsol.Position,VarMax);
                
                newsol.Cost=CostFunction(newsol.Position, source.position, o1,o2, nobs);

                if newsol1.Cost <= newsol.Cost
                    newsol=newsol1;
                end
                if newsol.Cost <= newpop(i).Cost  
                    newpop(i) = newsol;

                    if newpop(i).Cost<=BestSol.Cost
                        BestSol=newpop(i);

                    end                    
                end
                  
                                 
            end    
    
        end

        
    end



    pop=[pop
         newpop]; 
    
    % Sort
    [~, SortOrder]=sort([pop.Cost]);
    pop=pop(SortOrder);
    
    % Truncate
    pop=pop(1:nPop);
    
    % Store Best Cost Ever Found
    BestCost(it)=BestSol.Cost;
    source1.position = BestSol.Position;

     
     A = [temp.position(1) source1.position(1)];
     B = [temp.position(2) source1.position(2)];
     plot(A,B,'*');

    % Damp Mutation Coefficient
    BestSol.Cost = inf;
    alpha = alpha*alpha_damp;

 tolerance = 0.1;
       

 distTP = sqrt((source1.position(1) -temp.position(1))^2 + ((temp.position(2) - source1.position(2) )^2));
while distTP > tolerance 
   stepsize1=0.1;
   stepsize = 0.1;
   alpha1 = 0.1;
   alpha2= 0.1;
u1=alpha1*(source1.position(1)-x(1));
u2=alpha2*(source1.position(2)-x(2));

%Kinematic equations
Fx(1) = u1;
Fx(2) = u2;

%RK4 to solve for x(1),x(2):
for I = 1:2
kx1(I) = stepsize * Fx(I); x(I) = x(I) + kx1(I) / 2;
kx2(I) = stepsize * Fx(I); x(I) = x(I) + kx2(I) / 2;
kx3(I) = stepsize * Fx(I); x(I) = x(I) + kx3(I) / 2;
x(I) = x(I) + (kx1(I) + 2 * (kx2(I) + kx3(I)) + stepsize * Fx(I)) / 6;
end


 distance = distance + (sqrt(((temp.position(1) - x(1) )^2)+((temp.position(2) - x(2) )^2)));     
 disp(['Distance ' num2str(distance) ]);
      temp.position(1) = x(1);
      temp.position(2) = x(2);

  drawnow;
   set(h,'XData',x(1),'YData',x(2))
%Write the data values to files
   dlmwrite('Y.txt',[x(1),x(2)], 'newline', 'pc', '-append') 
% Draw the path
  X = dlmread('Y.txt');   set(hf,'XData',X(:,1),'YData',X(:,2))

   distTP = sqrt((source1.position(1) -x(1))^2 + ((x(2) - source1.position(2) )^2));
 
end 
     temp.position = source1.position; 

 cond1 = ((((o1(f) - source1.position(1))^2)+((o2(f) - source1.position(2))^2) ));

 if  (cond1 >= (ro(f) + 2)^2) 
     it = r+1;
     break;
 end
end


source.position = source1.position;
end

end

distTP = sqrt((source.position(1) -p1)^2 + ((source.position(2) - p2 )^2));
disp(['Distance ' num2str(distance) ]);
end
toc

%% Results
% fprintf(fileID,'%6s %12s\r\n','Run','Fitness');
function z=Objective(x,s,o1,o2, nobs)
    %Target position
p1 = 45; p2 = 5; rt =0.2;




         distFR = (sqrt(((s(1) - x(1))^2)+((s(2) - x(2))^2) )) ;

         
         if distFR > 3
             z = inf;
         else

                    z = 0;
                    for i = 1: nobs
                        z = z + (1/sqrt(((o1(i) - x(1))^2)+((o2(i) - x(2))^2) )); 
                    end 

                    z = 0.01*(z) + 0.01*(sqrt(((p1 - x(1) )^2)+((p2 - x(2) )^2)));

         end   
             


end