%--------------------------------------------------------------------------
% This function takes a DxN matrix of N data points in a D-dimensional 
% space and returns a NxN coefficient matrix of the sparse representation 
% of each data point in terms of the rest of the points
% Y: DxN data matrix
% affine: if true then enforce the affine constraint
% thr1: stopping threshold for the coefficient error ||Z-C||
% thr2: stopping threshold for the linear system error ||Y-YZ||
% maxIter: maximum number of iterations of ADMM
% C2: NxN sparse coefficient matrix
%--------------------------------------------------------------------------
% Copyright @ Ehsan Elhamifar, 2012
%--------------------------------------------------------------------------

function C2 = admmLasso_mat_func(Yt,Ys,affine,alpha,thr,maxIter)

if (nargin < 3)
    % default subspaces are linear
    affine = false; 
end
if (nargin < 4)
    % default regularizarion parameters
    alpha = 800;
end
if (nargin < 5)
    % default coefficient error threshold to stop ADMM
    % default linear system error threshold to stop ADMM
    thr = 2*10^(-8); 
end
if (nargin < 6)
    % default maximum number of iterations of ADMM
    maxIter = 100;%1000; 
end

if (length(alpha) == 1)
    alpha1 = alpha(1);
    alpha2 = alpha(1);
elseif (length(alpha) == 2)
    alpha1 = alpha(1);
    alpha2 = alpha(2);
end

if (length(thr) == 1)
    thr1 = thr(1);
    thr2 = thr(1);
elseif (length(thr) == 2)
    thr1 = thr(1);
    thr2 = thr(2);
end

Nt = size(Yt,2);
Ns = size(Ys,2);
% setting penalty parameters for the ADMM
mu1 = alpha1 * 1/computeLambda_mat(Yt);
mu2 = alpha2 * 1;

J=zeros(Ns,Nt);% modified
yita1=0;yita2=10^4; % modified

if (~affine)
    % initialization
    
    C1 = zeros(Ns,Nt);
    Lambda2 = zeros(Ns,Nt);
    err1 = 10*thr1; err2 = 10*thr2;
    i = 1;
    % ADMM iterations
    while ( err1(i) > thr1 && i < maxIter )
        A = inv(mu1*(Ys'*Ys)+mu2*eye(Ns));
%          A = inv(mu1*(Ys'*Ys)+mu2*eye(Ns)+yita2*eye(Ns));% modified
         
        % updating Z
        Z = A * (mu1*(Ys'*Yt)+mu2*(C1-Lambda2/mu2));
%         Z = A * (mu1*(Ys'*Yt)+mu2*(C1-Lambda2/mu2)+yita2*(J-yita1/yita2));%modified
%         Z = Z - diag(diag(Z));
%         z1=Z(1:80,:);z2=Z(80+1:end,:);z2=z2-diag(diag(z2));Z=[z1;z2];

        % updating C
        C2 = max(0,(abs(Z+Lambda2/mu2) - 1/mu2*ones(Ns,Nt))) .* sign(Z+Lambda2/mu2);
%         C2 = C2 - diag(diag(C2));
%         c1=C2(1:80,:);c2=C2(80+1:end,:);c2=c2-diag(diag(c2));C2=[c1;c2];
        % updating J (low rank) % modified
        temp = Z + yita1/yita2;
        [U,sigma,V] = svd(temp,'econ');
        sigma = diag(sigma);
        svp = length(find(sigma>1/yita2));
        if svp>=1
           sigma = sigma(1:svp)-1/yita2;
        else
           svp = 1;
           sigma = 0;
        end
        J = U(:,1:svp)*diag(sigma)*V(:,1:svp)';
        
        % updating Lagrange multipliers
        Lambda2 = Lambda2 + mu2 * (Z - C2);
        
        yita1=yita1+yita2*(Z-J);% modified
        yita2=min(yita2*1.1,10^6);% modified
        mu2=min(mu2*1.1,10^6);% modified
        
        % computing errors
        err1(i+1) = errorCoef(Z,C2);
%         err2(i+1) = errorLinSys(Y,Z);
        %
        C1 = C2;
        i = i + 1;
    end
    
%     fprintf('err1: %2.4f, err2: %2.4f, iter: %3.0f \n',err1(end),err2(end),i);
else
    % initialization
    
    C1 = zeros(Ns,Nt);
    Lambda2 = zeros(Ns,Nt);
    lambda3 = zeros(1,Nt);
    err1 = 10*thr1; err2 = 10*thr2; err3 = 10*thr1;
    i = 1;
    % ADMM iterations
    while ( (err1(i) > thr1 || err3(i) > thr1) && i < maxIter )
        A = inv(mu1*(Ys'*Ys)+mu2*eye(Ns)+mu2*ones(Ns,Ns));
        % updating Z
        Z = A * (mu1*(Ys'*Yt)+mu2*(C1-Lambda2/mu2)+mu2*ones(Ns,1)*(ones(1,Nt)-lambda3/mu2));
%         Z = Z - diag(diag(Z));
%         for kkk=1:Nt,Z(Ns-Nt+kkk,kkk)=0;end;
        % updating C
        C2 = max(0,(abs(Z+Lambda2/mu2) - 1/mu2*ones(Ns,Nt))) .* sign(Z+Lambda2/mu2);
%         C2 = C2 - diag(diag(C2));
%         for kkkk=1:Nt,C2(Ns-Nt+kkkk,kkkk)=0;end;
        % updating Lagrange multipliers
        Lambda2 = Lambda2 + mu2 * (Z - C2);
        lambda3 = lambda3 + mu2 * (ones(1,Ns)*Z - ones(1,Nt)); % affine constraint
        mu2=min(mu2*1.1,10^6);% modified
        % computing errors
        err1(i+1) = errorCoef(Z,C2);
%         err2(i+1) = errorLinSys(Y,Z);
        err3(i+1) = errorCoef(ones(1,Ns)*Z,ones(1,Nt));
        %
        C1 = C2;
        i = i + 1;
    end
%     fprintf('err1: %2.4f, err2: %2.4f, err3: %2.4f, iter: %3.0f \n',err1(end),err2(end),err3(end),i);
end