% Run of average consensus algorithm with noisy 
% updates using descending step size scheme gamma=a/(1+b)^c                     

%% graph definition
clear all; clc;
variance_noise=0.1; %variance of the gaussian noise added to the updates
iterations =100; %number of iterations of the algorithm
nodes=60; %number of graph nodes
G=graph(bucky); % using matlab default graph bucky
% i.e. 60 nodes with degree 3
%% variables initialization
equiv_noise_vector=zeros(1,nodes); 
x_0=zeros(1,nodes);
diag_D=zeros(nodes,nodes);
%plot the used graph
figure;
plot(G)
deg=degree(G);
for p=1:nodes
   diag_D(p,p)=deg(p); 
end
%observation initialization const 10 + wgn with variance 1 
for i=1:nodes     %initialize measurement
    x_0(i)=(10+wgn(1,1,log10(10*variance_noise)));    
end

%% variables definition
x_est_run(1,:) = x_0;
MSE_act(1)= sum((x_0-mean(x_0)).^2);
MSE_ave(1)=MSE_act(1);
VAR(1)=var(x_0);
noise_matrix=zeros(nodes);
%Laplacian using library functions
A=adjacency(G); %adjacency matrix
L=laplacian(G); %laplacian matrix
eig_L=eig(L);%eigenvalues of Laplacian
gamma=2/(eig_L(2)+eig_L(nodes));%the best possible coefficient
mean_0=mean(x_0);%value to converge to

%% Iterations of the algorithm
for i=1:iterations
   gamma=1/(i+42)^0.75; %selected descending step size
   P=eye(nodes)- gamma* L; %Perron matrix

   for j=1:nodes %computation of the nise affecting updates exchange
      noise_matrix(j,:)=wgn(nodes,1,10*log10(variance_noise)) ;
      noise_matrix(j,j)=0 ;
   end
    equiv_noise_matrix= P*noise_matrix;
   for j=1:nodes
   equiv_noise_vector(j)= equiv_noise_matrix(j,j);
   end
   x_est_run(i+1,:)=  P*x_est_run(i,:)'+equiv_noise_vector';  
end

%calculation of the run statistics
for i=1:iterations
  MSE_ave(i+1)= sum((x_est_run(i+1,:)-mean(x_0)).^2)/nodes;
  MSE_act(i+1)= sum((x_est_run(i+1,:)-mean(x_est_run(i+1,:))).^2)/nodes;   
  VAR(i+1)= var(x_est_run(i+1,:));
end

%%  Plot results
figure;
plot(0:iterations, x_est_run(:,:));
ttl1=title( 'Values in nodes'  )
ttl1=set(ttl1,'Interpreter','latex','FontSize', 15);
xlbl1=xlabel('Iterations [-]');
xlbl1=set(xlbl1,'Interpreter','latex');
ylbl1=ylabel('Value [-]');
ylbl1=set(ylbl1,'Interpreter','latex');
grid;

figure;
loglog(0:iterations, MSE_ave(:)   );
ttl1=title( 'Mean square error w.r.t. initial average '  )
ttl1=set(ttl1,'Interpreter','latex','FontSize', 15);
xlbl1=xlabel('Iterations [-]');
xlbl1=set(xlbl1,'Interpreter','latex');
ylbl1=ylabel('MSE [-]');
ylbl1=set(ylbl1,'Interpreter','latex');
grid;

figure;
loglog(0:iterations, VAR(:));
ttl1=title( 'Variance '  );
ttl1=set(ttl1,'Interpreter','latex','FontSize', 15);
xlbl1=xlabel('Iterations [-]');
xlbl1=set(xlbl1,'Interpreter','latex');
ylbl1=ylabel('Variance [-]');
ylbl1=set(ylbl1,'Interpreter','latex');
grid;