%      DelaySelfOrg.M
%
%   Self-connecting agents
% 
%
% J. Lunze
% 3.10.2023
%
% Version of 4.10.2023
% 
close all
clear all

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%      Resulting delay along the tree
%
figure(1)
subplot(3,1,[1,2]);
%  Agent enumeration:  P0  P1  P2  P3 ... PN1
%  Index in P               1   2   3      N1
N1=40;
Delta=1;
V=[];
k1=0;
Prob=[0, 0.03, 0.06, 0.1, 0.13, 0.4];
for p=Prob
    P=zeros(N1,N1);
    DVektor=Delta*ones(N1,1);     % every agent has delay D
    P(2,1)=1-p;        % agent A1 sends information to A2 with certainty
    P(3:N1,1)=p*(1-p);  % A1 sends information to A3 and all other agents with prob. p
    pij=p*(1-p);
    for k2=1:N1-1
        pij=pij*(1-p);
        P(k2+3:N1,k2+1)=pij;   % Ai receives information from Ak2 with prob. pij
    end
    for k2=3:N1
        sum1=0;
        for k3=1:k2-2
            sum1=sum1+P(k2,k3);
        end
        P(k2,k2-1)=1-sum1;
    end
    VVektor=inv(eye(N1,N1) - P)*DVektor;
    plot(VVektor,'b');
    hold on
end
axis([0 N1 0 12]);
latexxlabel('$$i$$');
latexylabel('$$v_i$$');
rotateY;
latextitle(['Average cumulative delay at agent $$i$$ ($$N= $$', num2str(N1), ')']);
%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;


%    from RandomRobots.m, figures 4, 5, 7  ->  SelfOrgRobotDemo.m in NCS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%   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 2, 3
figure(2)
subplot(4,1,[1,2]);
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(['DelaySelfOrg.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.6]);
hold off
%
figure(3)
subplot(3,1,[1,2]);
plot([2:N],DB,'b--');
hold on
axis([0 N 0 N*DVeh]);
latexxlabel('$$i$$');
latexylabel('$$\bar{\Delta}_i$$');
rotateY;
% 
%
figure(4)
subplot(3,1,[1,2]);
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
%DelaySelfOrg1=rng;                         % save s to restore the generator setting
%save('DelaySelfOrg1', 'DelaySelfOrg1');
load('DelaySelfOrg1');       % Seed (initialisation) of the number generator
rng(DelaySelfOrg1);
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(2)
subplot(4,1,[3,4]);
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(3)
subplot(3,1,[1,2]);
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, 48, ['$$N_{\rm add}=$$' num2str(Nlinks)]);
latexylabel('$$\bar{J}_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(4)
subplot(3,1,[1,2]);
% Adjacency matrix of the communication graph
%DelaySelfOrg2=rng;                         % save s to restore the generator setting
%save('DelaySelfOrg2', 'DelaySelfOrg2');
load('DelaySelfOrg2');       % Seed (initialisation) of the number generator
rng(DelaySelfOrg2);
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(4)
subplot(3,1,[1,2]);
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, 48, ['$$N_{\rm add}=$$' num2str(Nlinks)]);
latexylabel('$$\bar{J}_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(5)
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]);




%%
%
epsfigc15('DelaySelfOrg');

