function [X,z_e,z_l,z_u] = CaliMat(G,e,I_e,J_e,l,I_l,J_l,u,I_u,J_u,tau0)

%%%%%%%%%%%%% This code is designed to solve %%%%%%%%%%%%%%%%%%%%%
%%       min    0.5*<X-G, X-G>
%%       s.t.   X_ij =  e_ij     for (i,j) in (I_e,J_e)
%%              X_ij >= l_ij     for (i,j) in (I_l,J_l)
%%              X_ij <= u_ij     for (i,j) in (I_u,J_u)
%%              X >= tau0*I       X is SDP (tau0 >=0 and may be zero)
%%
%%%%%%%%%%%  Based on the algorithm  in  %%%%%%%%%%%%%%%%%%%
%%% "Calibrating Least Squares Covariance Matrix Problems
%%%  with Equality and Inequality Constraints", June 2008 
%%%%%%%%%  By Yan Gao and Defeng Sun  %%%%%%%%%%%%%%%%%%%%%%
  
%   Parameters:
%   Input
%   G       the given symmetric matrix
%   e       the right hand side of equality constraints
%   I_e     row indices of the fixed elements
%   J_e     column indices of the fixed elements
%   l       the right hand side of lower bound constraint
%   I_l     row indices of the lower bound elements
%   J_l     column indices of the lower bound elements
%   u       the right hand side of upper bound constraint
%   I_u     row indices of the upper bound elements
%   J_u     column indices of the upper bound elements
%   tau0    the lower bound of the smallest eigenvalue of X*
%
%   Output
%   X         the optimal primal solution
%   z_e       the optimal dual solution to equality constraints
%   z_l       the optimal dual solution to lower bound constraints
%   z_u       the optimal dual solution to upper bound constraints
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%% Last modified on June 12, 2008 %%% 


t0 = cputime;
n = length(G);
k_e = length(e);       % the number of fixed elements
k_l = length(l);       % the number of elements with lower bounds
k_u = length(u);       % the number of elements with upper bounds
k = k_e+k_l+k_u;

fprintf('\n ******************************************************** \n')
fprintf( '      Smoothing Newton Method with BICGSTAB Algorithm        ')
fprintf('\n ******************************************************** \n')
fprintf('\n The information of this problem is as follows: \n')
fprintf(' Dim. of    sdp      constr  = %d \n',n)
fprintf(' Num. of equality    constr  = %d \n',k_e)
fprintf(' Num. of lower bound constr  = %d \n',k_l)
fprintf(' Num. of upper bound constr  = %d \n',k_u)
fprintf(' The lower bounds: [ %2.1e, %2.1e ] \n',min(l),max(l))
fprintf(' The upper bounds: [ %2.1e, %2.1e ] \n',min(u),max(u))

if nargin == 11
    G = G - tau0*eye(n);         % reset G
    e = e - tau0*ones(k_e,1);    % reset e
    l = l - tau0*ones(k_l,1);    % reset l
    u = u - tau0*ones(k_u,1);    % reset u
end

G =(G+G')/2;           % make G symmetric

%%% set parameters
Iter_Whole = 200;
Iter_inner = 20;       % Maximum number of line search 
maxit = 200;           % Maximum number of iterations in CG/BICGSTB
tol = 1.0e-2;          % tolerance for CG/BICGSTAB   %tol = 2.0e-2;
error_tol = 1.0e-6;    % stopping criterion

eps_bar = 1.0e-2;      % eps_bar is in (0,1)
tau = 1;               % tau should be in (0,1] and it is used to control the decrease of epsilon
eta = 0.5;
r = 0.2;               % 0 <r,eta <1 and satisfy: sqrt(2)*max(eta,r*eps_bar)<1
sigma = 0.5e-6;        % tolerance in the line search of the smoothing Newton method
delta = 0.5;
kappa = 1.0e-2;        % regularized term

c = 2.0*ones(k,1);     
prec = ones(k,1);      % preconditioner

k1 = 0;
f_eval = 0;
iter_bicg = 0;

eig_time =0;
prec_time = 0;
CG_time = 0;

epsilon = eps_bar;         % initial epsilon
epsilon0 = epsilon;

num = 5;
phi_hist = zeros(num,1);

b_z = zeros(k,1);
b_ze = zeros(k_e,1);
b_zl = zeros(k_l,1);
b_zu = zeros(k_u,1);

d_z = zeros(k,1);
d_ze = zeros(k_e,1);
d_zl = zeros(k_l,1);
d_zu = zeros(k_u,1);

% initial value
z_e = zeros(k_e,1);
z_l = zeros(k_l,1);
z_u = zeros(k_u,1);

x0_ze = z_e;
x0_zl = z_l;
x0_zu = z_u;

Z_e = sparse(I_e,J_e,z_e,n,n);
Z_e = 0.5*(Z_e+Z_e');
Z_l = sparse(I_l,J_l,z_l,n,n);
Z_l = 0.5*(Z_l+Z_l');
Z_u = sparse(I_u,J_u,z_u,n,n);
Z_u = 0.5*(Z_u+Z_u');

C = G + Z_e + Z_l - Z_u;
C = (C + C')/2;

t1 = cputime;
[P,D] = eig(C);
eig_time = eig_time + cputime - t1;
lambda = diag(D);
P = real(P);
lambda = real(lambda);

f_eval = f_eval + 1;

Omega = omega_mat(lambda, epsilon);
            
[phi,g_ze,g_ze0,g_zl,g_zl0,g_zu,g_zu0] = ...
    Phi(epsilon,e,z_e,I_e,J_e,l,z_l,I_l,J_l,u,z_u,I_u,J_u,P,lambda,kappa,c); % the initial value of the merit function
theta = r * min(1,phi^((1+tau)/2));                                          % the initial term controlling the decrease of epsilon
delta_eps = -epsilon + eps_bar*theta;                                        % first compute delta_eps

phi_hist(1) = phi^0.5;

[b_ze,b_zl,b_zu] = ...
 rhs(epsilon,delta_eps,e,I_e,J_e,z_e,g_ze,l,I_l,J_l,z_l,g_zl,g_zl0,u,I_u,J_u,z_u,g_zu,g_zu0,P,lambda,kappa,c);  % to compute the right hand side term
b_z = [b_ze;b_zl;b_zu];

tt = cputime - t0;
[hh,mm,ss] = time(tt);
 
fprintf('\n   Iter.   Num. of BiCGstabs     Step length      Norm of smoothing fun.      time_used ')
fprintf('\n    %d         %2.0d                  %2.1e                        %3.2e              %d:%d:%d ',0,str2num('-'),str2num('-'),phi^0.5,hh,mm,ss)

while (phi^0.5>error_tol & k1 < Iter_Whole)
    
    % compute preconditioner
    t2 = cputime;  
    prec = precond_matrix(epsilon,e,I_e,J_e,l,I_l,J_l,z_l,g_zl0,u,I_u,J_u,z_u,g_zu0,Omega,P,kappa,c);
    prec_time = prec_time + cputime - t2;
   
    % CG/BICGStab starts   
    t3 = cputime;   
    if k_l+k_u == 0       % if no ineqalities, use CGs; otherwise bicgstab
        [d_z, flag, relres, iterk] = ...
     pre_cg(b_z,epsilon,e,I_e,J_e,l,I_l,J_l,z_l,g_zl0,u,I_u,J_u,z_u,g_zu0,Omega,P,maxit,tol,kappa,phi,eta,c,prec);
         if k1==0           
           fprintf( '\n $$$ No inequality constraints. BiCGStab is replaced by CG $$$ \n ')
        end
    else
        [d_z, relres, iterk, flag] = ...
    bicgstab(b_z,epsilon,e,I_e,J_e,l,I_l,J_l,z_l,g_zl0,u,I_u,J_u,z_u,g_zu0,Omega,P,maxit,tol,kappa,phi,eta,c,prec);
    end
    
    CG_time = CG_time + cputime - t3;
    iter_bicg = iter_bicg + iterk;
    
    d_ze = d_z(1:k_e);
    d_zl = d_z(k_e+1:k_e+k_l);
    d_zu = d_z(k_e+k_l+1:k);
    
    z_e = x0_ze + d_ze;                           % temporary z_e
    z_l = x0_zl + d_zl;                           % temporary z_l
    z_u = x0_zu + d_zu;                           % temporary z_u
    epsilon = epsilon0 + delta_eps;               % temporary epsilon
    
    Z_e = sparse(I_e,J_e,z_e,n,n);
    Z_e = 0.5*(Z_e+Z_e');
    Z_l = sparse(I_l,J_l,z_l,n,n);
    Z_l = 0.5*(Z_l+Z_l');
    Z_u = sparse(I_u,J_u,z_u,n,n);
    Z_u = 0.5*(Z_u+Z_u');
    
    C = G + Z_e + Z_l - Z_u;
    C = (C + C')/2;
    
    t1 = cputime;
    [P,D] = eig(C);                                 % Eig-decomposition: C = P*D*P^T
    eig_time = eig_time + cputime - t1;
    lambda = diag(D);
    P = real(P);
    lambda = real(lambda);
    
    phi0 = phi;
    [phi,g_ze,g_ze0,g_zl,g_zl0,g_zu,g_zu0] = ...
        Phi(epsilon,e,z_e,I_e,J_e,l,z_l,I_l,J_l,u,z_u,I_u,J_u,P,lambda,kappa,c); 

    %%% Line Search 
    k_inner=0;
    while( k_inner <=Iter_inner & phi>(1-2*sigma*(1-sqrt(2)*max(eta,r*eps_bar))*delta^k_inner)*phi0 + 1.0e-6 )       
        k_inner = k_inner+1;
        z_e = x0_ze + delta^k_inner*d_ze;
        z_l = x0_zl + delta^k_inner*d_zl;
        z_u = x0_zu + delta^k_inner*d_zu;
        epsilon = epsilon0 + delta^k_inner*delta_eps;
        
        Z_e = sparse(I_e,J_e,z_e,n,n);
        Z_e = 0.5*(Z_e+Z_e');
        Z_l = sparse(I_l,J_l,z_l,n,n);
        Z_l = 0.5*(Z_l+Z_l');
        Z_u = sparse(I_u,J_u,z_u,n,n);
        Z_u = 0.5*(Z_u+Z_u');

        C = G + Z_e + Z_l - Z_u;
        C = (C + C')/2;
        
        t1 =cputime;
        [P,D]=eig(C);
        eig_time = eig_time +cputime -t1;
        lambda = diag(D);
        P = real(P);
        lambda = real(lambda);
        
        [phi,g_ze,g_ze0,g_zl,g_zl0,g_zu,g_zu0] = ...
          Phi(epsilon,e,z_e,I_e,J_e,l,z_l,I_l,J_l,u,z_u,I_u,J_u,P,lambda,kappa,c); 
    end   
    
    k1 = k1+1;
    f_eval = f_eval+k_inner+1;
    
    x0_ze = z_e;
    x0_zl = z_l;
    x0_zu = z_u;
    epsilon0 = epsilon;

    % update the right hand side b_z
    theta = r * min(1,phi^((1+tau)/2));
    delta_eps = -epsilon + eps_bar*theta;
    
    [b_ze,b_zl,b_zu] = ...
      rhs(epsilon,delta_eps,e,I_e,J_e,z_e,g_ze,l,I_l,J_l,z_l,g_zl,g_zl0,u,I_u,J_u,z_u,g_zu,g_zu0,P,lambda,kappa,c);
    b_z = [b_ze;b_zl;b_zu];

    Omega = omega_mat(lambda,epsilon);
    
    tt = cputime - t0;
    [hh,mm,ss] = time(tt);
 
    fprintf('\n   %2.0d         %2.0d                  %2.1e              %3.2e              %d:%d:%d ',k1,iterk,delta^k_inner,phi^0.5,hh,mm,ss)

    % slow convergence test
    if  k1<num
        phi_hist(mod(k1,num)+1) = phi^0.5;
    else
        for i=1:num-1
            phi_hist(i) = phi_hist(i+1);
        end
        phi_hist(num) = phi^0.5;
    end  
    if ( k1 >= num-1 & phi_hist(1)-phi_hist(num) <= 1.0e-7 )
        fprintf('Progress is too slow! :( ')
        break
    end
        
end  % End for the outer loop;

% correct the final z_l & z_u
g_zl = g_zl - kappa*epsilon*z_l;
z_l = z_l - g_zl;
g_zu = g_zu - kappa*epsilon*z_u;
z_u = z_u - g_zu;

% optimal solution X*
P = real(P);
C = P';
s = smoothing_fun(epsilon,lambda);   % s = max(0,lambda);

i=1;
while (i<n+1)
    C(i,:) = s(i)*C(i,:);
    i=i+1;
end
X = P*C;                        
X = (X+X')/2;

% min_abs_lambda = min(abs(lambda));

% rank of X*
r_X = length( find(s > 1.0e-8) );

% rank of corresponding multiplier Z*
r_Z = length( find( abs(s-lambda)>1.0e-8 ) );

% optimal primal and negative dual value
C = (X-G);
C = (C+C')/2;
prim_val = sum((sum(C.*C)))/2;
dual_val = sum(sum(X.*X)) - sum(sum(G.*G));
dual_val = dual_val/2 - z_e'*e - z_l'*l + z_u'*u;

% convert to original X*
X = X + tau0 * eye(n);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%

time_used = cputime-t0;

fprintf('\n')
fprintf('\n ================ Final Information ================= \n');
fprintf(' Total number of iterations      = %2.0f \n',k1);
fprintf(' Number of func. evaluations     = %2.0f \n',f_eval)
fprintf(' Number of BiCGStabs             = %2.0f \n',iter_bicg)
fprintf(' Primal objective value          = %d \n', prim_val)
fprintf(' Dual objective value            = %d \n',-dual_val)
fprintf(' Norm of smoothing function      = %3.2e \n', phi^0.5)
fprintf(' Rank of  X* - (tau0*I)          = %2.0d \n', r_X)
fprintf(' Rank of optimal multiplier Z*   = %2.0d \n', r_Z)
fprintf(' CPU time for preconditioner     = %3.1f \n',prec_time)
fprintf(' CPU time for BICGSTAB           = %3.1f \n',CG_time)
fprintf(' CPU time for eigen-decom        = %3.1f \n',eig_time)
fprintf(' Total CPU time (secs)           = %3.1f \n',time_used)
fprintf(' ====================================================== \n');
    
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% end of the main program %%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%





 


%%  **************************************
%%  ******** All Sub-routines  ***********
%%  **************************************

%%% To change the format of time 
function [h,m,s] = time(t)
t = round(t); 
h = floor(t/3600);
m = floor(rem(t,3600)/60);
s = rem(rem(t,60),60);
%%% End of time.m

 



%%% To generate the smoothing function 
function s = smoothing_fun(epsilon,lambda)
n = length(lambda);
s = ones(n,1);
for i=1:n
    if lambda(i)>0
        s(i) = 0.5*(sqrt(lambda(i)^2+epsilon^2)+lambda(i));
    else
        s(i) = 0.5*epsilon^2/(sqrt(lambda(i)^2+epsilon^2)-lambda(i));
    end

end
return
%%% End of smoothing.m 





%%% To generate partial derivatives of G(epsilon,y) wrt epsilon and y 
function  [fe fx] = smoothing_ff(epsilon,x)
if x>0    
    fx = 0.5*(1+x/sqrt(epsilon^2+x^2));
else
    fx = 0.5*epsilon/sqrt(epsilon^2+x^2);
    fx = fx*epsilon/(sqrt(epsilon^2+x^2)-x);
end
fe = 0.5*epsilon/sqrt(epsilon^2+x^2);
return
%%% End of smoother_ff.m 





%%%% To generate the first order difference matrix 
function Omega = omega_mat(lambda,epsilon)
n = length(lambda);
Omega = zeros(n,n);
lambda_ep = sqrt(lambda.^2+epsilon^2);

Omega = lambda*ones(1,n)+ones(n,1)*lambda';
Omega = Omega./(lambda_ep *ones(1,n) + ones(n,1)*lambda_ep');
Omega = 0.5*(ones(n,n) + Omega);
return
%%%% End of omega_mat.m 





%%% To generate the merit function phi 
function  [phi,g_ze,g_ze0,g_zl,g_zl0,g_zu,g_zu0] = ...
   Phi(epsilon,e,z_e,I_e,J_e,l,z_l,I_l,J_l,u,z_u,I_u,J_u,P,lambda,kappa,c)

n = length(P);
k_e = length(e);
k_l = length(l);
k_u = length(u);
k = k_e+k_l+k_u;

const_sparse = 2;    % decides when chooses sparsity

g_ze = zeros(k_e,1);
g_zl = zeros(k_l,1);
g_zu = zeros(k_u,1);
g_ze0 = g_ze;
g_zl0 = g_zl; 
g_zu0 = g_zu;

s = ones(n,1);
s = smoothing_fun(epsilon,lambda);

% to generate the smoothing function for the SDP projection operator
M = P';
i=1;
while (i<=n)
    M(i,:) = s(i)*M(i,:);
    i=i+1;
end

if k < const_sparse*n;   % sparse form
    i=1;
    while (i<=k_e)
        g_ze0(i) = P(I_e(i),:)*M(:,J_e(i));
        i = i+1;
    end

    i=1;
    while (i<=k_l)
        g_zl0(i) = P(I_l(i),:)*M(:,J_l(i));
        i=i+1;
    end

    i=1;
    while (i<=k_u)
        g_zu0(i) = -P(I_u(i),:)*M(:,J_u(i));
        i=i+1;
    end
else     % dense form
    M = P*M;
    i=1;
    while (i<=k_e)
        g_ze0(i) = M(I_e(i),J_e(i));
        i=i+1;
    end

    i=1;
    while (i<=k_l)
        g_zl0(i) = M(I_l(i),J_l(i));
        i=i+1;
    end

    i=1;
    while (i<=k_u)
        g_zu0(i) = -M(I_u(i),J_u(i));
        i=i+1;
    end
end

g_ze = c(1:k_e).*(g_ze0-e) + kappa*epsilon*z_e;
g_zl = z_l - c(k_e+1:k_e+k_l).*(g_zl0-l);
g_zl = z_l - smoothing_fun(epsilon,g_zl) + kappa*epsilon*z_l;
g_zu = z_u - c(k_e+k_l+1:k).*(u+g_zu0);
g_zu = z_u - smoothing_fun(epsilon,g_zu) + kappa*epsilon*z_u;
phi = epsilon^2 + norm(g_ze)^2 + norm(g_zl)^2 + norm(g_zu)^2;
return
%%% End of Phi.m





%%% To generate the Jacobain product F'(y)(x) 
function [Ax_ze, Ax_zl, Ax_zu] = ...
    Jacobian_matrix(epsilon,x_ze,x_zl,x_zu,e,I_e,J_e,l,I_l,J_l,z_l,g_zl0,u,I_u,J_u,z_u,g_zu0,Omega,P,kappa,c)

n = length(P);
k_e = length(e);
k_l = length(l);
k_u = length(u);
k = k_e+k_l+k_u;

const_sparse = 2;      % decides when chooses sparsity

Ax_ze = zeros(k_e,1);
Ax_zl = zeros(k_l,1);
Ax_zu = zeros(k_u,1);

H = zeros(n,n);
fx = 0;
fe = 0;

Z_e = sparse(I_e,J_e,x_ze,n,n);
Z_e = 0.5*(Z_e+Z_e');
Z_l = sparse(I_l,J_l,x_zl,n,n);
Z_l = 0.5*(Z_l+Z_l');
Z_u = sparse(I_u,J_u,x_zu,n,n);
Z_u = 0.5*(Z_u+Z_u');
 
if k < const_sparse*n   % sparse form
    % disp('sparse form')
    H = P'*sparse(Z_e+Z_l-Z_u)*P;             % H = P^T* [Z_e+Z_l-Z_u]*P                        
    H = Omega.*H;
    H = H*P';
    
    %%% to generate the first part: Ax_ze    
    i=1;
    while (i<=k_e)
        Ax_ze(i) = c(i)*P(I_e(i),:)*H(:,J_e(i));
        Ax_ze(i) = Ax_ze(i) + kappa*epsilon*x_ze(i) + 1.0e-10*x_ze(i);     % small perturbation to avoid singularity
        i=i+1;
    end
    %%% to generate the second part: Ax_zl 
    i=1;
    while (i<=k_l)
        Ax_zl(i) = z_l(i) - c(k_e+i)*(g_zl0(i)-l(i)); 
        [fe fx] = smoothing_ff(epsilon,Ax_zl(i));
        Ax_zl(i) = ...
        (1-fx)*x_zl(i) + c(k_e+i)*fx*(P(I_l(i),:)*H(:,J_l(i))) + kappa*epsilon*x_zl(i) + 1.0e-10*x_zl(i); % add a small term to avoid singularity
        i= i+1;
    end
    %%% to generate the second part: Ax_zu 
    i=1;
    while (i<=k_u)
        Ax_zu(i) = z_u(i) - c(k_e+k_l+i)*(u(i)+g_zu0(i)); 
        [fe fx] = smoothing_ff(epsilon,Ax_zu(i));
        Ax_zu(i) = ...
        (1-fx)*x_zu(i) + c(k_e+k_l+i)*fx*(-P(I_u(i),:)*H(:,J_u(i))) + kappa*epsilon*x_zu(i) + 1.0e-10*x_zu(i); % add a small term to avoid singularity
        i= i+1;
    end
    
else   % the dense form
    % disp('dense form')
    H = P'*(Z_e+Z_l-Z_u)*P;
    H = Omega.*H;    
    H = H*P';
    H = P*H;

    %%% to generate the first part Ax_ze
    i=1;
    while (i<=k_e)
        Ax_ze(i) = c(i)*H(I_e(i),J_e(i)) + kappa*epsilon*x_ze(i) + 1.0e-10*x_ze(i);
        i=i+1;
    end
    %%% to generate the second part: Ax_zl 
    i=1;
    while (i<=k_l)
        Ax_zl(i) = z_l(i)- c(k_e+i)*(g_zl0(i)-l(i)); 
        [fe fx] = smoothing_ff(epsilon,Ax_zl(i));
        Ax_zl(i) = (1-fx)*x_zl(i) + c(k_e+i)*fx*H(I_l(i),J_l(i)) + kappa*epsilon*x_zl(i) + 1.0e-10*x_zl(i);
        i=i+1;
    end
    %%% to generate the second part: Ax_zu
    i=1;
    while (i<=k_u)
        Ax_zu(i) = z_u(i) - c(k_e+k_l+i)*(u(i)+g_zu0(i));
        [fe fx] = smoothing_ff(epsilon,Ax_zu(i));
        Ax_zu(i) = (1-fx)*x_zu(i)+ c(k_e+k_l+i)*fx*(-H(I_u(i),J_u(i))) + kappa*epsilon*x_zu(i) + 1.0e-10*x_zu(i);
        i=i+1;
    end
end
return
%%% End of Jacobian_matrix.m 





%%% To generate the right hand side of the linear system
function  [b_ze,b_zl,b_zu] = ...
      rhs(epsilon,delta_eps,e,I_e,J_e,z_e,g_ze,l,I_l,J_l,z_l,g_zl,g_zl0,u,I_u,J_u,z_u,g_zu,g_zu0,P,lambda,kappa,c)
  
n = length(P);
k_e = length(e);
k_l = length(l);
k_u = length(u);
k = k_e+k_l+k_u;

const_sparse = 2;    % decides when chooses sparsity

b_ze = -g_ze;
b_zl = -g_zl;
b_zu = -g_zu;

m1 = z_l - c(k_e+1:k_e+k_l).*(g_zl0-l);   
m2 = z_u - c(k_e+k_l+1:k).*(u+g_zu0);

D = P';
i=1;
while (i<=n)
    [fe,fx] = smoothing_ff(epsilon,lambda(i));
    D(i,:) = fe*D(i,:);
    i=i+1;
end

if  k < const_sparse*n      % sparse case   
    %%% to generate the first term b_ze 
    i =1;
    while (i<=k_e)
        b_ze(i)= b_ze(i) - c(i)*delta_eps*( P(I_e(i),:)*D(:,J_e(i)) ) - kappa*delta_eps*z_e(i);
        i=i+1;
    end
    %%% to generate the second term b_zl   
    i=1;
    while (i<=k_l)
        [fe,fx] = smoothing_ff(epsilon,m1(i));
        b_zl(i) = ...
          b_zl(i) + delta_eps*fe - c(k_e+i)* delta_eps*fx*( P(I_l(i),:)*D(:,J_l(i)) ) - kappa*delta_eps*z_l(i);
        i=i+1;
    end
    %%% to generate the second term b_zu
    i=1;
    while (i<=k_u)
        [fe,fx] = smoothing_ff(epsilon,m2(i));
        b_zu(i)= ...
          b_zu(i) + delta_eps*fe - c(k_e+k_l+i)*delta_eps*fx*( -P(I_u(i),:)*D(:,J_u(i)) ) - kappa*delta_eps*z_u(i);
        i=i+1;
    end
    
else   %%% dense case
    D = P*D;
    %%% to generate the first term b_ze 
    i=1;
    while(i<=k_e)
        b_ze(i) = b_ze(i) - c(i).*delta_eps*D(I_e(i),J_e(i)) - kappa*delta_eps*z_e(i);
        i=i+1;
    end    
   %%% to generate the second term b_zl  
    i=1;
    while (i<=k_l)
       [fe, fx] = smoothing_ff(epsilon,m1(i));
       b_zl(i)= b_zl(i) + delta_eps*fe - c(k_e+i)*delta_eps*fx*D(I_l(i),J_l(i)) - kappa*delta_eps*z_l(i);
       i=i+1;
    end    
    %%% to generate the second term b_zu  
    i=1;
    while (i<=k_u)
       [fe, fx] = smoothing_ff(epsilon,m2(i));
       b_zu(i)= b_zu(i) + delta_eps*fe - c(k_e+k_l+i)*delta_eps*fx*( -D(I_u(i),J_u(i)) ) - kappa*delta_eps*z_u(i);
       i=i+1;
    end
end
return
%%% End of rhs.m 





%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%% PCG method %%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% This is exactly the algorithm given by Hestenes and Stiefel (1952)
%% An iterative method to solve A(x) =b  
%% The symmetric positive definite matrix M is a preconditioner for A.
%% See Pages 527 and 534 of Golub and va Loan (1996)
function [p,flag,relres,iterk] = ...
   pre_cg( b,epsilon,e,I_e,J_e,l,I_l,J_l,z_l,g_zl0,u,I_u,J_u,z_u,g_zu0,Omega,P,maxit,tol,kappa,phi,eta,c,prec )

n = length(P);
k_e = length(e);
k_l = length(l);
k_u = length(u);
k = k_e+k_l+k_u;

r = b;  
n2b = norm(b);                       
tol = min( tol, eta*phi^0.5/n2b );
tolb = tol * n2b;       % relative tolerance 

p = zeros(k,1);

flag = 1;
iterk =0;
relres = 1000;       % give a big value on relres

%%% preconditioning 
z = r./prec;         % z = M\r; here M = diag(c); if M is not the identity matrix 
rz1 = r'*z; 
rz2 = 1; 
d = z;

%%% CG iteration
for k1 = 1:maxit
   if k1 > 1
       beta = rz1/rz2;
       d = z + beta*d;
   end
  
   x_ze = d(1:k_e);
   x_zl = d(k_e+1:k_e+k_l);
   x_zu = d(k_e+k_l+1:k);
   
   w = Jacobian_matrix(epsilon,x_ze,x_zl,x_zu,e,I_e,J_e,l,I_l,J_l,z_l,g_zl0,u,I_u,J_u,z_u,g_zu0,Omega,P,kappa,c);
   
   denom = d'*w;
   iterk = k1;
   relres = norm(r)/n2b;              % relative residue =norm(r) / norm(b)
   if denom <= 0 
       % sssss = 0;
       p = d/norm(d);                 % d is not a descent direction
       break  % exit
   else
       alpha = rz1/denom;
       p = p + alpha*d;
       r = r - alpha*w;
   end
   z = r./prec;                      
   if norm(r) <= tolb                % Exit if Hp=b is solved within the relative tolerance
       iterk = k1;
       relres = norm(r)/n2b;         % relative residue = norm(r)/norm(b)
       flag = 0;
       break
   end
   rz2 = rz1;
   rz1 = r'*z;
end
return
%%% End of pre_cg.m





%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%% BICGStab method %%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [ x, error_norm, iter, flag ] = ...
  bicgstab(b_z,epsilon,e,I_e,J_e,l,I_l,J_l,z_l,g_zl0,u,I_u,J_u,z_u,g_zu0,Omega,P,maxit,tol,kappa,phi,eta,c,prec)
%% BICGStab solves a linear system using the biconjugate gradient
%% stabilized method developed by 
%% H.A. van der Vorst,
%% "BI-CGSTAB: A fast and smoothly converging variant of BI-CG for the solution of nonsymmetric linear systems", 
%% SIAM Journal on Scientific and Statistical Computing 13 (1992), pp.631-644. 
%%
%    Output:
%    x               the optimal solution
%    error_norm      the norm of error
%    iter            the number of iterations needed
%    flag            the return flag
%                    ---  0 = the solution is found within the specified tolerance
%                    ---  1 = a satisfactory solution is not found and the iteration limit is exceeded
%                    --- -1 = the method broke down with RHO = 0 
%                    --- -2 = the method broke down with OMEGA = 0 

k_e = length(e);
k_l = length(l);
k_u = length(u);
k = k_e+k_l+k_u;

x_ze = zeros(k_e,1);
x_zl = zeros(k_l,1);
x_zu = zeros(k_u,1);
Ax_ze = zeros(k_e,1);
Ax_zl = zeros(k_l,1);
Ax_zu = zeros(k_u,1);

iter = 0;
omeg = 1.0;
flag = 1;

% intial x0=0
x = zeros(k,1);

r = b_z;
r_tld = r;

bnrm2 = norm( b_z );
tol = min( tol, eta*phi^0.5/bnrm2 );
error_norm = 1;                         % initial relative error = ||r||/||b||

for iter = 1 : maxit
    rho = r_tld' * r;
    if ( rho == 0.0 )
        flag = -1;
        break
    end
    if ( 1 < iter )
        beta  = ( rho / rho_1 ) * ( alpha0 / omeg );
        pp = r./prec + beta * ( pp - omeg * (v./prec));         % pp = r + beta * ( pp - omeg * v );
    else
        pp = r./prec;     % pp = r;
    end

    p_hat = pp;
    x_ze = p_hat(1:k_e);
    x_zl = p_hat(k_e+1:k_e+k_l);
    x_zu = p_hat(k_e+k_l+1:k);

    [Ax_ze, Ax_zl, Ax_zu] = ...
      Jacobian_matrix(epsilon,x_ze,x_zl,x_zu,e,I_e,J_e,l,I_l,J_l,z_l,g_zl0,u,I_u,J_u,z_u,g_zu0,Omega,P,kappa,c);
    v = [Ax_ze; Ax_zl; Ax_zu];
    
    alpha0 = rho / ( r_tld' * v );
    s = r - alpha0 * v;

    % Early convergence check
    if ( norm ( s ) < tol*bnrm2)
        flag = 0;
        x = x + alpha0 * p_hat;
        error_norm = norm(s) / bnrm2;        
        break;
    end

    s_hat = s./prec;
    x_ze = s_hat(1:k_e);
    x_zl = s_hat(k_e+1:k_e+k_l);
    x_zu = s_hat(k_e+k_l+1:k);

    [Ax_ze, Ax_zl, Ax_zu] = ...
     Jacobian_matrix(epsilon,x_ze,x_zl,x_zu,e,I_e,J_e,l,I_l,J_l,z_l,g_zl0,u,I_u,J_u,z_u,g_zu0,Omega,P,kappa,c);
    t = [Ax_ze; Ax_zl; Ax_zu];
    
    omeg = ( t' * s ) / ( t'*t );

    % update x
    x = x + alpha0 * p_hat + omeg * s_hat;
    r = s - omeg * t;
    error_norm = norm(r)/ bnrm2;

    if ( error_norm <= tol )
        flag = 0;
        break
    end
    if ( omeg == 0.0 )
        flag = -2;
        break
    end
    rho_1 = rho;
end
return
%%% End of bicgstab.m 
  




%%% To generate the diagonal preconditioner 
function prec = precond_matrix(epsilon,e,I_e,J_e,l,I_l,J_l,z_l,g_zl0,u,I_u,J_u,z_u,g_zu0,Omega,P,kappa,c)

n = length(P);
k_e = length(e);
k_l = length(l);
k_u = length(u);
k = k_e+k_l+k_u;

const_cg = 1;

H1 = zeros(n,n);
H2 = zeros(n,n);
prec = ones(k,1);

H = P';
H = H.*H;
Omega0 = Omega*H;
H = H';

%%% the first part: prec_ze
if k_e <= const_cg*n     % compute the exact diagonal preconditioner        
    k1 = ceil(k_e/n);
    k0 = k_e-n*(k1-1);
    for i=1:k1
        if ( i<k1 | k0==n )
            for j=(i-1)*n+1:i*n
                H1(:,j-(i-1)*n) = P(I_e(j),:)'.*P(J_e(j),:)';
            end
            H2 = Omega*H1;
            H1 = H1';
            for j=1:n                               
                prec(n*(i-1)+j) = H(I_e(n*(i-1)+j),:)*Omega0(:,J_e(n*(i-1)+j)) + H1(j,:)*H2(:,j);
                prec(n*(i-1)+j) = prec(n*(i-1)+j)/2;               
                prec(n*(i-1)+j) = c(n*(i-1)+j)*prec(n*(i-1)+j) + kappa*epsilon; 
                if prec(n*(i-1)+j) <= 1.0e-8
                    prec(n*(i-1)+j) = 1.0e-8;
                end
            end
        else
            for j = k_e-k0+1:k_e
                H1(:,j-(i-1)*n) = P(I_e(j),:)'.*P(J_e(j),:)';
            end
            H2(:,1:k0) = Omega*H1(:,1:k0);
            H1 = H1';
            for j=1:k0
                prec(n*(i-1)+j) = H(I_e(n*(i-1)+j),:)*Omega0(:,J_e(n*(i-1)+j)) + H1(j,:)*H2(:,j);
                prec(n*(i-1)+j) = prec(n*(i-1)+j)/2;
                prec(n*(i-1)+j) = c(n*(i-1)+j)*prec(n*(i-1)+j) + kappa*epsilon;
                if prec(n*(i-1)+j) <= 1.0e-8
                    prec(n*(i-1)+j) = 1.0e-8;
                end
            end
        end  % End of the second if 
    end  % End of the first for loop
else    % approximate the diagonal preconditioner
    i=1;
    while (i<=k_e)     
        prec(i) = H(I_e(i),:)*Omega0(:,J_e(i));
        prec(i) = c(i)*prec(i) + kappa*epsilon;
        if prec(i) <= 1.0e-8
            prec(i) = 1.0e-8;
        end
        i= i+1;
    end
end    

%%% the second part: prec_zl
if k_l <= const_cg*n    % compute the exact diagonal preconditioner        
    k1 = ceil(k_l/n);
    k0 = k_l-n*(k1-1);
    for i=1:k1
        if ( i<k1 | k0==n )
            for j=(i-1)*n+1:i*n
                H1(:,j-(i-1)*n) = P(I_l(j),:)'.*P(J_l(j),:)';
            end
            H2 = Omega*H1;
            H1 = H1';
            for j=1:n
                prec(k_e+n*(i-1)+j) = z_l(n*(i-1)+j) - c(k_e+n*(i-1)+j)*( g_zl0(n*(i-1)+j)-l(n*(i-1)+j) );
                [fe fx] = smoothing_ff(epsilon,prec(k_e+n*(i-1)+j));
                prec(k_e+n*(i-1)+j) = H(I_l(n*(i-1)+j),:)*Omega0(:,J_l(n*(i-1)+j)) + H1(j,:)*H2(:,j);
                prec(k_e+n*(i-1)+j) = prec(k_e+n*(i-1)+j)/2;
                prec(k_e+n*(i-1)+j) = (1-fx) + c(k_e+n*(i-1)+j)*fx*prec(k_e+n*(i-1)+j) + kappa*epsilon;
                if prec(k_e+n*(i-1)+j) <= 1.0e-8
                    prec(k_e+n*(i-1)+j) = 1.0e-8;
                end
            end
        else
            for j = k_l-k0+1:k_l
                H1(:,j-(i-1)*n) = P(I_l(j),:)'.*P(J_l(j),:)';
            end
            H2(:,1:k0) = Omega*H1(:,1:k0);
            H1 = H1';
            for j=1:k0
                prec(k_e+n*(i-1)+j) = z_l(n*(i-1)+j) - c(k_e+n*(i-1)+j)*( g_zl0(n*(i-1)+j)-l(n*(i-1)+j) );
                [fe fx] = smoothing_ff(epsilon,prec(k_e+n*(i-1)+j));
                prec(k_e+n*(i-1)+j) = H(I_l(n*(i-1)+j),:)*Omega0(:,J_l(n*(i-1)+j)) + H1(j,:)*H2(:,j);
                prec(k_e+n*(i-1)+j) = prec(k_e+n*(i-1)+j)/2;
                prec(k_e+n*(i-1)+j) = (1-fx) + c(k_e+n*(i-1)+j)*fx*prec(k_e+n*(i-1)+j) + kappa*epsilon;
                if prec(k_e+n*(i-1)+j)<=1.0e-8
                    prec(k_e+n*(i-1)+j) = 1.0e-8;
                end
            end
        end  % End of the second if 
    end  % End of the first for loop
else  % approximate the diagonal preconditioner   
    i=1;
    while (i<=k_l)
        prec(k_e+i) = z_l(i) - c(k_e+i)*(g_zl0(i)-l(i));
        [fe fx] = smoothing_ff(epsilon,prec(k_e+i));
        prec(k_e+i) = H(I_l(i),:)*Omega0(:,J_l(i));
        prec(k_e+i) = (1-fx) + c(k_e+i)*fx*prec(k_e+i) + kappa*epsilon;
        if prec(k_e+i) <= 1.0e-8
            prec(k_e+i) = 1.0e-8;
        end
        i= i+1;
    end    
end    

%%%% the third part: prec_zu
if k_u <= const_cg*n

    k1 = ceil(k_u/n);
    k0 = k_u-n*(k1-1);

    for i=1:k1
        if ( i<k1 | k0==n )
            for j=(i-1)*n+1:i*n
                H1(:,j-(i-1)*n) = P(I_u(j),:)'.*P(J_u(j),:)';
            end
            H2 = Omega*H1;
            H1 = H1';
            for j=1:n
              prec(k_e+k_l+n*(i-1)+j) = z_u(n*(i-1)+j) - c(k_e+k_l+n*(i-1)+j)*( u(n*(i-1)+j)+g_zu0(n*(i-1)+j) );
              [fe fx] = smoothing_ff(epsilon,prec(k_e+k_l+n*(i-1)+j));
              prec(k_e+k_l+n*(i-1)+j) = H(I_u(n*(i-1)+j),:)*Omega0(:,J_u(n*(i-1)+j)) + H1(j,:)*H2(:,j);
              prec(k_e+k_l+n*(i-1)+j) = (-1/2)*prec(k_e+k_l+n*(i-1)+j);
              prec(k_e+k_l+n*(i-1)+j) = (1-fx) + c(k_e+k_l+n*(i-1)+j)*fx*(-prec(k_e+k_l+n*(i-1)+j)) + kappa*epsilon;
              if prec(k_e+k_l+n*(i-1)+j) <= 1.0e-8
                prec(k_e+k_l+n*(i-1)+j) = 1.0e-8;
              end
            end
        else
            for j = k_u-k0+1:k_u
                H1(:,j-(i-1)*n) = P(I_u(j),:)'.*P(J_u(j),:)';
            end
            H2(:,1:k0) = Omega*H1(:,1:k0);
            H1 = H1';
            for j=1:k0
              prec(k_e+k_l+n*(i-1)+j) = z_u(n*(i-1)+j) - c(k_e+k_l+n*(i-1)+j)*( u(n*(i-1)+j)+g_zu0(n*(i-1)+j) );
              [fe fx] = smoothing_ff(epsilon,prec(k_e+k_l+n*(i-1)+j));
              prec(k_e+k_l+n*(i-1)+j) = H(I_u(n*(i-1)+j),:)*Omega0(:,J_u(n*(i-1)+j)) + H1(j,:)*H2(:,j);
              prec(k_e+k_l+n*(i-1)+j) = (-1/2)*prec(k_e+k_l+n*(i-1)+j);
              prec(k_e+k_l+n*(i-1)+j) = (1-fx) + c(k_e+k_l+n*(i-1)+j)*fx*(-prec(k_e+k_l+n*(i-1)+j)) + kappa*epsilon;
              if prec(k_e+k_l+n*(i-1)+j)<=1.0e-8
                  prec(k_e+k_l+n*(i-1)+j) = 1.0e-8;
              end
            end
        end  % End of the second if 
    end  % End of the first for loop
else
    i=1;
    while (i<=k_u)
        prec(k_e+k_l+i) = z_u(i) - c(k_e+k_l+i)*( u(i)+g_zu0(i) );
        [fe fx] = smoothing_ff(epsilon,prec(k_e+k_l+i));

        prec(k_e+k_l+i) = (-1)*H(I_u(i),:)*Omega0(:,J_u(i));
        prec(k_e+k_l+i) = (1-fx) + c(k_e+k_l+i)*fx*(-prec(k_e+k_l+i)) + kappa*epsilon;

        if prec(k_e+k_l+i) <= 1.0e-8
            prec(k_e+k_l+i) = 1.0e-8;
        end
        i= i+1;
    end
end
return
%%% End of precond_matrix.m