% inversio-ongelmien laskennallinen pk 2012

% Exercise 7

u = [zeros(1,100) ones(1,500) zeros(1,100)];

x = linspace(0,1,500);
s = [zeros(1,100) 100*x.*(x-1).*(x-0.2).*(x-0.7) zeros(1,100)];


% Boxcar kernel
k1 = ones(1,100)/100;

% Hann window kernel
k2 = hann(100);



% Note that the first argument of conmatrix must be
% COLUMN vector!
C1 = conmatrix(k1',700);
C2 = conmatrix(k2,700);

% Noiseless measurements, calculated using conv function
m1 = conv(k1,u);
m2 = conv(k2,u);
m3 = conv(k1,s);
m4 = conv(k2,s);


% Adding noise to the measuremnts
m1_n = m1 + 0.01 * max(u) * randn(size(m1));
m2_n = m2 + 0.01 * max(u) * randn(size(m2));
m3_n = m3 + 0.01 * max(s) * randn(size(m3));
m4_n = m4 + 0.01 * max(s) * randn(size(m4));


% Error norms
e1 = norm(m1_n - m1);
e2 = norm(m2_n - m2);
e3 = norm(m3_n - m3);
e4 = norm(m4_n - m4);



% We are going to solve here the problem
%
% m4_n = C2 * x
%
% (Real solution is s)
%
% Other problems are solved analogiously


% CGLS algorithm

% Choose x_1 = 0
sol = zeros(700,1);

% Discrepancy d_1 = m - A*x_1
d = m4_n';

% Residual
r = C2' * d;

% Search direction
sd = r;

y = C2 * sd;

% iteration loop, set max iteration to 500
maxiter = 50;

for i = 1:maxiter,
    
    alpha = (r'*r) / (y'*y);
    
    sol = sol + alpha * sd;
    
    d = d - alpha * y;
    
    % Check if discrepancy is less than error level
    % If so, we are done
    if ( norm(d) < e4)
        break;
    end
   %plot(s,'r')
   %hold on
   %plot(sol)
   %hold off
    
    old_r = r;
    
    r = C2'*d;
    
    beta = (r'*r) / (old_r'*old_r);
    
    sd = r + beta * sd;
    
    y = C2 * sd;
   
    
end

% Plot the solution or print warning if not done

if (i == maxiter & norm(d) > e4)
    disp('Iteration did not converge!')
else
    disp('Iteration converged!')
    i
    e4
    norm(d)
end
    



figure
plot(s,'r')
hold on
plot(sol)
hold off

