function [R, Y, Out] = ADMM_QAPs(L, Vhat, J, opts)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Alternating direction method of multiplier (ADMM) for the SDP relaxation
% of quadratic assignment problem (QAP) with sparse matrices
% min_{R,Y} trace(L*Y)
% subject to: Y(id) = 0 for all id in J\{(1,1)}, and Y(1,1) = 1;
%             0 <= Y <= 1,
%             Vhat*R*Vhat' = Y,
%             R is a psd matrix
%%%%%%%%%%%==================================================%%%%%%%%%%%%%%
%%% Input:
%           L: a symmetric matrix for the objective function
%           Vhat: an orthonormal matrix for the facial reduction
%           J: index set in the gangster constraint
%           opts. (default value)
%               maxit: maximum number of iterations (500)
%               tol: tolerance for stopping (0.1)
%               beta: penalty parameter (50)
%               low_rank: if there is low-rank constraint (FALSE)
%               K: rank that Y is constrained to, valid only if low_rank = TRUE
%               gamma: stepsize for multiplier update (1)
%
%%% Output:
%           R, Y: solutions
%           Out.
%               obj: history of objective values
%               iter: number of iterations
%               pr: primal residuals
%               dr: dual residuals
%               time: total running time
%               Z: dual variable for computing lower bound
%               pos_eig: track the number of positive eigenvalues
%%%%%%%%%%%%================================================%%%%%%%%%%%%%%
%%%
% Copyright: Danilo E. Oliveira, Henry Wolkowicz, Yangyang Xu, 2018
%
%
% Please cite to use the code:
% "Danilo E. Oliveira, Henry Wolkowicz, Yangyang Xu. ADMM for the SDP
% relaxation of the QAP. Mathematical Programming Computation, 2018."
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if isfield(opts,'maxit')     maxit = opts.maxit;       else maxit = 500;    end
if isfield(opts,'tol')       tol = opts.tol;           else tol = 1e-1;     end
if isfield(opts,'beta')      beta = opts.beta;         else beta = 50;      end
if isfield(opts,'low_rank')  low_rank = opts.low_rank; else low_rank = 0;   end
if isfield(opts,'K')         K = opts.K;               else K = 1;          end
if isfield(opts,'gamma')     gamma = opts.gamma;       else gamma = 1;      end
if isfield(opts,'cal_bd')    cal_bd = opts.cal_bd;     else cal_bd = 0;     end
if cal_bd 
    D = opts.B; F = opts.A;
end

if norm(L-L','fro') > 10*eps
    error('The matrix L is not symmetric! so symmetrizing here');
    L = (L+L')/2;
end

% set option parameters for eigs
opteigs.real=1;

R0 = opts.R0;
Y0 = opts.Y0;
Z0 = opts.Z0;
R = R0; Y = Y0; Z = Z0;
nrm_pR=1;
nrm_dR=1;

Y( (abs(Y)<min([nrm_pR,nrm_dR,1e-9])) ) = 0;
Y = sparse(Y);
Z( (abs(Z)<min([nrm_pR,nrm_dR,1e-9])) ) = 0;
Z = sparse(Z);

% scale matrix L for stability
n2 = size(Vhat,1)-1;
nrmL = norm(L,'fro');
L = L/nrmL*n2;
n = sqrt(n2);

In = speye(n);
en = ones(n,1);

feas = zeros(maxit,1);
obj = zeros(maxit,1);
pos_eig = zeros(maxit,1);
max_eig = zeros(maxit,K);
hist_pR = zeros(maxit,1);
hist_dR = zeros(maxit,1);

Out.lbd = [];
Out.ubd = [];


start_time = tic;   % start time
fprintf('\t iter# \t  \t     nrm_pR \t \t  nrm_dR \t   \t  time_toc \n');

for iter = 1:maxit
    
    start_iter_time = tic;
    %% update R     ....    step 1 of 3
    W = Y + Z/beta;
    WVhat = Vhat'*(W*Vhat);
    
    WVhat = (WVhat'+WVhat)/2;
    if ~low_rank
        
        % smooth the matrix first before eig
        nV = norm(WVhat,'fro')^2;
        nV = nV/numel(WVhat);
        
        WVhat( WVhat.^2<1e-11*nV ) = 0;
        WVhat = (WVhat'+WVhat)/2;
        [U,S] = eig(full(WVhat));
        
        s = diag(S);
        id = s>0;
        R = U(:,id)*diag(s(id))*U(:,id)';
        pos_eig(iter) = sum(id);
    else
        if ~issparse(WVhat)
            error('WVhat is NOT sparse before eigs in iter %i \n',iter)
        end
        opteigs.v0 = sum(WVhat)';
        [v,d] = eigs(WVhat,K,'LA',opteigs);  
        
        d = diag(d);
        id = d>0;
        if sum(id) > 0
            R = v(:,id)*diag(d(id))*v(:,id)';
        else
            R = zeros(size(R));
        end
        max_eig(iter,:) = d';
    end
    R = (R+R')/2;
    
    %% update Y    .....      step 2 of 3 with projections >=0 and <=1
    VRV = Vhat*R*Vhat';
    Y = VRV-(L+Z)/beta;
    Y=(Y+Y')/2;
    Y(J) = 0; Y(1,1) = 1;
    Y = min(1, max(0,Y));
    Y(J) = 0; Y(1,1) = 1;
    Y( (abs(Y)<min([nrm_pR,nrm_dR,1e-9])) ) = 0;
    Y = sparse(Y);
    
    
    %% update multiplier      step 3 of 3  update dual variable Z
    pR = Y-VRV;
    dR = Y-Y0; Y0 = Y;       % dR for cgnce and then update
    
    Z = Z + gamma*beta*pR;
    Z = (Z+Z')/2;
    Z( (abs(Z)<min([nrm_pR,nrm_dR,1e-9])) )=0;
    Z = sparse(Z);
    nrm_pR = norm(pR,'fro');
    feas(iter) = nrm_pR/norm(Y,'fro');
    
    obj(iter) = sum(L(:).*Y(:));
    
    
    nrm_dR = beta*norm(dR,'fro');
    hist_pR(iter) = nrm_pR;
    hist_dR(iter) = nrm_dR;
    
    if mod(iter, 100) == 0
        fprintf('\t %i \t \t    %5.4e \t \t %5.4e  \t \t  %g \n',...
            iter,nrm_pR,nrm_dR,(toc(start_iter_time)))
        if cal_bd
            cal_ubd;
            if ~low_rank
                cal_lbd;
            end
        end
    end
    
    if nrm_pR < tol && nrm_dR < tol
        nstall = nstall + 1;
    else
        nstall = 0;
    end
    
    if nstall >=5 
        break;
    end
    
    
end % of main iteration

cal_ubd;
if ~low_rank
    cal_lbd;
end

Out.toc = toc(start_time);
Out.obj = obj(1:iter)*nrmL/n2;
Out.iter = iter;
Out.feas = feas(1:iter);
Out.pr = hist_pR(1:iter);
Out.dr = hist_dR(1:iter);
Out.Z = Z*nrmL/n2;
Out.R = R;
Out.Y = Y;
if ~low_rank
    Out.pos_eig = pos_eig(1:iter);
else
    Out.max_eig = max_eig(1:iter);
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function cal_lbd
        
        % calculate lower bound
        
        That = [-ones(2*n,1),  [kron(In,en'); kron(en',In)] ];
        [Q,~] = qr(That',0);
        Q = Q(:,1:end-1);
        
        Uloc = [Vhat, Q];
        Zloc = Uloc'*Z*Uloc;    

        W12 = Zloc(1:(n-1)^2+1,(n-1)^2+2:end);
        W22 = Zloc((n-1)^2+2:end,(n-1)^2+2:end);
        W11 = Zloc(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 = Uloc*[W11,W12;W12',W22]*Uloc';
        Zp = (Zp+Zp')/2;
        Yp = zeros(n2+1);
        Yp(L+Zp < 0) = 1;
        Yp(J) = 0; Yp(1,1) = 1;
        Out.lbd = [Out.lbd; sum(sum((L+Zp).*Yp))*nrmL/n2];
    end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    function cal_ubd
        % calculate upper bound
        
        % get feasible solution with first dominant eigenvector of Y
        
        Yloc = (Y'+Y)/2;
        [u,d] = eigs(Yloc,1,'LA');
        Yloc_hat = d*(u*u');
        Xloc_hat = reshape(Yloc_hat(2:end,1),n,n);
        

        linopts =...
            optimoptions('linprog','Algorithm','dual-simplex','Display','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,:);
        [xloc,~] = linprog(-Xloc_hat(:),[],[],...
            CoefAd,rhsbd,zeros(n2,1),ones(n2,1),[],linopts);
        Xloc = reshape(xloc,n,n);
        
        feas_obj1 = round(trace(F*Xloc*D*Xloc'));
        
        % get feasible solution with first col of Y
        
        Xloc = full(reshape(Y(2:end,1),n,n));  % use first col of original Y11
        [xloc,~] = linprog(-Xloc(:),[],[],...
            CoefAd,rhsbd,zeros(n2,1),ones(n2,1),[],linopts);
        Xloc = reshape(xloc,n,n);
        feas_obj2 = round(trace(F*Xloc*D*Xloc'));
        
        Out.ubd = [Out.ubd; min(feas_obj1,feas_obj2)];
    end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

end   % of main function ADMM_QAP
