u0 = double(imread('data/einstein.png'));
u0 = mean(u0,3);

[M N] = size(u0);
MN = M*N;
I=reshape([1:M*N],M,N);
east=[I(:,2:end), I(:,end)];
north=[I(2:end,:); I(end,:)];
D1 = sparse(I,east,1,MN,MN) -speye(MN,MN);
D2 = sparse(I,north,1,MN,MN) -speye(MN,MN);
D = [D1 ; D2];

h0 = [ 1 2 1 ];
h = h0
for i=1:5 %% level of blur
    h = conv(h0,h);
end
size(h)
h = h'*h;
h = h/sum(sum(h));

stddev=5;
g = filter2(h,u0,'valid');
[Ms Ns]=size(g);
g = g + stddev*randn(Ms,Ns);

figure(1);
imagesc(u0); colormap(gray); %drawnow();
figure(2);
imagesc(g); colormap(gray); drawnow();

u=zeros(size(u0));
L2 = 8;
tau = 1; %% max value for tau = 1;
sig = 1/tau/L2;
lambda = .2;

p = zeros(2*MN,1);
%% primal-dual
for i=1:1000
    um = u;
    u = u-tau*(conv2(h,filter2(h,u,'valid')-g,'full') + reshape(D'*p,M,N));
    um = 2*u-um;
    p = p + sig*D*um(:);
    no= max(1,hypot(p(1:MN),p(MN+1:end))/lambda);
    p = p./[no;no]; 
    if mod(i,20)==0
        i
        figure(3);
        imagesc(u);
        colormap(gray); drawnow();
    end
end
