function[Data]=subAssemble(Mesh, Data, Solution, operation)

% subAssemble: Assembles the part of the internodes matrix which is
% changing from one iteration to the next
% INPUT:
% Mesh:         Structure containing all the mesh parameters
% Data:         Structure containing all the data parameters
% Solution:     Structure containing the solution
% operation:    Specifies the type of operation (initialization or
%               updating)
% OUTPUT:
% Data:         Updated structure containing all the data parameters

if strcmp(operation, 'initialization')
    [Data]=computeBoundaryMass(Mesh, Data, 'initialization');
end

% Assemble interpolation matrices
[Data]=assembleRs(Mesh, Data, Solution);
% Assemble boundary mass matrices
[Data]=computeBoundaryMass(Mesh, Data, 'updating');

    function[Data]=assembleRs(Mesh, Data, Solution)
        
        % assembleRs: Constructs the interpolation matrices
        % INPUT:
        % Mesh:     Structure containing all the mesh parameters
        % Data:     Structure containing all the data parameters
        % Solution: Structure containing the solution
        % OUTPUT:
        % Data:     Updated data structure with interpolation matrices
        
        % Retrieve mesh parameters
        nb_dof_local=Mesh.nb_dof_local;
        
        % Retrieve nodes making up the interfaces
        nodes1=Data.body{1}.interface_nodes;
        nodes2=Data.body{2}.interface_nodes;
        
        % Retrieve coordinates of nodes making up the interfaces
        coord1=Mesh.coord_mat(nodes1,:)+Solution.U_sort(nodes1,:);
        coord2=Mesh.coord_mat(nodes2,:)+Solution.U_sort(nodes2,:);
        
        [radiuses1, ~, ~, nnzR21, ~]=getRadius(Data, nodes1, nodes2, coord1, coord2);
        [radiuses2, ~, ~, nnzR12, ~]=getRadius(Data, nodes2, nodes1, coord2, coord1);
        
        i1=nnzR12==0;
        i2=nnzR21==0;
        
        % Check for distant bodies
        if all(i1) || all(i2)
            warning('One of the bodies will be translated because they are too far away from each other. Boundary conditions might be affected')
            [Mesh, coord1, coord2]=translateBodies(Mesh, Data, radiuses1, radiuses2, nodes1, nodes2, coord1, coord2);
            
            % Recompute radiuses
            [radiuses1, ~, ~, nnzR21, ~]=getRadius(Data, nodes1, nodes2, coord1, coord2);
            [radiuses2, ~, ~, nnzR12, ~]=getRadius(Data, nodes2, nodes1, coord2, coord1);
            
            i1=nnzR12==0;
            i2=nnzR21==0;
        end
        
        % Check for isolated nodes
        while any(i1) || any(i2)
            
            dump_nodes1=nodes1(i1);
            dump_nodes2=nodes2(i2);
            
            % Updating
            [Data]=updateInterface(Mesh, Data, 1, dump_nodes1, 'dump');
            [Data]=updateInterface(Mesh, Data, 2, dump_nodes2, 'dump');
            
            nodes1=nodes1(~i1);
            nodes2=nodes2(~i2);
            
            coord1=coord1(~i1,:);
            coord2=coord2(~i2,:);

            % Recompute radiuses
            [radiuses1, ~, ~, nnzR21, ~]=getRadius(Data, nodes1, nodes2, coord1, coord2);
            [radiuses2, ~, ~, nnzR12, ~]=getRadius(Data, nodes2, nodes1, coord2, coord1);
            
            i1=nnzR12==0;
            i2=nnzR21==0;
        end
        
        [Phi11] = assembleInterpMat(coord1, coord1, radiuses1);
        [Phi21] = assembleInterpMat(coord1, coord2, radiuses1);
        [Phi22] = assembleInterpMat(coord2, coord2, radiuses2);
        [Phi12] = assembleInterpMat(coord2, coord1, radiuses2);
        
        % Interpolation matrices
        R12=Phi12/Phi22;
        R21=Phi21/Phi11;
        
        s1=sum(R12,2);
        s2=sum(R21,2);
        
        % Rescaling
        R12=R12./s1;
        R21=R21./s2;
        
        Data.R12_normal=R12;
        Data.R21_normal=R21;
        
        % Expansion to full size
        Data.R12=kron(R12, speye(nb_dof_local));
        Data.R21=kron(R21, speye(nb_dof_local));
        
    end

    function[Data]=computeBoundaryMass(Mesh, Data, operation)
        
        % computeBoundaryMass: Assembles the boundary mass matrices
        % INPUT:
        % Mesh:         Structure containing all the mesh parameters
        % Data:         Structure containing all the data parameters
        % operation:    Specifies the type of operation (initialization or
        %               updating)
        % OUTPUT:
        % Data:         Updated data structure with boundary mass matrices
        
        
        % Retrieve mesh parameters
        % Number of local degrees of freedom
        nb_dof_local=Mesh.nb_dof_local;
        % Coordinate matrix
        coord=Mesh.coord_mat;
        % Order
        order=Mesh.order;
        % Get quadrature nodes and weights
        [points, weights]=getQuadrature(3, 'boundary');
        % Number of quadrature points
        nb_points=length(weights);
        
        % Retrive functions
        [Functions]=getFunctions(order);
        % Retrieve boundary basis functions
        phi_boundary=Functions.phi_boundary;
        
        % Retrieve bodies
        body=Data.body;
        % Number of bodies
        nb_bodies=length(body);
        
        if strcmp(operation, 'initialization')
            
            for n=1:nb_bodies
                % Retrieve data
                % Retriveve edges making up the interface
                Edges=body{n}.interface_connect;
                % Retrieve nodes making up the interface
                nodes=body{n}.interface_nodes;
                nb_nodes=length(nodes);
                s=order+1;
                
                % Initialization
                Phi=zeros(s, nb_points);
                
                % Map global nodes to local nodes
                func=@(x) find(nodes==x);
                connect=arrayfun(func, Edges);
                T=connect';
                G=T(:);
                
                I=repmat(connect, [1 s])';
                J=repmat(G', [s 1]);
                
                % Loop over the Gauss points
                for i = 1:nb_points
                    Phi(:,i)=phi_boundary(points(i,:));
                end
                
                Tr=Edges';
                Gr=Tr(:);
                % Coordinates of the nodes of the elements
                coord_nodes=coord(Gr,:);
                % Endpoints of the edges
                a=coord_nodes(1:s:end,:);
                b=coord_nodes(2:s:end,:);
                bk=b-a;
                norm_bk=vecnorm(bk,2,2);
                
                lambda=weights.*norm_bk';
                
                M=kr(Phi, Phi)*lambda;
                M=sparse(I(:), J(:), M(:), nb_nodes, nb_nodes);
                
                Data.body{n}.iinterface_mass=M;
                % Expansion to full size
                M=kron(M, speye(nb_dof_local));
                Data.body{n}.interface_mass=M;
            end
        else
            for n=1:nb_bodies
                active_set=Data.body{n}.active_set;                
                Data.body{n}.interface_mass=kron(Data.body{n}.iinterface_mass(active_set, active_set), speye(nb_dof_local));
            end
        end
    end

end