function [P,noiter,maxcosines]=altproj(n,m,A,B,toler,iterlimit,P0)
%        [P,noiter]=altproj(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 Alternating projection 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
%%%%%%%%%%%%%%%%%%%
%%
%%   june23/14  starting with AP 



%%%%%%%%%%%%%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 Alternating projection in file altproj.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=[];
normPdiffold=norm((P));    % using P_-1 = 0
%feasdetect=0;
vP=sHvec(P);
vPc=vP(pcols); % to coincide with block diagonal order
%residual=R-(Lsparse*vP(pcols));  % old way
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];
fprintf('Starting while loop for Alt-Proj algorithm')
fprintf('\niter norms-ratio       cos-vecs     norm-res   eig-proj-res PSD-proj-per.iter.time \n')
startwhiletime=cputime;   % start iterations
while noiter < iterlimit &&...
        max(norm(residual),normPdiffold)>toler,
       %max(norm(residual),normPdiffold)>toler && feasdetect<4, % ????
	noiter=noiter+1;
	%Pvec=pinvL*residual; % Pl-P;   % old way
	%%for top left offdiag block multiplication
	Pvec1=pinvMfinal*reshape(residual(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-P
	Pl=P+Pdiffold;  % (first) projection onto linear manifold
	%%%RLinL=2*Pl-P; 
	%%%RLinL=(RLinL+RLinL')/2;  % first reflection step
	%%%check Pl is on linear manifold
	normPdiffnew=norm(Pl-P,'fro');
	norms=[norms normPdiffnew/normPdiffold];
	%fprintf('norm of residual after proj onto lin manifold  %g \n',...
	%                norm(full(R-(L*sHvec(Pl)))));
	%fprintf('ratio of norms after proj onto lin manifold  %g \n',...
	%                normPdiffnew/normPdiffold);
	tic
	%%%[u,d]=eig(RLinL);
	[u,d]=eig(Pl);
	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)';
        P=u(:,indspos)*d(indspos,indspos)*u(:,indspos)';
	%%%%Rpsd=(Rpsd+Rpsd')/2;  % second reflection step
	timeforeigproj=toc;
        %P=u(:,indspos)*d(indspos,indspos)*u(:,indspos)';
	           % projection onto psd
        %%%%Pnew=(Rpsd+P)/2;
	%%%%%%%%%%%%%%%%%%%%%%%%%%%
	if size(P,1)==0,           % proj. of nsd is 0
		P=zeros(mn);
	end
	P=(P+P')/2;    
	P=(n/trace(P))*P;  % normalize for trace
	Pdiffnew=P-Pl;
	%%%%%%%%%%%%%%change cos values ... different angles?????
	%%%still need to fix this ?????
	cosvalue= -real(trace(Pdiffold*Pdiffnew)/...
	          (norm(Pdiffold,'fro')*norm(Pdiffnew,'fro')));
	cosines=...
	   [cosines cosvalue];
	RP=real(P);
	IP=imag(P);
	vP=[sqrt2*RP(svindu);sqrt2*IP(svindu);diag(P)];
	%if norm(vP-sHvec(Pnew))>1e-12,
	%	fprintf('errror in sHvec P')
	%	keyboard
	%end
        %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=R-(L*vP); % update residual
        %residual=R-(L*sHvec(P)); % update residual
	%normratio=normPdiffnew/normPdiffold;
	%%%P=Pnew;
	%%%?????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  %8.4g  %8.4g \n',...
	   noiter,cosvalue,...
	     norm(residual),norm(P-Pl),timeforeigproj);
	end
	normPdiffold=normPdiffnew;
	%%% check for separating hyperplane after ?? iterations???
	%%%%?????
end
	  fprintf(' %i  %14.4g  %14.4g  %8.4g  %8.4g \n',...
	   noiter,cosvalue,...
	     norm(residual),norm(P-Pl),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')
fprintf('final cosine value is  %g  \n',cosines(end));
%%%%%%%%%%%%%%%%%verify/check constraints
fprintf('checking feasibility errors for constraints in altproj.m using last iterate P \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
