function [P,noiter,maxcosines]=DRalg(n,m,A,B,toler,iterlimit,P0)
%        [P,noiter]=DRalg(n,m,A,B,toler,iterlimit,P0)
% We assume that the TP constraint is desired and the last T(A_i)=B_i
%  constraints is the unital constraint
% the Douglas-Rachford method is used to find a feas. TPCP P
%INPUT: 
%  n,m  input matrix sizes of A,B
%  A,B  cells of size k containing input of Hermitian matrices
%        for constraints   T(Ai)=Bi  i=1...k
%        We assume the last of these constraint is  unital T(In)=Im
%        We are including the trace preserving constraint
%  toler desired stopping tolerance   default .0001
% iterlimit   iteration limit    default 300
% P0          initial estimate for compl pos Choi matrix P, 
%                    use P0=[] if initial estimate not available
%     BUT! since we max rank here, we do not use this .. set to mnI_mn
% function matrixrep provides indices/blocks for matrix representation
%%%%%%
%OUTPUT: P      Hermitian size m*n for compl pos. T
%       noiter     iteration count
%    maxcosines   max value of the cos value of angle between subspaces
%%%%%%%%%%%%%%%%%%%
%%
%%%April 10/14:  Still to do:  feasibility check and correct cos values
%%%%%April 17/14 ???? need to debug addition of   indsrows ?????
%%%% and add the column indices?????
%%% april23/14   using with both TP and unital constraints
%%%% june14/14  Changing the order of the DR iterations???
%%%        i.e. putting psd projection first so as to get better 
%%%            stopping criteria and convergence iterate
%%%  june15/14  checking that we have the correct iterates for DR
%%%             second reflection and final iterate were changed!!
%%              Using lin residual on psd projected iterates.
%%   june21/14  R is only needed here ... so    "persistent R" is used
%%              other variables from matrixrep now 'persistent' here



%%%%%%%%%%%%%Initialize
%global R   % replaced by following line
persistent R    % unchanged R value is needed in this function for fixed r
persistent indsrows
persistent pcols pcolsinv Mfinal Mtemp pinvMfinal
persistent pinvMtemp Mtempnull finalblock
mn=m*n;
tmnm=mn*(mn-1)/2;
tnm=n*(n-1)/2;
svindu=logical(triu(ones(mn),1));  % indices for upper triangular part
sqrt2=sqrt(2);
k=length(A);
if ~exist('toler','var')
	toler=.0001;
end
if ~exist('iterlimit','var')
        iterlimit=300;
end
rankP=mn;   %%since max rank is used here
fprintf('\n **Solving for max-rank using Douglas-Rachford in file DRalg.m \n')
if ~exist('P0','var')   ||   size(P0,1)==0, 
	Mfinal=[];  % find new matrix rep and residual R
        P0=mn*(eye(rankP));  % initial starting point
	fprintf('\n\nStarting with new r value \n')
	fprintf('using [mn*I_mn ] as initial starting point for P \n')
end
P=P0;
clear P0;
if size(Mfinal)==0,  % skip the matrixrep part if L, RHS R, exists already
%find the matrix representation of the constraints and RHS
startLtime=cputime;
[indsrows,pcols,pcolsinv,Mfinal,Mtemp,pinvMfinal,pinvMtemp,...
           Mtempnull,finalblock]=matrixrep(m,n,A);
	    
% find matrix repr. of comp. pos.  T(A)
%%% L is permuted rows BUT!!! original cols from P basis order
%%  pinvL is both permuted rows and permuted cols to take advantage of structure
%%  pinvL=pinv(L(:,pcols)
%% pcols is the permutation so Lsparse=L(:,pcols)  in matrixrep
%%   has special block diag. structure
       R=zeros(k*m^2+n^2,1);
for ii=1:k,
	R((ii-1)*m^2+1:ii*m^2)=sHvec(B{ii});
end
    R(k*m^2+1:end)=[zeros(n^2-n,1);ones(n,1)];
R=R(indsrows,:);    % change rows to RHS
%%% change rows to get better block diagonal structure
%% as done for L in matrixrep
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf('time for generating essential blocks of Lsparse, pinvL and RHS is  %g   \n',...
               cputime-startLtime);

end   % end-for     if size(Mfinal)==0,

%%%%%%%%%%%%%%%%%%%%prepare for while loop
noiter=0;
norms=[];
cosines=[];
Reflpsd=P;   % P and reflection point assumed the same at start
%%%%%%%%%%%%%%%Calculate residual wrt lin. manifold for initial iterate P
%feasdetect=0;
vP=sHvec(P);
vPc=vP(pcols); % to coincide with block diagonal order
%residual=R-(Lsparse*vP(pcols));  % old way without blocks
residual2=Mfinal*(reshape(vPc(1:(tnm*(2*n^2))),2*n^2,tnm));
residual2=R(1:(tnm*(2*k)))-residual2(:); % for upper part
Preshape=reshape(vPc(tnm*2*n^2+1:end),n^2,n);
residual3=Mtemp*Preshape(:,1:end-1);
residual3=R(tnm*2*k+1:end)-[residual3(:);sum(Preshape,2)];
residual=[residual2;residual3];
residualrefl=residual;
normresidual=norm(residual);
fprintf('Starting while loop for Douglas-Rachford algorithm')
fprintf('\niter       cos-vecs     norm-proj.-resid.   PSD-proj-per.iter.time \n')
if normresidual<=toler,
	if min(eig(P))<0,   % not feasible
		normresidual=normresidual+2*toler; % ensure iteration
	else
		fprintf('zero DR iterations were needed \n')
	end
end
startwhiletime=cputime;   % start iterations
while noiter < iterlimit &&...
        normresidual>toler,   % residual of projected iterate used here
	noiter=noiter+1;
	%% Find reflection 2 - lin. manifold reflection
	% First find projection onto linear manifold using residual of refl 1
	%Pvec=pinvL*residualrefl; % Pl-P;   % old way ... P is current iterate
	%%for top left offdiag block multiplication
	Pvec1=pinvMfinal*reshape(residualrefl(1:(2*k*tnm)),2*k,tnm);
	%%for bottom right diag block multiplication
	restemp=reshape(residual(2*k*tnm+1:end-n^2),k,n-1);
	restemp2=pinvMtemp*restemp;
	%% add multn to each col.
	Mlast=(Mtempnull*residual(end-n^2+1:end))*ones(1,size(restemp,2));
	restemp3=restemp2(:)+Mlast(:);
	Msum=-sum(restemp2,2)+finalblock*residual(end-n^2+1:end);
	%restemp4=[Pvec1(:);restemp3(:);Msum];
	Pvec=[Pvec1(:);restemp3(:);Msum];
	Pvec=Pvec(pcolsinv); % using inverse cols perm. for perm in pinvL
	% construct Pdiffold=sHMat(Pvec) here
	Sympart=zeros(mn);
	Sympart(svindu)=Pvec(1:tmnm)/sqrt2;
	Sympart=Sympart+Sympart';
	Skewpart=zeros(mn);
	Skewpart(svindu)=Pvec(tmnm+1:2*tmnm)/sqrt2;
	Skewpart=Skewpart-Skewpart';
	Pdiffold=Sympart+1i*Skewpart+diag(Pvec(2*tmnm+1:end)); % 1i is sqrt(-1)
	%Pdiffold=sHMat(pinvL*residual); 
	Pl=Reflpsd+Pdiffold;  % (first) projection of Refl_1 onto linear manifold
	RLinL=2*Pl-Reflpsd;   % second refl step RLinL over lin. manif. 
	RLinL=(RLinL+RLinL')/2;  % second reflection step RLinL over lin. manif.
	%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
	Pnew=(P+RLinL)/2;   % new/next iterate  - done here in middle!!!
	Pnew=(Pnew+Pnew')/2;
	%Pnew=(n/trace(Pnew))*Pnew;  % normal. for trace-not needed after proj.
	%%%%%%%%%%%%%%%%%%%%%%%%%%%
	Pdifftwo=RLinL-Reflpsd;   % 2nd - 1st reflection points
	Pdiffone=P-Reflpsd;   % 1st reflection point - original point
	cosvalue= ...
	            real(trace(   Pdifftwo/norm(Pdifftwo,'fro')*...
		                  Pdiffone/norm(Pdiffone,'fro')...
				                                    ));
	cosines=...
	   [cosines cosvalue];
	%%%%%%%%%%%%%%%%%%%%%%%%%%%
	P=Pnew;
	%%%%%%%%%%%%%%%%%%%%%%%%%%%%project the current iterate P
	tic
	[u,d]=eig(P);
	d=real(d);  % to avoid roundoff in eig calculation
	[dd,indsall]=sort(diag(d),'descend');   
        indsneg=find(dd<=0);
        % project using largest pos. rankP eigs
	indspos=indsall(1:min(mn-length(indsneg),rankP)); 
        Rpsd=u(:,indspos)*d(indspos,indspos)*u(:,indspos)';
	if size(Rpsd,1)==0,           % proj. of nsd is 0
		Rpsd=zeros(mn);
	end
	Rpsd=(Rpsd+Rpsd')/2;  % projection of current iterate P onto psd cone
	timeforeigproj=toc;
        %P=u(:,indspos)*d(indspos,indspos)*u(:,indspos)';
	%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
	%%%%%%%%%Find residual .. residual of ****projected iterate****
	RP=real(Rpsd);
	IP=imag(Rpsd);
	vP=[sqrt2*RP(svindu);sqrt2*IP(svindu);diag(Rpsd)];
        %residual=R-(Lsparse*vP(pcols)); % update residual old way
        vPc=vP(pcols);
        residual2=Mfinal*(reshape(vPc(1:(tnm*(2*n^2))),2*n^2,tnm));
        %residual2=R(1:(tnm*(2*k)))-residual2(:);
	%residual(1:tnm*2*k)=residual2;  % need to update properly still??
        Preshape=reshape(vPc(tnm*2*n^2+1:end),n^2,n);
        residual3=Mtemp*Preshape(:,1:end-1);
        %residual3=R(tnm*2*k+1:end)-[residual3(:);sum(Preshape,2)];
        residual3=[residual3(:);sum(Preshape,2)];
        residual=R-[residual2(:);residual3]; % residual of proj. iterate
	normresidual=norm(residual);
	%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
	Reflpsd=2*Rpsd-P;
	%%%%%%%%%Find residualrefl .. residual of reflected pt Reflpsd
	RP=real(Reflpsd);
	IP=imag(Reflpsd);
	vP=[sqrt2*RP(svindu);sqrt2*IP(svindu);diag(Reflpsd)];
        %residual=R-(Lsparse*vP(pcols)); % update residual old way
        vPc=vP(pcols);
        residual2=Mfinal*(reshape(vPc(1:(tnm*(2*n^2))),2*n^2,tnm));
        %residual2=R(1:(tnm*(2*k)))-residual2(:);
	%residual(1:tnm*2*k)=residual2;  % need to update properly still??
        Preshape=reshape(vPc(tnm*2*n^2+1:end),n^2,n);
        residual3=Mtemp*Preshape(:,1:end-1);
        %residual3=R(tnm*2*k+1:end)-[residual3(:);sum(Preshape,2)];
        residual3=[residual3(:);sum(Preshape,2)];
        residualrefl=R-[residual2(:);residual3]; % residual of refl pt Reflpsd
        %residual=R-(L*vP); % update residual
        %residual=R-(L*sHvec(P)); % update residual
	%%%?????need to decide theoretically how to detect infeas
	%%%/??? what accuracy to use below
	%if abs(1-normratio)+abs(-1-cosvalue)<1e-6, % not converging
	%           feasdetect=feasdetect+1;
	%end
	if 10*round(noiter/10)==noiter,
	  fprintf(' %i   %14.4g  %14.4g   %14.4g \n',...
	   noiter,cosvalue,...
	     normresidual,timeforeigproj);
	end
	%%% check for separating hyperplane after ?? iterations???
	%%%%?????
end
	  fprintf(' %i   %14.4g  %14.4g   %14.4g \n',...
	   noiter,cosvalue,...
	     normresidual,timeforeigproj);
fprintf('time for iterations/while loop is  %g   \n',cputime-startwhiletime);
maxcosines=max(cosines);
%if feasdetect>=4,
%	fprintf('algorithm shows - infeasibile???\n')
%	fprintf('a separating hyperplane is ????\n')
%end
%%%%%%%%%%%remove comments to plot ratios of norms and cosines
%figure(1)
%plot(norms)
%title('ratios of norms of consecutive iterates')
%figure(2)
%plot(cosines)
%title('cosines of angles between consecutive iterates')
P=Rpsd;  % use the ***projected iterate*** to check feasibility!!!
fprintf('final cosine value is  %g  \n',cosines(end));
%%%%%%%%%%%%%%%%%verify/check constraints
fprintf('checking feasibility errors for constraints in DRalg.m using the ***projected*** last iterate Rpsd \n')
for kk=1:k,
	if norm(B{kk}-TP(m,n,P,A{kk}))>(max(m,n)*toler),
		if kk==k ,
		   fprintf('error for last k= %i for ||TP(A_k)-B_k||,  is  %g \n',...
		   kk,norm(B{kk}-TP(m,n,P,A{kk})));
		else
		   fprintf('error for k= %i for ||TP(A_k)-B_k||,  is  %g \n',...
		   kk,norm(B{kk}-TP(m,n,P,A{kk})));
	        end
	end
end
if norm(eye(n)-partialtrace(m,n,P))>((mn)*toler),
          fprintf('error for partial trace ||parttr(P)-I_n||,  is  %g \n',...
		   norm(partialtrace(m,n,P)-eye(n)));
end
