function   [optval,Lam,oX,X,oD,D,V,noiter]=main(A,H,toler) %revising nov20/14
%
%   call is: [optval,Lam,oX,X,oD,D,V,noiter]=main(A,H,toler);
%
%  solves the approx. closest distance matrix problem in Frobenius norm
%   min || spones(H).*(K_V(X)-A)||_F^2, X psd
%    using p-d i-p method with least squares (Gauss-Newton) search direction.
%
% INPUT: this function can be called from  gendist.m 
%     where data matrices A,H are generated and are defined.
%       A  is data to approximate by distance matrix
%       H  is weights on graph edges - can be 0-1
%    toler   is the desired tolerance in the duality gap
%            default is    1e-8
%
% OUTPUT:  optval =norm(H.*(A-KV(X,V,H)),'fro')^2;
%          Lam =(2*KWs(KW(X-B,V,H),V,H)); dual vrble for opt cond.
%          oX   is primal opt. psd matrix - possibly high rank
%               gap = trace(Lam*X);   % duality gap
%           X   is lower rank obtained using Eckert-Young theorem
%          oD   final distance matrix, oD=K_v(oX)=KV(oX) from high rank X
%           D   final distance matrix, D=K_v(X)=KV(X) from small rank X
%           V   V'e=0, basis of e^\perp for use in KV.m
%         noiter    number of iterations
%
if ~exist('toler','var')
	toler=1e-8;
end
%  Parameters:  % conservative at the start
sigmaa=1;      % centering parameter - changes adaptively
alpha=1.5;        % initial stepsize - changes adaptively
%
%Initialization:
n=size(H,1);
H=sparse(H);
H=H/normest(H);   % normalization for error growth
H(1:n+1:end)=0;   % ensure that diagonal is zero
% check if H has a zero row (or column)
%  Now check that the graph of H is connected - diag(H) =0 assumed.
label=1;
scan=1;
labelscan=[];
while ~isempty(label),
   %  scan 
     scan=label(1);
     labelscan=union(labelscan,scan);
     label=setdiff(label,scan);
     indnzs=find(H(scan,:)~=0);
     label=union(label,indnzs);
     label=setdiff(label,labelscan);
end
if length(labelscan)<size(H,1),
   disp('  ');
   disp('graph of H is disconnected - solve two smaller problems');
   disp('quit now please - or risk slow or nonconvergence');
   keyboard
end
%  continue initialization
A=sparse(spones(H).*A);    % free elements  might as well be 0
n=size(A,1);
n1=n-1;
n2=n1^2;
tn1=(n)*(n-1)/2;   % for dim n-1
on1=ones(n1);
indn1=find(triu(on1)==1);   % indices for upper triang. part
indn2=find(triu(ones(n1),1)==1);   % indices for strict upper triang.  part
%
% construct orthogonal V s.t. V'e=0
y=-1/sqrt(n);
x=-1/(n+sqrt(n));
V=[y*ones(1,n-1)
   (x*ones(n-1)+eye(n-1))  ];
%
V=sparse(V);
B=sparse(-.5*(V)'*A*(V));    %  A in dim n1 space
%  initial estimates 
X=sparse(B+1.2*normest(B,'fro')*speye(n1));
Lam=sparse(2*KWs(KW(X-B,V,H),V,H));
%  mineig is 1 if not p.d. and 0 if p.d.
[~,mineig]=chol(Lam);
if mineig > 0,
   fprintf('error - initial estimates  Lam not p.d. - mult of I added \n')
   Lam=sparse(Lam+1.2*normest(Lam));
end
[~,mineig]=chol(X);
if mineig > 0,  % X not p.d.
      fprintf('error - initial estimates  X not p.d. - mult of I added \n')
       X=sparse(X+1.2*normest(X));
end
gap = trace(Lam*X);   % duality gap
muu = gap /(2*n1);   % conservative decrease for first iteration
normHA=normest(H.*A,'fro');
Fd= (2*KWs(KW((X-B),V,H),V,H)-Lam);
Fd=(Fd+Fd')/2;
Fc = Lam*X - sigmaa*muu * eye(n1);
optval=norm(H.*(A-KV(X,V,H)),'fro')^2;
%   construct upper fixed part of operator for lss system F'muu*ds=-r
tforKu=cputime;
[Ku,lowinds]=Fpmuup(V,H,n1);  % finds Ku  - more efficiently!!!! to do still!!
fprintf('time for upper fixed part of operator F %g \n',cputime-tforKu);
Kop=[Ku
    zeros(n2,2*tn1) ];

noiter=0;
% clean up variable to same memory
clear inddiag x y  labelscan label scan on1;
%  check stopping condition, i.e. both gap and dual feas are small or
%    optimal value is small.
fprintf('noiter,    -log10(relgap),    -log10(Fd),      alpha,           sigma,         optval,       dirtime \n')
while min( ...
    (  max(   (gap/(optval+1)),   (norm(Fd,'fro')/normHA)     )      ) ...
                      ,optval ) > toler;
noiter=noiter+1;

%%%%  compute least squares direction
%tforKd=clock;
Kd=Fpmulow(Lam,X,lowinds,V,H);
%Kdtime=etime(clock,tforKd); 
%disp(['lower part of F :   ',num2str(Kdtime)]);
Kop(tn1+1:end,:)=Kd;
temp=Fd;
temp(indn2)=sqrt(2)*temp(indn2);
%  rhs=sparse([  temp(indn1)   %  RHS is NOT sparse????
rhs=([  temp(indn1)
       Fc(:) ]);
if any([isnan(Kop) isnan(rhs)]),
    fprintf('nan found in Kop or rhs \n')
     keyboard
end
tfordir=clock;
clear Kd temp   % clear memory for lss solution
dir = -Kop\rhs;     % revised nov/14; ???? is it full rank?
if any(isnan(dir)),
    fprintf('nan found in dir \n')
     keyboard
end
dir=sparse(dir);
dirtime=etime(clock,tfordir);
%  reshape separately
dX= zeros(n1);
dX(indn1)=dir(1:tn1);
dX=sparse(dX+triu(dX,1)');
dX=.5*sparse(dX+dX');
%
dLam= zeros(n1);
dLam(indn1)=dir(tn1+1:2*tn1);
dLam=sparse(dLam+triu(dLam,1)');
dLam=.5*sparse(dLam+dLam');
%
%%  check with normal operator equation for lss
[tFd,tFc]=Fpmu(dLam,dX,Lam,X,V,H);
[tdLam,tdX]=Fpmus(tFd,tFc,Lam,X,V,H);
check1=[tdX
      tdLam];
[trhs1,trhs2]=Fpmus(Fd,Fc,Lam,X,V,H);
check2=[trhs2
        trhs1];
check=check1+check2;
warning('off');
normcheck=normest(check)/max(1,normest(check2));
warning('on');
if normcheck> 1e-10,
 fprintf('normal eqns check failed, rel. norm error is: %0.5g \n',normcheck)
 %disp('re-solving using full with normal equations rather than sparse qr')
 %dir=-(  ( full(Kop)'*full(Kop) )\ (full(Kop')*full(rhs) )  );
 resolve=0;     % change to 1 if above resolve is done
 %  reshape separately again if resolve is done
 if resolve==1,
  dX= zeros(n1);
  dX(indn1)=dir(1:tn1);
  dX=dX+triu(dX,1)';
  dX=.5*sparse(dX+dX');
  %
  dLam= zeros(n1);
  dLam(indn1)=dir(tn1+1:2*tn1);
  dLam=dLam+triu(dLam,1)';
  dLam=.5*sparse(dLam+dLam');
  %
  %%  check with normal operator equation for lss again
  [tFd,tFc]=Fpmu(dLam,dX,Lam,X,V,H);
  [tdLam,tdX]=Fpmus(tFd,tFc,Lam,X,V,H);
  check1=[tdX
        tdLam];
  [trhs1,trhs2]=Fpmus(Fd,Fc,Lam,X,V,H);
  check2=[trhs2
          trhs1];
  check=check1+check2;
  normcheck=normest(check)/max(1,normest(check2));
  if normcheck> 1e-10,
 fprintf('normal eqns check fail again, rel. error is: %0.5g \n',normcheck)
  end
 end    % end for double check after resolve
end
%
%   symmetrize to avoid complex eigs.
X=.5*sparse(X+X');
Lam=.5*sparse(Lam+Lam');
dX=.5*sparse(dX+dX');
dLam=.5*sparse(dLam+dLam');
mineig=1;
while mineig > 0,
  Xs = X + alpha * dX; 
  Lams = Lam + alpha * dLam; 
%  mineig is 1 if not p.d. and 0 if p.d.
  [~,mineig]=chol(Lams);
  if mineig ==0,
     [~,mineig]=chol(Xs);
  end
  alpha=.95*alpha;
  if alpha < 10^(-5),  %  set to 0 first if small,  not just break!!!
      disp('WARNING alpha too small and reset to 0');
      alpha=0;  
      break
  end
end
 Xs = sparse(X + alpha * dX);
  Lams =sparse(Lam + alpha * dLam);


% update
X=Xs;
Lam=Lams;
gap = trace(Lam*X);   % duality gap
muu = gap /(2*n1);
%Fd= sparse(2*KWs(KW((X-B),V,H),V,H)-Lam);    % is it sparse?????
Fd= (2*KWs(KW((X-B),V,H),V,H)-Lam);
Fc = Lam*X - sigmaa*muu * eye(n1);
optval=norm(H.*(A-KV(X,V,H)),'fro')^2;

%  reler is relative error: duality-gap/n1/(optval+1)
fprintf(' %3i         %g       %e   %e   %e     %e    %e  \n', ...
       noiter,-log10(full(gap)/(optval+1)), ...
        -log10( (norm(Fd,'fro')/normHA) ),alpha,sigmaa,optval,dirtime)
% update continued after iteration display
% update muu and sigmaa depending on stepsize
      if alpha < .001,muu = muu * 2.1;sigmaa=1;alpha=.9;  %muu increased
      elseif alpha < .01, muu = muu * 2;sigmaa=1;alpha=1;  % muu unchanged
      elseif alpha < .25, muu = muu * 1.5; sigmaa=1;alpha=1; 
      elseif alpha < .95, muu = muu * 1.2; sigmaa=1-.3*min(alpha,1);alpha=1.5;
      elseif alpha < 1.05, muu = muu *.5; sigmaa=1-.3*min(alpha,1); alpha=2.0;
      else  sigmaa=1-.3*alpha;alpha=2;muu=muu*.8;
      end
%% check for emergency stop at end of iteration and save data
%emerg
save saveiter X Lam noiter optval
end;   %  end of while
% verify that distance matrix was found
oX=X;   % save original X
oD=KV(oX,V,H);   % construct the distance matrix from final oX=X high rank
     [~,mineig]=chol(-V'*oD*V);
if any(diag(oD)~=0) || mineig>0 ,
  fprintf('min eig of -VtDV: %g \n ',num2str(min(eig(-full(V'*oD*V)))));
  fprintf('largest diagonal in abs value: %g \n ',num2str(max(abs(diag(oD)))));
end
rankX=rank(full(oX),.0001);
if rankX > 3,
   fprintf('original rank is %i and is decreased to 3 \n',rankX)
   [P,G]=eig(full(X));
   dG=diag(G);
   [~,indeigs]=sort(dG,'descend');  
   dG=dG(indeigs(1:3));
   P=P(:,indeigs(1:3));
   X=P*diag(dG)*P';
   X=.5*(X+X');
   rankX=3;   % reset in order to plot
end
figure(2)
spy(H)
title('adjacency (nonzeros) for graph of problem')
figure(1)
% plot points if possible
  [u,d]=eig(full(X));
  d=diag(d);
  [~,inds]=sort(d,'descend');
  R=u(:,inds(1:rankX));
  R=V*R*diag(sqrt(d(inds(1:rankX))));
  clf
  [iii,jjj]=find(triu(H)~=0);
  if rankX==1,
     for iind=1:length(iii),
       ii=iii(iind);jj=jjj(iind);
       xp=[ R(jj,1) R(ii,1)];
       yp=[ 0 0];
       distpt=norm(R(jj,1)-R(ii,1))^2;
       pos=R(ii,1)+(R(jj,1)-R(ii,1))/2;
       plot(xp,yp,'marker','square','markersize',5,...
           'markeredgecolor','y','markerfacecolor',[.6 0 .6],...
                 'linestyle','-','color','r','linewidth',1);
       text(pos,.2,num2str(distpt,2),'fontsize',7);   % place distance on line
       hold on
      end
      %legend('line from point to point');
  elseif rankX==2,
     for iind=1:length(iii),
       ii=iii(iind);jj=jjj(iind);
       xp=[ R(jj,1) R(ii,1)];
       yp=[ R(jj,2) R(ii,2)];
       distpt=norm(R(jj,1:2)-R(ii,1:2))^2;
       pos=R(ii,1:2)+(R(jj,1:2)-R(ii,1:2))/2;
       plot(xp,yp,'marker','square','markersize',5,...
           'markeredgecolor','y','markerfacecolor',[.6 0 .6],...
                 'linestyle','-','color','r','linewidth',1);
       text(pos(1),pos(2),num2str(distpt,2),'fontsize',7);   
                                 % place distance on line
       hold on
     end
      %legend('line from point to point');
   elseif rankX==3,
     for iind=1:length(iii),
       ii=iii(iind);jj=jjj(iind);
       xp=[ R(jj,1) R(ii,1)];
       yp=[ R(jj,2) R(ii,2)];
       zp=[ R(jj,3) R(ii,3)];
       distpt=norm(R(jj,1:3)-R(ii,1:3))^2;
       pos=R(ii,1:3)+(R(jj,1:3)-R(ii,1:3))/2;
       plot3(xp,yp,zp,'marker','square','markersize',5,...
           'markeredgecolor','y','markerfacecolor',[.6 0 .6],...
                 'linestyle','-','color','r','linewidth',1);
       text(pos(1),pos(2),pos(3),num2str(distpt,2),'fontsize',7);   
                                           % place distance on line
       hold on
      end
      %legend('line from point to point');
  end
D=KV(X,V,H); % new distance matrix from small rank X
newoptval=norm(H.*(A-KV(X,V,H)),'fro')^2; % optval with low rank X
fprintf('optvals with orig. high rank & with new low rank X: %g %g  \n ',...
           optval,newoptval);
