% MATLAB FEM code for 2D homogeneous Helmholtz equation: Delta U + k^2*U = 0
% P1 triangular elements
% Dirichlet condition on the source (Antenna): U(Antenna) = Source_Amplitude
% Absorbing condition on the square bounds: n.grad U + ik*U = 0 with n the outwardly normal vector at the boundary of the square

% Remarks:
% - the definition of the mesh requires the pdetool box of Matlab, but I can provide if needed another geometry file. 
% - all the assembly and resolution are compatible with all Matlab versions.
% - the assembly could have been fully vectorized (much more efficient), but this intermediate level way to proceed allows to see the usual steps in detail to debug the code

% I consider a point source rather than a very small circle and wonder if this is the problem ?


%%%%%%%%%%%%%%%%%%%%%%%%%
%% PHYSICAL PARAMETERS %%
%%%%%%%%%%%%%%%%%%%%%%%%%
k = 30;                % Wavenumber of the Helmholtz equation
Source_Amplitude = 1;  % Amplitude of emission


%%%%%%%%%%%%%%%%%%%%%%%%%
%% SET GEOMETRY & MESH %%
%%%%%%%%%%%%%%%%%%%%%%%%%
model   = createpde;
Domain  = [3 4 0 1 1 0 0 0 1 1]';  % Define unite square domain
g       = decsg(Domain,'D',char('D')');
geometryFromEdges(model,g);
mesh    = generateMesh(model,'Hmax',0.01,'Hmin',0.001,'GeometricOrder','Linear'); % Hmin and Hmax give the min and max element size (adapted mesh)
[p,e,t] = meshToPet(model.Mesh);   % Triangulation

t = t(1 : 3, :).';   % Get column PET format with usefull labels
p = p.';
e = e([1 2 5],:)';

T = triangulation(t(:,1 : 3),p); % Triangulation data from mesh
Antenna = [0.5 0.5]; % Expected position of the source


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% INDICES OF PHYSICAL REGIONS %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
id_E1 = find(e(:,3) == 1);  % Get edge indices of each of the 4 sides of the domain
id_E2 = find(e(:,3) == 2);
id_E3 = find(e(:,3) == 3);
id_E4 = find(e(:,3) == 4);

id_ABC    = [id_E1; id_E2; id_E3; id_E4];  % Choose edges supporting Absorbing Boundary Condition
Dir_guess = [];                % Choose edges supporting homogeneous Dirichlet condition

id_Dir      = unique(e(Dir_guess,1:2));   % Extract indices of the nodes supporting homogeneous Dirichlet boundary conditions (reflecting parts)
id_Antenna  = nearestNeighbor(T,Antenna);               % Index of the node where we apply inhomogeneous Dirichlet: i.e. the antenna (found via nearestNeighbor)
id_DirNodes = [id_Antenna; setdiff(id_Dir,id_Antenna)]; % Indices of all the Dirichlet nodes (Antenna + reflecting parts)

nb_t    = size(t,1);               % Number of elements
nb_p    = size(p,1);               % Number of nodes
nb_eABC = length(id_ABC);          % Number of ABC edges
nb_DirNodes = length(id_DirNodes); % Number of Dirichlet nodes

id_DoF = setdiff((1 : nb_p).',id_DirNodes);  % Indices of the free nodes (ABC nodes + interior nodes)


%%%%%%%%%%%%%%%%%%
%% FEM ASSEMBLY %%
%%%%%%%%%%%%%%%%%%
% The mass M, stiffness K and edge mass bM are assembled as follows:
% - loop over the elements and compute each 3 x 3 elementary matrix Me, Ke, bMe
% - reshape each elementary matrix into 9 x 1
% - store each of them in corresponding tables (size 9 x number of elements) tM, tK and tbM: this allows to optimize the storage
% - deal with sparse matrices: look for connectivity list between nodes and elements in order to spread the tables tM,tK,tbM (containing the elementary matrices of each element) 
% into the global square sparse matrices M,K and bM at the good places.

tM  = zeros(9,nb_t);   % Initialize tables of elementary matrices (M: mass, K: stiffness, i-th column = reshaped elementary matrix of triangle i)
tK  = zeros(9,nb_t);
tbM = zeros(9,nb_t);   % Initialize table of edge mass elementary matrices in (explain)

for i = 1 : nb_t  % Loop of the triangles
%---Characterization of triangular element i
    nodes_t = t(i,:);                     % Indices of the 3 nodes of triangle i
    Pe      = [ones(3,1), p(nodes_t,1:2)];  % 3x3 matrix with lines (1 Xcorner Ycorner)
    CoefT   = inv(Pe);                      % Columns of CoefT are coefs a,b,c in shape function phi = a+bx+cy 
    Area    = abs(det(Pe))/2;
%---Elementary stiffness of element i 
    grad = CoefT(2:3,:);            % P1 elements: grad = the coefs
    Ke   = Area * (grad.' * grad);  % Elementary stiffness
    Ke   = reshape(Ke,9,1);         % Reshape in 9x1 to load in tK
%---Elementary mass of element i
    GaussPt = [1/2, 1/2, 0; 0, 1/2, 1/2; 1/2, 0, 1/2] * Pe(:,2 : 3); % Location of gauss points on ref triangle 
    MeanPt  = [ones(3,1) GaussPt] * CoefT;                  % Apply to shape function
    Me      = Area/3 * (MeanPt(1,:).' * MeanPt(1,:) + ...
              MeanPt(2,:).' * MeanPt(2,:) + MeanPt(3,:).' * MeanPt(3,:)); % Mass elementary matrix: jac * (Gauss quad of phi * phi)
    Me = reshape(Me,9,1);    % Reshape in 9x1 to load in tM
%---Load the elementary matrices on the tables
    tM(:,i) = Me; 
    tK(:,i) = Ke; 
end

% For simplicity, the table tbM is rewritten in the format of the other tables tM and tK, using the connectivity lists.
I  = t(:,[1 2 3 1 2 3 1 2 3]).';  % Connectivity list of triangles, rows
J  = t(:,[1 1 1 2 2 2 3 3 3]).';  % Connectivity list of triangles, columns

Ib = e(id_ABC(:),[1 2 1 2]).';    % Same for lines (edge elements)
Jb = e(id_ABC(:),[1 1 2 2]).';

ID = edgeAttachments(T,e(id_ABC,1),e(id_ABC,2)); % From each considered edges, find associated triangle

for i = 1 : nb_eABC % Loop over the ABC edges
%---Elementary edge mass matrix bMe of edge i 
    nodes_e = e(id_ABC(i),1 : 2);  % Indices of the 2 nodes of edge i
    Le      = norm(p(nodes_e(2),1 : 2) - p(nodes_e(1),1 : 2)); % Length of edge i
    bMe     = Le/6 * [2; 1; 1; 2]; % Resolution of 1D integral
%---Spread contributions of bMe, according to triangle format of the previous tables
    idTRIelem = [I(:,ID{i}) J(:,ID{i})]; % Find connectivity list of triangle linked to edge i
    iE1  = [Ib(1,i) Jb(1,i)];            % Connectivity of each contribution of the elementary matrices
    iE2  = [Ib(2,i) Jb(2,i)];
    iE3  = [Ib(3,i) Jb(3,i)];
    iE4  = [Ib(4,i) Jb(4,i)];
    Pos1 = find(idTRIelem(:,1) == iE1(1) & idTRIelem(:,2) == iE1(2)); % Find the corresponding position in the global table
    Pos2 = find(idTRIelem(:,1) == iE2(1) & idTRIelem(:,2) == iE2(2));
    Pos3 = find(idTRIelem(:,1) == iE3(1) & idTRIelem(:,2) == iE3(2));
    Pos4 = find(idTRIelem(:,1) == iE4(1) & idTRIelem(:,2) == iE4(2));
%---Load the contributions in tbM
    tbM(Pos1,ID{i}) = tbM(Pos1,ID{i}) + bMe(1); 
    tbM(Pos2,ID{i}) = tbM(Pos2,ID{i}) + bMe(2);
    tbM(Pos3,ID{i}) = tbM(Pos3,ID{i}) + bMe(3);
    tbM(Pos4,ID{i}) = tbM(Pos4,ID{i}) + bMe(4);
end

% Build the global matrix: global table tA and sparsification under matrix format.
tA    = k^2 * tM + 1i * k * tbM - tK;         % Table with elementary matrices of global system
A     = sparse(I,J,tA,nb_p,nb_p); % Assemble sparse global FEM matrix
A_DoF = A(id_DoF,id_DoF);  % Extract the DoF for the resolution (Dirichlet nodes sent to right hand side)

% Right hand side with Dirichlet conditions
DirBound = [Source_Amplitude; zeros(nb_DirNodes - length(Source_Amplitude),1)];    % Set Dirichlet condition vector
b        = -A(id_DoF,id_DirNodes) * DirBound;     % Right hand side, enforcing the Dirichlet conditions


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% RESOLUTION OVER THE DoF'S %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
U_vect = A_DoF\b; % Here a priori Matlab's LU direct solver, efficient for these settings
U              = zeros(nb_p,1); % Initialize the full solution vector
U(id_DirNodes) = DirBound;  % Fill the solution with nodes with Dirichlet condition (Antenna + reflecting parts)
U(id_DoF)      = U_vect;    % Fill with DoF (interior nodes + ABS edge nodes)


%%%%%%%%%%
%% PLOT %%
%%%%%%%%%%
trisurf(t(:,1:3), p(:,1), p(:,2),real(U),'facecolor','interp');
set(0,'DefaultFigureColormap',jet()); shading interp;