im          = imread('noisy_input.png');
[nx,ny,nc]   = size(im);
f = double(im(:)) / 255.; 
m = ones(size(f));
                          
e                  = ones(ny, 1);
Dx_tilde           = spdiags([e -e], -1:0, ny, ny);
Dx_tilde(:, end)   = 0;
e                  = ones(nx, 1);
Dy_tilde           = spdiags([-e e], 0:1, nx, nx);
Dy_tilde(end, end) = 0;
Dx                 = kron(Dx_tilde', speye(nx));
Dy                 = kron(speye(ny), Dy_tilde);
D                  = cat(1, kron(speye(3), Dx), kron(speye(3), Dy));

u = f;
lambda = 10;

max_iter = 500;
energy = zeros(max_iter, 1);

q = zeros(ny*nx*nc*2, 1);
tau = 1/(normest(D).^2);

for k=1:max_iter
    u = f - (D' * q) / lambda;

    Du = reshape(D*u, [ny*nx, nc, 2]);
    norm_Du = sqrt(sum(sum(Du.^2,2), 3));
    energy(k) = (lambda/2) * sum((m .* (u-f)).^2) + sum(norm_Du);
    
    grad = D * (D' * q - lambda * f);
    q = q - tau * grad;
    
    qproj          = reshape(q,nx*ny,2*nc);
    normq       = sqrt(sum(qproj.^2,2));
    idx         = (normq>1);
    qproj(idx,:)   = qproj(idx,:)./(repmat(normq(idx),1,2*nc));
    q           = qproj(:);
    
        
    fprintf('It=%d En=%f\n', k, energy(k));
end

imshow([reshape(f, [ny,nx,nc]) reshape(u, [ny,nx,nc])]);