% problem data
dirichlet_data = @(x) (x);
%f = @(x) (100*x.*(1-x));
f = @(x) (0*x);

%lower_f = @(x) 0*x;
%upper_f = @(x) 0*x+1;
lower_f = @(x) x.*(1-x)*2;
upper_f = @(x) 0*x+10;

% generate grid hierarchy
gridHierarchy = grid_hierarchy(2, 10);
PP = transfer_operators(gridHierarchy);

% iteration steps
maxnu = 40;








% the finest grid
grid = gridHierarchy{end};

% grid points
n = length(grid)

% mesh size
h = grid(2) - grid(1);

% assemble rhs by trapezoidal rule
fx = f(grid);
b = fx * h;
b(1) = fx(1) * h/2;
b(n) = fx(n) * h/2;

% assemble matrix
A_diags = [-ones(n,1), 2*ones(n,1), -ones(n,1)] / h;
A = spdiags(A_diags, [-1 0 1], n, n);
A(1,1) = 1/h;
A(n,n) = 1/h;

% setup solution vector
x = zeros(n, 1);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% incorporate dirichlet bc

% sample dirichlet data on boundary
x([1 end]) = dirichlet_data(grid([1 end]));

% shift problem using dirichlet data
b = b - A*x;

% make problem homogenous be restricting to inner vertices
A([1 end],:) = 0;
A(:, [1 end]) = 0;
b([1 end]) = 0;

% remove contributions for dirichlet data from coarse grid functions
PP{end}([1 end],:) = 0;


% interpolate obstacles
lower = lower_f(grid);
upper = upper_f(grid);

% ensure that initial value respects obstacles
x = max(min(x, upper),lower);
x([1 end]) = dirichlet_data(grid([1 end]));

x0 = x;

% Additional last argument for switching storage of all iterates on
% Second return value is a matrix containg x_0, ..., x_maxnu as
% column vectors
[x, xcache] = mmg_solver(A, b, lower, upper, x0, PP, maxnu, 1);





% compute energy for visualization
E = zeros(maxnu+1,1);
for nu=0:maxnu
    E(nu+1) = 1/2 * dot(A*xcache(:,nu+1),xcache(:,nu+1)) - dot(b,xcache(:,nu+1));
end

% compute approximate error for visualization
corr_norm = zeros(maxnu+1,1);
for nu = 0:maxnu
    corr = xcache(:,nu+1) - x;
    corr_norm(nu+1) = dot(A*corr, corr)^.5;
end
% plot only nonzero error values in semi log scale
nu = 0:maxnu;
nz = find(corr_norm);




% plot solution
figure(1);
clf;
for i = 1:maxnu
    clf

    subplot(2,2,1)
    plot(grid, x, 'o-r');
    hold on;
    plot(grid, lower, '-k');
    plot(grid, xcache(:,i), 'x-b');
    title('Exact solution, iterate, lower obstacle')

    subplot(2,2,2)
%    clf
    plot(grid, x-xcache(:,i), 'o-r');
    title('Error function')

    subplot(2,2,3)
    plot(0:maxnu, E, 'o-');
    title('Energy over iteration steps')

    subplot(2,2,4)
    semilogy(nu(nz), corr_norm(nz), 'o-');
    title('Approximate error over iteration steps')


    pause
end
