clc;
clear;
close all;
delete('XZ.txt');
delete('XY.txt');
delete('X.txt');
p1 = 45; p2 = 45; rt =0.2;
target1 = 45; target2 = 5;
target3 = 5; target4 = 25;
start(1) = 5; start(2) = 45; start(3) = 0;
start1(1) = 45; start1(2) = 25; start1(3) = 0;
nobs = 10;
a = [25, 25, 30, 30,25];
b = [20, 30, 30, 20, 20 ];
%Obstacles position and size
for i = 1:nobs
o1(i) = 40*rand(1,1); 
o2(i) = 40*rand(1,1); 
ro(i)=2*rand(1,1);
end
x(1) = 5;  x(2) = 5;  x(3) = 0;  x(4) = 1; x(5) = 1;
startsource(1) = 5;
startsource(2) = 45;
%% Problem Definition

CostFunction=@(x,s,start,startsource,start1,startsource1,o1,o2,nobs,a ,b) Objective(x,s, start, startsource,start1, startsource1,o1,o2,nobs,a,b);        % Cost Function
CostFunction1=@(x,s,start,startsource,start1, startsource1, o1,o2,nobs,a ,b) Objective1(x,s, start, startsource,start1, startsource1,o1,o2,nobs,a,b); 
CostFunction2=@(x,s,start,startsource,start1,startsource1, o1,o2,nobs,a ,b) Objective2(x,s, start, startsource,start1, startsource1,o1,o2,nobs,a,b); 
nVar=2;             % Number of Decision Variables

VarSize=[1 nVar];   % Variables Matrix Size

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

%% ACOR Parameters

MaxIt=1000;          % Maximum Number of Iterations

nPop=150;            % Population Size (Archive Size)

nSample=10000;         % Sample Size

q=0.5;              % Intensification Factor (Selection Pressure)

zeta=1;             % Deviation-Distance Ratio

%% Initialization

% Create Empty Individual Structure
empty_individual.Position=[];
empty_individual.Cost=[];
source.position(1) = 5;
source.position(2) = 5;
temp.position(1) = 5;
temp.position(2) = 5;

source1.position(1) = 5;
source1.position(2) = 45;
temp1.position(1) = 5;
temp1.position(2) = 45;

source2.position(1) = 45;
source2.position(2) = 25;
temp2.position(1) = 45;
temp2.position(2) = 25;
distance = 0;
% Create Population Matrix
pop=repmat(empty_individual,nPop,1); %first point-mass robot

pop1=repmat(empty_individual,nPop,1);%second point mass robot

pop2=repmat(empty_individual,nPop,1);%third point mass robot
% Initialize Population Members
for i=1:nPop
    
    % Create Random Solution
    
       pop(i).Position=unifrnd(VarMin,VarMax,VarSize);
       pop1(i).Position=unifrnd(VarMin,VarMax,VarSize);
       pop2(i).Position=unifrnd(VarMin,VarMax,VarSize);
    
    
    
    % Evaluation
    pop(i).Cost=CostFunction(pop(i).Position, source.position,pop1(i).Position, source1.position,pop2(i).Position, source2.position, o1,o2,nobs,a,b);
    pop1(i).Cost=CostFunction1(pop(i).Position, source.position,pop1(i).Position, source1.position,pop2(i).Position, source2.position, o1,o2,nobs,a,b);
    pop2(i).Cost=CostFunction2(pop(i).Position, source.position,pop1(i).Position, source1.position, pop2(i).Position, source2.position, o1,o2,nobs,a,b);
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

line(a,b);
%Draw target

ht = plot(rt*cos(t)+p1,rt*sin(t)+p2,'.');set(ht,'MarkerSize',10);
hs = plot(rt*cos(t)+x(1),rt*sin(t)+x(2),'.');set(ht,'MarkerSize',10);
ht1 = plot(rt*cos(t)+target1,rt*sin(t)+target2,'.');set(ht,'MarkerSize',10);
hs1 = plot(rt*cos(t)+start(1),rt*sin(t)+start(2),'.');set(ht,'MarkerSize',10);
ht2 = plot(rt*cos(t)+target3,rt*sin(t)+target4,'.');set(ht,'MarkerSize',10);
hs2 = plot(rt*cos(t)+start1(1),rt*sin(t)+start1(2),'.');set(ht,'MarkerSize',10);
hold on 

h = plot(x(1),x(2),'.','MarkerSize',18);
hf = plot(x(1),x(2),'.','MarkerSize',1); 

h1 = plot(start(1),start(2),'.','MarkerSize',18);
hf1 = plot(start(1),start(2),'.','MarkerSize',1); 

h2 = plot(start(1),start(2),'.','MarkerSize',18);
hf2 = plot(start1(1),start1(2),'.','MarkerSize',1); 
% Sort Population

[~, SortOrder]=sort([pop.Cost]);
pop=pop(SortOrder);

[~, SortOrder1]=sort([pop1.Cost]);
pop1=pop1(SortOrder1);

[~, SortOrder2]=sort([pop1.Cost]);
pop2=pop2(SortOrder2);
% Update Best Solution Ever Found
BestSol=pop(1);
BestSol1=pop1(1);
BestSol2=pop2(1);
% Array to Hold Best Cost Values
BestCost=zeros(MaxIt,1);
BestCost1=zeros(MaxIt,1);
BestCost2=zeros(MaxIt,1);
% Solution Weights
w=1/(sqrt(2*pi)*q*nPop)*exp(-0.5*(((1:nPop)-1)/(q*nPop)).^2);

% Selection Probabilities
p=w/sum(w);


%% ACOR Main Loop

for it=1:MaxIt
    
    % Means
    s=zeros(nPop,nVar);
    s1=zeros(nPop,nVar);
    s2=zeros(nPop,nVar);
    for l=1:nPop
        s(l,:)=pop(l).Position;
        s1(l,:)=pop1(l).Position;
        s2(l,:)=pop2(l).Position;
    end
    
    % Standard Deviations
    sigma=zeros(nPop,nVar);
    sigma1=zeros(nPop,nVar);
    sigma2=zeros(nPop,nVar);
    for l=1:nPop
        D=0;
        D1=0;
        D2=0;
        for r=1:nPop
            D=D+abs(s(l,:)-s(r,:));
            D1=D1+abs(s1(l,:)-s1(r,:));
            D2=D2+abs(s2(l,:)-s2(r,:));
        end
        sigma(l,:)=zeta*D/(nPop-1);
        sigma1(l,:)=zeta*D1/(nPop-1);
        sigma2(l,:)=zeta*D2/(nPop-1);
    end
    
    % Create New Population Array
    newpop=repmat(empty_individual,nSample,1);
    newpop1=repmat(empty_individual,nSample,1);
    newpop2=repmat(empty_individual,nSample,1);
    for t=1:nSample
        
        % Initialize Position Matrix
        newpop(t).Position=zeros(VarSize);
        newpop1(t).Position=zeros(VarSize);
        newpop2(t).Position=zeros(VarSize);
        % Solution Construction
        for i=1:nVar
            
            % Select Gaussian Kernel
            l=RouletteWheelSelection(p);
            l1=RouletteWheelSelection(p);
            l2=RouletteWheelSelection(p);
            % Generate Gaussian Random Variable
            newpop(t).Position(i)=s(l,i)+sigma(l,i)*randn;
            newpop1(t).Position(i)=s1(l1,i)+sigma1(l1,i)*randn;
            newpop2(t).Position(i)=s2(l2,i)+sigma2(l2,i)*randn;
        end
        
        % Evaluation
        newpop(t).Cost=CostFunction(newpop(t).Position, source.position,newpop1(t).Position, source1.position,newpop2(t).Position, source2.position,o1,o2, nobs, a, b);
        newpop1(t).Cost=CostFunction1(newpop(t).Position, source.position,newpop1(t).Position, source1.position,newpop2(t).Position, source2.position,o1,o2, nobs, a, b);
        newpop2(t).Cost=CostFunction2(newpop(t).Position, source.position,newpop1(t).Position, source1.position,newpop2(t).Position, source2.position,o1,o2, nobs, a, b);
    end
    
    % Merge Main Population (Archive) and New Population (Samples)
    pop=[pop
         newpop]; %#ok
    pop1=[pop1
         newpop1]; %#ok
    pop2=[pop2
         newpop2]; %#ok
    % Sort Population
    [~, SortOrder]=sort([pop.Cost]);
    pop=pop(SortOrder);
    
    [~, SortOrder]=sort([pop1.Cost]);
    pop1=pop1(SortOrder);
    
     [~, SortOrder]=sort([pop2.Cost]);
    pop2=pop2(SortOrder);
    % Delete Extra Members
    pop=pop(1:nPop);
    pop1=pop1(1:nPop);
    pop2=pop2(1:nPop);
    % Update Best Solution Ever Found
    BestSol=pop(1);
    BestSol1=pop1(1);
    BestSol2=pop2(1);
    % Store Best Cost
    BestCost(it)=BestSol.Cost;
     BestCost1(it)=BestSol1.Cost;
     BestCost2(it)=BestSol2.Cost;
    % Show Iteration Information
   source.position = BestSol.Position;
   source1.position = BestSol1.Position;
   source2.position = BestSol2.Position;
  
 %   disp(['Iteration ' num2str(source.position) ': Best Cost = ' num2str(BestCost(it))]);
   
     
     A = [temp.position(1) source.position(1)];
     B = [temp.position(2) source.position(2)];
     plot(A,B,'*');
      C = [temp1.position(1) source1.position(1)];
     D = [temp1.position(2) source1.position(2)];
     plot(C,D,'.');
     
     E = [temp2.position(1) source2.position(1)];
     F = [temp2.position(2) source2.position(2)];
     plot(E,F,'+');
 
     drawnow;
     distance = distance + (sqrt(((temp.position(1) - source.position(1) )^2)+((temp.position(2) - source.position(2) )^2)));
       disp(['Distance ' num2str(distance) ]);

%point mass one ODE
     ttheta = 0;     
       tolerance = 0.9;
       
       xi = atan2(source.position(2)-x(2),source.position(1)-x(1)) -x(3); 
 % xi = 0;
 distTP = sqrt((source.position(1) -temp.position(1))^2 + ((temp.position(2) - source.position(2) )^2));
while distTP > tolerance 
   stepsize1=0.1;
   stepsize = 0.1;
   
Fxi=((source.position(2)-x(2))*cos(x(3))-(source.position(1)-x(1))*sin(x(3)))/(sqrt((source.position(2)-x(2))^2+(source.position(1)-x(1))^2)+0.01)-xi;%;

%RK4 to solve for xi:
ky1 = stepsize1 * Fxi; xi = xi + ky1/ 2;
ky2 = stepsize1 * Fxi; xi = xi + ky2/ 2;
ky3 = stepsize1 * Fxi; xi = xi + ky3/ 2;
xi = xi + (ky1 + 2 * (ky2 + ky3) + stepsize1 * Fxi) / 6;  
 
Phi=7/9* atan(xi);
v=0.05*((source.position(1)-x(1))^2+(source.position(2)-x(2))^2)^0.5;

%ODE
Fx(1) =v*cos(x(3)) - v*tan(Phi)*sin(x(3))/2;
Fx(2) = v*sin(x(3)) + v*tan(Phi)*cos(x(3))/2;
Fx(3) = v*tan(Phi);%/Ll;

%RK4 to solve the ODE
for I = 1:3
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

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

   distTP = sqrt((source.position(1) -x(1))^2 + ((x(2) - source.position(2) )^2));
  temp.position = source.position; 
%  disp(x(3))
end
 
 %point mass two ODE

       
       starti = atan2(source1.position(2)-start(2),source1.position(1)-start(1)) -start(3); 
 % xi = 0;
 distTP1 = sqrt((source1.position(1) -temp1.position(1))^2 + ((temp1.position(2) - source1.position(2) )^2));
while distTP1 > tolerance 
   stepsize1=0.1;
   stepsize = 0.1;
   
Fstarti=((source1.position(2)-start(2))*cos(start(3))-(source1.position(1)-start(1))*sin(start(3)))/(sqrt((source1.position(2)-start(2))^2+(source1.position(1)-start(1))^2)+0.01)-starti;%;

%RK4 to solve for xi:
starty1 = stepsize1 * Fstarti; starti = starti + starty1/ 2;
starty2 = stepsize1 * Fstarti; starti = starti + starty2/ 2;
starty3 = stepsize1 * Fstarti; starti = starti + starty3/ 2;
starti = starti + (starty1 + 2 * (starty2 + starty3) + stepsize1 * Fstarti) / 6;  
 
Phi1=7/9* atan(starti);
v1=0.05*((source1.position(1)-start(1))^2+(source1.position(2)-start(2))^2)^0.5;

%ODE
Fstart(1) =v1*cos(start(3)) - v1*tan(Phi1)*sin(start(3))/2;
Fstart(2) = v1*sin(start(3)) + v1*tan(Phi1)*cos(start(3))/2;
Fstart(3) = v1*tan(Phi1);%/Ll;

%RK4 to solve the ODE
for I = 1:3
startx1(I) = stepsize * Fstart(I); start(I) = start(I) + startx1(I) / 2;
startx2(I) = stepsize * Fstart(I); start(I) = start(I) + startx2(I) / 2;
startx3(I) = stepsize * Fstart(I); start(I) = start(I) + startx3(I) / 2;
start(I) = start(I) + (startx1(I) + 2 * (startx2(I) + startx3(I)) + stepsize * Fstart(I)) / 6;
end

      
      temp1.position(1) = start(1);
      temp1.position(2) = start(2);
      theta1 = Fstart(3);
     
  temptheta1 = theta1;

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

   distTP1 = sqrt((source1.position(1) -start(1))^2 + ((start(2) - source1.position(2) )^2));
  temp1.position = source1.position; 
end 


%point mass three ODE

       
       start1i = atan2(source2.position(2)-start1(2),source2.position(1)-start1(1)) -start1(3); 
 % xi = 0;
 distTP2 = sqrt((source2.position(1) -temp2.position(1))^2 + ((temp2.position(2) - source2.position(2) )^2));
while distTP2 > tolerance 
   stepsize1=0.1;
   stepsize = 0.1;
   
Fstart1i=((source2.position(2)-start1(2))*cos(start1(3))-(source2.position(1)-start1(1))*sin(start1(3)))/(sqrt((source2.position(2)-start1(2))^2+(source2.position(1)-start1(1))^2)+0.01)-start1i;%;

%RK4 to solve for xi:
start1y1 = stepsize1 * Fstart1i; start1i = start1i + start1y1/ 2;
start1y2 = stepsize1 * Fstart1i; start1i = start1i + start1y2/ 2;
start1y3 = stepsize1 * Fstart1i; start1i = start1i + start1y3/ 2;
start1i = start1i + (start1y1 + 2 * (start1y2 + start1y3) + stepsize1 * Fstart1i) / 6;  
 
Phi2=7/9* atan(start1i);
v2=0.05*((source2.position(1)-start1(1))^2+(source2.position(2)-start1(2))^2)^0.5;

%ODE
Fstart1(1) =v2*cos(start1(3)) - v2*tan(Phi2)*sin(start1(3))/2;
Fstart1(2) = v2*sin(start1(3)) + v2*tan(Phi2)*cos(start1(3))/2;
Fstart1(3) = v2*tan(Phi2);%/Ll;

%RK4 to solve the ODE
for I = 1:3
start1x1(I) = stepsize * Fstart1(I); start1(I) = start1(I) - start1x1(I) / 2;
start1x2(I) = stepsize * Fstart1(I); start1(I) = start1(I) - start1x2(I) / 2;
start1x3(I) = stepsize * Fstart1(I); start1(I) = start1(I) - start1x3(I) / 2;
start1(I) = start1(I) - (start1x1(I) + 2 * (start1x2(I) - start1x3(I)) - stepsize * Fstart1(I)) / 6;
end

      
      temp2.position(1) = start1(1);
      temp2.position(2) = start1(2);
      theta2 = Fstart1(3);
     
  temptheta2 = theta2;

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

   distTP2 = sqrt((source2.position(1) -start1(1))^2 + ((start1(2) - source2.position(2) )^2));
  temp2.position = source2.position; 
end 

end

%% Results

figure;
%plot(BestCost,'LineWidth',2);
semilogy(BestCost,'LineWidth',2);
xlabel('Iteration');
ylabel('Best Cost');
grid on;


function z=Objective(x,s,start, startsource,start1, startsource1, o1,o2, nobs, a, b)
    %Target position
p1 = 45; p2 = 45; rt =0.2;
target1 =45; target2=5;

u = linspace(a(1),a(2),4);
y = linspace(b(1),b(2),4);

e = linspace(a(2),a(3),4);
f = linspace(b(2),b(3),4);

g = linspace(a(3),a(4),4);
h = linspace(b(3),b(4),4);

m = linspace(a(4),a(5),4);
n = linspace(b(4),b(5),4);

         distFR = (sqrt(((s(1) - x(1))^2)+((s(2) - x(2))^2) )) ;
         if distFR > 3 
             z = inf;
         else

                    z = 0;
                    c = 0;
                    d = 0;
                    for i = 1: nobs
                        z = z + (1/sqrt(((o1(i) - x(1))^2)+((o2(i) - x(2))^2) )) ;
                    end 
                    for j = 1:4
                        c = c + (1/sqrt(((u(j) - x(1))^2)+((y(j) - x(2))^2) ))+(1/sqrt(((e(j) - x(1))^2)+((f(j) - x(2))^2) ))+(1/sqrt(((g(j) - x(1))^2)+((h(j) - x(2))^2) ))+(1/sqrt(((m(j) - x(1))^2)+((n(j) - x(2))^2) )); %use minimum distance technique
                    end 
                 
                     d = 0.1*(1/sqrt(((x(1) - startsource(1))^2)+((x(2) - startsource(2))^2) ))+ 0.1*(1/sqrt(((x(1) - startsource1(1))^2)+((x(2) - startsource1(2))^2) ));

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

         end   
             


end

function z=Objective1(x,s,start, startsource,start1, startsource1, o1,o2, nobs, a, b)
    %Target position
p1 = 25; p2 = 25; rt =0.2;
target1 =45; target2=5;

u = linspace(a(1),a(2),4);
y = linspace(b(1),b(2),4);

e = linspace(a(2),a(3),4);
f = linspace(b(2),b(3),4);

g = linspace(a(3),a(4),4);
h = linspace(b(3),b(4),4);

m = linspace(a(4),a(5),4);
n = linspace(b(4),b(5),4);

         distFR1 = (sqrt(((startsource(1) - start(1))^2)+((startsource(2) - start(2))^2) )) ;

         if distFR1 > 3
             z = inf;
         else

                    z = 0;
                    c = 0;
                    d = 0;
                    for i = 1: nobs
                        z = z + (1/sqrt(((o1(i) - start(1))^2)+((o2(i) - start(2))^2) ));
                    end 
                    for j = 1:4
                        c = c + (1/sqrt(((u(j) - start(1))^2)+((y(j) - start(2))^2) ))+(1/sqrt(((e(j) - start(1))^2)+((f(j) - start(2))^2) ))+(1/sqrt(((g(j) - start(1))^2)+((h(j) - start(2))^2) ))+(1/sqrt(((m(j) - start(1))^2)+((n(j) - start(2))^2) )); %use minimum distance technique

                    end 
                   
                     d = 0.2*(1/sqrt(((startsource1(1) - start(1))^2)+ ((startsource1(2) - start(2))^2) ))+ 0.2*(1/sqrt(((s(1) - start(1))^2)+((s(2) - start(2))^2) ));

                    z =  d+ 0.1*(z) + 0.1*(c)+  0.01*(sqrt(((target1 - start(1) )^2)+((target2 - start(2) )^2)));

         end   
             


end

function z=Objective2(x,s,start, startsource,start1, startsource1, o1,o2, nobs, a, b)
    %Target position
p1 = 45; p2 = 45; rt =0.2;
target3 =5; target4=25;

u = linspace(a(1),a(2),4);
y = linspace(b(1),b(2),4);

e = linspace(a(2),a(3),4);
f = linspace(b(2),b(3),4);

g = linspace(a(3),a(4),4);
h = linspace(b(3),b(4),4);

m = linspace(a(4),a(5),4);
n = linspace(b(4),b(5),4);

         distFR2 = (sqrt(((startsource1(1) - start1(1))^2)+((startsource1(2) - start1(2))^2) )) ;

         if distFR2 > 3
             z = inf;
         else

                    z = 0;
                    c = 0;
                    d = 0;
                    for i = 1: nobs
                        z = z + (1/sqrt(((o1(i) - start1(1))^2)+((o2(i) - start1(2))^2) ));
                    end 
                    for j = 1:4
                        c = c + (1/sqrt(((u(j) - start1(1))^2)+((y(j) - start1(2))^2) ))+(1/sqrt(((e(j) - start1(1))^2)+((f(j) - start1(2))^2) ))+(1/sqrt(((g(j) - start1(1))^2)+((h(j) - start1(2))^2) ))+(1/sqrt(((m(j) - start1(1))^2)+((n(j) - start1(2))^2) )); %use minimum distance technique
 
                    end 
                    d = 0.1*(1/sqrt(((start1(1) - startsource(1))^2)+((start1(2) - startsource(2))^2) ))+ 0.1*(1/sqrt(((s(1) - start1(1))^2)+((s(2) - start1(2))^2) ));
                    z = d + 0.1*(z) + 0.1*(c)+  0.01*(sqrt(((target3 - start1(1) )^2)+((target4 - start1(2) )^2)));

         end   
             


end