load SP500
data = normlization(data);
S = cov(data);  %数据协方差
[X, W] = glasso_1(double(S), 0.5);
%X:sigma^(-1), W:sigma
[~, idx] = sort(info(:,3));
colormap gray
imagesc(X(idx, idx) == 0)
axis off

%% Data Normalization
function data = normlization(data)
data = bsxfun(@minus, data, mean(data));
data = bsxfun(@rdivide, data, std(data));
end

function [X, W] = glasso_1(S, lambda)
%% Graphical Lasso - Friedman et. al, Biostatistics, 2008
% Input:
%   data - n * p matrix with n samples and p variables
%   S - 样本的协方差矩阵
%   lambda - thresholding parameter
% Output:
%   X - Concentration matrix    sigma^(-1)
%   W - Covariance matrix   sigma
%%
p = size(S,1);  %数据维度
W = S + lambda * eye(p);  %W=S+λI
beta = zeros(p) - lambda * eye(p);   %β=-λI
eps = 1e-4;
finished = false(p);   %finished:p*p的逻辑0矩阵
while true
    for j = 1 : p
        idx = 1 : p; idx(j) = [];
        beta(idx, j) = lasso(W(idx, idx), S(idx, j), lambda, beta(idx, j));
        W(idx, j) = W(idx,idx) * beta(idx, j);  %W=W*β
        W(j, idx) = W(idx, j);
    end
    index = (beta == 0);
    finished(index) = (abs(W(index) - S(index)) <= lambda);
    finished(~index) = (abs(W(~index) -S(~index) + lambda * sign(beta(~index))) < eps);
    if finished
        break;
    end
end
X = zeros(p);
for j = 1 : p
    idx = 1 : p; idx(j) = [];
    X(j,j) = 1 / (W(j,j) - dot(W(idx,j), beta(idx,j)));
    X(idx, j) = -1 * X(j, j) * beta(idx,j);
end
% X = sparse(X);
end

function w = lasso(A, b, lambda, w)
% Lasso 
p = size(A,1);
df = A * w - b;
eps = 1e-4;
finished = false(1, p);
while true
    for j = 1 : p
        wtmp = w(j);
        w(j) = soft(wtmp - df(j) / A(j,j), lambda / A(j,j));
        if w(j) ~= wtmp
            df = df + (w(j) - wtmp) * A(:, j); % update df
        end
    end
    index = (w == 0);
    finished(index) = (abs(df(index)) <= lambda);
    finished(~index) = (abs(df(~index) + lambda * sign(w(~index))) < eps);
    if finished
        break;
    end
end
end
%% Soft thresholding
function x = soft(x, lambda)
x = sign(x) * max(0, abs(x) - lambda);
end