%%here Centre free resource alloc with quadratic cost function implemented
clear all;
clc;
%ctrl+r-->comment
%ctrl+T-->uncomment 
%% Network

n_iter=1000;

%random network with 20 nodes (%Random Network the same as Boyd-Xiao Fig.1 .)

A = [0 1 1 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
           1 0 1 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0;
           1 1 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
           0 0 1 0 1 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0;
           1 0 0 1 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
           0 0 0 0 1 0 1 0 0 0 0 0 0 0 0 0 0 0 0 1;
           0 0 0 0 0 1 0 1 0 0 0 0 0 0 0 0 1 0 0 0;
           0 0 0 0 0 0 1 0 1 1 0 0 0 0 0 0 0 0 0 0;
           0 0 0 1 0 0 0 1 0 1 0 0 0 0 0 0 0 0 0 0;
           0 0 0 0 0 0 0 1 1 0 1 0 0 0 0 0 0 0 0 0;
           0 0 0 0 0 0 0 0 0 1 0 1 1 0 0 0 0 0 0 0;
           0 0 0 0 0 0 0 0 0 0 1 0 1 1 0 0 0 0 0 0;
           0 0 0 0 0 0 0 0 0 0 1 1 0 1 0 0 0 0 0 0;
           0 0 0 0 0 0 0 0 0 0 0 1 1 0 1 0 0 0 0 0;
           0 1 0 0 0 0 0 0 0 0 0 0 0 1 0 1 0 0 0 0;
           0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 1 0 1 0;
           0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 1 0 1 0 0;
           0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 1 1;
           0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 1 0 1;
           0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 1 1 0];


[L,N]=size(A);
degvec = zeros(N,1);
for i=1:N
    degvec(i, 1)=nnz(A(i,:)); 
end
degreeMat = diag(degvec);
Lap = degreeMat-A;
%% creating function coeficient
%f(xi) = (1/2)a_i(xi-ci)^2+log[1+exp(bi(xi-di))];
%f'(xi) = a_i(xi-ci)+(bi exp(bi(xi-di)))/(1+exp(bi(xi-di))); 
% f"(xi) = ai + [bi^2 exp(bi(xi-di))]/[1+exp(bi(xi-di))]^2
%ai = rand[0,2] bi=rand[-2,2], ci=rand[-10,10], di=rand[-10,10];
%sum xi = 0
% random_vector = (x2-x1)*rand(1,100) + x1;

% a = 0.1*rand(N,1);
% b= 20*(1+rand(N,1));

%  a = 0.05*ones(N,1);
%  b=4*ones(N,1);
 
a = 2*rand(N,1);
 b= 4*rand(N,1)-2;
c=20*rand(N,1)-10;
d=20*rand(N,1)-10;
%lower & upper bounds 
l = a;
u = a+1/4*b.^2;
L = diag(l);
U = diag(u);


%% weigh matrices

 
alpha = -0.2030;
w_bestconst = -1*alpha*Lap;

%metrpolis

w_metro = zeros(size(A));
[m,n]=size(w_metro);
for i=1:m
   s=0;
    for j=1:n
         if A(i,j)~=0 && i~=j
        w_metro(i,j)=-min(1/(nnz(A(i,:))*u(i,:)) , 1/(nnz(A(j,:))*u(j,:)));
        s=s+ w_metro(i,j);
        end        
    end
    w_metro(i,i)=-s;
end

%% xio-boyd convergence factors

% %guaranteed convergence rate
% %xiao boyd

%for optimal boyd
[w_opt_boyd, cond_no_boyd] = BoydOptimalWeight(A,L,U);

eta = L.^(1/2)*(w_opt_boyd+w_opt_boyd'-w_opt_boyd'*U*w_opt_boyd)*L.^(1/2);
eigs = sort(abs(eig(eta))); 
r_opt = 1-eigs(2)

%bestconst
eta = L.^(1/2)*(w_bestconst+w_bestconst'-w_bestconst'*U*w_bestconst)*L.^(1/2);
eigs = sort(abs(eig(eta))); 
r_bestconst = 1-eigs(2)

%metropolis
eta = L.^(1/2)*(w_metro+w_metro'-w_metro'*U*w_metro)*L.^(1/2);
eigs = sort(abs(eig(eta))); 
r_metro = 1-eigs(2)



%% Hb Guaranteddd convergence factor

% % parameters
w_upper = w_metro*U;
w_lower = w_metro*L;
eigsU_metro=sort(abs(eig(w_upper)));
eigsL_metro = sort(abs(eig(w_lower)));
Lmax_metro=max(eigsU_metro); lmin_metro=eigsL_metro(2);
a_metro=4/(sqrt(Lmax_metro)+sqrt(lmin_metro))^2; b_metro=((sqrt(Lmax_metro)-sqrt(lmin_metro))/(sqrt(Lmax_metro)+sqrt(lmin_metro)))^2;

w_upper = w_bestconst*U;
w_lower = w_bestconst*L;
eigsU_bestconst=sort(abs(eig(w_upper)));
eigsL_bestconst = sort(abs(eig(w_lower)));

Lmax_bestconst=max(eigsU_bestconst); lmin_bestconst=eigsL_bestconst(2);
a_bestconst=4/(sqrt(Lmax_bestconst)+sqrt(lmin_bestconst))^2; b_bestconst=((sqrt(Lmax_bestconst)-sqrt(lmin_bestconst))/(sqrt(Lmax_bestconst)+sqrt(lmin_bestconst)))^2;


%test run 
x0=20*rand(N,1)-10*ones(N,1);
xvec_test = [x0];

x_test = x0;
for k=1:n_iter,
    
    x_test=x_test-w_bestconst*(a.*(x_test-c)+(b+exp(b.*(x_test-d)))./(ones(N,1)+exp(b.*(x_test-d))));
    xvec_test=[xvec_test,x_test];
end;

% % first compute H at the optimum
x_opt = x_test;
H = a + (b.^2.* exp(b.*(x_opt-d)))./(ones()+exp(b.*(x_opt-d))).^2;
H = diag(H);


% % % % % Optimal weight 
% % % either with the Hessian of the optimal point or without this one
 [w_opt, cond_no] = HBOptimalWeight(A,H); 
% [w_opt, cond_no] = HBOptimalWeight(A,eye(N,N)); 

% % % setting the alpha beta
w_upper = w_opt*U;
w_lower = w_opt*L;
eigsU_opt=sort(abs(eig(w_upper)));
eigsL_opt = sort(abs(eig(w_lower)));

Lmax_opt=max(eigsU_opt); lmin_opt=eigsL_opt(2);

a_opt=4/(sqrt(Lmax_opt)+sqrt(lmin_opt))^2; b_opt=((sqrt(Lmax_opt)-sqrt(lmin_opt))/(sqrt(Lmax_opt)+sqrt(lmin_opt)))^2;

w=w_opt;
ah = a_opt; bh = b_opt;
HB = [(1+bh)*eye(N,N)-ah*w*H, -bh*eye(N,N); eye(N,N), zeros(N,N)];
eigs = sort(abs(eig(HB)));
rhb1_opt = eigs(N-1)


w=w_metro;
ah = a_metro; bh = b_metro;
HB = [(1+bh)*eye(N,N)-ah*w*H, -bh*eye(N,N); eye(N,N), zeros(N,N)];
eigs = sort(abs(eig(HB)));
rhb1_metro = eigs(N-1)


%bestconst
% H = (L+U)/2;
w=w_bestconst;
ah = a_bestconst; bh = b_bestconst;
HB = [(1+bh)*eye(N,N)-ah*w*H, -bh*eye(N,N); eye(N,N), zeros(N,N)];
eigs = sort(abs(eig(HB)));
rhb1_bestconst = eigs(N-1)



%% Iterations

 % % % xiao-Boyd Xiao-Boyd
% % % % % % % % % % % % % % % % % % % xiao-Boyd Xiao-Boyd
% % % % metro
% % % best-cohnst
% % % % optimal
x0=20*rand(N,1)-10*ones(N,1);
% x0 = [x01;-x01];

xvec_metro=[x0];
xvec_bestconst = [x0];
xvec_opt_boyd = [x0];

x_metro = x0;
x_bestconst = x0;
x_opt_boyd = x0;

for k=1:n_iter,
    x_metro=x_metro - w_metro*(a.*(x_metro-c)+(b+exp(b.*(x_metro-d)))./(ones(N,1)+exp(b.*(x_metro-d))));
    xvec_metro = [xvec_metro,x_metro];
    

    x_bestconst=x_bestconst-w_bestconst*(a.*(x_bestconst-c)+(b+exp(b.*(x_bestconst-d)))./(ones(N,1)+exp(b.*(x_bestconst-d))));
    xvec_bestconst=[xvec_bestconst,x_bestconst];
    
    x_opt_boyd=x_opt_boyd - w_opt_boyd*(a.*(x_opt_boyd-c)+(b+exp(b.*(x_opt_boyd-d)))./(ones(N,1)+exp(b.*(x_opt_boyd-d))));
    xvec_opt_boyd=[xvec_opt_boyd,x_opt_boyd];
end;

% % % % % % % % % % % % % % % % % % % % % % % % % % % % 
% % % % % % % % % % % % % % % % % HB HB HB HB HB

x1vec_metro=[x0];
x1vec_bestconst = [x0];
x1vec_opt = [x0];

x_metro = x0;
x_bestconst = x0;
x_opt = x0;

xold_metro = x_metro;
xold_bestconst = x_bestconst;
xold_opt = x_opt;
for k=1:n_iter,
        
    xnext_metro=x_metro-a_metro*w_metro*(a.*(x_metro-c)+(b+exp(b.*(x_metro-d)))./(ones(N,1)+exp(b.*(x_metro-d)))) + b_metro*(x_metro- xold_metro);
    xold_metro = x_metro;
    x_metro =xnext_metro;    
    x1vec_metro = [x1vec_metro,x_metro];
    
   
    
    xnext_bestconst=x_bestconst-a_bestconst*w_bestconst*(a.*(x_bestconst-c)+(b+exp(b.*(x_bestconst-d)))./(ones(N,1)+exp(b.*(x_bestconst-d)))) + b_bestconst*(x_bestconst- xold_bestconst);
    xold_bestconst = x_bestconst;
    x_bestconst =xnext_bestconst;    
    x1vec_bestconst = [x1vec_bestconst,x_bestconst];
    

    xnext_opt=x_opt-a_opt*w_opt*(a.*(x_opt-c)+(b+exp(b.*(x_opt-d)))./(ones(N,1)+exp(b.*(x_opt-d)))) + b_opt*(x_opt- xold_opt);
    xold_opt = x_opt;
    x_opt =xnext_opt;    
    x1vec_opt = [x1vec_opt,x_opt];
    
end;



%%  PLOTS

% % % load already existing data
%  load('resource_alloc_data.mat')


%first compute f(x)
a_vec = a*ones(1,n_iter+1);
b_vec = b*ones(1,n_iter+1);
c_vec = c*ones(1,n_iter+1);
d_vec = d*ones(1,n_iter+1);


% x_vec = xvec_maxdegree;
% fvec_maxdegree = (1/2)*a_vec.*(x_vec-c_vec).^2+log(ones(size(x_vec))+exp(b_vec.*(x_vec-d_vec)));


x_vec = xvec_bestconst;
fvec_bestconst = (1/2)*a_vec.*(x_vec-c_vec).^2+log(ones(size(x_vec))+exp(b_vec.*(x_vec-d_vec)));

x_vec = xvec_metro;
fvec_metro = (1/2)*a_vec.*(x_vec-c_vec).^2+log(ones(size(x_vec))+exp(b_vec.*(x_vec-d_vec)));


x_vec = xvec_opt_boyd;
fvec_opt_boyd = (1/2)*a_vec.*(x_vec-c_vec).^2+log(ones(size(x_vec))+exp(b_vec.*(x_vec-d_vec)));



semilogy(sum(abs(fvec_metro-fvec_metro(:,n_iter+1)*ones(1,n_iter+1))), '-k', 'LineWidth',3);
hold on;

semilogy(sum(abs(fvec_bestconst-fvec_bestconst(:,n_iter+1)*ones(1,n_iter+1))), '--k', 'LineWidth', 3);
hold on;

semilogy(sum(abs(fvec_opt_boyd-fvec_opt_boyd(:,n_iter+1)*ones(1,n_iter+1))), '-.k', 'LineWidth', 3);
hold on;

% % % % % HB
% % % % % 
% % % % % 
x_vec = x1vec_metro;
f1vec_metro = (1/2)*a_vec.*(x_vec-c_vec).^2+log(ones(size(x_vec))+exp(b_vec.*(x_vec-d_vec)));



x_vec = x1vec_bestconst;
f1vec_bestconst = (1/2)*a_vec.*(x_vec-c_vec).^2+log(ones(size(x_vec))+exp(b_vec.*(x_vec-d_vec)));


x_vec = x1vec_opt;
f1vec_opt = (1/2)*a_vec.*(x_vec-c_vec).^2+log(ones(size(x_vec))+exp(b_vec.*(x_vec-d_vec)));


semilogy(sum(abs(f1vec_bestconst-f1vec_bestconst(:,n_iter+1)*ones(1,n_iter+1))), '-b', 'LineWidth', 3);
hold on;

semilogy(sum(abs(f1vec_metro-f1vec_metro(:,n_iter+1)*ones(1,n_iter+1))), '--b', 'LineWidth',3);
hold on;

semilogy(sum(abs(f1vec_opt-f1vec_opt(:,n_iter+1)*ones(1,n_iter+1))), '-.b', 'LineWidth', 3);
hold on;



% % plot setting
% % % % % % % % % % % % % % % % % % % % % 

legend('Xiao-Boyd-Best-Constant','Xiao-Boyd-Metropolis', 'Xiao-Boyd-SDP-Symmetric', 'Multi-step-Best-Constant', 'Multi-step-Metropolis', 'Multi-step-SDP-Symmetric');

ylim([1e-7 1e3]);
 xlim([0 n_iter/2]);

hx=xlabel('k');
hy=ylabel('f(x(k))-f^{*}');
font_name='times';


set(hx, 'FontName', font_name, 'FontWeight', 'Bold', 'FontSize', 14);
set(hy, 'FontName', font_name, 'FontWeight', 'Bold', 'FontSize', 14);
set(gca, 'FontName', font_name, 'FontSize', 14, 'FontWeight', 'Demi', 'LineWidth', 2);
