function   [optval,Lam,X,D,noiter]=main(A,H,toler,algor)
%
%   call is:  [optval,Lam,X,D,noiter]=main(A,H,toler,algor)
%
%  solves the approx. closest distance matrix problem
%    using p-d i-p method with Gauss-Newton (least squares) direction.
%  This solves moderate sized problems.
%
%   Input:   A,H   - given matrix to approx. and weight matrix
%            toler - stopping tolerance for relative duality gap
%   algor    -  1 for QR solution of lss using qls.m
%               2  for NAG with Lanczos approach for lss solution
%               (1 is the default if only 3 inputs are used)
%
%   Output:  optval   -  optimal value
%            X,Lam    - primal and dual optimum matrices
%            D        - optimal distance matrix found from X
%            noiter   - number of iterations used
%
%  Parameters:  % conservative at the start of the iterations
[m,n]=size(A);
if m ~=n,
  disp(['ERROR: A is NOT a square matrix  ']);
  return
end

if nargin == 4,
  disp(['Starting new closest EDM problem dimension:  ',num2str(n)]);
elseif nargin ==3,
  algor=1;    % default lss algor
elseif nargin ==2,
  toler=1*10^(-9);algor=1;    % defaults
else
  disp(['ERROR: Not enough input arguments  ']);
  return
end
[m1,n1]=size(H);
if m1~=n1 | m1~=m,
  disp(['ERROR: dimension of H does not match that of A  ']);
  return
end
global Kop
sigmaa=1;      % centering parameter - changes adaptively
alpha=1.5;        % initial stepsize - changes adaptively
%
%Initialization:
H=sparse(H);
H=H/normest(H);   % normalization for error growth
% check if H has a zero row (or column)
inddiag=find(speye(size(H,1))==1);
H(inddiag)=zeros(length(inddiag),1);
if any(sum(H)==0),
   disp('WARNING: H has a zero column - Slater CQ will fail')
   disp('slow or no convergence may result')
   disp('0 columns (and rows) are now deleted to reduce size of problem')
   ind0=find(sum(H)~=0);
   H=H(ind0,ind0);
   A=A(ind0,ind0);
end
% skip connected check for matlab 4
%%  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
%%%%%%%%%%%%%%%%%%%%%%%%%%
% initializ. for sparse least squares call to f04qap
if algor==2,
   damp=0.0;    % for +damp*norm(x)^2 in lss
   atol=100*eps;
   btol=100*eps;
   conlim=1/atol;
   itnlim=4*tn1;
   msglvl=0;
   aprod='my_aprod';
end
%%%%%%%%%%%%%%%%%
%  Alternate (simpler?) V skipped for now.
%V=[ones(1,n1)
%   -eye(n1) ];   % sparse V though not orthogonal
%Vi=pinv(V);
%%%%%%%%%%%%%%%%%
% 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.
[tmp,mineig]=chol(Lam);
if mineig > 0,
   'error - initial estimates  Lam not p.d. - mult of I added'
   Lam=sparse(Lam+1.2*normest(Lam));
end
[tmp,mineig]=chol(X);
if mineig > 0,  % X not p.d.
      'error - initial estimates  X not p.d. - mult of I added'
       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= sparse(2*KWs(KW((X-B),V,H),V,H)-Lam);
[Fc] = Fmerit(muu,Lam,X,B,V,H,sigmaa);
Fc=sparse(Fc);
keyboard
optval=norm(H.*(A-KV(X,V,H)),'fro')^2;
%
%if algor==1,
   %   construct upper fixed part of operator for lss system F'muu*ds=-r
   tforKu=cputime;
   [Ku,lowinds]=Fpmuup(V,H,n1);  
   disp('time for upper fixed part of operator F');
   disp([cputime-tforKu]);
   Kop=[Ku
       zeros(size(Ku)) ];
%end

noiter=0;
% clean up variables to save memory
clear inddiag x y Ku labelscan label scan tmp on1;
%  check stopping condition, i.e. both gap and dual feas are small or
%    optimal value is small.
while min( ...
    (  max(   (gap/(optval+1)),   (norm(Fd,'fro')/normHA)     )      ) ...
                      ,optval ) > toler;
noiter=noiter+1;

%%%%  compute least squares direction
%  Find rhs of linear system - Fd,Fc calculated below during update
rhs=sparse([  Fd(:)
       Fc(:)
  ]);
if any(isnan(rhs)),
    'nan found in rhs'
     keyboard
end
%if algor==1,
    tforKd=clock;
    Kd=Fpmulow(Lam,X,lowinds,V,H);
    Kdtime=etime(clock,tforKd); 
    disp(['time for lower fixed part of operator F :   ',num2str(Kdtime)]);
    Kop=sparse([Kop(1:n1^2,1:2*tn1)
        Kd]);
    if any(isnan(Kop)),
        'nan found in Kop '
         keyboard
    end
%end
tfordir=clock;
clear Kd tmp   % clear memory for lss solution
if algor==1,
   dir=-qls(Kop,rhs);   % previous sqr package
elseif algor==2,
%   rhs1=full(rhs+Kop*sparse(ones(2*tn1,1) )  ); % avoid 0's in f04qaf
%   if any(rhs1==0),
%      'rhs1 has a 0'
%      keyboard
%   end
rhs=full(rhs);   % for nag routine
   [dir,se,itn,anorm,acond,rnorm,arnorm,xnorm,inform, ...
      ifail]=f04qaf((2*tn1),(-rhs),aprod,damp ...
            ,atol,btol,conlim,itnlim,msglvl);
%  dir=dir+ones(2*tn1,1);   % recover solution for original rhs
   dir=sparse(dir);
end
if any(isnan(dir)),
    'nan found in dir'
     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;
normcheck=normest(check)/max(1,normest(check2));
if normcheck> 1e-10,
 disp(sprintf('normal eqns check failed, rel. norm error is: %0.5g',normcheck))
 disp('re-solving using full with normal equations rather than sparse qr')
 dir=-(  ( full(Kop)'*full(Kop) )\ (full(Kop')*full(rhs) )  );
end
%  reshape separately again
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,
 disp(sprintf('normal eqns check failed again, rel. error is: %0.5g',normcheck))
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;
tforalpha=clock;
while mineig > 0,
  Xs = X + alpha * dX; 
  Lams = Lam + alpha * dLam; 
%  mineig is 1 if not p.d. and 0 if p.d.
  [tmp,mineig]=chol(Lams);
  if mineig ==0,
     [tmp,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
alphatime=etime(clock,tforalpha);
 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);
[Fc] = Fmerit(muu,Lam,X,B,V,H,sigmaa);
Fc=sparse(Fc);
optval=norm(H.*(A-KV(X,V,H)),'fro')^2;

%  reler is relative error: duality-gap/n1/(optval+1)
disp('~~~noiter,-log10(relgap),-log10(Fd),alpha,~~sigma,~~optval,~~dirtime,')
%disp('~~~noiter,-log10(relgap),-log10(Fd),alpha,sigma,~~optval,~~dirtime,~alphatime')
%disp(sprintf('%2d,%0.5g',[noiter,-log10(gap/(optval+1)), ...
%     -log10( (norm(Fd,'fro')/normHA) ),alpha,sigmaa,optval,dirtime,alphatime]))
disp(full([noiter,-log10(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.8; sigmaa=1;alpha=1; 
      elseif alpha < .95, muu = muu * 1.5; sigmaa=1-.2*min(alpha,1);alpha=1.1;
      elseif alpha < 1.15, muu = muu *.5; sigmaa=1-.3*min(alpha,1); alpha=1.5;
      else  sigmaa=.9;alpha=2;muu=muu*.5;
      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
D=KV(X,V,H);
     [tmp,mineig]=chol(-V'*D*V);
if any(diag(D)~=0) | mineig>0 ,
  disp(['min eig of -VtDV: ',num2str(min(eig(-full(V'*D*V))))]);
  disp(['largest diagonal in abs value:  ',num2str(max(abs(diag(D))))]);
 %keyboard
end
  rankX=rank(full(X),.0001);
  orankX=rankX;   % save original rank
if rankX > 3,
   disp(['original rank is ',num2str(rankX),' and is decreased to 3'])
   oX=X;  % save original optimal X
   [P,G]=eig(full(X));
   dG=diag(G);
   [eigs,indeigs]=sort(dG);  
   dG(indeigs(1:(n1-3)))=zeros(size(indeigs(1:(n1-3))));
   X=P'*diag(dG)*P;
   X=.5*(X+X');
   newoptval=norm(H.*(A-KV(X,V,H)),'fro')^2;
   relerror=abs(optval-newoptval)/((optval+(optval<.001)));
   disp(['old, new, and relerror optimal values are: ' ...
      ,num2str(optval),' ',num2str(newoptval),' and ',num2str(relerror)]);
   rankX=3;   % reset in order to plot
end
%  skip plots for matlab 4
%% plot points if possible
%  [u,d,v]=svd(full(X));
%  R=u(:,1:rankX);
%  dvec=(diag(d(1:rankX,1:rankX))).^(.5);
%  R=R*diag(dvec);
%  R=V*R;
%  clf
%  if rankX==1,
%     for ii=1:n,
%      for jj=1:(ii-1),
%       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),'fontsize',7);   % place distance on line
%       hold on
%      end
%      end
%      %legend('line from point to point');
%  elseif rankX==2,
%     for ii=1:n,
%      for jj=1:(ii-1),
%       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),'fontsize',7);   
%                                 % place distance on line
%       hold on
%      end
%      end
%      %legend('line from point to point');
%   elseif rankX==3,
%     for ii=1:n,
%      for jj=1:(ii-1),
%       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),'fontsize',7);   
%                                           % place distance on line
%       hold on
%      end
%      end
%      %legend('line from point to point');
%  end
