clear all
addpath datasets
% 45 datasets in total
datafiles = {'esc16a','esc16b','esc16c','esc16d','esc16e','esc16g','esc16h','esc16i','esc16j',...
    'had12','had14','had16','had18','had20','tho30','kra30a','kra30b','kra32',...
    'nug12','nug14','nug15','nug16a','nug16b','nug17','nug18','nug20','nug21','nug22',...
    'nug24','nug25','nug27','nug28','nug30','rou12','rou15','rou20','scr12','scr15','scr20',...
    'tai12a','tai15a','tai17a','tai20a','tai25a','tai30a'};

%% load data
data_name = 'esc16b'; % change this name to test other datasets

load(data_name);

fprintf('\nUsing/loading new problem from file  %s \n',data_name)

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

%% set parameter matrices and starting points
n2 = n^2;
n21 = n2+1;
n2m1 = (n-1)^2-1;
In = speye(n);
En = ones(n);
en = ones(n,1);

% Vchoice: 1 is QR, 2 is triangular, 3 sparsest
Vchoice = 3;   
V = generate_V(Vchoice,n);
Vhat = [sqrt(1/2), sparse(1,(n-1)^2); sqrt(1/2)/n*ones(n2,1), kron(V,V)];

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;

Es21 = triu(ones(n),1);
YJ = kron(In,triu(En,1)) + kron(Es21,In);
YJ = blkdiag(1,YJ);
YJ = YJ + YJ';
J = (YJ~=0);     % fix 0 elements in barycenter to stay 0
Yhat(J)=0; Yhat(1,1)=1;
Jorig=J;

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

%% ADMM for SDP relaxation
opts = [];
opts.R0 = R0; opts.Y0 = Y0; opts.Z0 = Z0;
opts.beta = n/3;
opts.maxit = 10000;
opts.tol = 1e-5;
opts.gamma = 1.618;

fprintf('\nRunning ADMM for SDP relaxation ...')
time1 = tic;
[R11,Y11,Out11] = ADMM_QAP(L,Vhat,J,opts);

% get lower bound
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. 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 by projecting to best rank-one approximation
Y11 = (Y11'+Y11)/2;
[u,d] = eigs(Y11,1,'LA');
Y11hat = d*(u*u');
X11hat = reshape(Y11hat(2:end,1),n,n);

%%% choose the options according to the version of Matlab
% 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 = reshape(x11,n,n);

feas_obj1 = round(trace(F*X11*D*X11'));
% get feasible solution by using the first col/row
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'));

% choose the better one from two feasible solutions
feas1 = min(feas_obj1, feas_obj1row1);
time1 = toc(time1);
%% ADMM for SDP relaxation with rank-1 constraint
opts = [];
opts.R0 = R0; opts.Y0 = Y0; opts.Z0 = Z0;
opts.beta = n/3;
opts.maxit = 1000;
opts.tol = 1e-5;
opts.low_rank = 1;
opts.K = 1;
opts.gamma = 1; % gamma =1 because of nonconvexity

fprintf('\n\nRunning ADMM for SDP relaxation with rank-1 constraint...')
time2 = tic;
[R22,Y22,Out22] = ADMM_QAP(L,Vhat,J,opts);

% Y22 should be rank-1 PSD matrix but to be proved
% in case not, get feasible solution by projecting to best rank-1
% approximation

linopts = optimset('Simplex','on','LargeScale','off');
Y22 = (Y22'+Y22)/2;  
primalfeasvrv=norm(Y22-Vhat*R22*Vhat');
[u,d] = eigs(Y22,1,'LA');
Y22hat = d*(u*u');
X22hat = reshape(Y22hat(2:end,1),n,n);

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,fvalperm] = linprog(-X22hat(:),[],[], CoefA,rhsb,zeros(n2,1),ones(n2,1),[],linopts);

X22 = reshape(x22,n,n);
feas_obj2 = round(trace(F*X22*D*X22'));

% get feasible solution by using first col/row of Y22
X22hatrow1 = reshape(Y22(2:end,1),n,n);  
[x22row1,fvalpermrow1]= linprog(-X22hatrow1(:),CoefA,rhsb,[],[],zeros(n2,1),[],[],linopts);
X22row1 = reshape(x22row1,n,n);
feas_obj2row1 = round(trace(F*X22row1*D*X22row1'));

% choose the better one from two feasible solutions
feas2 = min(feas_obj2, feas_obj2row1);
time2 = toc(time2);
%% Output results
fprintf('\n\nLower bound given by ADMM is %d\n\n',ceil(lbd));
fprintf('Objective of Feasible solution given by ADMM is %d\n', feas1);
fprintf('The time taken by ADMM is %5.4f\n\n',time1);
fprintf('Objective of Feasible solution given by rank-1 ADMM is %d\n', feas2);
fprintf('The time taken by rank-1 ADMM is %5.4f\n\n',time2);