%% --------------------------------------------------------------------- %%
%                         ** Elasticity class **                          %
%-------------------------------------------------------------------------%

classdef Elasticity
    
    %% Properties
    properties
        
        % Finite element mesh
        mesh
        
        % Material properties
        E         % Young's modulus
        nu        % Poisson's ratio
        rho       % Solid material density
        th = 1.0; % thickness
        
        % ID matrix (relation node-DOF)
        % (lines = DOF's, columns = nodes)
        ID
        number_of_equations

        % Equation matrices
        F % forces
        K % stiffness
        M % mass
        U % displacements (state variable)
        
        % Sensitivity numerical filter matrix
        H
        
    end
    
    %% Methods
    methods
        
        %% Constructor
        function self = Elasticity(mesh)
            
            % Generate finite element mesh
            self.mesh = mesh;
            
            disp([' '])
            disp(['         Preparing FEA.'])
            
            % Creating ID matrix (relation node-DOF)
            self.ID = ones(2,size(self.mesh.coordinates,1));
            self.ID(1,:) = 1+2*[0:(size(self.mesh.coordinates,1)-1)];
            self.ID(2,:) = 2+2*[0:(size(self.mesh.coordinates,1)-1)];

            % Number of equations
            self.number_of_equations = self.ID(2,end);
            
            disp(['         Number of equations: ', sprintf('%10i',self.number_of_equations)])

        end % end Constructor
        
        %% Add solid material
        function self = AddSolidMaterial(self, E_in, nu_in, rho_in)
            
            % Assign material
            self.E = E_in;
            self.nu = nu_in;
            self.rho = rho_in;
            
        end % end AddSolidMaterial
        
        %% Build filter matrix
        function self = BuildFilterMatrix(self,radius)
            
            disp(['         Building filter matrix.'])
            
            % Estimating the number of elements in filter matrix: elements within filter
            % area multiplied by the number of elements.
            nfilterel = length(self.design_domain)*4*round(radius/(self.mesh.coordinates(self.mesh.incidence(1,3),1)-self.mesh.coordinates(self.mesh.incidence(1,2),1)))^2;

            % Allocating vectors
            % Element weights
            R = zeros(nfilterel,1);
            % Index vectors
            ifilt = zeros(nfilterel,1);
            jfilt = zeros(nfilterel,1);

            % Selecting coordinates of design nodes
            X_den = self.mesh.centroids(self.design_domain,1);
            Y_den = self.mesh.centroids(self.design_domain,2);

            % Auxiliary counter
            k = 0;

            % Looping at the elements inside the design domain
            for i = 1:length(self.design_domain)

                % Element
                el = self.design_domain(i);

                % Coordinates of the centroid of the element
                x = self.mesh.centroids(el,1);
                y = self.mesh.centroids(el,2);

                % Weight function (linear with radial distance)
                r = max(0,1-(((X_den-x).^2+(Y_den-y).^2).^(1/2)/radius));

                % Auxiliary counter update
                k = k+nnz(r);

                % Index vectors building
                ifilt(k-nnz(r)+1:k) = i*ones(nnz(r),1);
                jfilt(k-nnz(r)+1:k) = find(r);

                % Weights update
                R(k-nnz(r)+1:k) = r(r>0)/sum(r);

            end

            % H filter matrix
            self.H = sparse(ifilt(1:k),jfilt(1:k),R(1:k),length(self.design_domain),length(self.design_domain));

        end % end BuildFilterMatrix
        
        %% Assemble point loads
        function self = AssemblePointLoads(self)
            
            % Load vector assembly
            self.F = sparse(zeros(2,1));
            self.F(self.number_of_equations) = 0;
            % Applied forces
            for i = 1:size(self.mesh.neumann_boundary,1)
                if (self.mesh.neumann_boundary(i,1) ~= 0)
                    self.F(self.ID(self.mesh.neumann_boundary(i,2),self.mesh.neumann_boundary(i,1)),1) = self.mesh.neumann_boundary(i,3);
                end
            end
            
        end % end AssemblePointLoads
        
        %% Compute structural element stiffness matrix
        function Ke = ComputeStructuralKe(self,stress_case)

            % Initializing matrix and getting FEA info
            Ke = zeros(8);
            el = 1; % Structure grid, element 1
            coord = self.mesh.coordinates;
            inci = self.mesh.incidence;
            E = self.E;
            nu = self.nu;

            % Selecting stress case
            switch stress_case
                case 1 % Plane stress elasticity matrix
                    D = (E/(1-nu*nu))*[1 nu 0; nu 1 0; 0 0 (1-nu)/2];
                case 2 % Plane strain elasticity matrix
                    D = (E/((1+nu)*(1-2*nu)))*[(1-nu) nu 0; nu (1-nu) 0; 0 0 (1-2*nu)/2];
            end
            
            % Gaussian points (2 x 2)
            W = (1/3)*[sqrt(3) sqrt(3); sqrt(3) -sqrt(3); -sqrt(3) sqrt(3); -sqrt(3) -sqrt(3)];

            % Integral
            for j = 1:4
                N = W(j,1);  % Eta
                Q = W(j,2);  % Xi

                % Shape functions derivatives with respect to Xi(Q)
                N1r = (1/4)*(N-1);
                N2r = (1/4)*(1-N);
                N3r = (1/4)*(1+N);
                N4r = -(1/4)*(1+N);

                % Shape functions derivatives with respect to Eta(N)
                N1s = (1/4)*(Q-1);
                N2s = -(1/4)*(1+Q);
                N3s = (1/4)*(1+Q);
                N4s = (1/4)*(1-Q);

                % Coeficientes da Matriz Jacobiana
                J11 = N1r*coord(inci(el,2),1)+N2r*coord(inci(el,3),1)+N3r*coord(inci(el,4),1)+N4r*coord(inci(el,5),1);
                J12 = N1r*coord(inci(el,2),2)+N2r*coord(inci(el,3),2)+N3r*coord(inci(el,4),2)+N4r*coord(inci(el,5),2);
                J21 = N1s*coord(inci(el,2),1)+N2s*coord(inci(el,3),1)+N3s*coord(inci(el,4),1)+N4s*coord(inci(el,5),1);
                J22 = N1s*coord(inci(el,2),2)+N2s*coord(inci(el,3),2)+N3s*coord(inci(el,4),2)+N4s*coord(inci(el,5),2);

                % Jacobian matrix
                J = [J11 J12; J21 J22];

                % Auxiliary matrix [G]
                jacob = det(J);
                % solid
                G = (1/jacob)*[J22 0 -J12 0; 0 -J21 0 J11; -J21 J22 J11 -J12];

                % Auxiliary matrix [P]
                % Solid
                P = [N1r 0 N2r 0 N3r 0 N4r 0; 0 N1r 0 N2r 0 N3r 0 N4r; N1s 0 N2s 0 N3s 0 N4s 0; 0 N1s 0 N2s 0 N3s 0 N4s];

                % Matrix [B] with the derivatives of shape functions
                B = G*P;

                % Stiffness matrix integral
                Ke = Ke + self.th*B'*D*B*jacob;

            end
            
        end % end ComputeStructuralKe
        
        %% Assemble stiffness matrix
        function self = AssembleStructuralK(self,densities,stress_case)
            
            clear K
            
            % Compute element stiffness matrix
            Ke = ComputeStructuralKe(self,stress_case);
            
            % Number of elements
            nel = size(self.mesh.incidence,1);
            
            % Assemble global stiffness matrix
            % Solid elements DOF's
            loc = [self.ID(1,self.mesh.incidence([1:nel]',2));
                   self.ID(2,self.mesh.incidence([1:nel]',2));
                   self.ID(1,self.mesh.incidence([1:nel]',3));
                   self.ID(2,self.mesh.incidence([1:nel]',3));
                   self.ID(1,self.mesh.incidence([1:nel]',4));
                   self.ID(2,self.mesh.incidence([1:nel]',4));
                   self.ID(1,self.mesh.incidence([1:nel]',5));
                   self.ID(2,self.mesh.incidence([1:nel]',5))];

            % Index vectors
            I = reshape(repmat(loc,8,1),nel*64,1);
            J = kron(loc(:),ones(8,1));
            Kg = repmat(Ke(:),nel,1);

            % Mulyiplying physical densities
            for i = 1:nel
                Kg([1:64]+64*(i-1),1) = Kg([1:64]+64*(i-1),1)*densities(i);
            end

            % Assembly
            self.K = sparse(I,J,Kg);
            
        end % end AssembleStructuralK
        
        %% Solve static case of FEA (Ku = F)
        function self = SolveStaticStructuralFEA(self)
            
            % Identifying active DOF's
            dofn = [1:self.number_of_equations]';
            % Blocking DOF's with dirichlet boundary conditions = 0
            for i = 1:size(self.mesh.dirichlet_boundary,1)
                if (self.mesh.dirichlet_boundary(i,1) ~= 0) && (self.mesh.dirichlet_boundary(i,3) == 0)
                    dofn(self.ID(self.mesh.dirichlet_boundary(i,2),self.mesh.dirichlet_boundary(i,1))) = 0;
                end
            end
            % Active DOF's
            dofa = nonzeros(dofn);
            
            % Solving for displacements
            self.U = sparse(zeros(10,1));
            self.U(self.number_of_equations) = 0;

            % Matlab linear solver
            self.U(dofa) = self.K(dofa,dofa)\self.F(dofa);
            
        end % end SolveStaticFEA
        
    end % end methods
end