%      SelfOrgRobotDemo.M
%
%   Properties of self-organising robots
%    
%
% J. Lunze
% 20.3.2018
%
% Version of 28.10.2018
% for 2nd edition: 26.4.2021
% 
close all
clear all

%%             Basic communication structure:  chain of agents No. 1 - N
%        from RandomTreeCDC16.m  of 9.2.2016,fugres 1 and 2
%
figure(1)
%  additional edges occur with probability p
%  agent 0 is the leader
%  each agent has the delay D=1
%  determine the overall delay between agent 0 and agent N
N=20;
D=1;
V=[];     % v_i is the average path length of 0 \rightarrow i
k1=0;
Prob=[0:0.01:1];
for p=Prob
P=zeros(N,N);
VVector=zeros(1,N);
VVector(1)=1;
for i1=2:N
    for j1=1:i1-1
        if j1<i1-1
            P(i1,j1)=(1-p)^j1*p;
        else
            P(i1,j1)=(1-p)^(i1-1);
        end
    end
    for j1=1:i1-1
        VVector(i1)=VVector(i1)+P(i1,j1)*VVector(j1);
    end
    VVector(i1)=VVector(i1)+1;
end
k1=k1+1;
V(k1,:)=VVector;
end
subplot(1,3,1)
plot(Prob,V, 'b')
axis([0 0.1 0 N]);
latexxlabel('$$p$$');
latexylabel('$$v_i$$');
rotateY;
xticks([0 0.1]);
yticks([0 5 10 15 20]);
hold off;
%
subplot(1,3,[2,3])
plot(Prob,V, 'b');
axis([0 1 0 N]);
latexxlabel('$$p$$');
%latexylabel('$$v_i$$');
yleer;
rotateY;
latextitle(['Average path length to agent $$i$$ ($$N= $$', num2str(N), ')']);
xticks([0 0.5 1]);
hold off;

%%      Resulting delay along the tree
%
figure(2)
subplot(3,1,[1,2]);
N=40;
D=1;
V=[];
k1=0;
Prob=[0, 0.03, 0.06, 0.1, 0.13, 0.4];
for p=Prob
P=zeros(N,N);
DVektor=D*ones(N,1);     % every agent has delay D
P(2,1)=1-p;      % agent A1 sends information to A2 with certainty
P(3:N,1)=p*(1-p);  % A1 sends information to A3 and all other agents with prob. p
pij=p*(1-p);
for k2=1:N-1
    pij=pij*(1-p);
    P(k2+3:N,k2+1)=pij;   % Ai receives information from Ak2 with prob. pij
end
for k2=3:N
    sum1=0;
    for k3=1:k2-2
        sum1=sum1+P(k2,k3);
    end
    P(k2,k2-1)=1-sum1;
end
VVektor=inv(eye(N, N) - P)*DVektor;
plot(VVektor,'b');
hold on
end
axis([0 N 0 12]);
latexxlabel('$$i$$');
latexylabel('$$v_i$$');
rotateY;
latextitle(['Average cumulative delay at agent $$i$$ ($$N= $$', num2str(N), ')']);
%latextext(N/4,2,['$$p=$$', num2str(Prob)]);
latextext(3, 11, '$$p=0$$');
latextext(20, 9, '$$0.03$$');
latextext(20, 7, '$$0.06$$');
%latextext(20, 5, '$$0.09$$');
%latextext(20, 4.5, '$$0.12$$');
latextext(20, 2, '$$0.4$$');
xticks([0 10 20 30 40]);
yticks([0 3 6 9 12]);
hold off;

%%  Inclusion of the bound \bar{v} into an interval [v_min, v_max]
%      RandomTreeCDC16.m, figure 6
%
figure(3)
subplot(2,1,1)
%  Determination of v_i for i=1,2,...,M
M=8;
Vmax=[];
Vmin=[];
Prob=[0:0.003:1];
k1=0;
for p=Prob
Deltav=1;     % \Delta v_1 = 1
vm1=Deltav;   % v_{m-1}= Delta v_1 + \sum_{j=2}^{M-1} \Delta v_j
for i1=2:M-1
    Deltav=(1-p)^(i1-1)*Deltav;
    vm1=vm1+Deltav;
end
q=(1-p)^(M-1);
vmax=vm1+q/(1-q)*Deltav;   % Deltav = \Delta v_{m-1}
k1=k1+1;
Vmin(k1)=vm1;
Vmax(k1)=vmax;
end
plot(Prob, Vmin, 'b', Prob, Vmax,'b');
hold on
plot([0 0.1],[4 4],'k--');
plot([0.096 0.096],[4 0],'k--');
latexxlabel('$$p$$');
latexylabel('$$v_{\rm min}, v_{\rm max}$$');
rotateY;
latextitle('Inclusion of $$\bar{v}$$ into the interval $$[v_{\rm min}, v_{\rm max}]$$');
latextext(1.02, 1, '$$1$$');
axis([0 1 0 10])
xticks([0 0.1 0.5 1]);
yticks([0 4  10]);
hold off


%    from RandomRobots.m, figures 4, 5, 7
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%   Robot formation problem
%   
%  new robot parameters
%  Vehicle model with velocity and position control loops
m=10;           % m= 10 kg  mass
c=0.2;          %  c=1kg/s friction constant
                % forces in N,  hence input  u=acceleration in N
                % time in seconds
                % distance in m
                % velocity in m/s,   
                %         hence output  v=velocity in m/s
%               PI velocity controller
kI=5;          % kI=5
kP=14;          % kP=10
%               Controlled vehicle
%               input = set-point for velocity controller
%               output = velocity
Ac=[-c/m-kP/m -kI/m; 1 0];
Bc=[kP/m; -1];
Cc=[1 0];
Dc=0;
na=2;            % dynamical order of vehicle with velocity controller
%
%
%  Vehicle with output=position
A=[0 Cc; zeros(na,1) Ac];
B=[0; Bc];
C=[1 zeros(1,na)];
D=0;
Robot=ss(A, B, C, D);

kP=0.3;          % feedback gain
Abar=A-kP*B*C;
Bbar=kP*B;
Cbar=C;
Dbar=D;
n=na+1;
contrVehicle=ss(Abar, Bbar, Cbar, Dbar);
N=20;
Tend=100;
dt=0.5;
Time=0:dt:Tend;
Y=step(contrVehicle,Time);
%  Determination of the delay
DVeh=trapz(1-Y)*dt;

%   Robot model
% with new matrices so as to adapt the model to the situation 
% used in RandomPT1Tree.m  of 10.12.2015
%  here: robot model (AR, BR, CR, DR)
AR=Abar;
BR=Bbar;
CR=Cbar;
DR=Dbar;
n=length(Abar);


%% Behaviour of the multi-agent system for the basic communication structure
%  Basic communication structure: direct links  A_i -> A_i+1
%     RandomRobots.m, figures 4 and 5;
%     here as figures 4, 5
figure(4)
Adj=zeros(N,N);
%  Basic communication structure
for k1=2:N
    Adj(k1,k1-1)=1;
end
AGS=kron(eye(N,N), AR);
BGS=kron(eye(N,N), BR);
CGS=kron(eye(N,N), CR);
DGS=kron(eye(N,N), DR);
Abar=AGS+BGS*Adj*CGS;
Bbar=zeros(N*n,1);
Bbar(1:n,1)=BR;
Cbar=CGS;
Dbar=zeros(N,1);
MASwithBasic=ss(Abar, Bbar, Cbar, Dbar);
Y=step(MASwithBasic, Time);
subplot(2,1,1)
plot(Time, Y, 'b');
hold on
plot([-2 0 0 Tend],[0 0 1 1],'b--');
axis([-2 Tend -0.1 1.1]);
%latexxlabel('$$t$$');
xleer;
latexylabel('$$y_i$$');
rotateY;
latextitle(['RandomRobots.m: Behaviour with basic communication']);
%  Determination of delay for the basic communication structure
DB=[];
k2=0;
dt=Time(2)-Time(1);
for k1=2:N
    k2=k2+1;
   DB(k2)=trapz(1-Y(:,k1))*dt;
end
%  Insert the maximum delay in the figure showing the robot behaviour
plot([max(DB) max(DB)],[0 1],'b--');
yticks([0 0.5 1]);
xticks([66]);
hold off
%
figure(5)
plot([2:N],DB,'b--');
hold on
axis([0 N 0 N*DVeh]);
latexxlabel('$$i$$');
latexylabel('$$\bar{\Delta}_i$$');
rotateY;
% 
%
figure(6)
plot([2:N],DB,'b--');
hold on
axis([0 N 0 N*DVeh]);
latexxlabel('$$i$$');
latexylabel('$$\bar{\Delta}_i$$');
rotateY;

%%       Agents with additional communication links
%
%  additional edges occur with probability p
%  agent 1 is the leader
%  each agent has the delay D=1
%  determine the overall delay between agent 0 and agent N
p=0.1;         % Probability of connection
% Adjacency matrix of the communication graph
%RandomSelfOrg=rng;                         % save s to restore the generator setting
%save('RandomSelfOrg', 'RandomSelfOrg');
load('RandomSelfOrg');       % Seed (initialisation) of the number generator
rng(RandomSelfOrg);
Adj=ceil(rand(N,N)-(1-p)*ones(N,N));
Adj=Adj-diag(diag(Adj));        % Delete elements A_ii
%  Add the basic communication structure
for k1=2:N
    Adj(k1,k1-1)=1;
end
% Select the first connection and delete the others
% Count additional communication links: Nlinks
Adj(1,1:N)=0;
Nlinks=0;
for k1=2:N
    k2=1;
    while Adj(k1,k2)==0
        k2=k2+1;
    end
    Adj(k1,k2+1:N)=0;
    if k2<k1-1
        Nlinks=Nlinks+1;
    end
end
Abar=AGS+BGS*Adj*CGS;
MASwithAdd=ss(Abar, Bbar, Cbar, Dbar);
[Y, Time]=step(MASwithAdd, Time);
%
figure(4)
subplot(2,1,2)
plot(Time, Y, 'b');
hold on
plot([-2 0 0 Tend],[0 0 1 1],'b--');
axis([-2 Tend -0.1 1.1]);
latexxlabel('$$t$$');
latexylabel('$$y_i$$');
rotateY;
%
%latextitle(['RandomRobots.m: Behaviour with additional links ($$p=$$' num2str(p) ')']);
%hold off;
%  Determination of delay for effective communication structure
D=[];
k2=0;
dt=Time(2)-Time(1);
for k1=1:N
    k2=k2+1;
   D(k2)=trapz(1-Y(:,k1))*dt;
end
%  Mark the maximum delay of all robots in the figure showing the behaviour
plot([max(D) max(D)],[0 1],'b--');
yticks([0 0.5 1]);
xticks([0 23 50 100 160]);
hold off
%
figure(5)
plot([1:N],D, 'b');
hold on;
plot([1:N],D, 'bo');
latexxlabel('$$i$$');
latextext(1, 55, ['$$\Delta_{\rm max}=$$' num2str(max(D),'%.1f')]);
latextext(1, 50, ['$$N_{\rm add}=$$' num2str(Nlinks)]);
latexylabel('$$\bar{\Delta}_i$$');
rotateY;
%latextext(N+1, 1, num2str(Nlinks));
%Nlinks

%  for comparison: draw average delay
%  from RandomTree.m
V=[];
V(1)=1;
Xold=[1; 1];
for k1=1:N-1
    ak=(1-p)^(k1-1);
    bk=ak*p;
    Xnext=[ak 1; bk 1]*Xold;
    XDelta(k1,1:2)=(Xnext-Xold)';
    Xold=Xnext;
    V(k1+1)=Xnext(1);
end
plot(V*DVeh, 'b--');
yticks([0 20 40 60 80]);
xticks([0 10 20 30 40]);
axis([0 N 0 60]);
hold off


%%    Repetition with another random communication graph
%
figure(6)
% Adjacency matrix of the communication graph
%RandomSelfOrg2=rng;                         % save s to restore the generator setting
%save('RandomSelfOrg2', 'RandomSelfOrg2');
load('RandomSelfOrg2');       % Seed (initialisation) of the number generator
rng(RandomSelfOrg2);
Adj=ceil(rand(N,N)-(1-p)*ones(N,N));
Adj=Adj-diag(diag(Adj));        % Delete elements A_ii
%  Add the basic communication structure
for k1=2:N
    Adj(k1,k1-1)=1;
end
% Select the first connection and delete the others
% Count additional communication links: Nlinks
Adj(1,1:N)=0;
Nlinks=0;
for k1=2:N
    k2=1;
    while Adj(k1,k2)==0
        k2=k2+1;
    end
    Adj(k1,k2+1:N)=0;
    if k2<k1-1
        Nlinks=Nlinks+1;
    end
end
Abar=AGS+BGS*Adj*CGS;
MASwithAdd=ss(Abar, Bbar, Cbar, Dbar);
[Y, Time]=step(MASwithAdd, Time);
%
%  Determination of delay for effective communication structure
D=[];
k2=0;
dt=Time(2)-Time(1);
for k1=1:N
    k2=k2+1;
   D(k2)=trapz(1-Y(:,k1))*dt;
end
%
figure(6)
plot([1:N],D, 'b');
hold on;
plot([1:N],D, 'bo');
latexxlabel('$$i$$');
latextext(1, 55, ['$$\Delta_{\rm max}=$$' num2str(max(D),'%.1f')]);
latextext(1, 50, ['$$N_{\rm add}=$$' num2str(Nlinks)]);
latexylabel('$$\bar{\Delta}_i$$');
rotateY;
%latextext(N+1, 1, num2str(Nlinks));
%Nlinks

%  for comparison: draw average delay
%  from RandomTree.m
V=[];
V(1)=1;
Xold=[1; 1];
for k1=1:N-1
    ak=(1-p)^(k1-1);
    bk=ak*p;
    Xnext=[ak 1; bk 1]*Xold;
    XDelta(k1,1:2)=(Xnext-Xold)';
    Xold=Xnext;
    V(k1+1)=Xnext(1);
end
plot(V*DVeh, 'b--');
yticks([0 20 40 60 80]);
xticks([0 10 20 30 40]);
axis([0 N 0 60]);
hold off


%%  Probability that an edge i-1 -> i is also an edge
%   of the effective communication graph
%     from RandomTree.m, figure 7
%
figure(7)
subplot(2,1,1)
N=20;
p=0.1;
pij=1;
Psurvive(1)=1;
for i1=2:N
    pij=pij*(1-p);
    Psurvive(i1)=pij;
end
plot(Psurvive, 'b')
latexxlabel('$$i$$');
latexylabel('$$p_{i,i-1}$$');
rotateY;
latextitle('Probability of an edge of $${\cal G}_{\rm B}$$ to be an edge of $${\cal G}$$');
yticks([0 0.5 1]);
xticks([0 5 10 15 20]);
axis([0 N 0 1]);



%%          Chain and additional links towards a restricted interval
%           of agents i+1 ... i+M
%       from RandomTreeMss.m , figure 1
%
figure(8)
subplot(3,1,[1,2])
%  additional edges occur with probability p
%  towards agents in the neighbourhood of at most M agents.
%  Determine the average path length between vertex 0 and vertex N:
N=30;
%M=4;   % agent i can reach at most agent i+M (M >1)
pRobot=0.1;
p=pRobot;
MRobot=5;  % M for robot example
for M=[1,2, 3, 4, 5, 6, 7];
%  The average path lengths v_0,..., v_{M-1} are determined 
%  with the same formulas as for unrestricted communication
%  (cf. RandomTree.m)
V=[];
P=zeros(M,M);
VVector=zeros(1,M);
VVector(1)=1;
for i1=2:M
    for j1=1:i1-1
        if j1<i1-1
            P(i1,j1)=(1-p)^j1*p;
        else
            P(i1,j1)=(1-p)^(i1-1);
        end
    end
    for j1=1:i1-1
        VVector(i1)=VVector(i1)+P(i1,j1)*VVector(j1);
    end
    VVector(i1)=VVector(i1)+1;
end
%   Now the further average path length are determined by the 
%   following state-space model (A, B, C)
A=zeros(M,M);
for i1=1:M-1
    A(i1,i1+1)=1;
end
A(M,1)=p;
for i1=2:M-1
     A(M,i1)=A(M,i1-1)*(1-p);
end
A(M,M)=(1-p)^(M-1);
%
B=zeros(M,1);
B(M,1)=1;
C=zeros(1,M);
C(1,M)=1;
Sys=ss(A, B, C, 0);
%  Use the model (A, B, C) to determine the next average path lengths
%  which are represented as the output of this model
x=VVector';
for k1=M+1:N
   xneu=A*x+B;
   VVector(k1)=C*xneu;
   %x-xneu
   x=xneu;
end
if M==1
   plot(VVector, 'b--');
else
   plot(VVector, 'b');
end
hold on
if M==MRobot
    VRobot=VVector;
end
end
latexxlabel('$$i$$');
latexylabel('$$v_i$$');
rotateY;
latextitle(['$${\tt SelfOrgRobotDemo}:$$ Average path length for $$p= \;$$' num2str(p)]);
latextext(3,10,'$$M=1$$');
%  for comparison: include the average path length for unlimited reach 
%  of the agents as dashed line in the figure
V=[];
P=zeros(N,N);
VVector=zeros(1,N);
VVector(1)=1;
for i1=2:N
    for j1=1:i1-1
        if j1<i1-1
            P(i1,j1)=(1-p)^j1*p;
        else
            P(i1,j1)=(1-p)^(i1-1);
        end
    end
    for j1=1:i1-1
        VVector(i1)=VVector(i1)+P(i1,j1)*VVector(j1);
    end
    VVector(i1)=VVector(i1)+1;
end
plot(VVector,'b--');
latextext(0.8*N,2,'$$\infty$$');
latextext(21,7,'7');
xticks([0 10 20 30]);
yticks([0 10 20 30]);
hold off


%%   Determination of the increment for large i
%       from RandomTreeMss.m , figure 1
%
%
figure(9)
subplot(3,1,[1,2])
Mmax=20;
MVector=[1:Mmax];
Prob=[0.03 0.1 0.4];
for p=Prob
Delta=[];
k1=0;
for M=MVector
    % model
    A=zeros(M,M);
    for i1=1:M-1
        A(i1,i1+1)=1;
    end
    A(M,1)=p;
    for i1=2:M-1
         A(M,i1)=A(M,i1-1)*(1-p);
    end
    A(M,M)=(1-p)^(M-1);
    %
    B=zeros(M,1);
    B(M,1)=1;
    % Transformation in canonical normal form
    [V, D]=eig(A);
    % Find the eigenvalue = 1 and the corresponding eigenvector
    Lambda=diag(D);
    LVsort=sortrows([Lambda V']);   % result has eigenvalue = 1 in last row
    Vsort=LVsort(:,2:end)';         % eigenvector for eigenvalue 1 in last column
    % Generate a column with 1's as last column of V
    for k2=2:M
        Vsort(k2,M)=Vsort(k2,M)/Vsort(1,M);
    end
    Vsort(1,M)=1;
    W=inv(Vsort);    % W(M,:) is the left eigenvector for eigenvalue 1
    b1tilde=real(W(M,M));
    k1=k1+1;
    Delta(k1)=b1tilde;
end
plot(MVector,Delta,'b');
hold on
end
axis([0, Mmax, 0, 1]);
latexxlabel('$$M$$');
latexylabel('$$\overline{\Delta v}$$');
rotateY;
latextitle('$${\tt SelfOrgRobotDemo}:$$ Increment of average path length for large $$i$$');
latextext(6.5, 0.08,'$$0.4$$');
latextext(12,0.5,'$$p=0.03$$');
latextext(12,0.24,'$$0.1$$');
plot([3, 5, 9], 0.5, 'ko');
plot([0 9], [0.5 0.5], 'k--');
plot([3 3],[0.5 0],'k--');
plot([5 5],[0.5 0],'k--');
plot([9 9],[0.5 0],'k--');
xticks([0 3 5 9 15 20]);
yticks([0 0.5 1]);
hold off


%%       Agents with additional communication links in a restricted neighbourhood
%         from RandomTreeMss.m,  figure 4 and 5
%
figure(10)
N=30;
AdjB=zeros(N,N);
for k1=2:N
    AdjB(k1,k1-1)=1;
end
AGS=kron(eye(N,N), AR);
BGS=kron(eye(N,N), BR);
CGS=kron(eye(N,N), CR);
DGS=kron(eye(N,N), DR);
Abar=AGS+BGS*AdjB*CGS;
Bbar=zeros(N*n,1);
Bbar(1:n,1)=BR;
Cbar=CGS;
Dbar=zeros(N,1);
MASwithBasic=ss(Abar, Bbar, Cbar, Dbar);
[Y, Time]=step(MASwithBasic);
%  Determination of delay for the basic communication structure
DB=[];
k2=0;
dt=Time(2)-Time(1);
for k1=2:N
    k2=k2+1;
   DB(k2)=trapz(1-Y(:,k1))*dt;
end
plot([2:N],DB,'b--');
hold on
axis([0 N 0 N*DVeh]);
latexxlabel('$$i$$');
latexylabel('$$\bar{D}_i$$');
rotateY;
%
%  Additional edges occur with probability p between agent i and agent i+M.
%
M=MRobot;
p=pRobot;
AGS=kron(eye(N,N), AR);
BGS=kron(eye(N,N), BR);
CGS=kron(eye(N,N), CR);
DGS=kron(eye(N,N), DR);
Bbar=zeros(N*n,1);
Bbar(1:n,1)=BR;
Cbar=CGS;
Dbar=zeros(N,1);
%
%  Agent 1 is the leader.
% Adjacency matrix of the communication graph
Adj=ceil(rand(N,N)-(1-p)*ones(N,N));
Adj=Adj-diag(diag(Adj));        % Delete elements A_ii
%  Add the basic communication structure
for k1=2:N
    Adj(k1,k1-1)=1;
end
%  Delete connnections i -> j with j>i+M
for k1=1:N
    for k2=k1+M+1: N
        Adj(k2,k1)=0;
    end
end
% Select the first connection and delete the others
% Count additional communication links: Nlinks
Adj(1,1:N)=0;
Nlinks=0;
for k1=2:N
    k2=1;
    while Adj(k1,k2)==0
        k2=k2+1;
    end
    Adj(k1,k2+1:N)=0;
    if k2<k1-1
        Nlinks=Nlinks+1;
    end
end
Abar=AGS+BGS*Adj*CGS;
MASwithAdd=ss(Abar, Bbar, Cbar, Dbar);
[Y, Time]=step(MASwithAdd, Time);
%  Determination of delay for effective communication structure
D=[];
k2=0;
dt=Time(2)-Time(1);
for k1=2:N
    k2=k2+1;
   D(k2)=trapz(1-Y(:,k1))*dt;
end
plot([max(D) max(D)],[-0.1 1.1],'--');
plot([2:N],D, 'b');
hold on;
plot([2:N],D, 'bo');
latexxlabel('$$i$$');
latextext(1, 0.9*N*DVeh, ['$$\Delta_{\rm max}=$$' sprintf('%4.0f', max(D))...
       ', $$\quad N_{\rm add}=$$' num2str(Nlinks)]);
latexylabel('$$J_i$$');
rotateY;
latextitle(['$${\tt SelfOrgRobotDemo}:$$ Communication with $$p= \;$$',...
    num2str(p), ', $$M= \;$$', num2str(M)]);
%  for comparison: draw average delay
%  determined above for MRobot
plot(VRobot*DVeh, 'b--');
axis([0 N 0 N*DVeh]);
xticks([0 10 20 30]);
yticks([0 20 40 60 80 100]);
hold off


%% Behaviour for two step changes
figure(11)
%  Agents with basic communication structure
%  model: MASwithBasic
%  command input: 0 ... t_1:    w=0
%                 t_1 ... t_2:  w=1
%                 t_2 ... t_3:  w=0
%  for better interpretation, the time t=0 is set to the time point
%  when the command signal changes its value first
t1=30;
t2=200+t1;
t3=380;
Tend=t3;
dt=2;
Time=[0:dt:Tend]';
k1end=ceil(t1/dt)+1;
k2end=ceil(t2/dt);
kend=ceil(Tend/dt)+1;
W=zeros(kend,1);
for k1=k1end+1:k2end
    W(k1)=1;
end
Y=lsim(MASwithBasic,W,Time);
subplot(4,1,[1,2])
for k1=1:5:N
    plot(Time-t1, Y(:,k1), 'b');
    hold on
end
plot(Time-t1, W,'b--');
plot([99.6 99.6],[1 0],'b--');
axis([-t1 Time(end)-t1 -0.1 1.1]);
xleer;
latexylabel('$$y_i$$');
rotateY;
latextitle(['$${\tt SelfOrgRobotDemo}:$$ Comparison for $$p= \;$$' num2str(p)]);
latextext(90, -0.2, '$$99.6$$');
yticks([0 0.5 1]);
xticksempty([0 100 200 300]);
hold off;
%  Agents with additional communication links
%  model: MASwithAdd
Y=lsim(MASwithAdd,W,Time);
%
subplot(4,1,[3,4])
for k1=1:5:N
    plot(Time-t1, Y(:,k1), 'b');
    hold on
end
plot(Time-t1, W,'b--');
plot([43.3 43.3],[1 0],'b--');
axis([0-t1 Time(end)-t1 -0.1 1.1]);
latexxlabel('$$t$$');
latexylabel('$$y_i$$');
rotateY;
yticks([0 0.5 1]);
xticks([0 43.3 100 200 300]);
hold off;



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%  Systems with the ring structure as basic communication
%%%%%%%%%%%%%%  graph
%%      Resulting delay along the tree
%     from RandomRing.m, figure 2
%
figure(12)
subplot(3,1,[1,2]);
Vvector=[];
PVector=[];
N=30;
D=1;
V=[];
k1=0;
Prob=[0, 0.03, 0.06, 0.1, 0.17 0.4];
for p=Prob
   ptilde=2*p-p*p;
   VVector=[];
   VVector(1)=1;
   VVector(2)=2;
   VVector(3)=p*VVector(1)+(1-p)*VVector(2)+1;
   PVector=[];
   PVector(1)=p;
   PVector(2)=(1-p)*ptilde;
   PVector(3)=1-p-(1-p)*ptilde;
   for i1=4:N
       VVector(i1)=PVector(1:i1-1)*VVector(1:i1-1)'+1;
       PVector(i1-1)=PVector(i1-2)*(1-ptilde);
       PVector(i1)=1-sum(PVector(1:i1-1));
   end
plot(VVector, 'b');
hold on
end
axis([0 N 0 12]);
latexxlabel('$$k$$');
latexylabel('$$v_k$$');
rotateY;
latextitle(['$${\tt SelfOrgRobotDemo}:$$ Average cumulative delay at agent $$i$$ ($$N= $$', num2str(N), ')']);
%latextext(N/4,2,['$$p=$$', num2str(Prob)]);
latextext(3, 9, '$$p=0$$');
latextext(20, 7.3, '$$0.03$$');
latextext(20, 5.5, '$$0.06$$');
%latextext(20, 5, '$$0.09$$');
%latextext(20, 4.5, '$$0.12$$');
latextext(20, 2, '$$0.4$$');
yticks([0 3 6 9 12]);
xticks([0 10 20 30 40]);
hold off;


%%  Inclusion of the bound \bar{v} into an interval [v_min, v_max]
%     from RandomRing.m, figure 4
%
figure(13)
subplot(2,1,1)
%  Determination of v_i for i=1,2,...,M
M=8;
Vmax=[];
Vmin=[];
Prob=[0:0.0001:0.1 0.103:0.003:1];
k1=0;
for p=Prob
    ptilde=2*p-p*p;
Deltav=1;     % \Delta v_2 = 1
vm1=2;        % v2=2
% v_{m-1}= Delta v_1 + \sum_{j=2}^{M-1} \Delta v_j
for i1=3:M-1
    Deltav=(1-p)*(1-ptilde)^(i1-3)*Deltav;
    vm1=vm1+Deltav;
end
q=(1-p)*(1-ptilde)^(M-3);
vmax=vm1+q/(1-q)*Deltav;   % Deltav = \Delta v_{m-1}
k1=k1+1;
Vmin(k1)=vm1;
Vmax(k1)=vmax;
end
plot(Prob, Vmin, 'b', Prob, Vmax, 'b');
latexxlabel('$$p$$');
latexylabel('$$v_{\rm min}, v_{\rm max}$$');
rotateY;
latextitle('$${\tt SelfOrgRobotDemo}:$$ Inclusion of $$\bar{v}$$ into the interval $$[v_{\rm min}, v_{\rm max}]$$');
axis([0 1 0 10])
yticks([0 5 10]);
xticks([0 0.1 0.5 1]);
%
subplot(2,1,2)
%plot(Prob, Vmin, 'b', Prob, Vmax, 'b');    % broader tolerance band
%deleted
%    Repeat calculations for another value of M
M=15;
Vmax=[];
Vmin=[];
k1=0;
for p=Prob
    ptilde=2*p-p*p;
Deltav=1;     % \Delta v_2 = 1
vm1=2;        % v2=2
% v_{m-1}= Delta v_1 + \sum_{j=2}^{M-1} \Delta v_j
for i1=3:M-1
    Deltav=(1-p)*(1-ptilde)^(i1-3)*Deltav;
    vm1=vm1+Deltav;
end
q=(1-p)*(1-ptilde)^(M-3);
vmax=vm1+q/(1-q)*Deltav;   % Deltav = \Delta v_{m-1}
k1=k1+1;
Vmin(k1)=vm1;
Vmax(k1)=vmax;
end
plot(Prob, Vmin, 'b', Prob, Vmax, 'b');
latexxlabel('$$p$$');
latexylabel('$$v_{\rm min}, v_{\rm max}$$');
rotateY;
axis([0 0.1 0 30])
yticks([0 10 20 30]);
xticks([0 0.02 0.04 0.06 0.08 0.1]);
hold off;



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%  Change of the leader -> change of the communication graph
%%      Generation of the effective communication graph for leader A_1
%       from RandomRing.m, figure 7
%
figure(14)
N=20;
load Adj;
%p=0.1;         % Probability of connection
%
% ARing=zeros(N,N);
% for k1=2:N
%     ARing(k1,k1-1)=1;
%     ARing(k1-1,k1)=1;
% end
% ARing(1,N)=1;
% ARing(N,1)=1;
% %  Add additional bi-directional links
% Add=ceil(rand(N,N)-(1-p)*ones(N,N));
% Add=Add-diag(diag(Add));        % Delete elements A_ii
% %  Add the basic communication structure
% Adj=max(ARing,Add);
% % for bi-directional links, make the matrix symmetric
% for k1=1:N
%     for k2=1:k1
%         Adj(k1,k2)=Adj(k2,k1);
%     end
% end
% sorting the vertices in dependence upon their distance from the leader
VNumbers=[1:N]';             % vector of agent indices
LeaderEntry=1;
Distances=min(mod(VNumbers-LeaderEntry,N), mod(LeaderEntry-VNumbers,N))+1; 
                            % vector of the rank
SortNumbers=sortrows([Distances VNumbers]);  % matrix with rows sorted
                             % according to the rank
%  build permutation matrix to sort the agent indices according to the rank
Perm=zeros(N,N);
for k1=1:N
    Perm(k1,SortNumbers(k1,2))=1;
end
AdjGE=Perm*Adj*Perm';        % now the agents are enumerated according to 
                             % their rank
                             % after the leader entry point pairs of 
                             % vertices have the same rank
%  Interpret AdjGE as adjacency matrix of a directed tree by
%  deleting the edges from agents with higher number
for k1=1:N
    for k2=k1:N
        AdjGE(k1,k2)=0;   
    end
end
% Select the first connection and delete the others
% Count additional communication links: Nlinks
AdjGE(1,1:N)=0;
Nlinks=0;
for k1=2:N
    k2=1;
    while AdjGE(k1,k2)==0
        k2=k2+1;
    end
    AdjGE(k1,k2+1:N)=0;
    if k2<k1-1
        Nlinks=Nlinks+1;
    end
end
%
%  Determine distance from leader
D=eye(N,1);      % Vector with the path lengths
                 % Leader has path length = 1
D=max(D, 2*AdjGE(:,1)); % Every direct link implies path length = 2
Anext=AdjGE(:,1);
pathlength=2;
while prod(D)==0    % while there are unspecified path lengths
      Anext=AdjGE*Anext;
      pathlength=pathlength+1;
      for k1=1:N
          if D(k1,1)==0 && Anext(k1,1) ~=0
                    D(k1,1)=pathlength;
                end
            end
end
plot([1:N], Perm'*D*DVeh, 'b');   % plot with the original agent numbers
hold on
plot([1:N], Perm'*D*DVeh, 'bo');   % plot with the original agent numbers
plot(1, Perm(1,1)*D(1,1)*DVeh, 'bo',...
            'MarkerEdgeColor','b',...
            'MarkerFaceColor','b');
%   add average path length for i-th agent
ptilde=2*p-p*p;
VVector=[];
VVector(1)=1;                 
VVector(2)=2;
VVector(3)=p*VVector(1)+(1-p)*VVector(2)+1;
PVector=[];
PVector(1)=p;
PVector(2)=(1-p)*ptilde;
PVector(3)=1-p-(1-p)*ptilde;
for i1=4:ceil(N/2)+1     % i1 is the distance from leader
    VVector(i1)=PVector(1:i1-1)*VVector(1:i1-1)'+1;
    PVector(i1-1)=PVector(i1-2)*(1-ptilde);
    PVector(i1)=1-sum(PVector(1:i1-1));
end
% two succeding agents have the same length
LVector=[];
LVector(1)=VVector(1);
for i1=2:ceil(N/2)
   LVector(2*i1-2)=VVector(i1);
   LVector(2*i1-1)=VVector(i1);
end
if ceil(N/2)==N/2
    LVector(N)=VVector(N/2+1);
end
plot([1, N],[max(LVector), max(LVector)]*DVeh ,'b--');
latexxlabel('$$i$$');
latexylabel('$$J_i$$');
rotateY;
latextitle(['$${\tt SelfOrgRobotDemo}:$$ Distance in the effective communication graph ($$p=$$' num2str(p) ')']);
latextext(1, 18, '$$l=1$$'); 
axis([0 21 0 21]);
xticks([1 5 10 15 20]);
yticks([0 5 10 15 20]);
hold off


%%   With new entry point of the leader
%
figure(15)
VNumbers=[1:N]';             % vector of agent indices
LeaderEntry=8;
Distances=min(mod(VNumbers-LeaderEntry,N), mod(LeaderEntry-VNumbers,N))+1; 
                            % vector of the rank
SortNumbers=sortrows([Distances VNumbers]);  % matrix with rows sorted
                             % according to the rank
%  build permutation matrix to sort the agent indices according to the rank
Perm=zeros(N,N);
for k1=1:N
    Perm(k1,SortNumbers(k1,2))=1;
end
AdjGE=Perm*Adj*Perm';        % now agent are enumerated according to 
                             % their rank
                             % after the leader entry point pairs of 
                             % vertices have the same rank
%  Interpret AdjGE as adjacency matrix of a directed tree by
%  deleting the edges from agents with higher number
for k1=1:N
    for k2=k1:N
        AdjGE(k1,k2)=0;   
    end
end
% Select the first connection and delete the others
% Count additional communication links: Nlinks
AdjGE(1,1:N)=0;
Nlinks=0;
for k1=2:N
    k2=1;
    while AdjGE(k1,k2)==0
        k2=k2+1;
    end
    AdjGE(k1,k2+1:N)=0;
    if k2<k1-1
        Nlinks=Nlinks+1;
    end
end
%
%  Determine distance from leader
D=eye(N,1);      % Vector with the path lengths
                 % Leader has path length = 1
D=max(D, 2*AdjGE(:,1)); % Every direct link implies path length = 2
Anext=AdjGE(:,1);
pathlength=2;
while prod(D)==0    % while there are unspecified path lengths
      Anext=AdjGE*Anext;
      pathlength=pathlength+1;
      for k1=1:N
          if D(k1,1)==0 && Anext(k1,1) ~=0
                    D(k1,1)=pathlength;
                end
            end
end
plot([1:N], Perm'*D*DVeh, 'b');   % plot with the original agent numbers
hold on
plot([1:N], Perm'*D*DVeh, 'bo');   % plot with the original agent numbers
plot(8, D(1,1)*DVeh, 'bo',...
            'MarkerEdgeColor','b',...
            'MarkerFaceColor','b');
%   add average path length for i-th agent
plot([1, N],[max(LVector), max(LVector)]*DVeh ,'b--');
latextext(1, 18, '$$l=8$$'); 
axis([0 21 0 21]);
latexxlabel('$$i$$');
latexylabel('$$J_i$$');
rotateY;
latextitle(['$${\tt SelfOrgRobotDemo}:$$ Distance in the effective communication graph ($$p=$$' num2str(p) ')']);
xticks([1 5 10 15 20]);
yticks([0 5 10 15 20]);
hold off


%%
%
epsfigc15('SelfOrgRobotDemo');
