clc;
clear;
close all;
delete('XYZ.txt');
p1 = 45; p2 = 5; rt =0.2; 
nobs = 10;
a = [25, 25];
b = [30, 15];
%Obstacles position and size
for i = 1:nobs
o1(i) = 50*rand(1,1); 
o2(i) = 50*rand(1,1); 
ro(i)=2*rand(1,1);
end
x(1) = 5;  x(2) = 45;  x(3) = 0;  x(4) = 0; x(5) = 1;
Ll = 1.5;Lw = 0.7; Lt=3; epsilon1 = .2; epsilon2 = .2;
vmax = 3; pmin = Ll / tan(80 * pi / 180);
Pht = x(3);
%% Problem Definition

CostFunction=@(x,s, o1,o2,nobs,a ,b) Objective(x,s,o1,o2,nobs,a,b);        % Cost Function

nVar=2;             % Number of Decision Variables

VarSize=[1 nVar];   % Variables Matrix Size

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

%% ACOR Parameters

MaxIt=1000;          % Maximum Number of Iterations

nPop=300;            % 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) = 45;
temp.position(1) = 5;
temp.position(2) = 45;
distance = 0;
% Create Population Matrix
pop=repmat(empty_individual,nPop,1);

% Initialize Population Members
for i=1:nPop
    
    % Create Random Solution
    
       pop(i).Position=unifrnd(VarMin,VarMax,VarSize);
  
    
    
    % Evaluation
    pop(i).Cost=CostFunction(pop(i).Position, source.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);
hold on 

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

t1 = linspace(-1,1);c=0.75;
t3 = linspace(-1,1);
hm = plot(x(1)-c*cos(x(3)) + (Ll+c)*t1*cos(x(3)),x(2)-c*sin(x(3)) + (Ll+c)*t1*sin(x(3)),'.'); 
hf = plot(x(1) + Ll*cos(x(3)) + Lw*(t1)*sin(x(3)),x(2) + Ll*sin(x(3)) - Lw*(t1)*cos(x(3)),'.'); 
hr = plot(x(1) - Ll*cos(x(3)) - Lw*(t1)*sin(x(3)),x(2) - Ll*sin(x(3)) + Lw*(t1)*cos(x(3)),'.');
hrl = plot(x(1) - Ll*cos(x(3)) - Lw*sin(x(3))+ 0.5*Lw*t1*cos(x(3)),x(2) - Ll*sin(x(3)) + Lw*cos(x(3))+ 0.5*Lw*t1*sin(x(3)),'.');
hrr = plot(x(1) - Ll*cos(x(3)) + Lw*sin(x(3))+ 0.5*Lw*t1*cos(x(3)),x(2) - Ll*sin(x(3)) - Lw*cos(x(3))+ 0.5*Lw*t1*sin(x(3)),'.');
hfl = plot(x(1) + Ll*cos(x(3)) + Lw*sin(x(3))+ 0.5*Lw*t1*cos(x(3)),x(2) + Ll*sin(x(3)) - Lw*cos(x(3))+ 0.5*Lw*t1*sin(x(3)),'.');
hfr = plot(x(1) + Ll*cos(x(3)) - Lw*sin(x(3))+ 0.5*Lw*t1*cos(x(3)),x(2) + Ll*sin(x(3)) + Lw*cos(x(3))+ 0.5*Lw*t1*sin(x(3)),'.');
hlbox = plot(x(1) - 1.5*Lw*sin(x(3)) + 1.5*Ll*t1*cos(x(3)),x(2)+ 1.5*Lw*cos(x(3)) + 1.5*Ll*t1*sin(x(3)),'.'); 
hrbox = plot(x(1) + 1.5*Lw*sin(x(3)) + 1.5*Ll*t1*cos(x(3)),x(2)- 1.5*Lw*cos(x(3)) + 1.5*Ll*t1*sin(x(3)),'.'); 
fbox = plot(x(1) + 1.5*Ll*cos(x(3)) + 1.5*Lw*(t1)*sin(x(3)),x(2) + 1.5*Ll*sin(x(3)) - 1.5*Lw*(t1)*cos(x(3)),'.'); 
rbox = plot(x(1) - 1.5*Ll*cos(x(3)) - 1.5*Lw*(t1)*sin(x(3)),x(2) - 1.5*Ll*sin(x(3)) + 1.5*Lw*(t1)*cos(x(3)),'.');
set(hm,'MarkerSize',3, 'Color','k'); set(hf,'MarkerSize',3, 'Color','k'); set(hr,'MarkerSize',3, 'Color','k');
set(hrl,'MarkerSize',5, 'Color','k');set(hrr,'MarkerSize',5, 'Color','k');set(hfl,'MarkerSize',5, 'Color','k');set(hfr,'MarkerSize',5, 'Color','k');
set(hlbox,'MarkerSize',4, 'Color','k');set(hrbox,'MarkerSize',4, 'Color','k');set(fbox,'MarkerSize',4, 'Color','k');set(rbox,'MarkerSize',4, 'Color','k');

%Draw the trailor
t2=linspace(0,1);
t3 = linspace(-0.5,1);
hmt = plot(x(1) - (Ll+2*c)*cos(x(3))-(Lt)*t2*cos(x(4)),x(2) - (Ll+2*c)*sin(x(3))-(Lt)*t2*sin(x(4)),'.'); 
hrt = plot(x(1) - (Ll+2*c)*cos(x(3))-(Lt)*cos(x(4)) - Lw*(t1)*sin(x(4)),x(2) - (Ll+2*c)*sin(x(3))-(Lt)*sin(x(4))+ Lw*(t1)*cos(x(4)),'.');
lt = plot(x(1) - (Ll+2*c)*cos(x(3))-(Lt)*cos(x(4)) - Lw*(1)*sin(x(4))+ 0.5*Lw*t1*cos(x(4)),x(2) - (Ll+2*c)*sin(x(3))-(Lt)*sin(x(4))+ Lw*(1)*cos(x(4))+ 0.5*Lw*t1*sin(x(4)),'.');
rt = plot(x(1) - (Ll+2*c)*cos(x(3))-(Lt)*cos(x(4)) - Lw*(-1)*sin(x(4))+ 0.5*Lw*t1*cos(x(4)),x(2) - (Ll+2*c)*sin(x(3))-(Lt)*sin(x(4))+ Lw*(-1)*cos(x(4))+ 0.5*Lw*t1*sin(x(4)),'.');
trbox = plot(x(1) - (Ll+2*c)*cos(x(3))-(Lt+0.25*Lt)*cos(x(4)) - (1.5*Lw)*(t1)*sin(x(4)),x(2) - (Ll+2*c)*sin(x(3))-(Lt+0.25*Lt)*sin(x(4))+ (1.5*Lw)*(t1)*cos(x(4)),'.');
tfbox = plot(x(1) - (Ll+2*c)*cos(x(3))-(Lt-0.25*Lt)*cos(x(4)) - (1.5*Lw)*(t1)*sin(x(4)),x(2) - (Ll+2*c)*sin(x(3))-(Lt-0.25*Lt)*sin(x(4))+ (1.5*Lw)*(t1)*cos(x(4)),'.');
rrbox = plot(x(1) - (Ll+2*c)*cos(x(3))-(Lt)*cos(x(4)) - (1.5*Lw)*(1)*sin(x(4))+ 1*(2*Lw)*t3*cos(x(4)),x(2) - (Ll+2*c)*sin(x(3))-(Lt)*sin(x(4))+ (1.5*Lw)*(1)*cos(x(4))+ 1*(2*Lw)*t3*sin(x(4)),'.');
llbox = plot(x(1) - (Ll+2*c)*cos(x(3))-(Lt)*cos(x(4)) - (1.5*Lw)*(-1)*sin(x(4))+ 15*(2*Lw)*t3*cos(x(4)),x(2) - (Ll+2*c)*sin(x(3))-(Lt)*sin(x(4))+ (1.5*Lw)*(-1)*cos(x(4))+ 1*(2*Lw)*t3*sin(x(4)),'.');
set(hmt,'MarkerSize',3, 'Color','k');set(hrt,'MarkerSize',3, 'Color','k');set(lt,'MarkerSize',5, 'Color','k');set(rt,'MarkerSize',5, 'Color','k'); 
set(trbox,'MarkerSize',4, 'Color','k');set(tfbox,'MarkerSize',4, 'Color','k');set(rrbox,'MarkerSize',4, 'Color','k');set(llbox,'MarkerSize',4, 'Color','k');

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

% Update Best Solution Ever Found
BestSol=pop(1);

% Array to Hold Best Cost Values
BestCost=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);
    for l=1:nPop
        s(l,:)=pop(l).Position;
    end
    
    % Standard Deviations
    sigma=zeros(nPop,nVar);
    for l=1:nPop
        D=0;
        for r=1:nPop
            D=D+abs(s(l,:)-s(r,:));
        end
        sigma(l,:)=zeta*D/(nPop-1);
    end
    
    % Create New Population Array
    newpop=repmat(empty_individual,nSample,1);
    for t=1:nSample
        
        % Initialize Position Matrix
        newpop(t).Position=zeros(VarSize);
        
        % Solution Construction
        for i=1:nVar
            
            % Select Gaussian Kernel
            l=RouletteWheelSelection(p);
            
            % Generate Gaussian Random Variable
            newpop(t).Position(i)=s(l,i)+sigma(l,i)*randn;
            
        end
        
        % Evaluation
        newpop(t).Cost=CostFunction(newpop(t).Position, source.position,o1,o2, nobs, a, b);
        
    end
    
    % Merge Main Population (Archive) and New Population (Samples)
    pop=[pop
         newpop]; %#ok
     
    % Sort Population
    [~, SortOrder]=sort([pop.Cost]);
    pop=pop(SortOrder);
    
    % Delete Extra Members
    pop=pop(1:nPop);
    
    % Update Best Solution Ever Found
    BestSol=pop(1);
    
    % Store Best Cost
    BestCost(it)=BestSol.Cost;
    
    % Show Iteration Information
   source.position = BestSol.Position;
    % Show Iteration Information

    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,'*');

     drawnow;
     distance = distance + (sqrt(((temp.position(1) - source.position(1) )^2)+((temp.position(2) - source.position(2) )^2)));
       disp(['Distance ' num2str(distance) ]);
      
     ttheta = 0;     
       tolerance = 0.9;
       
       xi = atan2(source.position(2)-x(2),source.position(1)-x(1)) -x(3); 
 
 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;
Fx(4) = v*sin(x(3)-x(4))/Lt-v*(2*c)*tan(Phi)*cos(x(3)-x(4))/(Lt*Ll);
Ph=(x(3)-Pht)/0.08;
%RK4 to solve the ODE
for I = 1:4
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;
 disp(x(3))
 drawnow;
  set(hm,'XData',x(1)-c*cos(x(3)) + (Ll+c)*t1*cos(x(3)),'YData',x(2)-c*sin(x(3)) +(Ll+c)*t1*sin(x(3)))
   set(hf,'XData',x(1) + Ll*cos(x(3)) + Lw*(t1)*sin(x(3)),'YData',x(2) + Ll*sin(x(3)) - Lw*(t1)*cos(x(3)))
   set(hr,'XData',x(1) - Ll*cos(x(3)) - Lw*(t1)*sin(x(3)),'YData',x(2) - Ll*sin(x(3)) + Lw*(t1)*cos(x(3)))
   set(hrl,'XData',x(1) - Ll*cos(x(3)) - Lw*sin(x(3))+ 0.5*Lw*t1*cos(x(3)),'YData',x(2) - Ll*sin(x(3)) + Lw*cos(x(3))+ 0.5*Lw*t1*sin(x(3)))
   set(hrr,'XData',x(1) - Ll*cos(x(3)) + Lw*sin(x(3))+ 0.5*Lw*t1*cos(x(3)),'YData',x(2) - Ll*sin(x(3)) - Lw*cos(x(3))+ 0.5*Lw*t1*sin(x(3)))
   set(hfl,'XData',x(1) + Ll*cos(x(3)) + Lw*sin(x(3))+ 0.5*Lw*t1*cos(x(3)+Ph),'YData',x(2) + Ll*sin(x(3)) - Lw*cos(x(3))+ 0.5*Lw*t1*sin(x(3)+Ph))
   set(hfr,'XData',x(1) + Ll*cos(x(3)) - Lw*sin(x(3))+ 0.5*Lw*t1*cos(x(3)+Ph),'YData',x(2) + Ll*sin(x(3)) + Lw*cos(x(3))+ 0.5*Lw*t1*sin(x(3)+Ph))
   
   set(hlbox,'XData',x(1) - 2*Lw*sin(x(3)) + 1.5*Ll*t1*cos(x(3)),'YData',x(2)+ 2*Lw*cos(x(3)) + 1.5*Ll*t1*sin(x(3)))
   set(hrbox,'XData',x(1) + 2*Lw*sin(x(3)) + 1.5*Ll*t1*cos(x(3)),'YData',x(2)- 2*Lw*cos(x(3)) + 1.5*Ll*t1*sin(x(3)))
   set(fbox,'XData',x(1) + 1.5*Ll*cos(x(3)) + 2*Lw*(t1)*sin(x(3)),'YData',x(2) + 1.5*Ll*sin(x(3)) - 2*Lw*(t1)*cos(x(3)))
   set(rbox,'XData',x(1) - 1.5*Ll*cos(x(3)) - 2*Lw*(t1)*sin(x(3)),'YData',x(2) - 1.5*Ll*sin(x(3)) + 2*Lw*(t1)*cos(x(3)))

   set(hmt,'XData',x(1) - (Ll+2*c)*cos(x(3))-(Lt)*t2*cos(x(4)),'YData',x(2) - (Ll+2*c)*sin(x(3))-(Lt)*t2*sin(x(4)))
   set(hrt,'XData',x(1) - (Ll+2*c)*cos(x(3))-(Lt)*cos(x(4)) - Lw*(t1)*sin(x(4)),'YData',x(2) - (Ll+2*c)*sin(x(3))-(Lt)*sin(x(4))+ Lw*(t1)*cos(x(4)))
   set(lt,'XData',x(1) - (Ll+2*c)*cos(x(3))-(Lt)*cos(x(4)) - Lw*(1)*sin(x(4))+ 0.5*Lw*t1*cos(x(4)),'YData',x(2) - (Ll+2*c)*sin(x(3))-(Lt)*sin(x(4))+ Lw*(1)*cos(x(4))+ 0.5*Lw*t1*sin(x(4)))
   set(rt,'XData',x(1) - (Ll+2*c)*cos(x(3))-(Lt)*cos(x(4)) - Lw*(-1)*sin(x(4))+ 0.5*Lw*t1*cos(x(4)),'YData',x(2) - (Ll+2*c)*sin(x(3))-(Lt)*sin(x(4))+ Lw*(-1)*cos(x(4))+ 0.5*Lw*t1*sin(x(4)))
   set(trbox,'XData',x(1) - (Ll+2*c)*cos(x(3))-(Lt+0.25*Lt)*cos(x(4)) - (2*Lw)*(t1)*sin(x(4)),'YData',x(2) - (Ll+2*c)*sin(x(3))-(Lt+0.25*Lt)*sin(x(4))+ (2*Lw)*(t1)*cos(x(4)))
   set(tfbox,'XData',x(1) - (Ll+2*c)*cos(x(3))-(Lt-0.5*Lt)*cos(x(4)) - (2*Lw)*(t1)*sin(x(4)),'YData',x(2) - (Ll+2*c)*sin(x(3))-(Lt-0.5*Lt)*sin(x(4))+ (2*Lw)*(t1)*cos(x(4)))
   set(rrbox,'XData',x(1) - (Ll+2*c)*cos(x(3))-(Lt)*cos(x(4)) - (2*Lw)*(1)*sin(x(4))+ 1*(2*Lw)*t3*cos(x(4)),'YData',x(2) - (Ll+2*c)*sin(x(3))-(Lt)*sin(x(4))+ (2*Lw)*(1)*cos(x(4))+ 1*(2*Lw)*t3*sin(x(4)))
   set(llbox,'XData',x(1) - (Ll+2*c)*cos(x(3))-(Lt)*cos(x(4)) - (2*Lw)*(-1)*sin(x(4))+ 1*(2*Lw)*t3*cos(x(4)),'YData',x(2) - (Ll+2*c)*sin(x(3))-(Lt)*sin(x(4))+ (2*Lw)*(-1)*cos(x(4))+ 1*(2*Lw)*t3*sin(x(4)))

 

   
   set(h,'XData',x(1),'YData',x(2))
%Write the data values to files
   dlmwrite('XYZ.txt',[x(1),x(2)], 'newline', 'pc', '-append') 
% Draw the path
  X = dlmread('XYZ.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; 
end 

 distRT = sqrt((source.position(1) - p1)^2 + ((p2 - source.position(2) )^2));
  if (mod(it,4) == 0 && distRT > 4 )
       % fprintf('Snapshot at t = %d\n',time); 
plot(x(1)-c*cos(x(3)) + (Ll+c)*t1*cos(x(3)),x(2)-c*sin(x(3)) + (Ll+c)*t1*sin(x(3)),'.','MarkerSize',1, 'Color','k'); 
plot(x(1) + Ll*cos(x(3)) + Lw*(t1)*sin(x(3)),x(2) + Ll*sin(x(3)) - Lw*(t1)*cos(x(3)),'.','MarkerSize',1, 'Color','k'); 
plot(x(1) - Ll*cos(x(3)) - Lw*(t1)*sin(x(3)),x(2) - Ll*sin(x(3)) + Lw*(t1)*cos(x(3)),'.','MarkerSize',1, 'Color','k');
plot(x(1) - Ll*cos(x(3)) - Lw*sin(x(3))+ 0.5*Lw*t1*cos(x(3)),x(2) - Ll*sin(x(3)) + Lw*cos(x(3))+ 0.5*Lw*t1*sin(x(3)),'.','MarkerSize',1, 'Color','k');
plot(x(1) - Ll*cos(x(3)) + Lw*sin(x(3))+ 0.5*Lw*t1*cos(x(3)),x(2) - Ll*sin(x(3)) - Lw*cos(x(3))+ 0.5*Lw*t1*sin(x(3)),'.','MarkerSize',1, 'Color','k');
plot(x(1) + Ll*cos(x(3)) + Lw*sin(x(3))+ 0.5*Lw*t1*cos(x(3)),x(2) + Ll*sin(x(3)) - Lw*cos(x(3))+ 0.5*Lw*t1*sin(x(3)),'.','MarkerSize',1, 'Color','k');
plot(x(1) + Ll*cos(x(3)) - Lw*sin(x(3))+ 0.5*Lw*t1*cos(x(3)),x(2) + Ll*sin(x(3)) + Lw*cos(x(3))+ 0.5*Lw*t1*sin(x(3)),'.','MarkerSize',1, 'Color','k');
plot(x(1) - 2*Lw*sin(x(3)) + 1.5*Ll*t1*cos(x(3)),x(2)+ 2*Lw*cos(x(3)) + 1.5*Ll*t1*sin(x(3)),'.','MarkerSize',1, 'Color','k'); 
plot(x(1) + 2*Lw*sin(x(3)) + 1.5*Ll*t1*cos(x(3)),x(2)- 2*Lw*cos(x(3)) + 1.5*Ll*t1*sin(x(3)),'.','MarkerSize',1, 'Color','k'); 
plot(x(1) + 1.5*Ll*cos(x(3)) +2*Lw*(t1)*sin(x(3)),x(2) + 1.5*Ll*sin(x(3)) -2*Lw*(t1)*cos(x(3)),'.','MarkerSize',1, 'Color','k'); 
 plot(x(1) - 1.5*Ll*cos(x(3)) - 2*Lw*(t1)*sin(x(3)),x(2) - 1.5*Ll*sin(x(3)) + 2*Lw*(t1)*cos(x(3)),'.','MarkerSize',1, 'Color','k');
%Draw the trailor
 plot(x(1) - (Ll+2*c)*cos(x(3))-(Lt)*t2*cos(x(4)),x(2) - (Ll+2*c)*sin(x(3))-(Lt)*t2*sin(x(4)),'.','MarkerSize',1, 'Color','k'); 
plot(x(1) - (Ll+2*c)*cos(x(3))-(Lt)*cos(x(4)) - Lw*(t1)*sin(x(4)),x(2) - (Ll+2*c)*sin(x(3))-(Lt)*sin(x(4))+ Lw*(t1)*cos(x(4)),'.','MarkerSize',1, 'Color','k');
plot(x(1) - (Ll+2*c)*cos(x(3))-(Lt)*cos(x(4)) - Lw*(1)*sin(x(4))+ 0.5*Lw*t1*cos(x(4)),x(2) - (Ll+2*c)*sin(x(3))-(Lt)*sin(x(4))+ Lw*(1)*cos(x(4))+ 0.5*Lw*t1*sin(x(4)),'.','MarkerSize',1, 'Color','k');
plot(x(1) - (Ll+2*c)*cos(x(3))-(Lt)*cos(x(4)) - Lw*(-1)*sin(x(4))+ 0.5*Lw*t1*cos(x(4)),x(2) - (Ll+2*c)*sin(x(3))-(Lt)*sin(x(4))+ Lw*(-1)*cos(x(4))+ 0.5*Lw*t1*sin(x(4)),'.','MarkerSize',1, 'Color','k');
 plot(x(1) - (Ll+2*c)*cos(x(3))-(Lt+0.25*Lt)*cos(x(4)) - (2*Lw)*(t1)*sin(x(4)),x(2) - (Ll+2*c)*sin(x(3))-(Lt+0.25*Lt)*sin(x(4))+ (2*Lw)*(t1)*cos(x(4)),'.','MarkerSize',1, 'Color','k');
plot(x(1) - (Ll+2*c)*cos(x(3))-(Lt-0.5*Lt)*cos(x(4)) - (2*Lw)*(t1)*sin(x(4)),x(2) - (Ll+2*c)*sin(x(3))-(Lt-0.5*Lt)*sin(x(4))+ (2*Lw)*(t1)*cos(x(4)),'.','MarkerSize',1, 'Color','k');
plot(x(1) - (Ll+2*c)*cos(x(3))-(Lt)*cos(x(4)) - (2*Lw)*(1)*sin(x(4))+ 1*(2*Lw)*t3*cos(x(4)),x(2) - (Ll+2*c)*sin(x(3))-(Lt)*sin(x(4))+ (2*Lw)*(1)*cos(x(4))+ 1*(2*Lw)*t3*sin(x(4)),'.','MarkerSize',1, 'Color','k');
plot(x(1) - (Ll+2*c)*cos(x(3))-(Lt)*cos(x(4)) - (2*Lw)*(-1)*sin(x(4))+ 1*(2*Lw)*t3*cos(x(4)),x(2) - (Ll+2*c)*sin(x(3))-(Lt)*sin(x(4))+ (2*Lw)*(-1)*cos(x(4))+ 1*(2*Lw)*t3*sin(x(4)),'.','MarkerSize',1, 'Color','k');
end

end

%% Results

figure;

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


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

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


         distFR = (sqrt(((s(1) - x(1))^2)+((s(2) - x(2))^2) )) ;
         
         if distFR > 3
             z = inf;
         else
%               
                    z = 0;
                    c = 0;
                    for i = 1: nobs
                        z = z + (1/sqrt(((o1(i) - x(1))^2)+((o2(i) - x(2))^2) ));
                    end 
                    for j = 1:10
                        c = c + (1/sqrt(((u(j) - x(1))^2)+((y(j) - x(2))^2) )); 
                    end 
                    z = 0.1*(z) + 0.1*(c)+ 0.01*(sqrt(((p1 - x(1) )^2)+((p2 - x(2) )^2)));

         end   
             


end