close all
clear 

image1  = imread('jacobi.jpg');
image2  = imread('dyoung.jpg');

N = length(image1);

%% Create 5-point finite difference Laplacian on a square
G = numgrid('S',N+2);
D = delsq(G);
A = D; 

%% Increment the digaonal by diagonalIncremement

diagonalIncremement = 0; 
for i=1:length(A)
    A(i,i) = A(i,i) + diagonalIncremement; 
end

%% Create b such that image2 is answer to Ax = b
b = A*cast(reshape(image2,N^2,1),'double');

%% Make image1 our initial guess to Ax = b

x0 = cast(reshape(image1,N^2,1),'double');

%% Visualize initial guess

image(reshape(x0,N,N))
colormap(gray(256))
axis equal off tight
drawnow


%% Set up matrix splitting of A
D = diag(diag(A));
L = -1*tril(A,-1);
U = -1*triu(A,1);

%% Prepare diagonal splitting version of Jacobi method

Dinv = inv(D);
LPU = (L+U);


%% Perform Jacobi iteration

x = x0;
iterates{1} = x; 
e0 = norm(x - cast(reshape(image2,N^2,1),'double'));

[rows,columns] = size(A); 

numIterations = 50; 
for iter = 2:numIterations
    xold = x;

    x = Dinv*LPU*x + Dinv*b;
    eiter = norm(x - cast(reshape(image2,N^2,1),'double'))/e0
    
    cla
    image(reshape(x,N,N))
    colormap(gray(256))
    axis equal off tight
    pause(.25)
        
end