% The program demonstrates the use of CGLS algorithm for calculating
% numerical derivative from noisy data. 

% Generating the matrix describimg the forward problem (antiderivative)

n = 128;
col = ones(n,1);
row = zeros(1,n);
row(1) = 1;
A = (1/n)*toeplitz(col,row);

% Generating the noiseless data and the true derivative 

t = linspace(0,1,n)';
b0 = 3*t - 1;
b0 = max(zeros(n,1),b0);
b0 = min(b0,ones(n,1));
xtrue = 3*(t>1/3).*(t<2/3);

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

%sigma = 0.0005;
sigma = 0.005;
errvect = sigma*randn(n,1);
eta = norm(errvect);          %  Norm of the noise vector
b = b0 + errvect;

figure(1)
plot(t,b,'k-','LineWidth',2)
pause


save_iterates = 'yes';

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

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

j = 0;
lw = 1;
plotcolor = 'b';
figure(2)
plot(t,xtrue,'r')
axis([0 1 -0.5 3.5])
hold on

x = zeros(n,1);
X = x;
d = b - A*x;
r = A'*d;
p = r;
aux0 = r'*r;
j = 0;

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

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

    



