function [fval, X, time] = lssdp_sdpt3_p(G,P,q,Bindex,d)

% This code writes input to SDPT3 for the primal formulation of the robust least square SDP
% problem, in primal form:
% 
% min 0.5*||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.
%
% It writes input in SeDuMi format and then transform using SDPT3 default
% 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. 
%
%
% Output
%
% fval - optimal value
%
% X    - optimal solution
%
% Written by Guoyin Li, Alfred Ka Chun Ma and Ting Kei Pong
% Last modified: Oct 6, 2012

% variables v t u X

n = size(G,1);
m = size(Bindex,1);
r = size(q,1);
k = size(q,2);
nn = n*(n+1)/2 - n - m; % Taking away the equality constraints

A = sparse(1,k*r + nn + n^2 + 1);

% X_ii = 1
b = [ones(n,1);d];
for i = 1:n
  e = sparse(n,n);
  e(i,i) = 1;
  A(i,k*r+nn+2:end) = e(:)';
end
for i = 1:m
  e = sparse(n^2,1);
  e(Bindex(i,1)) = 1/2;
  e(Bindex(i,2)) = 1/2;
  A(n+i,k*r+nn+2:end) = e';
end
  
% SOC constraints: K.q
count = n + m;
for i = 1:r
  A(count+1:count+k,k*(i-1)+1:k*i) = eye(k);
  A(count+1:count+k,k*r+nn+2:end) = P{i};
  b = [b; q(i,:)'];
  count = count + k;
end

% objective
counter = 1;
for i = 1:n-1
  for j = i+1:n
    % Note that the first column of Bindex is n*(i-1)+j with j > i
    tmp = n*(i-1) + j;
    Btmp = abs(Bindex(:,1) - tmp);
    if min(Btmp) > 0
      A(count+counter,k*r+1+counter) = 1;
      A(count+counter,k*r+nn+1+n*(i-1)+j) = 1/2;
      A(count+counter,k*r+nn+1+n*(j-1)+i) = 1/2;
      b = [b;G(i,j)];
      counter = counter + 1;
    end
  end
end

c = sparse(1,k*r + nn + n^2 + 1);
c(k*r+1) = 1;

K.q = [k*ones(r,1);nn+1];

% SDP cone: K.s
 
K.s = n;
[blk,At,C,b] = read_sedumi(A,b,c,K);
clear A c K
tstart = tic;
[obj,X,y,Z,info] = sdpt3(blk,At,C,b,[]);
time = toc(tstart);

if info.termcode > 0
  X = zeros(n,n);
  fval = -1;
  return
end

X = X{2};
X = (X + X')/2;
fval = norm(X - G,'fro')^2/2;
    
    
     