%%%%%??? questions ???? should we use feasible starting points????
%function [R22,Y22,Out22,R11,Y11,Out11] = quicktest
%%      [R11,Y11,Out11] = quicktest
%%% variables kUU,kUUt etc for sparse eigs have been commented out
%%%dec132015 dec132015matrixoutput.mat is full tests on office PC
%%%%              with 1e12 tolerances
clear all
addpath datasets
%profile clear
%profile on

%%%%% enter choice for which of three V representations are desired
%%%% do this in variable Vchoice below
%%%%% enter the problems to solve into variable  probsi -- in for loop

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% for small random problem
%%n=8;
%%fprintf('use a random small problem n= %i \n',n)
%%A=round(20*(rand(n)+1));
%%A=A-diag(diag(A));
%%A=(A+A');
%%B=round(20*(rand(n)+1));
%%B=B-diag(diag(B));
%%B=(B+B');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% for small random problem
%for i=15:45
Vchoice=3;   % 3 sparsest choice .... 1 is QR, 2 is triangular
probsi=[1:45];
%probsi=[2:2];
totali=length(probsi);
outputmatrix11=zeros(totali,6);
outputmatrix22=zeros(totali,6);
iprob=0;
for i=probsi,
clearvars -except i Vchoice probsi outputmatrix11 outputmatrix22 iprob;
   iprob=iprob+1;
   if i== 1
        loadfile='esc16a';
    elseif i==2
        loadfile='esc16b';
    elseif i==3
        loadfile='esc16c';
    elseif i==4
        loadfile='esc16d';        
    elseif i==5
        loadfile='esc16e';
    elseif i==6
        loadfile='esc16g';
    elseif i==7
        loadfile='esc16h';
    elseif i==8
        loadfile='esc16i';
    elseif i==9
        loadfile='esc16j'; 
    elseif i==10
        loadfile='had12';
    elseif i==11
        loadfile='had14';
    elseif i==12
        loadfile='had16';        
    elseif i==13
        loadfile='had18';
    elseif i==14
        loadfile='had20';
    elseif i==15
        loadfile='tho30';
    elseif i==16
        loadfile='kra30b';
    elseif i==17
        loadfile='kra32';
    elseif i==18
        loadfile='nug12';
    elseif i==19
        loadfile='nug14';
    elseif i==20
        loadfile='nug15';        
    elseif i==21
        loadfile='nug16a';
    elseif i==22
        loadfile='nug16b';
    elseif i==23
        loadfile='nug17';
    elseif i==24
        loadfile='nug18';
    elseif i==25
        loadfile='nug20';
    elseif i==26
        loadfile='nug21';
    elseif i==27
        loadfile='nug22';
    elseif i==28
        loadfile='nug24';        
    elseif i==29
        loadfile='nug25';
    elseif i==30
        loadfile='nug27';
    elseif i==31
        loadfile='nug28';
    elseif i==32
        loadfile='nug30';
    elseif i==33
        loadfile='rou12';
    elseif i==34
        loadfile='rou15';
    elseif i==35
        loadfile='rou20';
    elseif i==36
        loadfile='scr12';        
    elseif i==37
        loadfile='scr15';
    elseif i==38
        loadfile='scr20';
    elseif i==39
        loadfile='tai12a';
    elseif i==40
        loadfile='tai15a';
    elseif i==41
        loadfile='tai17a';
    elseif i==42
        loadfile='tai20a';
    elseif i==43
        loadfile='tai25a';
    elseif i==44
        loadfile='tai30a';        
    elseif i==45
        loadfile='kra30a';
    end        
load(loadfile)

fprintf('\n Using/loading new problem i= %i  from file      %s \n',i,loadfile)
n2=n^2;
n21=n2+1;
n2m1=(n-1)^2-1;
In=speye(n);
En=ones(n);
en = ones(n,1);
D = B; F = A;    % to do row/col sums to get 'zeros'??????
L=blkdiag(0,kron(D,F));   %objective matrix
if norm(L - L','fro') > 10*eps
    fprintf('the data is not symmetric\n');
    %fprintf(fid,'%s: the data is not symmetric\n');
    return
end

%%%%three possible choices for V
if Vchoice==1,
	fprintf('using Vchoice=1 standard V with qr for Vhat \n')
%% using qr 
    Veye = [eye(n-1); -ones(1,n-1)];
    [V,~] = qr(Veye,0);
elseif Vchoice==2,
	fprintf('using  Vchoice=2 uppertriangular V for Vhat \n')
% using upper triangular ones
    V0=ones(n,n-1);
    V0=triu(V0);
    V0(2:n+1:end)=-(1:n-1);
    DV0=sqrt((1:1:(n-1))+(1:1:(n-1)).^2);
    V0=V0/diag(DV0);
    V=V0;
elseif Vchoice==3,
	fprintf('using  Vchoice=3 sparsest V by blocks for Vhat \n')
%%% using orthog. blocks
  iblk=1;                            % number of current block
  ii=1;                              % current beginning column of block iblk
  temp=(spalloc(n,n-1,n*(n+1)/2));     % matrix for final V
  sizeblocks=zeros(floor(n/2),1);    % initialize vector
  normalize=zeros(floor(n/2),1);    % initialize vector
  dscale=ones(n-1,1);              % scaling vector
  sizeblocks(iblk)=floor(n/(2^iblk));     % size/#cols of blk iblk
  normalize(iblk)=norm(ones(2^iblk,1));   %  normalization scalars
  while sizeblocks(iblk) >= 1,         % room for at least one column
     tspeye=speye(sizeblocks(iblk));
     ncols=sizeblocks(iblk);    % do not exceed # cols left % TK: this will never exceed cols left
     dscale(ii:ii+ncols-1)=normalize(iblk);
     %%%%%change to proper indexing for sparse matrix!!???????
     temp(:,ii:ii+ncols-1)=...
       [ kron(tspeye(:,1:ncols),[ones(2^(iblk-1),1);-ones(2^(iblk-1),1)])
         zeros(n-sizeblocks(iblk)*2^iblk,ncols) ];
     ii=ii+ncols;
     iblk=iblk+1;
     sizeblocks(iblk)=floor(n/(2^iblk));
     normalize(iblk)=norm(ones(2^iblk,1));
  end
  itest= (sum(diag(temp'*temp)==0));
  if itest > 0,
  	ttemp=sparse(null(full([temp ones(n,1)]')));
  	temp(:,end-itest+1:end)=ttemp(:,1:itest);
  end
  dd=sqrt(diag(temp'*temp));
  Ddd=spdiags(dd,0,n-1,n-1);
  V=temp/Ddd;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end    % end if for Vchoice

%%%% test sparse representation with function for eigs
%%%normest(V- (temp/Ddd))
%%kVV=kron(V,V);
%%kUU=kron(temp,temp);
%%kUUt=kUU';
%%kDD=kron(Ddd,Ddd);
%%kDD=diag(kDD);
%normest(kVV - kUU/kDD)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%




KVV=sparse(kron(V,V));
Vhat = [sqrt(1/2), sparse(1,(n-1)^2); sqrt(1/2)/n*ones(n2,1), KVV];
testorth=Vhat'*Vhat;
errornormVhat=norm(full((testorth-speye(length(testorth)))));
if errornormVhat>1e-12,
   fprintf('error orthog. norm %g Vhat - enter continue if desired \n',...
             errornormVhat)
   keyboard
end
%Vhat = [sqrt(1/2), zeros(1,(n-1)^2); sqrt(1/2)/n*ones(n2,1), (kUU/kDD)];



Yhat = ([0, 1/n*ones(1,n2);
    1/n*ones(n2,1), 1/n2*ones(n2)+1/(n2*(n-1))*kron(n*In-En,n*In-En)]);
Yhat(1,1) = 1;
Yhat=sparse(Yhat);

Es21=triu(ones(n),1);
YJ = sparse(kron(In,triu(En,1)) + kron(Es21,In));
YJ = blkdiag(1,YJ); 
YJ = YJ + YJ';
J = sparse(YJ~=0);     % fix 0 elements in barycenter to stay 0
Yhat(J)=0;Yhat(1,1)=1;
Jorig=J;
%% to remove the redundant off diagonal constraints
%Es21(:,end)=0;               % added to remove redundant off-diag blocks
%Es21(end-2,end-1)=0;         % added to remove redundant off-diag blocks
%YJ = kron(In,triu(En,1)) + kron(Es21,In);
%YJ = blkdiag(1,YJ); 
%YJ = YJ + YJ';
%J = sparse(YJ~=0);     % fix 0 elements in barycenter to stay 0

R0 = Vhat'*Yhat*Vhat;
R0 = (R0 + R0')/2;
Y0 = Yhat;
Z0 = Y0 - Vhat*R0*Vhat';
%% ADMM without low-rank constraint
opts = [];
opts.R0 = R0; opts.Y0 = Y0; opts.Z0 = Z0;
opts.beta = n/3;
%opts.maxit = 5000;
opts.maxit = 40000;
%opts.maxit = 100000;
%opts.tol = 1e-12;
opts.tol = 1e-5;
opts.gamma = 1.618;
%%%%solve the high rank case
[R11,Y11,Out11] = ADMM_QAP(L,Vhat,J,opts);
%[R11,Y11,Out11] = ADMM_QAP(L,Vhat,J,opts,kUU,kUUt,kDD);

% get lower bound
%That = [kron(In,en'); kron(en',In)];
%That = [-ones(2*n,1), That];
That = [-ones(2*n,1),  [kron(In,en'); kron(en',In)] ];
[Q,~] = qr(That',0);
Q = Q(:,1:end-1);

U = [Vhat, Q];
Z11 = U'*Out11.Z*U;    % this means that Out11.Z is DUAL** feas
%%%%%%%%%%%%%%%%%%%   i.e. it satisfies g(Out11.Z) is a lower bound
W12 = Z11(1:(n-1)^2+1,(n-1)^2+2:end);
W22 = Z11((n-1)^2+2:end,(n-1)^2+2:end);
W11 = Z11(1:(n-1)^2+1,1:(n-1)^2+1); W11 = (W11+W11')/2;
[Uw,Dw] = eig(full(W11));
dw = diag(Dw);
id = dw<0;
W11 = Uw(:,id)*diag(dw(id))*Uw(:,id)';
Zp = U*[W11,W12;W12',W22]*U';
Zp = (Zp+Zp')/2;
Yp = zeros(n2+1);
Yp(L+Zp < 0) = 1;
Yp(J) = 0; Yp(1,1) = 1;
lbd = sum(sum((L+Zp).*Yp));


% get feasible solution

Y11 = (Y11'+Y11)/2;
[u,d] = eigs(Y11,1,'LA');
Y11hat = d*(u*u');
X11hat = reshape(Y11hat(2:end,1),n,n);

%%% simplex changed to dual-simplex option by henry oct21/15
%linopts = optimoptions('linprog','Diagnostics','off');
%linopts = optimoptions('linprog','Display','off');
linopts =...
optimoptions('linprog','Algorithm','dual-simplex','Display','off');
% linopts = optimset('Simplex','on','LargeScale','off');
CoefA = [kron(eye(n),en'); kron(en',eye(n))];
CoefAd=CoefA(1:end-1,:);
rhsb = ones(2*n,1);
rhsbd=rhsb(1:end-1,:);
[x11,~] = linprog(-X11hat(:),[],[],...
     CoefAd,rhsbd,zeros(n2,1),ones(n2,1),[],linopts);
%[x11,fvalperm] = linprog(-X11hat(:),[],[],...
%     CoefAd,rhsbd,zeros(n2,1),ones(n2,1),[],linopts);
X11 = reshape(x11,n,n);

feas_obj1 = round(trace(F*X11*D*X11'));
%%%%%%%%%%%%%%%%%%%try with only first row of Y11
X11hatrow1 = full(reshape(Y11(2:end,1),n,n));  % use first col of original Y11
[x11row1,~] = linprog(-X11hatrow1(:),[],[],...
     CoefAd,rhsbd,zeros(n2,1),ones(n2,1),[],linopts);
X11row1 = reshape(x11row1,n,n);
feas_obj1row1 = round(trace(F*X11row1*D*X11row1'));

[hh,mm,ss] = mytime(sum(Out11.toc));
fprintf('\n  \t\t\t Primal Obj\t\t Dual Obj\t\t Feas. Val \t\t Feas. ValRow1 \t Iter \t Cpu\n');
fprintf('  high rank:   %8.10g\t\t   %8.10g\t\t   %d\t  \t%d\t     %d      %s:%s:%s\n',...
                    Out11.obj(end),lbd,feas_obj1,feas_obj1row1,Out11.iter,hh,mm,ss);
outputmatrix11(iprob,:)=...
   [Out11.obj(end) lbd feas_obj1 feas_obj1row1 Out11.iter Out11.toc];


%% ADMM with low-rank constraint
opts = [];
opts.R0 = R0; opts.Y0 = Y0; opts.Z0 = Z0;
opts.beta = n/3;
%opts.maxit = 10000;
%opts.tol = 1e-5;
opts.maxit = 40000;
opts.tol = 1e-12;
opts.low_rank = 1;
opts.K = 1;
opts.gamma = 1;

%[R22,Y22,Out22] = ADMM_QAP(L,Vhat,J,opts,kUU,kUUt,kDD);
[R22,Y22,Out22] = ADMM_QAP(L,Vhat,J,opts);

% get feasible solution

Y22 = (Y22'+Y22)/2;   % should be rank 1 psd.
primalfeasvrv=norm(Y22-Vhat*R22*Vhat');
[u,d] = eigs(Y22,1,'LA');
Y22hat = d*(u*u');
X22hat = reshape(Y22hat(2:end,1),n,n);

%linopts = optimoptions('linprog','Algorithm','dual-simplex');
%linopts = optimoptions('linprog','Display','off');
linopts =...
optimoptions('linprog','Algorithm','dual-simplex','Display','off');
% linopts = optimset('Simplex','on','LargeScale','off');
CoefA = [kron(eye(n),en'); kron(en',eye(n))];
CoefAd=CoefA(1:end-1,:);
rhsb = ones(2*n,1);
rhsbd=rhsb(1:end-1,:);
%x22 = linprog(-X22hat(:),CoefA,rhsb,[],[],zeros(n2,1),[],[],linopts);
[x22,fvalperm] = linprog(-X22hat(:),[],[],...
     CoefA,rhsb,zeros(n2,1),ones(n2,1),[],linopts);
%[x22,fvalperm] = linprog(-X22hat(:),[],[],...
%     CoefAd,rhsbd,zeros(n2,1),ones(n2,1),[],linopts);

X22 = reshape(x22,n,n);

feas_obj2 = round(trace(F*X22*D*X22'));
%%%%%%%%%%%%%%%%%%%try with only first row of Y11
X22hatrow1 = reshape(Y22(2:end,1),n,n);  % use first col of original Y11
[x22row1,fvalpermrow1]= linprog(-X22hatrow1(:),CoefA,rhsb,[],[],zeros(n2,1),[],[],linopts);
X22row1 = reshape(x22row1,n,n);
feas_obj2row1 = round(trace(F*X22row1*D*X22row1'));


%%%%????? need to update lbd appropr??? it makes no sense????
%fprintf('  low  rank:   %5.8g\t\t   %5.8g\t\t   %d\t     %d     %d\n',...
%                    Out22.obj(end),lbd,feas_obj2,Out22.iter,Out22.toc);
%fprintf('\n  \t\t\t Primal Obj\t\t Dual Obj\t\t Feas. Val \t\t Feas. ValRow1 \t Iter \t Cpu\n');
[hh,mm,ss] = mytime(sum(Out22.toc));
fprintf('  low rank:   %8.10g\t\t   %8.10g\t\t   %d\t  \t%d\t     %d     %s:%s:%s\n',...
                    Out22.obj(end),lbd,feas_obj2,feas_obj2row1,Out22.iter,hh,mm,ss);
outputmatrix22(iprob,:)=...
   [Out22.obj(end) lbd feas_obj2 feas_obj2row1 Out22.iter Out22.toc];

save('matrixoutput','outputmatrix11','outputmatrix22')
end %% end for
%profile report
%%%% test cuts feasible
%%Yoff=Y11-diag(diag(Y11));
%%Ymax=Yoff-diag(Y11)*ones(1,size(Y11,1));
%%[val,inds]=max(Ymax(:));
%%[ii,jj]=ind2sub(size(Ymax),inds);
%%fprintf('%g  %g   %g   %g  \n',val,Y11(ii,ii),Y11(jj,jj),Y11(ii,jj))
%end
