% The program demonstrates the use of CG algorithm for calculating
% deconvolution from noisy data. 

% Generating the matrix describing the forward problem (convolution)

T = 3;
n = 128;
t = linspace(0,T,n)';
gamma = 0.2;
hh = (T/n)*1/(sqrt(2*pi*gamma^2))*exp(-1/(2*gamma^2)*t.^2);
A = toeplitz(hh,hh);          % convolution matrix

% Generating the noiseless data and the true derivative 

xtrue = 3*(t>1).*(t<2);
b0 = A*xtrue;

% Adding random noise. We assume that the norm of the noise vector can be
% assumed to be known

sigma = 5e-7;                 %  Noise level very low
errvect = sigma*randn(n,1);
eta = norm(errvect);          %  Norm of the noise vector
b = b0 + errvect;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

save_iterates = 'yes';

iterate = 'yes';  % Iterations stopped if iterate == 'no'
maxit = 50;       %  Maximum number of iterations
tau = 1.5;        %  The fudge factor used in discrepancy principle
discr = tau*eta;  %  The discrepancy limit

% CG solver. The iteration will stop if either the maximum number
% of iteration is reached, the discrepancy limit is attained.

j = 0;
plotcolor = 'k';
lw = 1;  %  Line width 
figure(1)
plot(t,xtrue,'g')
hold on

x = zeros(n,1);
X = x;             % Iterare storage
y = A*x;           % We make dummy y, y=zeros(1,n) would have done.    
r = b - y;
p = r;
aux0 = r'*r,
j = 0;

while (j<=maxit)&strcmp(iterate,'yes')
    j = j + 1;
    y = A*p;
    alpha = aux0/(p'*y);
    x = x + alpha*p;       % New update
    if strcmp(save_iterates,'yes')
        % Saving all the iterates
        X = [X,x];
    end
    r = r - alpha*y;       % New residual
     % Checking the discrepancy condition
     if norm(r) < discr
        % The discrepancy limit is reached; stop iteration
        iterate = 'no';
        disp(['Discrepancy limit attained at iteration j =' num2str(j)]);
        plotcolor = 'r';
        lw = 2;
    end
    aux1 = r'*r;
    beta = aux1/aux0;
    p = r + beta*p;        % New search direction
    aux0 = aux1;
    figure(1)
    plot(t,x,plotcolor,'LineWidth',lw)
    pause
end


hold off
title(['Number of iterations =' num2str(j)])