clear; close all;
profile clear
profile on
addpath ../data/datasets1
addpath ../data/datasets2
addpath ../data/datasets3

seed = 1;
rngsave = rng(seed);
Vchoice = 3;   % 3 sparsest choice .... 1 is QR, 2 is triangular

maxit = 40000;

% choose test data set
i = 26;


if i== 1
    loadfile='esc16a'; % opt value 68;
elseif i==2
    loadfile='esc16b'; % opt value 292
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'; % opt value 1652
elseif i==11
    loadfile='had14';
elseif i==12
    loadfile='had16';   % opt value 3720
elseif i==13
    loadfile='had18';
elseif i==14
    loadfile='had20';
elseif i==15
    loadfile='kra30a';
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';  % opt value is 2570
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='tho30';
end
load(loadfile)

fprintf('\n Starting 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;    
L = blkdiag(0,kron(D,F));   %objective matrix
if norm(L - L','fro') > 10*eps
    error('the data is not symmetric\n');
end

%% setting 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);    
        dscale(ii:ii+ncols-1)=normalize(iblk);

        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;
    V(abs(V)<1e-12)=0;  % zero out any tiny elements
    
end    % end if for Vchoice

KVV=sparse(kron(V,V));
Vhat = [sqrt(1/2), sparse(1,(n-1)^2); sqrt(1/2)/n*ones(n2,1), KVV];
Vhat(abs(Vhat)<1e-12)=0;  % zero out any tiny elements
testorth = Vhat'*Vhat;
errornormVhat=norm(full((testorth-speye(length(testorth)))));
fprintf('error in ||Vhat''Vhat-I||  is= %g; sparsity Vhat is nnz/numel=  %g \n',...
    errornormVhat,nnz(Vhat)/numel(Vhat));

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;

%% set initial points and parameters
R0 = Vhat'*Yhat*Vhat;
R0 = (R0 + R0')/2;
Y0 = Yhat;
Z0 = Y0 - Vhat*R0*Vhat';

tol = 1e-5;
beta = n/3;

%% ADMM without rank-1 constraint
opts = [];
opts.R0 = R0; opts.Y0 = Y0; opts.Z0 = Z0;
opts.beta = beta;
opts.maxit = maxit;
opts.tol = tol;
opts.gamma = 1.618;
opts.A = A;
opts.B = B;
opts.cal_bd = 1;  % calculate upper and lower bounds every 100 iters.
fprintf('max # iterations = %i; tolerance = %e \n',...
    opts.maxit,opts.tol)
fprintf('\n\nADMM_QAP without rank-1 constraint\n');

% for large-scale data set, use sparse version ADMM_QAPs
[R11,Y11,Out11] = ADMM_QAP(L,Vhat,J,opts);

%[hh,mm,ss] = mytime(sum(Out11.toc));
[hh,mm,ss,~] = secs2hms(sum(Out11.toc));

final_obj = Out11.obj(end);
final_res = max(Out11.pr(end), Out11.dr(end));

fprintf('total toc/cpu-sec time %g \n',Out11.toc);
fprintf('Lbd.Val: %g\n Feas.Val: %g\n Iter : %i\n CPU: %s:%s:%s\n\n',...
    max(Out11.lbd), min(Out11.ubd), Out11.iter,hh,mm,ss);

%% ADMM with rank-1 constraint
opts = [];
opts.R0 = R0; opts.Y0 = Y0; opts.Z0 = Z0;
opts.beta = n/3;
opts.tol = tol;
opts.maxit = maxit;
opts.low_rank = 1;
opts.K = 1;     
opts.gamma = 1;
opts.A = A;
opts.B = B;
opts.cal_bd = 1;

fprintf('\nADMM_QAP with rank-1 constraint\n');

% for large-scale data set, use sparse version ADMM_QAPs
[R22,Y22,Out22] = ADMM_QAP(L,Vhat,J,opts);

%[hh,mm,ss] = mytime(sum(Out22.toc));
[hh,mm,ss,~] = secs2hms(sum(Out22.toc));

final_obj = Out22.obj(end);
final_res = max(Out22.pr(end), Out22.dr(end));

fprintf('total toc/cpu-sec time %g \n',Out22.toc);
fprintf('Feas.Val: %g\n Iter : %i\n CPU: %s:%s:%s\n\n',...
    min(Out22.ubd),Out22.iter,hh,mm,ss);

profile report
