function FEMatricesOut = assembleFEMatricesInternal(self,BCEnforcementOption,gmatrices,state)
% assembleFEMatricesInternal - Internal function to assemble the intermediate FE matrices

% Copyright 2015-2023 The MathWorks, Inc.

narginchk(4, 4);
[p,~,t] = self.Mesh.meshToPet();

if isa(self,'pde.StructuralModel')
    performSolverPrecheck(self, false);
    [coefstruct,~,coefstructForThermalStress,hystereticDamping] = self.packCoefficients;
    if ~isprop(self,'PackedDomainData')
        dprop = self.addprop('PackedDomainData');
        dprop.Transient = true;
        dprop.Hidden = true;
    end
    clearDProp = onCleanup(@() delete(dprop));
    self.PackedDomainData = coefstructForThermalStress;
    dDefinedFlag = self.DCoefficientFlag;
elseif self.ContainsFemodel
    [coefstruct,~,self.PackedDomainData,hystereticDamping] = packCoefficients(self.ParentModel);
    self.PackedBCs = packBC(self.ParentModel,self.PackedDomainData);
    dDefinedFlag = contains(self.ParentModel.AnalysisType,'thermal');
    if any(cellfun(@isempty,coefstruct.Coefficients.m))
        coefstruct.Coefficients.m = repmat({0},1,numel(coefstruct.Coefficients.m));
    end
else % PDEModel or pde.ThermalModel
    performSolverPrecheck(self, false);
    coefstruct = self.packCoefficients();
    dDefinedFlag = self.DCoefficientFlag;
end

Np = size(p,2);
N = self.PDESystemSize;
selectedmatrix = gmatrices;
if isempty(gmatrices)
    selectedmatrix = 'AFGHKMQR';
end

%Perform a sample assembly to determine if either 'u' or 'time' is needed to
%complete the requested assembly. Note that only the PDE coefficients that
%affect the requested matrices are checked. However, the Dirichlet BC pair
%(H, R) and Neumann BC pair (Q, G) are assembled even if one of the entries
%in the pair is requested, the output is filtered to contain only the
%matrix that was requested.
%
coefstructTemp = coefstruct;
coefstructTemp.NumElementsInSubdomain = ones(size(coefstruct.NumElementsInSubdomain));
% Check for u dependency in the model for selected matrices.
u = nan(N*Np,1);
timeORfreq = 0;
femodel = self.assembleSelectedFEMatrices(p, t, coefstructTemp, u, timeORfreq, selectedmatrix, dDefinedFlag);
needU = checkForNaNOutput(femodel);
if needU && (isempty(state) || ~isfield(state,'u'))
    error(message('pde:assembleFEMatrices:needUforNonlinearModel'));
end
% Check for time dependency in the model for selected matrices.
u = zeros(N*Np,1);
timeORfreq = nan;
femodel = self.assembleSelectedFEMatrices(p, t, coefstructTemp, u, timeORfreq, selectedmatrix,dDefinedFlag);
needTime = checkForNaNOutput(femodel);

requiredStateField = "time";
if (isprop(self,'AnalysisType') && contains(self.AnalysisType,"frequency",IgnoreCase=true)) || ...
    (self.ContainsFemodel && contains(self.ParentModel.AnalysisType,"frequency",IgnoreCase=true) )
    requiredStateField = "frequency";
end

if needTime && (isempty(state) || ~isfield(state,requiredStateField))
    if isequal(requiredStateField,"time")
        error(message('pde:assembleFEMatrices:needTforTransientModel'))
    else
        error(message('pde:assembleFEMatrices:needFforFrequencyModel'))
    end

end

% Perform the full assembly after validating data provided in the state struct.
u = [];
timeORfreq = [];
if isstruct(state)
    [u,timeORfreq] = validataInputStruct(state,N,Np,needU,needTime,requiredStateField);
end
femodel = self.assembleSelectedFEMatrices(p, t, coefstruct, u, timeORfreq, selectedmatrix,dDefinedFlag);
if contains(selectedmatrix,'K') && ...
    ((isa(self,'pde.StructuralModel') && contains(self.AnalysisType,"frequency","IgnoreCase",true)) || ...
     (self.ContainsFemodel && contains(self.ParentModel.AnalysisType,"frequency","IgnoreCase",true)) )
    % Compute damping matrices.
    massMat = self.assembleSelectedFEMatrices(p, t, coefstruct, u, timeORfreq, 'M',dDefinedFlag);
    femodelFR.K = femodel.K;
    femodelFR.Mass = massMat.M;
    femodelFR.p=p;
    femodelFR.t=t;
    [~, Khd] = evaluateDampingMatrix(self,femodelFR,coefstruct,hystereticDamping);
    femodel.K = femodel.K + 1i*Khd;
end


if ~isempty(gmatrices)
    for term = gmatrices
        FEMatrices.(term) = femodel.(term);
    end
else
    FEMatrices.K = femodel.K;
    FEMatrices.A = femodel.A;
    FEMatrices.F = femodel.F;
    FEMatrices.Q = femodel.Q;
    FEMatrices.G = femodel.G;
    FEMatrices.H = femodel.H;
    FEMatrices.R = femodel.R;
    FEMatrices.M = femodel.M;
end

if startsWith("nullspace",BCEnforcementOption,'IgnoreCase',true)
    [FEMatricesOut.Kc,FEMatricesOut.Fc,FEMatricesOut.B,FEMatricesOut.ud, FEMatricesOut.M] ...
        = applyBCNullSpace(FEMatrices);
elseif startsWith("stiff-spring",BCEnforcementOption,'IgnoreCase',true)
    [FEMatricesOut.Ks,FEMatricesOut.Fs,FEMatricesOut.M] = applyBCStiffSpring(FEMatrices,p);
else
    FEMatricesOut = FEMatrices;
end
end

function [Kc, Fc, null, ud, M] = applyBCNullSpace(FEMatrices)
K = FEMatrices.K;
A = FEMatrices.A;
F = FEMatrices.F;
Q = FEMatrices.Q;
G = FEMatrices.G;
H = FEMatrices.H;
R = FEMatrices.R;
M = FEMatrices.M;

[null,orth]=pdenullorth(H);
if size(orth,2)==0
    ud=zeros(size(K,2),1);
else
    ud=full(orth*((H*orth)\R));
end
Kc=K+A+Q;
Fc=null'*((F+G)-Kc*ud);
Kc=null'*Kc*null;
Kc = pde.PDEModel.checkForSymmetry(Kc);
M = null'*M*null;
M = pde.PDEModel.checkForSymmetry(M);
end

function [Ks, Fs, M] = applyBCStiffSpring(FEMatrices,p)

K = FEMatrices.K;
A = FEMatrices.A;
F = FEMatrices.F;
Q = FEMatrices.Q;
G = FEMatrices.G;
H = FEMatrices.H;
R = FEMatrices.R;
M = FEMatrices.M;

if nnz(H)==0 % No constraints
    K=K+A+Q;
    Ks = pde.PDEModel.checkForSymmetry(K);
    Fs=F+G;
    return
end

% Scaling
d1=full(diag(H*H'));
l=find(d1==0);
d1(l)=Inf*ones(size(l));
id1=diag(sparse(1./d1));
% Form normal equations
R=H'*id1*R;
H=H'*id1*H;

% Incorporate constraints (Dirichlet conditions)
% Find a positive spring constant of suitable size.

np=size(p,2);
tscale=1/sqrt(np);
kscale=max(max(abs(K)));
mscale=max(max(abs(A)));
qscale=max(max(abs(Q)));
hscale=1;

% Spring constant
spc=max(max(kscale*tscale^2,mscale),qscale*tscale)/(hscale*tscale^3);

K=K+A+Q+spc*H;
Ks =pde.PDEModel.checkForSymmetry(K);
Fs= F+G+spc*R;
M = M +spc*H;
M =pde.PDEModel.checkForSymmetry(M);

end

function [u,timeOrFreq] = validataInputStruct(inStruct,N,Np,needU,needTime,requiredStateField)
u = [];
timeOrFreq = [];
inStructFields = lower(fields(inStruct));

if any(contains(inStructFields,'u')) && needU
    u = inStruct.u;
    if ( ~isnumeric(u) || issparse(u) || isscalar(u) ...
            || any(isnan(u(:))) || any(~isfinite(u(:))))
        error(message('pde:assembleFEMatrices:invalidSolution'));
    end
    
    if ~isequal(size(u),[N,Np])
        error(message('pde:assembleFEMatrices:invalidSolutionSize',N,Np));
    end
end

if any(contains(inStructFields,requiredStateField)) && needTime
    timeOrFreq = inStruct.(requiredStateField);
    if ( ~isnumeric(timeOrFreq) || ~isscalar(timeOrFreq) || ~isreal(timeOrFreq))
        error(message('pde:assembleFEMatrices:invalidTimeOrFreq'));
    end
end
end

function nanoutput = checkForNaNOutput(femodel)
matrixIDs = fields(femodel);
matrixNaNs = false(numel(matrixIDs),1);
for n = 1:numel(matrixIDs)
    matrixNaNs(n) = any(isnan(femodel.(matrixIDs{n})),'all');
end
nanoutput = any(matrixNaNs);
end