function output=linearize (self,inputOptions)
%LINEARIZE  Linearize structural model.
%
%   LINEARIZE extracts a sparse linear model of a structural model. 
%   For transient structural models, LINEARIZE uses time t=0. Use 
%   LINEARIZEINPUT to specify the inputs of the linear model that 
%   correspond to external forcing, such as loads. Use LINEARIZEOUTPUT 
%   to specify the outputs of the linear model that correspond to
%   degree-of-freedom (DoF) values in the selected regions of the 
%   geometry.
%
%   SYS = LINEARIZE(structuralmodel) returns the linearized model SYS 
%   in MECHSS form. This syntax requires Control System Toolbox. Use 
%   SYS.InputName and SYS.OutputGroup to locate the various inputs and 
%   output groups.
%
%   MX = LINEARIZE(structuralmodel,'OutputType','matrices') returns the
%   corresponding A,B,C,D,E or M,K,B,F matrices in a structure array MX.
%
%   See also pde.StructuralModel/linearizeInput, pde.StructuralModel/linearizeOutput

%    Copyright 2021-2022 The MathWorks, Inc.

arguments
    self pde.StructuralModel
    inputOptions.OutputType {mustBeTextScalar}
end

state.time=0;
data = assembleFEMatrices(self,state);
M=data.M;
K=data.K;
H=data.H;
R=data.R;
Fnbc=data.G;

% Get M and K
if ~isequal(nnz(H),0)
    M = blkdiag(M,zeros(size(R,1)));
    K = [K H';H zeros(size(R,1))];
end

%Inputs B: loads and BCs
[B,InputName,InputGroup]=getB(self,K,Fnbc,H);

% Outputs C: regions of interest
[F,OutputName,OutputGroup]=getF(self,K);

if isfield(inputOptions,'OutputType')
    if ~startsWith('matrices',inputOptions.OutputType,'IgnoreCase',true)
        error(message('pde:linearize:invalidVal',inputOptions.OutputType));
    end
    %formatted matrices as linearize output
    output.M=M;
    output.K=K;
    output.B=B;
    output.F=F;
    return
end
output = mechss(M,[],K,B,F,...
    'InputName',InputName, 'InputGroup', InputGroup,...
    'OutputName',OutputName, 'OutputGroup', OutputGroup);
end

function [bCstb,InputName,InputGroup]=getB(userModel,K,Fnbc,H)
% Get input-to-DoF matrix, input names, and input groups
nBCRows=numel(Fnbc);   % Rows for Neuman BC entries in B
aRows=size(K,1);       % Num of rows in matrix A
[gnodes,nodesH]=find(H);
N = size(userModel.Mesh.Nodes,2);  % Num of DoFs
if userModel.IsTwoD
   ND = 2;
else
   ND = 3;
end
inputs=userModel.LinearizeInputs;
numInputs=numel(inputs);
bCstb = sparse(aRows,numInputs*ND);%Allocate enough space first, then remove empty ones before exiting
InputName=strings(numInputs*ND,1);
InputGroup=struct;
axis=["x" "y" "z"];

bcl=0; %Used to track which column of B an assignment should happen
state.time=0;
for i=1:numInputs
  try
      % Structural BC
      actBC=findStructuralBC(userModel.BoundaryConditions,'Label',inputs(i).Label);

      % Consolidate Dirichlet constraints
      if ~isempty(actBC.Displacement) || strcmp(actBC.Constraint,'fixed')
         XYZConstraint = [true(1,ND) false(1,3-ND)];
      else
         XYZConstraint = ~[isempty(actBC.XDisplacement) ...
            isempty(actBC.YDisplacement) isempty(actBC.ZDisplacement)];
      end

      if any(XYZConstraint)
         nodes=findNodes(userModel.Mesh,'region',inputs(i).RegionType,inputs(i).RegionID);
         for j=1:3
            if XYZConstraint(j)
               bcl=bcl+1;
               [~,idx]=intersect(nodesH,nodes+(j-1)*N);
               bCstb(nBCRows+gnodes(idx),:)=0;
               bCstb(nBCRows+gnodes(idx),bcl)=1;
               InputName(bcl,:)=actBC.Label+"_"+axis(j);
            end
         end

      elseif ~isempty(actBC.Pressure) && isempty(actBC.SurfaceTraction)
         % 'Pressure' is scalar
         bcl=bcl+1;
         model=createModelWithSingleLoad(userModel,actBC,'Pressure');
         DATA = assembleFEMatrices(model,'G',state);
         Bcol = DATA.G;
         bCstb(1:nBCRows,bcl)=Bcol;
         InputName(bcl,:)=actBC.Label;

      elseif ~isempty(actBC.Force) || ~isempty(actBC.SurfaceTraction)
         %Force/SurfaceTraction is a vector and they can't be applied
         %together because they are for different region types
         %If both Pressure and SurfaceTraction are specified together,
         %processing SurfaceTraction only is enough
         p=getParameter(actBC);
         model=createModelWithSingleLoad(userModel,actBC,p);
         DATA = assembleFEMatrices(model,'G',state);
         Bcol = DATA.G;
         for j=1:ND
            Btemp=sparse(aRows,1);
            bcl=bcl+1;
            Btemp((j-1)*N+1:j*N) = Bcol((j-1)*N+1:j*N);
            bCstb(:,bcl)=Btemp;
            InputName(bcl,:)=actBC.Label+"_"+axis(j);
         end
      end   

   catch
      % Body load
      actBC=findBodyLoad(userModel.BodyLoads,'Label',inputs(i).Label);
      if isprop(actBC,'GravitationalAcceleration')
         model=createModelWithSingleLoad(userModel,actBC,'GravitationalAcceleration');
         DATA = assembleFEMatrices(model,'F',state);
         Bcol = DATA.F;
         for j=1:ND
            Btemp=sparse(aRows,1);
            bcl=bcl+1;
            Btemp((j-1)*N+1:j*N)=Bcol((j-1)*N+1:j*N);
            bCstb(:,bcl)=Btemp;
            InputName(bcl,:)=actBC.Label+"_"+axis(j);
         end
      end
   end
end
InputName = InputName(1:bcl,:);
bCstb = bCstb( :,1:bcl);
end

function [fCstb,OutputName,OutputGroup]=getF(userModel,K)
numOutputs=numel(userModel.LinearizeOutputs);
f = strings(3*numOutputs,1);
c = cell(3*numOutputs,1);
ny = 0; % number outputs, row size of F
ng = 0;
OutputName = strings(0,1);
axis=["x" "y" "z"];

N = size(userModel.Mesh.Nodes,2);
iy = [];
for i=1:numOutputs
    regionType=userModel.LinearizeOutputs(i).RegionType;
    regionID=userModel.LinearizeOutputs(i).RegionID;
    
    try
        nodes = findNodes(userModel.Mesh,'region',regionType,regionID);
    catch
        error(message('pde:linearize:outdatedMesh',regionID));
    end
    
    nn = numel(nodes);
    componentAxis=getComponentAxis(userModel,i);
    for j=1:numel(componentAxis)
       if componentAxis(j)
          iy = [iy , nodes+(j-1)*N];
          yName = string(regionType) + regionID + "_" + axis(j);
          if nn==1
             % Scalar output: log in output name
             OutputName = [OutputName ; yName];
          else
             % Vector output: log in output group
             ng = ng+1;
             f(ng) = yName;
             c{ng} = ny+1:ny+nn;
             OutputName = [OutputName ; strings(nn,1)];
          end
          ny = ny+nn;
       end
    end
end
fCstb = sparse(1:ny,iy,ones(1,ny),ny,size(K,1));
OutputGroup = cell2struct(c(1:ng,:),f(1:ng,:),1);
end


function pdem=createModelWithSingleLoad(userModel,bc,parameter)
pdem = createpde('structural',userModel.AnalysisType);
pdem.Geometry=userModel.Geometry;
pdem.Mesh=userModel.Mesh;
pdem.MaterialProperties=userModel.MaterialProperties;

%Apply unit load
if strcmp(parameter,'GravitationalAcceleration')
    structuralBodyLoad(pdem,'GravitationalAcceleration',ones(numel(bc.(parameter)),1));%,bc.RegionType,bc.RegionID);
else
    structuralBoundaryLoad(pdem,bc.RegionType,bc.RegionID,parameter,ones(numel(bc.(parameter)),1));
end
end

function p=getParameter(bc)
if ~isempty(bc.Force)
    p='Force';
elseif ~isempty(bc.SurfaceTraction)
    p='SurfaceTraction';
end
end

function componentAxis=getComponentAxis(userModel,i)
Component=userModel.LinearizeOutputs(i).Component;
if userModel.IsTwoD
    xyz=[contains(Component,'x') contains(Component,'y')];
else
    xyz=[contains(Component,'x') contains(Component,'y') contains(Component,'z')];
end
componentAxis=xyz;
end
