function [J,A]=diffusion_lag(I,iter,lam,g,h,I0)
%% Total Variation denoising.
%% Example: J=chambolle1(I,iter,lam,I0)
%% Input: I    - image (double array gray level 0-255),
%%        iter - num of iterations,
%%        dt   - time step, should be 0.25 at most  [0.125] 
%%        lam  - fidelity term lambda; choosing the correct value
%%               depends on the size of the image and the range of 
%%               pixel values; for the range issue, both commands
%%                 J1 = diffusion_lag(I,iter,dt,lam,G,I0)
%%                 J2 = diffusion_lag(I/a,iter,dt,a*lam,G,I0)
%%               give the same result; for the size issue,
%%               images whose pixel values are in [0..1] and 
%%               whose spatial stepsize is h,
%%               the value  lam=h*400/p  and TV-minimization
%%               (see lag_TV.m) will shrink the jump
%%               in an image with black left half (I=0) and 
%%               white right half (I=1) by precisely p percent.
%%        g    - name of the m-file for minimization method [g='glag_TV']
%%        h    - spatial stepsize [h=1/max(size(I))]
%%        I0   - starting point for fixed-point iteration  [I0=I]
%%       (default values are in [])
%% Output: evolved image
%%
%%   On the aforementioned example,
%%   about 10000 Iterations give precision 3-5 * 10^(-3)  with dt=0.5

[ny,nx]=size(I); 
I=double(I);

if ~exist('iter')
    iter=5;
end
if ~exist('h')
    h=1/max([ny,nx]);
end
if ~exist('lam')
    lam=400; % shrink jumps in TV-denoising by at most 1 percent
end
if ~exist('I0')
    I0=I;
else
    I0=double(I0);
end
if ~exist('g')
    g='glag_TV';
end

% right hand side
f=h^2*lam*I(:);

% prepare for plots
hFig=figure;
res=get(0,'ScreenSize');
set(hFig,'Position',[1 1 res(3) res(4)-64]); %Leave room for title bar

% prepare sparse matrix for difference method
i=zeros(1,8*nx*ny);
j=zeros(1,8*nx*ny);
s=zeros(1,8*nx*ny);
fak=zeros(1,8*nx*ny);
indk=zeros(1,8*nx*ny); % index into matrix g_lag
indl=zeros(1,8*nx*ny);
% put fixed part of diagonal entries
i(1:(nx*ny))=(1:(nx*ny));
j(1:(nx*ny))=(1:(nx*ny));
s(1:(nx*ny))=lam*h^2*ones(1,nx*ny);
num=nx*ny;
% construct index vectors and signs for diffusion_lag entries in the
% matrix
for k=1:nx-1
    kk=(k-1)*ny;
    for l=1:ny-1
        kkl=kk+l; % row number for diagonal entry 2*g(l,k)
        num=num+1;
        i(num)=kkl;
        j(num)=kkl;
        fak(num)=2;
        indk(num)=k;
        indl(num)=l;
        num=num+1;
        i(num)=kkl+1;
        j(num)=kkl+1;
        fak(num)=1;
        indk(num)=k;
        indl(num)=l;
        num=num+1;
        i(num)=kkl+nx;
        j(num)=kkl+nx;
        fak(num)=1;
        indk(num)=k;
        indl(num)=l;
        num=num+1;
        i(num)=kkl;
        j(num)=kkl+1;
        fak(num)=-1;
        indk(num)=k;
        indl(num)=l;
        num=num+1;
        i(num)=kkl;
        j(num)=kkl+nx;
        fak(num)=-1;
        indk(num)=k;
        indl(num)=l;
        num=num+1;
        i(num)=kkl+1;
        j(num)=kkl;
        fak(num)=-1;
        indk(num)=k;
        indl(num)=l;
        num=num+1;
        i(num)=kkl+nx;
        j(num)=kkl;
        fak(num)=-1;
        indk(num)=k;
        indl(num)=l;
    end
end
numend=num;
i=i(1:numend);
j=j(1:numend);
s=s(1:numend);
fak=fak(1:numend);
indtotal=(nx*ny+1):numend;
% convert indk,indl to single index 
indg=sub2ind([ny,nx],indl(indtotal),indk(indtotal));

for k=1:iter  % do iterations
    % nabla and div are discretized as in Chambolle's dual method
    % estimate nabla(I0)
    I0_x = [I0(:,[2:nx]),zeros(ny,1)]-[I0(:,[1:nx-1]),zeros(ny,1)];
	I0_y = [I0([2:ny],:);zeros(1,nx)]-[I0([1:ny-1],:);zeros(1,nx)];
    % include the stepsize h for computation of the diffusivity function
	arg_lag=1/h*sqrt(I0_x.^2+I0_y.^2);
    arg_lag=arg_lag(:).';
    g_lag=feval(g,arg_lag);    
    % construct sparse matrix for difference scheme
    s(indtotal)=fak(indtotal).*g_lag(indg);
    A=sparse(i,j,s,nx*ny,nx*ny,numend);
    I_new=A\f;  % the new image 
    I_new=reshape(I_new,ny,nx);
   imshow(I_new);
   pause
   % compute the next iterate
    I0=I_new;
end % for i
%% return image
%J=I*Imean/mean(mean(I)); % normalize to original mean
J=I_new;