function [Y, C, infos] = CENewton_d(D, w, pars)
%
% call: [Y, C, infos] = CENewton_d(D, w, pars)
% 
% This code is designed to solve %%%%%%%%%%%%%
%   min 0.5*\| W^{1/2}(Y-D)W^{1/2}\|^2,   W is diagonal
%   s.t. Y_ii - Y_{imax,imax} =0, i= 2,...,n                   (P)
%        Y \in \K^n_- (almost negative semidefinite cone in \S^n)
% where i_0 comes from [wmax, imax] = max(w)
% Date: January 28, 2015; April 11, 2016
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Input: D  -- predistance matrix (D >= 0)
%        w  -- diaginal weight (strictly positive)
%       
% pars: -- parameters needed as input
%
% pars.y -- starting point, a vector of length(D)
%              default: use map_initial to get one;
% pars.eigenlevel -- 1.0e-5 (yes, treat >= 1.0e-5 as positive eigenvalues)
% pars.scaling -- = 0 no scaling 
%                 = max(max(D))  (default)
% pars.tol -- error tolerance (default: 1.0e-6)
% pars.nls -- maxumum number of linear search (default: 3)
% pars.printyes -- printyes = 1 print information
%                             0 no output information (default)
% Output:
%      Y -- Best almost nonpositive matrix from D (optimal solution of (P)
%      C -- Constant that defines the diagonals
%           Y-C or Y - 0.5*(a*ones(1,n)+ones(n,1)*a') is the EDM
%           where a = diag(Y)
%    infos:
%      infos.y  = the optimal Lagrange multiplier corresponding 
%                 to the diagonal constraints
%      infos.rank = Embedding_Dimension;
%      infos.Iter = k;
%      infos.feval = f_eval; number of function evaluations;
%      infos.t = time_used; total cpu used
%      infos.res = norm_b; norm of the gradient at the final iterate
%                  This is the residual one finally got.
%      infos.f = val_obj; final objective function value 
%      infos.EigenD -- Total number of eigen decompositions
%%          
% Send your comments and suggestions to    %%%%%%
%        hdqi@soton.ac.uk      %%%%%%
%
%
%%%%% Warning: Accuracy may not be guaranteed!!!!! %%%%%%%%
%%
%
t0 = tic;
n  = size(D, 1);

% set up parameters
if ~isfield(pars, 'tol')
    pars.tol = 1.0e-6;
end

if ~isfield(pars, 'printyes')
    pars.printyes = 0;
end
prnt = pars.printyes;
    
if ~isfield(pars, 'y')
    [~, ~, infos] = CENewton(D, pars);
    y = infos.y;
else
    y = pars.y;
end

if prnt
   fprintf('\n ******************************************************** \n')
   fprintf( '          The Semismooth Newton-CG Method (CNENewton_d.m)        ')
   fprintf('\n ******************************************************** \n')
   fprintf('\n The information of this problem is as follows: \n')
   fprintf(' Dim. of    sdp      constr  = %d \n',n)
end

% put the lastest w(i) at the end (as w(n))
% w = Qw, where Q(i,i) = 1; i \not= imax, n
%               Q(imax, n) = P(n, imax)  = 1;
%               Q(i,j) = 0 otherwsie
%               Q is a symmetric permulation matrix
[wmax, imax] = max(w);
imax         = imax(1);
w(imax)      = w(n);
w(n)         = wmax;

% let D = QDQ
D1         = D(:, imax);
Dn         = D(:, n);
D(:, n)    = D1;
D(:, imax) = Dn;
D1         = D(imax, :);
Dn         = D(n, :);
D(n, :)    = D1;
D(imax, :) = Dn;

D = (D+D')/2; % make sure D symmetric

% reset D to \tilde{D}, denoted by TD
sqrtw = sqrt(w);
SrW   = bsxfun(@times, sqrtw, sqrtw');  %=(sqrtw*sqrtw');
TD    = D.*SrW;

% scale TD so that D_{ij} \in [0,1]
Sfactor = sum(sum(abs(TD)))/nnz(TD); % Sfactor = 1 to return to unscaled version
% Sfactor = max(max(D));
if ~isfield(pars, 'scaling') % default
   pars.scaling = Sfactor;
end

Sfactor = pars.scaling; % do not use scaling
TD      = TD/Sfactor;
prnt    = pars.printyes;
y       = y/Sfactor;
error_tol = pars.tol/Sfactor;
%%%%%
% calculate JDJ
%
sumw  = sum(w);
Dw    = TD*sqrtw;
td    = sqrtw'*Dw;
JDJ   = bsxfun(@times, sqrtw, Dw'); % =sqrtw*Dw';
JDJ   = (JDJ+JDJ')/sumw;
JDJ   = TD - JDJ + td/(sumw^2)*SrW;

% define eta \in \Re^{n-1}
eta = w(1:n-1)/w(n);

k      = 0;
f_eval = 0;
EigenD      = 0; % eigen decomposition number
Iter_Whole  = 20; % 200 maximum iterations
if ~isfield(pars, 'nls') % nls: number of linear search
    pars.nls = 3; % maximum number of linear search is three
end
Iter_inner  = pars.nls; % Maximum number of Line Search in Newton method
maxit   = 200; %Maximum number of iterations in PCG
tol     = 1.0e-2; %relative accuracy for CGs
sigma_1 = 1.0e-4; %tolerance in the line search of the Newton method
%
prec_time = 0;
pcg_time  = 0;
eig_time  =0;

% calculate Y = J(D+\A^*(y))J
Y = JyJ(y, w);
Y = JDJ + Y;
Y = (Y+Y')/2;

%%%%% eigendecomposition
eig_time0  = tic;
[P,lambda] = qieig(Y); 
eig_time   = eig_time + toc(eig_time0); 
EigenD     = EigenD + 1;
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[f0,Fy] = gradient(y, w, TD, lambda, P);
f       = f0;
f_eval  = f_eval + 1; % number of function evaluations increased by 1
b       = - Fy;
norm_b  = norm(b);

Initial_f = - f0;
if prnt
    fprintf('Initial Dual Objective Function value==== %d \n', Initial_f)
    fprintf('Newton: Norm of Gradient %d \n',norm_b)
end

Omega12 = omega_mat(lambda,n);
x0 = y;

tt = toc(t0);
[hh,mm,ss] = time(tt);

if prnt
    fprintf('\n   Iter.   Num. of CGs     Step length      Norm of gradient     time_used ')
    fprintf('\n    %d         %2.0d            %3.2e                      %3.2e         %d:%d:%d ',0,str2double('-'),str2double('-'),norm_b,hh,mm,ss)
end

while (norm_b>error_tol && k < Iter_Whole)

    prec_time0 = tic;
    c = precond_matrix(w, Omega12, P); % comment this line for  no preconditioning
    prec_time = prec_time + toc(prec_time0);
    pcg_time0 = tic;
    %tol = min(5.0e-3, 0.1*norm(b));
    [d,flag,~,iterk]  = pre_cg(b,w,tol,maxit,c,Omega12,P); % pcg is best 
    pcg_time = pcg_time + toc(pcg_time0);
  
    if (flag~=0); % if CG is unsuccessful, use the negative gradient direction
      d = -Fy;
         disp('..... Not a full Newton step......')
    end
    slope = (Fy)'*d; %%% nabla f d 

    k_inner = 0;
    while ( 1~= 2)
       rho = 0.5^k_inner;
       y   = x0 + rho*d;
       Y   = JyJ(y, w);
       Y   = (JDJ + Y);
       Y   = (Y+Y')/2;
       ta  = tic;
       [P, lambda] = qieig(Y);
       eig_time = eig_time + toc(ta);
    
       [f, Fy] = gradient(y,w,TD,lambda,P);
    
       if (k_inner >= Iter_inner) || ( f <= f0 + sigma_1*rho*slope + 1.e-6)
          break
       end
       k_inner = k_inner + 1;
    end

    if (k_inner > 1)
        fprintf('\n number of line serach: %d', k_inner)
    end
    f_eval = f_eval + k_inner + 1; % number of function evaluations
                         % is increased over the line search
                         % and the one before it
    x0 = y;
    f0 = f;

    k = k+1;
    b = -Fy;
    norm_b = norm(b);
    tt = toc(t0);
    [hh,mm,ss] = time(tt); 
    
    if prnt      
        fprintf('\n   %2.0d         %2.0d             %3.2e          %3.2e         %d:%d:%d', ...
        k,iterk,0.5^k_inner,norm_b,hh,mm,ss)
    end   
    Omega12 = omega_mat(lambda,n);
 end
 %end loop for while i=1;

Ip = find(lambda>0); % could set to 1.0e-7
In = find(lambda < 0);
r    = length(Ip);
rank = length(In); 
infos.rank = rank;

% The eigen-decomposition is on -J(D+\A^*y)J
% The imbedding dimension is the number of negative eigenvalues of
% -J(D+\A^*y)J.
% This result is based on Haydan-Wells projection formula
% see Eq. (37) in my paper.

% \A^*(y) = diag(-sum(y); y);
y0 = eta'*y;
z  = [y; -y0];
if (r==0)
    Y = TD + diag(z);
elseif (r==n)
    Y = TD + diag(z) - Y;
elseif (r<=n/2)
    lambda1 = abs(lambda(Ip));
    lambda1 = lambda1.^0.5;
    P1 = P(:, 1:r);
    P1 = P1*sparse(diag(lambda1));
    Y = P1*P1';
    Y = TD+ diag(z) -  Y;% Optimal solution X* 
else     
    lambda2 = abs(lambda(n-rank+1:n));
    lambda2 = lambda2.^0.5;
    P2 = P(:, n-rank+1:n);
    P2 = P2*sparse(diag(lambda2));
    Y = Y + P2*P2'; 
    Y = TD + diag(z) - Y;% Optimal solution X* 
end

 Y = (Y+Y')/2;
 Final_f = - Sfactor^2*f;

% dual matrix Z
 Z =  Y - (TD + diag(z));      % Z belong to the dual cone of (\K^n_w)^*;
                             % see note on 14/04/2016
% comp_gap = sum(sum(Y.*Z)); % complementarity gap trace(YZ) = 0 in theory
                             % Since Y, Z are computed according the
                             % formula, comp_gap should be near zero.

val_obj   = sum(sum((Y-TD).*(Y-TD)))/2;
val_obj   = val_obj*Sfactor^2;
norm_b    = norm_b*Sfactor;
y         = Sfactor*y;
% lambda    = Sfactor*lambda;
time_used = toc(t0); 

% set output information
% infos.P      = P;
% infos.lambda = lambda;
infos.Iter   = k;
infos.feval  = f_eval;
infos.t      = time_used;
infos.res    = norm_b;
infos.f      = val_obj;
infos.EigenD = EigenD;
infos.dualVal   = Final_f;
infos.primVal   = val_obj;

% scale back of Y
Y = Sfactor*Y;

% put diagonal weights back
Y = Y./SrW;
C = Y(1,1);
Z = Z.*SrW;

% Put back the right order of Y
D1         = Y(:, imax);
Dn         = Y(:, n);
Y(:, imax) = Dn;
Y(:, n)    = D1;
D1         = Y(imax, :);
Dn         = Y(n, :);
Y(imax, :) = Dn;
Y(n, :)    = D1;

% Put back the right order of Z
D1         = Z(:, imax);
Dn         = Z(:, n);
Z(:, imax) = Dn;
Z(:, n)    = D1;
D1         = Z(imax, :);
Dn         = Z(n, :);
Z(imax, :) = Dn;
Z(n, :)    = D1;
infos.Z    = Sfactor*Z;

% Put back the right order of y
y(imax) = - y0;
infos.y = y;

% Not on computing the embedding points
% Embedding points cannot be computed as the unweighted case, where we have
% J_w = J = I - (1/n) ee^T.
% If we need to compute the embedding points, we have to do one more
% eigen-decomposition on - 0.5JYJ
% sumY   = sum(Y, 2);
% totalY = sum(sumY);
% JYJ = bsxfun(@times, sumY/n, ones(1,n));
% JYJ = Y - (JYJ+JYJ') + totalY/n^2;
% [P, lambda] = qieig(JYJ);
% lambda = abs(lambda);       % lambda has been mulitplied by Sfactor
% lambda = lambda(end:-1:1);
% P = P(:, end:-1:1);
% infos.lambda = lambda;
% infos.P = P;
% lambda = 0.5*lambda(1:rank); % positive eigenvalues in -0.5JYJ
% P = P(:, 1:rank);
% 
% P = P*diag(lambda.^(0.5));
% P = P';
% infos.X = P;                 % embedding points in columns



if prnt
    fprintf('\n\n')
    fprintf('Norm of Gradient == %d \n', full(norm_b))
    fprintf('Number of Iterations == %d \n', k)
    fprintf('Number of Function Evaluations == %d \n', f_eval)
    fprintf('Final Dual Objective Function value ========== %d \n', full(Final_f))
    fprintf('Final Original Objective Function value ====== %d \n', full(val_obj))
    fprintf('Embedding dimension ================= %d \n', full(rank))

    fprintf('Computing time for computing preconditioners == %d \n', prec_time)
    fprintf('Computing time for linear systems solving (cgs time) ====%d \n', pcg_time)
    fprintf('Computing time for  eigenvalue decompostions (calling mexeig time)==%d \n', eig_time) 
    fprintf('Total computing time (in s) ==== =====================%d \n',time_used)
end
% 


%%% end of the main program

%%% 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);
return
%%% End of time.m

function [P,lambda] = qieig(X)
X = full(X);
[P, Lambda] =  eig(X);   %% X= P*diag(D)*P'
P           =  real(P);
lambda      =  real(diag(Lambda));
    
% make sure eigenvalues are in decreasing order
if issorted(lambda)
    lambda = lambda(end:-1:1);
    P      = P(:,end:-1:1);
elseif issorted(lambda(end:-1:1))
    return;
else
    [lambda, Inx] = sort(lambda,'descend');
    P = P(:,Inx);
end
return

%%%%%% To generate J*diag(y)J
%
function Y = JyJ(y, w)
% y is column vector, to compute J\A^*(y)J
n     = length(w);
sqrtw = sqrt(w);
sumw  = sum(w);
eta   = w(1:n-1)/w(n);
y0    = -(eta'*y);
z     = [y; y0]; 

wz = sqrtw.*z;
Y  = bsxfun(@times, sqrtw, wz'); % = sqrtw*wz';
Y  = (Y+Y')/sumw;

Y = diag(z) - Y; % the sqrtw'*diag(z)*sqrtw = 0 (the last term is zero)

return
%% end of JyJ

%%%%%% To generate F(y) %%%%%%%
%%%%%%%
% this function iu being tested and is right
function [f,Fy]= gradient(y, w, TD, lambda, P)
% d = diag(TD) in the main programme

n = length(w);
d = diag(TD);
f = 0.0;

eta = w(1:n-1)/w(n);
y0  = - eta'*y;
z   = [y; y0]; % \A^*(y) = diag(z)
 
%lambdap=max(0,lambda);
%H =diag(lambdap); %% H =P^T* H^0.5*H^0.5 *P

r = sum(lambda>0);
lambda1 = abs(lambda(1:r)).^0.5;
P = P(:,1:r)';
P = bsxfun(@times, lambda1, P);
% for i = 1:r;
%     P(i,:) = lambda1(i)*P(i,:);
% end
 
 P = P.*P;
 Fy = d + z - (sum(P))';
 Fy = Fy(1:n-1) - Fy(n)*eta;

% Compute the objective function 
r       = sum(lambda<0); % negative eigenvalues
lambdan = lambda((n-r+1):end);
f       = f + lambdan'*lambdan;
%
sqrtw = sqrt(w);
sumw  = sum(w);
z0    = TD*sqrtw;
z     = z0 + (z.*sqrtw);
a0    = (z0'*sqrtw)/sumw; 
 
f = f + 2*sum(z.^2)/sumw - a0^2;
f = 0.5*(f - sum(sum(TD.^2)));

return

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%% end of gradient.m %%%%%%

%%%%%%%%%%%%%%        %%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%% To generate the first -order difference of lambda
%%%%%%%

%%%%%%%%%%%%%%        %%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%% To generate the essential part of the first -order difference of d
%%%%%%%
function Omega12 = omega_mat(lambda,n)
%We compute omega only for 1<=|idx|<=n-1
idx.idp = find(lambda > 1.0e-9);
idx.idm = setdiff(1:n,idx.idp);
n =length(lambda);
r = length(idx.idp);
 
if ~isempty(idx.idp)
    if (r == n)
        Omega12 = ones(n,n);
    else
        s = n-r;
        dp = lambda(1:r);
        dn = lambda(r+1:n);
        Omega12 = (dp*ones(1,s))./(abs(dp)*ones(1,s) + ones(r,1)*abs(dn'));
        %  Omega12 = max(1e-15,Omega12);

    end
else
    Omega12 =[];
end

    %%***** perturbation *****
    return

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%% end of omega_mat.m %%%%%%%%%%

%%%%%% PCG method %%%%%%%
%%%%%%% This is exactly the algorithm 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,w,tol,maxit,c,Omega12,P)

% Initializations
n      = length(w);
r      = b;  %We take the initial guess x0=0 to save time in calculating A(x0) 
n2b    = norm(b);    % norm of b
tolb   = tol * n2b;  % relative tolerance 
p      = zeros(n-1,1);
flag   = 1;
iterk  = 0;
relres =1000; %%% To give a big value on relres

% Precondition 
z   =r./c;  %%%%% z = M\r; here M =diag(c); if M is not the identity matrix 
rz1 = r'*z; 
rz2 = 1; 
d   = z;
% CG iteration
for k = 1:maxit
   if k > 1
       beta = rz1/rz2;
       d = z + beta*d;
   end
   ww = Jacobian_matrix(d,w,Omega12,P); % W =A(d)
%   wT = Tw_Jacobian_matrix(d,w,Omega12,P,n);
%    norm(ww-wT)
%    pause
   denom = d'*ww;
   iterk =k;
   relres = norm(r)/n2b;              %relative residue = norm(r) / norm(b)
   if denom <= 0 
       disp('pcg not working')
       p = d/norm(d); % d is not a descent direction
       break % exit
   else
       alpha = rz1/denom;
       p = p + alpha*d;
       r = r - alpha*ww;
   end
   z = r./c; %  z = M\r; here M =diag(c); if M is not the identity matrix ;
   if norm(r) <= tolb % Exit if Hp=b solved within the relative tolerance
       iterk =k;
       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%%%%%%%%%%%


%%% To generate the Jacobian product with x: F'(y)(x)
function Ax = Jacobian_matrix(x, w, Omega12, P)

n      = length(w);
[r, s] = size(Omega12); 
%tau = 1.0e-10;
tau = 0;

sqrtw = sqrt(w);
sumw  = sum(w);
eta   = w(1:n-1)/w(n);

x0 = eta'*x;
z  = [x; -x0]; % \A^*(z) = diag(z)

if (r==0) % Omega = 0
    Ax = (1 + tau)*x + x0*eta;
    return
elseif (r==n) % Omega = E this case is impossible
    Ax = (2/sumw)*( w(1:n-1).*x + w(n)*x0*eta) + tau*x;
    return
end

zbar = sqrtw.*z;

P1 = P(:,1:r);
P2 = P(:,r+1:n);
q  = P1'*zbar;
q0 = P1*q;
q0 = q0.*sqrtw;

q0 = q0(1:n-1) - q0(n)*eta;
q0 = (2/sumw)*q0; 

if (r<n/2)
   H1 = bsxfun(@times, P1', z'); %H1 = P1'*sparse(diag(z));
   Omega12 = Omega12.*(H1*P2);
   H = [(H1*P1)*P1' + Omega12*P2'; Omega12'*P1'];
%%%%%%%%%%%%%%%%%% 1st way
   H = P.*H';
   d = sum(H, 2);
   d = d(1:n-1) - d(n)*eta;
%%%%%%%%%%%%%%%%%% 2nd way
%    i=1;
%     while (i<=n)
%         Ax(i) = P(i,:)*H(:,i); % part from the digonal part
%         i = i+1;
%     end
%%%%%%%%%%%%%%%%%%%% end of 2nd way
    Ax = ( x+ x0*eta ) - d + q0 + tau*x; 
%
else % if r>=n/2, use a complementary formula.
     %H = ((E-Omega).*(P'*Z*P))*P';               
     H2 = bsxfun(@times, P2', z'); %H2 = P2'*sparse(diag(z));
     Omega12 = ones(r,s)- Omega12;
     Omega12 = Omega12.*((H2*P1)');
     H = [Omega12*P2'; Omega12'*P1' + (H2*P2)*P2'];
%%%%%%%%%%%%%%%%%% 1st way
     H = P.*H';
     d = sum(H, 2);
     d = d(1:n-1) - d(n)*eta;
%%%%%%%%%%%%%%%%%% 2nd way
%    i=1;
%     while (i<=n)
%         Ax(i) = P(i,:)*H(:,i); % part from the digonal part
%         i = i+1;
%     end
%%%%%%%%%%%%%%%%%%%% end of 2nd way
     Ax = d + q0 + tau*x;
end
return
%%% End of Jacobian_matrix.m  
   

%%%%%% To generate the diagonal preconditioner%%%%%%%
%%%%%%%

function c = precond_matrix(w, Omega12, P)

n           = length(w);
r           = size(Omega12, 1);
sumw        = sum(w);
eta         = w(1:n-1)/w(n);
sqrtw       = sqrt(w);

c = 1 + eta.^2;
% cn = zeros(n,1);
% dn = zeros(n,1);

if (r==0) % Omega = 0
    return % c = 2*ones(n-1,1);
elseif (r==n) % Omega = E (this case is impossible)
    t = 2/sumw;
    c = t*( w(1:n-1) + w(n)*(eta.^2) );
    return
end
%
H = P';
H = H.*H;

% compute c0
P1T = P(:, 1:r)';
a   = P1T(:, n);
A   = sqrtw(n)*bsxfun(@times, a, eta');
P1T = bsxfun(@times, P1T, sqrtw');
% A = a(:, ones(1, n-1));
% A = sqrtw(1)*( A*sparse(diag(eta)) );
% P1T = P1T*sparse(diag(sqrtw));
P1T = P1T(:, 1:n-1) - A;
P1T = P1T.^2;
c0  = (2/sumw)*sum(P1T);
c0  = c0';

% compute tau0
pn2 = H(:, n);

if r < n/2
    tau0 = sum(pn2(1:r))^2 + 2*pn2(1:r)'*(Omega12*pn2(r+1:n));
    for i = 1:n-1
        pi2 = H(:, i);
        tau1 = sum(pi2(1:r))^2 + 2*pi2(1:r)'*(Omega12*pi2(r+1:n));
        pin = P(i, :) .* P(n, :);
        pin = pin';
        tau2 = sum(pin(1:r))^2 + 2*pin(1:r)'*(Omega12*pin(r+1:n));
        tau = tau1 - 2*eta(i)*tau2 + eta(i)^2*tau0;
        c(i) = 1 + eta(i)^2 - tau;
    end
else
    Omega12 = 1 - Omega12;
    tau0 = sum(pn2(r+1:n))^2 + 2*(pn2(1:r)'*Omega12)*pn2(r+1:n);

    for i = 1:n-1
        pi2 = H(:, i);
        tau1 = sum(pi2(r+1:n))^2 + 2*(pi2(1:r)'*Omega12)*pi2(r+1:n);
        pin = P(i, :) .* P(n, :);
        pin = pin';
        tau2 = sum(pin(r+1:n))^2 + 2*(pin(1:r)'*Omega12)*pin(r+1:n);
        tau = tau1 - 2*eta(i)*tau2 + eta(i)^2*tau0;
        c(i) = tau;
    end
end        

c = c+c0;
c = max(1.0e-8, c);
return

