function [fval, dval, pfeas, gap, X, iter, time, exitflag] = lssdp_gp(G,P,q,Bindex,d,tol,itbnd)

% This code uses gradient projection algorithm to solve the dual formulation of the robust least square SDP
% problem:
% 
% min ||X - G||^2   s.t. q_i - P_i(X) in SOC, X_ii = 1 for all i, X_rs = d_j, X is p.s.d.
%
% This code is solving the dual problem as a minimization problem, i.e.,
% minimizing the negative of the dual objective function
%
% Input (k must be >= 2)
%
% G     - n by n matrix
%
% P     - r cells of k by n^2 matrix. For fixed r, the k by n^2 matrix corresponds to the
%        rth linear map in kth the uncertainty center
%
% q     - r by k matrix, whose columns are the vector corresponding to the kth
%        uncertainty center
%
% Bindex- m by 2 matrix, for the indices (r,s) of the m additional equality constraints.
%
% d     - m by 1 vector. 
%
% tol   - tolerance for convergence
%
% itbnd - max number of iterations
%
%
% Output
%
% fval,dval- optimal values
% 
% pfeas    - absolute primal feasibility
%
% gap      - relative primal dual duality gap
%
% X        - optimal solution
%
% iter     - number of iterations
%
% exitflag - exitflag. 1 - successfully solved the problem up to
%                          desired accuracy within the given number of iterations
%                      0 - quitted with max iteration or tiny stepsize
%                     -1 - infeasible instance
%
% time     - cpu time, measured by tic-toc
%
% Written by Guoyin Li, Alfred Ka Chun Ma and Ting Kei Pong
% Last modified: Nov 6, 2012

fprintf(' ********* start grad proj *********\n')

% Initialization
tstart = tic;
sigma = 1e-4;
M = 200;
n = size(G,1);
r = size(q,1);
k = size(q,2);
m = size(Bindex,1);
d = 2*d; % multiply by 2 to account for the input type of B
exitflag = 1;

nG = norm(G,'fro');
p_bnd = 0.5*(n + nG)^2;

nG = nG^2/2;
vecG = G(:);
mu = zeros(n + m,1);
s = zeros(k,r);
ds = s;

step_init = ones(r+1,1);

% initial function value and gradient
[dval, X] = objective(m,n,r,vecG,P,q,Bindex,d,mu,s,nG);
pval = norm(X - G,'fro')^2/2;
grad = gradient(m,n,k,r,P,q,Bindex,d,X);

d1 = -inf*ones(M,1); % for nonmonotone line-search
d1(1) = dval;
dmax = max(d1);
dbest = dval; % record the best dual objective for termination check

pfeas = 0;
for i = 1:r
  v = grad(k*(i-1)+1:k*i,1);
  pfeas = max(pfeas, norm(v - soc_proj(v)));
end
pfeas = max([pfeas, norm(grad(k*r+1:k*r+n)), norm(grad(k*r+n+1:end)/2)]);

fprintf('  iter          pval         dval     step        pfeas         time\n')

% main loop
iter = 1;
while iter <= itbnd
  
  gap = abs(pval + dbest)/max(abs(pval),1);
  if gap < 100*tol && pfeas < tol
    % terminate by small relative duality gap and absolute feasibility
    break
  end
    
  % compute directional derivative
  dderiv = 0;
  for i = 1:r
    ds(:,i) = soc_proj(s(:,i) - step_init(i)*grad(k*(i-1)+1:k*i)) - s(:,i);
    dderiv = dderiv + grad(k*(i-1)+1:k*i)'*ds(:,i);
  end
  dmu = -step_init(r+1)*grad(k*r+1:end);
  dderiv = dderiv + grad(k*r+1:end)'*dmu;
    
  stepsize = 1;
  snew = s + stepsize*ds;
  munew = mu + stepsize*dmu;
  [dval,X] = objective(m,n,r,vecG,P,q,Bindex,d,munew,snew,nG);
    
  while dval > dmax + sigma*stepsize*dderiv && stepsize > 1e-15 % Armijo rule for descent algorithm
        
    stepsize = stepsize/2;
    snew = s + stepsize*ds;
    munew = mu + stepsize*dmu;
    [dval,X] = objective(m,n,r,vecG,P,q,Bindex,d,munew,snew,nG);
        
  end
    
  if stepsize <= 1e-15
    fprintf(' Terminate due to small step size in line search: stepsize %g\n', stepsize)
    exitflag = 0;
    break
  end
    
  % update step_init
  gradnew = gradient(m,n,k,r,P,q,Bindex,d,X);
    
  Dg = gradnew - grad;
  Ds = stepsize*ds; % snew - s
  Dmu = stepsize*dmu; % munew - mu
    
  for i = 1:r
    step_init(i) = max(1e-8,min(norm(Ds(:,i))^2/(Ds(:,i)'*Dg(k*(i-1)+1:k*i)),1e5));
  end
  step_init(r+1) = max(1e-8,min((Dmu'*Dmu)/(Dmu'*Dg(k*r+1:end)),1e5));
    
  % update variables
    
  mu = munew;
  s = snew;
  grad = gradnew;
    
  % update pval, dval and pfeas
  d1(mod(iter,M) + 1) = dval;
  % check for infeasibility
  if -dval > p_bnd
    fprintf(' Primal infeasible instances: primal bound = %g\n', p_bnd)
    pval = inf;
    X = -eye(n);
    exitflag = -1;
    break
  end
  
  dmax  = max(d1);
  pval  = norm(X - G,'fro')^2/2;    
  pfeas = 0;
  for i = 1:r
    v = grad(k*(i-1)+1:k*i,1);
    pfeas = max(pfeas, norm(v - soc_proj(v)));
  end
  pfeas = max([pfeas, norm(grad(k*r+1:k*r+n)), norm(grad(k*r+n+1:end))/2]);
  dbest = min(dbest,dval);  % record the best dual objective for termination check
    
  if mod(iter,500) == 0
    fprintf(' %6d %12.7g %12.7g %8.4g %12.7g %12.7g\n', iter, pval, -dbest, stepsize, pfeas, toc(tstart))
  end
    
  iter = iter + 1;    
end
time = toc(tstart);
fval = pval;
dval = -dbest;

if iter > itbnd
  exitflag = 0;
end

fprintf(' iter = %d time = %g, fval = %g, pfeas = %g\n', iter, time, pval, pfeas)