function sparsefem
% This function solves the elliptic measure space control problem
%      min 1/2 |y-yd|_L^2 + alpha |u|_M    s.t.   -\Delta y = u
% using a semismooth Newton method described in the paper
%    "Approximation of elliptic control problems in measure spaces with
%     sparse solutions"
% by Eduardo Casas, Christian Clason, and Karl Kunisch, to appear in SIAM
% Journal on Control and Optimization.
%
% April 28, 2012            Christian Clason <christian.clason@uni-graz.at>
%                                            http://www.uni-graz.at/~clason

%% setup
N     = 128;      % number of spatial grid points
alpha = 1e-3;     % penalty parameter
maxit = 20;       % max iterations in semismooth Newton method

% setup grid, assemble stiffness and mass matrix
[A,M,xx,yy] = assembleFEM(N);
tplot = @(n,f,s) tplot_(n,f,s,N,xx,yy);

% target
yd = exp(-50*(xx.^2+yy.^2)); yd = yd(:);
tplot(1,yd,'target');

%% initialization
Ap = zeros(N*N,1); Ap_old = Ap; % active set for upper bound
Am = zeros(N*N,1); Am_old = Am; % active set for lower bound

% continuation strategy
for gamma = 10.^(0:12)
    disp(['Solving for gamma = ' num2str(gamma,'%1.0e')]);
    
    % semismooth Newton iteration
    for iter = 1:maxit
        % setup Newton step
        As = spdiags(gamma*(Ap+Am),0,N*N,N*N);
        C  = [A As; -M A];
        b  = [alpha*gamma*(Ap-Am); -M*yd];
        
        % solve for Newton step
        dx = C\b;
        p = dx(N*N+1:end);
        
        % compute active sets
        Ap = (p >  alpha);
        Am = (p < -alpha);
        
        update = nnz((Ap-Ap_old)|(Am-Am_old));
        fprintf('Iteration %d: %d points changed in active set\n', ...
            iter, update);
        if update == 0
            break
        end
        
        Ap_old = Ap;
        Am_old = Am;
    end
    
    % terminate continuation if Newton iteration converged in one step
    if iter == 1
        break
    end
end

% compute starting point for control, active sets
u  = -gamma*(max(0,p-alpha)+min(0,p+alpha));
Ap = (-u + p - alpha > 0);
Am = (-u + p + alpha < 0);

%% compute optimal measure space control
disp('Solving original problem');

I = speye(N*N);
O = spalloc(N*N,N*N,0);

for iter = 1:maxit
    % setup Newton step
    As = spdiags(Ap+Am,0,N*N,N*N);
    C  = [A O -I; -M A O; O As I-As];
    b  = [0*yd; -M*yd; alpha*(Ap-Am)];
    
    % solve for Newton step
    dx = C\b;
    y = dx(1:N*N);
    p = dx(N*N+1:2*N*N);
    u = dx(2*N*N+1:end);
    
    % compute active sets
    Ap = (-u + p - alpha > 0);
    Am = (-u + p + alpha < 0);
    
    update = nnz((Ap-Ap_old)|(Am-Am_old));
    fprintf('Iteration %d: %d points changed in active set\n', ...
        iter, update);
    if update == 0
        break
    end
    
    Ap_old = Ap;
    Am_old = Am;
end

%% plot results
tplot(2,u,'control');
tplot(3,y,'state');


function [K,M,xx,yy] = assembleFEM(n)
nel = 2*(n-1)^2;         % number of nodes
h2  = (2/(n-1))^2;       % Jacobi determinant of transformation (2*area(T))

% nodes
[xx,yy] = meshgrid(linspace(-1,1,n));

% triangulation
tri = zeros(nel,3);
ind = 1;
for i = 1:n-1
    for j = 1:n-1
        node         = (i-1)*n+j+1;              % two triangles meeting at node
        tri(ind,:)   = [node node-1 node+n];     % triangle 1 (lower left)
        tri(ind+1,:) = [node+n-1 node+n node-1]; % triangle 2 (upper right)
        ind = ind+2;
    end
end

% Mass and stiffness matrices
Ke = 1/2 * [2 -1 -1 -1 1 0 -1 0 1]'; % elemental stiffness matrix
Me = h2/24 * [2 1 1 1 2 1 1 1 2]';   % elemental mass matrix

ent = 9*nel;
row = zeros(ent,1);
col = zeros(ent,1);
valk = zeros(ent,1);
valm = zeros(ent,1);

ind = 1;
for el=1:nel
    ll       = ind:(ind+8);             % local node indices
    gl       = tri(el,:);               % global node indices
    row(ll)  = gl([1;1;1],:); rg = gl';
    col(ll)  = rg(:,[1 1 1]);
    valk(ll) = Ke;
    valm(ll) = Me;
    ind      = ind+9;
end
M = sparse(row,col,valm);
K = sparse(row,col,valk);

% modify matrices for homogenenous Dirichlet conditions
bdnod = [find(xx == -1); find(yy == -1); find(xx == 1); find(yy == 1)];
M(bdnod,:) = 0;
K(bdnod,:) = 0;  K(:,bdnod) = 0;
for j = bdnod'
    K(j,j) = 1; %#ok<SPRIX>
end


function tplot_(n,f,s,N,xx,yy)
figure(n); surf(xx,yy,reshape(f,N,N)); axis tight
shading interp;colormap summer;lighting phong;camlight headlight;alpha(0.8)
title(s); xlabel('x'); ylabel('y');
drawnow;
