function TP3_2D
close all
Mx	= 20;
My	= 20;
Lx	= 1;
Ly	= 1;

x	= linspace(0,Lx,Mx+1);x=x(2:end-1);
dx	= x(2) - x(1);
y	= linspace(0,Ly,My+1);y=y(2:end-1);
dy	= y(2) - y(1);

[X,Y]	= meshgrid(x,y);

H	= -(X-Lx/2).^2-(Y-Ly/2).^2-.25;%+0.1*sin(20*(Y.*X));[(X-Lx/2).^2+(Y-Ly/2).^2 -1]
h	= H(:);

% Generalize (u_{n+1} - 2 u_n + u_{n-1})/dx^2 to 2D.
% In 2D :
% (ue - 2 u + uw})/dx^2 + (un - 2 u + us})/dy^2

%		   xn
%		xw x  xe
%		   xs

Nodes 	= 1:(Mx-1) * (My-1);
Nodes2D	= reshape(Nodes,(Mx-1) , (My-1));	
NodesN	= Nodes2D(1:end-1,:);
NodesS	= Nodes2D(2:end,:);
NodesE	= Nodes2D(:,2:end);
NodesW	= Nodes2D(:,1:end-1);

% Implementation of the 2D Laplace operator
 
A	= sparse([NodesE(:)],[NodesW(:)],1/dx^2,(Mx-1) * (My-1),(Mx-1) * (My-1)) ... %Lines corresponding to nodes that have a neighbourgh on their west side. 
        + sparse([NodesW(:)],[NodesE(:)],1/dx^2,(Mx-1) * (My-1),(Mx-1) * (My-1)) ... %Lines corresponding to nodes that have a neighbourgh on their east side. 
        + sparse([NodesN(:)],[NodesS(:)],1/dy^2,(Mx-1) * (My-1),(Mx-1) * (My-1)) ... %Lines corresponding to nodes that have a neighbourgh on their south side. 
        + sparse([NodesS(:)],[NodesN(:)],1/dy^2,(Mx-1) * (My-1),(Mx-1) * (My-1)) ... %Lines corresponding to nodes that have a neighbourgh on their north side. 
	- 2*(1/dx^2+1/dy^2)*eye((Mx-1) * (My-1));
A	= -A ;

Id	= eye((Mx-1) * (My-1));

%h	= x-1.5+0.1*sin(20*x);%2.5*(x-1);
%h_red	= h(2:end-1)';

g	= 10 ;
f	= -g*ones((Mx-1) * (My-1),1);
c	= 1;


%Debugging + Initialization of u : check the "free" solution
	u	= A\f;
	%plot(x(2:end-1),u)
	U		= reshape(u,My-1,Mx-1);
if 0
figure(1)	

	mesh(X,Y,U)
	mesh(X,Y,H)
%1
%pause
end

%Initialization of lambda
lambda 	= 0*u;
u_old	= u+1;


%Newton Algorithm  (Primal-Dual Active Set Strategy) 
while norm(u-u_old)>=1e-10

	u_old		= u;
	In		= find(lambda - c*(u-h) <=0 );
	Ac		= find(lambda - c*(u-h) > 0 );

	lambda(In) 	= 0;
	u(Ac)	   	= h(Ac);

	Matrix		= [A(:,In) , -Id(:,Ac)] ;
	Rhs		= f 	- A(:,Ac)*u(Ac)  ;
	W		= Matrix\Rhs;

	lambda(Ac) 	= W(1+length(In):end);
	u(In)	   	= W(1:length(In));
	
%	plot(x(2:end-1),u,x(2:end-1),h_red,x(2:end-1),dx*lambda,'linewidth',4)

	U		= reshape(u,My-1,Mx-1);



end
figure(2)

hold on
	surf(X,Y,U)%,X,Y,H)
	mesh(X,Y,H)%,X,Y,H)
hold off
view(-40,30)

%	legend({'Solution','Obstacle'},'location','Southeast')
%	pause
end
