function  [X, y, Z, muu, itersinfo, meritfn] = grssdp(C, A, b, toler, precond, crosstoler, lsqr, itoler, X_0, y_0)
%function  [X, y, Z, muu, itersinfo, meritfn] = grssdp(C, A, b, toler, precond, crosstoler, lsqr, itoler, X_0, y_0)
%% 	INPUT:
%
%%   C  -  given real symmetric n by n matrix
%%   A  -  given m real symmetric n by n matrices in the form of Matlab cell structure
%	 b  -  given real m by 1 vector  
%
%%   toler - relative tolerance for stopping criteria of the pd-ip algorithm
%    precond - indicator for type of preconditioning 
%                                   0 for no preconditioning
%                                   1 currently for opt diag precond.
%                                   2 currently for Q-less QR block diagonal precond
%    crosstoler - (optional input) tolerance for start of 
%                         			crossover to sigmaa=0 and no pos. check. 
%                       			default is .1 (see below)
%	 lsqr - indicator for least-square solver
%									0 for lsmr (default)
%									1 for lsqr
%	 itoler - parameter to select initial solution for X and Z ((1/toler)^{itoler}I)
%									default is itoler = 0 (I is the initial solution)
%	 X_0 - optional initial primal feasible solution
%	 y_0 - optional initial dual feasible solution
%
%% 	OUTPUT:
%%	 
%%	 X - optimum matrix
%%   y, Z - optimum dual vector,matrix
%%   muu - duality gap at end (trace XZ)
%%   itersinfo - 2 dim vector with
%%           	(number of iterations used, crossover iteration no.)
%%   meritfn - if included as desired input (optional), this is a 
%%             	vector with the value of ||F||| at each iteration
%%             	and a plot at completion.
%%
% 	PARAMETERS:
%
%      iterbnd   - upper bound on number of iterations
%
% 	Copyright (c) 2010 by
%		Xuan Vinh Doan, Serge Kruk, and Henry Wolkowicz
%
% 	DEVELOPMENTS:
%
% 	 December 16, 2009:
%          Start to modify and develop the algorithm.
%	 January 27, 2010:
%		   Include the option for infeasible starting point
%	 January 30, 2010:
%		   Change how to perform ssMat with sMind indices (inverse of svind indices)
%		   Change bases for both row and null space of A: A = [I AQ] and Q = [-AQ; I]
% 		by finding a well-conditioned m by m block in the original A
%		   Pass both permutation indf and the inverse permutation iindf (for the well-conditioned
%		block in A) in ssdpj.m to calculate the Jacobian and its adjoint
%		   Keep X and Z as current available and pass them to ssdpj.m
%	 March 08, 2010:
%		   Start the diagonal preconditioner. Need to update ssdpj.m
%  	 March 12, 2010:
%		   Start the block diagonal preconditioner. Need to add ssdpx.m and ssdpz.m to calculate Z(.) and X(.)
%	 March 18, 2010:
%		   Change crossover condition to include measure of infeasibility. Add display information
%	 June 07, 2010:
%		   Change from lsqr to lsmr, need to use sdplsmr.m code
%	 August 25, 2010:
%		   Add DIMACS error measures at the end
%		   Calculate preconditioners more efficiently by using original matrices 
%		   Remove matrix transpose in calculation of Jacobian and its adjoint
%	 September 01, 2010:
%		   Remove full-rank checking for matrix mA (to test with larger matrices)
%		   Use updated empirical distribution of condition numbers to select a well-conditioned sub-matrix
%		   Keep geometric scale for step sizes
%	 September 03, 2010:
%		   Add possible premature stopping criteria if precond=2, tolerance<1e-10, and lsqr iteration reaches the limit
%	 October 26, 2010:
%		   Change time calculation from cputime to tic and toc 
%	 October 29, 2010:
%		   Add option to use either lsmr or lsqr
%		   Add option to use (1/toler)^{itoler}I for both Z and X as initial solution
%	 November 25, 2010:
%		   Complete version 0.1
%
rand('state',sum(100*clock));
n = size(C,1); %matrix size
m = length(b); %number of constraints
n2 = n^2; %number of equations in G-N formulation
tn = n*(n+1)/2; %number of variables in G-N formulation
svind = find(triu(ones(n),1)); %linear indices of the strictly upper triangle
sMind = zeros(n2,1); %matrix indices with respect to linear indices of the diagonal and the strictly upper triangle
sMind(svind) = 1:tn-n; %strictly upper triangle
sMind(1:n+1:n2) = tn-n+1:tn;%the diagonal
%calculate the linear indices of the strictly lower triangle
indM = zeros(n); %matrix indices
indM(:) = 1:n2;
indM = indM';
lsvind = indM(svind); %strictly lower triangular 
sMind(lsvind) = 1:tn-n; %strictly lower triangular

%check the arguments
if ~exist('crosstoler','var')
   crosstoler = .1;    %default tolerance to start crossover
end
if ~exist('precond','var')
   precond = 0;        %default is no preconditioning
end
if ~exist('toler','var')
   toler = 1e-09;	   %default tolerance
end
if ~exist('lsqr','var')
	lsqr = 0;		%default solver is lsmr
end
if ~exist('itoler','var')
	itoler = 0;		%default initial solution is identity matrix
end
wantmeritfn = nargout>=6;

disp(['solve new problem with crosstoler dim-n, tn1=n(n+1)/2 tn1^2: ',...
   num2str(crosstoler),'  ',num2str(n),'  ',num2str(n*(n+1)/2),...
   ' ',num2str((n*(n+1)/2)^2)]);

%initial parameters
sigmaa = 1;     % centering parameter - changes adaptively
step = 1;       % initial stepsize - changes adaptively
				% initially was 2 - using 1 for cond. numb tests
tau1 = .05;     % initial backtrack value
				% using: step = min(.99997,1-min(tau1,abs(tau1*gapp)))*step;
iterbnd = 50;   % bound on number of major iterations
nnzchol = 0;    % nnz for cholinc

%%Initializations
mA = zeros(m,tn); %matrix form of linear operator A
for i=1:m
	mA(i,:) = ssvec(A{i},svind)';
end
%check the rank of A
%if (rank(mA)<m)
%	disp('A is not full-rank');
%	keyboard;
%end
%the constant value x_0 and c
%tmA = mA';
%x_0 = tmA*((mA*tmA)\b);
x_0 = mA\b;
c = ssvec(C,svind);
%find an orthonomal basis for A
%Q = null(mA);
fnum = 0.0;
indf = 1:tn;
%find a square submatrix with low condition number
disp('Find new bases for the row space and null space of A ...');
vnum = [];
for i=1:tn
	rind = randperm(tn);
	mmA = mA(:,rind(1:m));
	cnum = rcond(mmA);
	vnum = [vnum cnum];
	tnum = mean(vnum) + 2*std(vnum);
	if (cnum > fnum)
		indf = rind;
		fnum = cnum;
		%disp([num2str(i) ': ' num2str(1/fnum)]);
	end
	%keyboard;
	%stop if fnum > mean(vnum) + 2*std(vnum)
	if (fnum > tnum)
		break;
	end
end
%disp(['Checked ' num2str(i) ' submatrices to get a submatrix with condition number ' num2str(1/fnum)]);
T = mA(:,indf(1:m)); %the transformation matrix for basis change 
mA = T\mA; %change basis for the row space of A
%construct new A in matrix form
pA = cell(m,1);
for i=1:m
	pA{i} = ssMat(mA(i,:),n,sMind);
end
%the common block between A and Q
AQ = mA;
AQ(:,indf(1:m)) = []; %remove the identity matrix
% basis for the null space of A
Q = [-AQ; eye(tn-m)];
%shift the rows of Q
indf(m+1:tn) = sort(indf(m+1:tn));
%the inverse permutation
iindf = zeros(tn,1);
iindf(indf) = 1:tn;
Q = Q(iindf,:);
%construct Q in matrix form
pQ = cell(tn-m,1);
for i=1:tn-m
	pQ{i} = ssMat(Q(:,i),n,sMind);
end

v = zeros(tn-m,1); %initial solution v
%there is no initial primal solution
if ~exist('X_0','var')
	x = ssvec(1/toler^itoler*eye(n),svind);
else
	%check semidefiniteness of primal solution
	[~,choltrueX] = chol(X_0);
	if choltrueX > 0 %switch to primal infeasibility
		disp('The initial solution X_0 is not positive semidefinite. Switch to primal infeasible approach');
		x = ssvec(1/toler^itoler*eye(n),svind);
	else
		x_0 = ssvec(X_0,svind);
		x = x_0;
	end
end
X = ssMat(x,n,sMind); %the original matrix variable
%there is no initial dual solution
if ~exist('y_0','var')
	y = zeros(m,1);
	Z = 1/toler^itoler*eye(n);
	z = ssvec(Z,svind);
else
	%check semidefiniteness of dual solution
	z = c - (y_0'*mA)';
	Z = ssMat(z,n,sMind);
	[~,choltrueZ] = chol(Z);
	if choltrueZ > 0 %switch to dual infeasibility
		disp('The initial solution Z_0 is not positive semidefinite. Switch to dual infeasible approach');
		y = zeros(m,1);
		Z = 1/toler^itoler*eye(n);
		z = ssvec(Z,svind);
	else
		y = y_0;
	end
end
%calculate the residuals
rx = x - x_0 - Q*v;
rd = z - c + (y'*mA)';		
%calculate the optimality matrix ZX
ZX = Z*X;
gapp = trace(ZX);
muu=gapp/n; %initial parameter muu
ZXnorm = norm(ZX,'fro'); %for crossover
%calculate the current objective
optval = c'*x;

noiter = 0;
crossover = 0; %indicator for crossover algor.
itersinfo(2) = 0; %in case no crossover takes place

if wantmeritfn, 
	meritfn = []; 
end

stagnant = 0; %flag to check if the gap is not improved
%add display information
disp('noiter   -log10(relgapp)    step        sigmaa         optval         lsqrtime      cputimeprec    lsqr.iter  nnz-X  nnz-chol');

while ( ~crossover*(gapp/(abs(optval)+1) > toler)  & (noiter<iterbnd)  )...
         |(crossover*(ZXnorm)/(abs(optval)+1) > toler& (noiter<iterbnd));

	noiter=noiter+1;
	%compute RHS at current iterate and current muu estimate
	Fmu = ZX - sigmaa*muu*eye(n);
    Fmu = Fmu - Z*ssMat(rx,n,sMind);
    Fmu = Fmu - ssMat(rd,n,sMind)*X;


	%find the search direction at the current iterate
	tfordirc = cputime;
	damp = 0;
	localsize = 0;
   %  change accuracy: adaptive depending on muu!!! and no iterations
   %%  not working properly???i.e. precond fails when adaptive - why???
   %%                 needs more accuracy for large dimensions??
   %%            ?????to add: nnzQ influence with dimensions????
   %atol   = 1.0e-6;
   %btol   = 1.0e-6;
   %%%?????need to include change in atol,btol after crossover?????
%   atol   = 1e-14;
%   btol   = max(1.0e-1*toler,...
%             (1.0e-6)*(min(muu,1))^(1/3)  ); 
%   btol = max(btol,(1e-14)/muu);
%%%%%%%%%%%%%%%%%%sept/04 working tolerances below%%%%%%%%%%%%%
    atol = max(1.0e-1*toler,...
			(1.0e-6)*(min(muu,1))^(1/3)  );  
    btol = max(1.0e-1*toler,...
			(1.0e-6)*(min(muu,1))^(1/3)  ); 
	%atol = max(1.0e-5*toler,...
	%		(1.0e-10)*(min(muu,1))^(1/3)  );  
    %btol = max(1.0e-5*toler,...
	%		(1.0e-10)*(min(muu,1))^(1/3)  ); 
%%%%%%%%%%%%%%%%%%sept/04 working tolerances above%%%%%%%%%%%%%
   %atol   = max(1.0e-1*toler,...
   %          (1.0e-6)*sqrt(min(muu,1)));  %adaptive not working???why??
   %btol   = max(1.0e-1*toler,...
   %          (1.0e-6)*sqrt(min(muu,1)));
	conlim = 1.0e+10;
	itnlim = 10*tn;
	show   = 0;
   
	%PRECONDITIONING
    stimeprec = tic;
	Tv = [];
	Ty = [];
	%diagonal preconditioner
	if precond == 1
		Tv = zeros(tn-m,1);
		Ty = zeros(m,1);
		%calculate Jacobian J
		for i=1:tn-m
			%ei = zeros(tn,1);
			%ei(i) = 1;
			%Ji = ssdpj(1, n2, tn, ei, svind, sMind, X, Z, AQ, indf, iindf, 0, Tv, Ty);
			Ji = Z*pQ{i};
			Tv(i) = norm(Ji,'fro');
			%keyboard;
		end
		for i=1:m
			%ei = zeros(tn,1);
			%ei(tn-m+i) = 1;
			%Ji = ssdpj(1, n2, tn, ei, svind, sMind, X, Z, AQ, indf, iindf, 0, Tv, Ty);
			Ji = pA{i}*X;
			Ty(i) = norm(Ji,'fro');
			%keyboard;
		end
    else
		%block diagonal preconditioner
		if precond == 2
			%calculate transformation matrix for Z: R^{tn-m} -> M^n
			mZ = zeros(n2,tn-m);
			for i=1:tn-m
				%ei = zeros(tn-m,1);
				%ei(i) = 1;
				%Zi = ssdpz(n, ei, sMind, Z, AQ, iindf);
				Zi = Z*pQ{i};
				mZ(:,i) = Zi(:);
				%keyboard;
			end
			%Q-less QR decomposition
			Tv = triu(qr(mZ));
			Tv = Tv(1:tn-m,:);
			%calculate transformation matrix for X: R^m -> M^n
			mX = zeros(n2,m);
			for i=1:m
				%ei = zeros(m,1);
				%ei(i) = 1;
				%Xi = ssdpx(n, ei, sMind, X, AQ, iindf);
				Xi = -pA{i}*X;
				mX(:,i) = Xi(:);
				%keyboard;
			end
			%Q-less QR decomposition
			Ty = triu(qr(mX));
			Ty = Ty(1:m,:);
		end
	end
	timeprec=toc(stimeprec);       
   %%%%%%%%%%%%%%%%%%%
   
	%assign data to calculate J and J* at each point (v,y)
    %xz = [x z]; 
	stlsqr = tic;
	%%[ deltas, istop, lsqriter, rnorm, xnorm, var ] =  ...  %  for var variable
	if lsqr==1
		[ deltas, istop, lsqriter, rnorm, xnorm      ] =  ...
		sdplsqr( n2, tn, 'ssdpj', svind, sMind, X, Z, AQ, indf, iindf, precond, Tv, Ty, -Fmu(:), damp, atol, btol, conlim, itnlim, show );
	else
		[ deltas, istop, lsqriter, rnorm, normAr, normA, condA, xnorm      ] =  ...
		sdplsmr( n2, tn, 'ssdpj', svind, sMind, X, Z, AQ, indf, iindf, precond, Tv, Ty, -Fmu(:), damp, atol, btol, conlim, itnlim, localsize, show );
	end
	%sdpj is used to calculate J(v,y) given A and Q and also J*(M)
	tlsqr = toc(stlsqr);
   %  disp(['solvlss-lsqr: itns rnorm, xnorm cputime:  ',num2str(itn),' ',...
   %      num2str(rnorm),' ',num2str(rnorm),' ',num2str(tlsqr)]);
   %deltax=deltas(1:tnm1);deltay=deltas(tnm1+1:length(deltas));
   %  disp(['solvlss-lsqr: atol btol :  ',num2str(atol),' ',num2str(btol)]);
   
   % if precond==2,
     % deltas(1:tnm1)=cholmat11\deltas(1:tnm1);
     % deltas(1:tnm1)=deltas(invpcols);
     % deltas(end-n+1:end)=deltas(end-n+1:end)./dataxy(end-n+1:end);
   % elseif precond==1,
   % deltas=deltas./dataxy;   % old diagonal preconditioner
   % elseif precond==0,   % no preconditioner
   % end
	%calculate the directions
	deltav = deltas(1:tn-m);
	deltay = deltas(tn-m+1:end);
	if precond == 1
		deltav = deltav./Tv; %scale dv back according to Tv;
		deltay = deltay./Ty; %scale dy back according to Ty;
	else
		if precond == 2
			deltav = Tv\deltav;
			deltay = Ty\deltay;
		end
	end
    deltax = Q*deltav - rx;
	deltaX = ssMat(deltax,n,sMind);
    deltaz = -(deltay'*mA)' - rd;
	deltaZ = ssMat(deltaz,n,sMind);
	%total time to find the directions
	dirtimec = cputime - tfordirc;
%%%%%%%%%%%%%%%%%%%%%%%
%%% start crossover - needs lots of polishing
	if ((norm(rx)+norm(rd)+gapp)/(abs(optval)+1) < crosstoler)  & ~crossover ,
		crossover = 1;
		step = 1; %step stays 1 after crossover starts
		sigmaa = 0; %stays 0 after crossover starts
		itersinfo(2) = noiter;
		disp(['*********CROSSover started at: ',num2str((norm(rx)+norm(rd)+gapp)/(abs(optval)+1))]);
   %% disp('noiter   -log10(relgapp)    step        sigmaa         optval         lsqrtime      cputimeprec    lsqr.iter  nnz-X  nnz-chol');
     %disp('noiter -log10(relnormZX)     step       sigmaa     optval          cputime.dir,');
	end
	
	if ~crossover,
%     disp(['starting the back tracking  ']);
		%vp = v + step*deltav;
		%yp = y + step*deltay;
        %xp = x + step*deltax;
        %zp = z + step*deltaz;
		Xp = X + step*deltaX;
		Zp = Z + step*deltaZ;
		%check the semidefiniteness of solution
        [~,choltrueX] = chol(Xp);
        [~,choltrueZ] = chol(Zp);
		%[~,choltrueXZ] = chol(Xp+Zp-min(1e-3,sqrt(muu))*eye(n));
		while choltrueX + choltrueZ > 0,
%%testing ?????
        %step=.9999*step;   % small backtrack to get bndry
                       % change to eig approach?????
%%testing ?????
			step = .8*step;  % original backtracking
			%vp = v + step*deltav;
			%yp = y + step*deltay;
            %xp = x + step*deltax;
            %zp = z + step*deltaz;
			Xp = X + step*deltaX;
			Zp = Z + step*deltaZ;
			[~,choltrueX] = chol(Xp);
			[~,choltrueZ] = chol(Zp);
			%[~,choltrueXZ] = chol(Xp+Zp-min(1e-3,sqrt(muu))*eye(n));
		end
  %  step=.999997*step;     % original backtrack strategy
%%testing ?????
     %step=min(1.07,1.32-min(tau1,abs(tau1*gapp)   )    )*step;
%%testing ?????
		step = min(.99997,1-min(tau1,abs(tau1*gapp)))*step; %proper way
  %  step=.97*step;
	end
%%% end crossover modif - below for sigmaa as well.
	%update all the variables
	v = v + step*deltav;
	y = y + step*deltay;
    x = x + step*deltax;
    z = z + step*deltaz;
	X = X + step*deltaX;
	Z = Z + step*deltaZ;
	% v = vp;
	% y = yp;
    % x = xp;
    % z = zp;
	% X = Xp;
	% Z = Zp;
    rx = x - x_0 - Q*v;
    rd = z - c + (y'*mA)';
	%update the optimality matrix ZX
    ZX = Z*X;
	ZXnorm = norm(ZX,'fro'); %%for crossover check
	gapp = trace(ZX);
	muu = gapp/(2*n);
	%calculate the current objective
    optval = c'*x;
%  reler is relative error: duality-gapp/n1/(optval+1)
	if crossover,
		muu = ZXnorm/(abs(optval)+1);  % need positive for lsqr tolerance atol,btol

		fprintf(' %3g      %6.2e         %6.2e    %8.2e     %5.4e    %2.4e     %2.4e   %5g   %5g   %5g\n', ...
			noiter,-log10(muu), ...
			1,0,optval,tlsqr,timeprec,lsqriter,nnz(X),nnzchol);
	else
		fprintf(' %3g      %6.2e         %6.2e    %8.2e     %5.4e    %2.4e     %2.4e   %5g   %5g   %5g\n', ...
		    noiter,-log10(full(gapp)/(optval+1)), ...
			step,sigmaa,optval,tlsqr,timeprec,lsqriter,nnz(X),nnzchol);


%disp([sprintf(' %3g      %6.2e         %4g    %10g     %5.4e      %2.4e  %2.4e %2.4e', ...
%       noiter,-log10(ZXnorm/(abs(optval)+1)), ...
%        1,0,optval,dirtimec)]);
%  else
%disp([sprintf(' %3g      %6.2e       %7.4e    %2.1e     %2.4e      %2.4e  %2.4e %2.4e   %2.4', ...
%       noiter,-log10(gapp/(optval+1)), ...
%        step,sigmaa,optval,dirtimec,min(min(eig(Z),eig(X))))]);


% update muu and sigmaa depending on stepsize
		if step < .001,
			muu = muu * 2.1;
			sigmaa = 1;
			step = .9;  %muu increased
		elseif step < .01, 
			muu = muu * 2;
			sigmaa = 1;
			step = 1;  % muu unchanged
		elseif step < .25, 
			muu = muu * 1.5; 
			sigmaa = 1;
			step = 1; 
%%   testing for influence of condition number on lsqr
%%% steplength restricted to one when using lsqr - seems to help
%%% the next 3 lines are changed to get steplength <=1
%% This appears to be TRUE in case of crossover soon after,
%%   i.e. stay well centered till crossover.
%     elseif step < .95, muu = muu * 1.2; sigmaa=1-.3*min(step,1);step=1.5;
%     elseif step < 1.05, muu = muu *.5; sigmaa=1-.3*min(step,1); step=2.0;
%     else  sigmaa=1-.3*step;step=2;muu=muu*.8;
		elseif step < .95, 
			muu = muu * 1.2; 
			sigmaa = 1-.3*min(step,1);
			step = 1.0;
		elseif step < 1.05, 
			muu = muu *.5; 
			sigmaa = 1-.3*min(step,1); 
			step = 1.0;
		else  
			sigmaa = 1-.3*step;
			step = 1;
			muu = muu*.8;
		end
	end
%%% end crossover modif - see above as well
	if wantmeritfn, 
		meritfn = [meritfn ZXnorm];
	end
%%% check to see whether the lsrqr is stagnant
	if (precond==2 & ~crossover & lsqriter==itnlim) %& gapp/(abs(optval)+1) < 1e-10)
		disp(['to USER from grssdp.m: Possible maximum tolerance reached - alogrithms ends']);
		stagnant = 1;
		break;
	end
end          % end of main while loop
%calculate the final solution
%X = ssMat(x,n,sMind);
%Z = ssMat(z,n,sMind);
y = (y'/T)'; %change back to the original basis of the row space of A
%  if wantmeritfn,
%    plot(-log10(meritfn/(abs(optval)+1)));
%    title('plot of -log10(norm(ZX)/(abs(optval)+1))')
%  end
if noiter < iterbnd & stagnant==0
	disp('to USER from grssdp.m: Tolerance achieved - algorithm ends')
end
if noiter >= iterbnd,
	disp(['to USER from grssdp.m: ERROR;  exceeded iteration bound:  ',...
		num2str(noiter)]);
end
disp(['for crossover; final rel. norm(ZX) is: ',num2str(ZXnorm/(abs(optval)+1))]); 
disp(['for crossover; rel. min of eigs is: ',...
	num2str( min(  min(eig(X)),min(eig(Z)) )/(abs(optval)+1)  ) ]); 
itersinfo(1) = noiter;
%DIMACS errors
pfeas = b;
for i=1:m
	pfeas(i) = pfeas(i) - sum(sum(A{i}.*X));
end
err1 = norm(pfeas)/(1+max(abs(b)));
err2 = max([0 -min(eig(X))/(1+max(abs(b)))]);
dfeas = Z - C;
for i=1:m
	dfeas = dfeas + y(i)*A{i};
end
err3 = norm(dfeas,'fro')/(1+max(abs(C(:))));
err4 = max([0 -min(eig(Z))/(1+max(abs(C(:))))]);
err5 = (sum(sum(C.*X))-b'*y)/(1+abs(sum(sum(C.*X)))+abs(b'*y));
err6 = sum(sum(Z.*X))/(1+abs(sum(sum(C.*X)))+abs(b'*y));
disp(['DIMACS error measures: ' num2str(err1) ' ' num2str(err2) ' ' num2str(err3) ' ' num2str(err4) ' ' num2str(err5) ' ' num2str(err6)]);
