function [u,dudt,dudt2] = solveModalTimeDependent(self, coefstruct, u0, ut0, tlist,Rm)
% solveModalTimeDependent - Internal function for solving a time-dependent
% structural model using modal basis of the structure.
%

% Copyright 2018-2022 The MathWorks, Inc.

%Validate modal results input.
if (isempty(Rm.NaturalFrequencies) || ...
        isempty(Rm.ModeShapes) || ....
        isempty(Rm.Mesh) )
    error(message('pde:StructuralModel:emptyModalBasis'));
end

if isempty(Rm.ModalConstraintsMap)
    error(message('pde:StructuralModel:modalResultsCreatedPriorTo19a'));
end

if ~isequal(Rm.Mesh.Nodes, self.Mesh.Nodes)
    error(message('pde:StructuralModel:inconsistentMeshModalAndTransient'));
end

Np = size(Rm.ModeShapes.ux,2);

p = self.addprop('PackedBCs');
p.Transient = true;
p.Hidden = true;

dbc = self.addprop('DiscretizedBCs');
dbc.Transient = true;
dbc.Hidden = true;

clearN = onCleanup(@() temporalSolverCleanup(p,dbc));

global femodel

rtol =  self.SolverOptions.RelativeTolerance;
atol =  self.SolverOptions.AbsoluteTolerance;

stats = self.SolverOptions.ReportStatistics;
[p,e,t] = self.Mesh.meshToPet();

% Discretize the model.
femodel=pde.DynamicDiscretizedPDEModel(self,p,e,t,coefstruct,u0,tlist,true);

%Verify that constraints remain unchanged between modal analysis and
%transient simulation. 
if ~isequal(femodel.B,Rm.ModalConstraintsMap)
    error(message('pde:StructuralModel:inconsistentConstraintsModalAndTransient'));
end

% Construct modal matrix.
Phi = [Rm.ModeShapes.ux(:,1:Np); Rm.ModeShapes.uy(:,1:Np)];
if ~Rm.IsTwoD
    Phi = [Phi; Rm.ModeShapes.uz(:,1:Np)];
end

% Impose constraints on the modes.
Phi = femodel.B'*Phi;
femodel.Phi = Phi;
femodel.numConstrainedEqns = Np;

FF = femodel.F+femodel.G;
FFr = Phi.'*femodel.B'*FF;

KKr = diag(Rm.NaturalFrequencies(1:Np).^2);
MMr = eye(Np);

% Account for damping, if specified.
femodel.Cr = sparse(Np,Np);
if ~isempty(self.DampingModels)
    hystereticDamping = [];
    C =  evaluateDampingMatrix(self,femodel,coefstruct,hystereticDamping,Rm.NaturalFrequencies(1:Np));
    if contains("proportional",coefstruct.Coefficients.d.dampingModel) 
        C = Phi'*femodel.B'*C*femodel.B*Phi;
    end
    femodel.Cr = C;
end

% Form first order system of ODE integration.
femodel.Fr=[zeros(Np,1);FFr];
femodel.Kr=[sparse(Np,Np) -speye(Np,Np); KKr sparse(Np,Np)];
Mr = [speye(Np,Np), sparse(Np,Np); femodel.Cr, sparse(MMr)];

% Jacobian and mass matrix for ODE system.
dfcn = -femodel.Kr;
mfcn = Mr;

% Setup ODE system
[u0, ut0] = imposeBConICsecondOrderODE(self, u0,ut0,tlist);
uu0=[u0;ut0];
odeoptions = constructODEoptions(uu0,rtol,atol,stats,Np,dfcn,mfcn,true);

sol=ode15s(@modalODEf,tlist,uu0,odeoptions);

if(sol.x(end) < tlist(end))
    error(message('pde:pdetool:incompSoln'))
end

[y,yp] = deval(sol,tlist);

% Expansion phase
DoFIndex=1:Np; % vector of 1 to number of degrees of freedom

u  = y(DoFIndex,:);
dudt = zeros(size(u));
dudt2 = zeros(size(u));

velIndex=Np+1:size(y,1);
dudt(DoFIndex,:) = y(velIndex,:);
dudt2(DoFIndex,:) = yp(velIndex,:);

% If Dirichlet constraints are functions of time or u, recovery of the
% full solution requires reevaluating H and R matrices. TODO - see if this reevaluated of BC matrices can be avoided.
if femodel.vr
    u = Phi*u;
    dudt = Phi*dudt;
    dudt2 = Phi*dudt2;
    [u, dudt, dudt2] = recoverFullSolutionLinear(self,u,dudt,dudt2,tlist,femodel.tspan,femodel.H);
else
    u =femodel.B*Phi*u;
    dudt = femodel.B*Phi*dudt;
    dudt2 = femodel.B*Phi*dudt2;
end

end % End of solverModalTimeDependent


function temporalSolverCleanup(p,dbc)
% Clear the packed BCs for the current invocation of solver.
delete(p);
delete(dbc);
%Clear global data.
clear('global', 'femodel')
end


function [u0, ut0] = imposeBConICsecondOrderODE(thePde, u0,ut0,tlist)
global femodel
if ~(femodel.vh || femodel.vr)
    % No Dirichlet BC that varies with time.
    u0 = femodel.Phi.'*femodel.B'*femodel.Mass*u0;
    ut0 = femodel.Phi.'*femodel.B'*femodel.Mass*ut0;
else
    % Match ICs to BC.
    t0=tlist(1);
    t1=t0+femodel.tspan*sqrt(eps);
    dt=t1-t0;
    [~,~,H,R]=thePde.assembleBoundary(u0,t0);
    [N,O]=pdenullorth(H);
    ud=O*((H*O)\R);
    [~,~,H,R]=thePde.assembleBoundary(u0,t1);
    [N1,O]=pdenullorth(H);
    ud1=O*((H*O)\R);
    u0=N'*u0;
    ut0=N'*(ut0-((N1-N)*u0-(ud1-ud))/dt);
    
    u0 = femodel.Phi.'*femodel.B'*femodel.Mass*femodel.B*u0;
    ut0 = femodel.Phi.'*femodel.B'*femodel.Mass*femodel.B*ut0;
    
end
end


function odeoptions = constructODEoptions(~,rtol,atol,stats,Np,dfcn,mfcn,tsecondOrder)
global femodel

odeoptions = odeset;
odeoptions=odeset(odeoptions, 'Jacobian',dfcn);
if(~(femodel.vc || femodel.va || femodel.vq || femodel.vh || femodel.vr))
    odeoptions=odeset(odeoptions,'JConstant','on');
end

odeoptions=odeset(odeoptions,'Mass',mfcn);
odeoptions=odeset(odeoptions,'MassSingular',false);
odeoptions=odeset(odeoptions,'RelTol',rtol);

if (tsecondOrder)
    % Mask out dudt
    atol=atol*ones(2*Np,1);
    atol(Np+1:Np+Np)=Inf*ones(Np,1);
    odeoptions=odeset(odeoptions,'MaxOrder',2);
end

odeoptions=odeset(odeoptions,'AbsTol',atol);
odeoptions=odeset(odeoptions,'Stats',stats);

end



function [uf, dudtf, dudt2f] = recoverFullSolutionLinear(thePde,ur,durdt,dur2dt2,t,tspan,H)
%This function is used to transform the reduced solution with constrained DOFs
% removed to the full solution with all DOFs at all FE nodes for linear
% system.

dt=tspan*eps^(1/3);
[null,orth]=pdenullorth(H);

% Dirichlet values and its derivatives.
ud = zeros(size(H,2),numel(t));
ud1 = zeros(size(H,2),numel(t));
udm1 = zeros(size(H,2),numel(t));

for n = 1:numel(t)
    t0 = t(n);
    t1 = t0+dt;
    tm1 = t0-dt;
    ufn=null*ur(:,n) + ud(:,n);
    
    [~,~,H,R]=thePde.assembleBoundary(ufn,t0);
    ud(:,n)=full(orth*((H*orth)\R));
    
    [~,~,H,R]=thePde.assembleBoundary(ufn,t1);
    ud1(:,n) = full(orth*((H*orth)\R));
    
    [~,~,H,R]=thePde.assembleBoundary(ufn,tm1);
    udm1(:,n) = full(orth*((H*orth)\R));
end

udDot = (ud1 - udm1)./(2*dt);
udDDot = (ud1 -2*ud+ udm1)./dt.^2;

uf = null*ur + ud;
dudtf = null*durdt + udDot;
dudt2f = null*dur2dt2 + udDDot;
end



function f=modalODEf(t,u)
%modalODEf - Residue function for second-order ODE system
%Computes residue of discretized PDE with second-order time derivative.

global femodel

if ~(femodel.vq || femodel.vg || femodel.vh || femodel.vr || ...
        femodel.vc || femodel.va || femodel.vf)
    
    f=-femodel.Kr*u+femodel.Fr;
    return
end

nu=femodel.numConstrainedEqns;
uFull = femodel.B*femodel.Phi*u(1:nu);

if femodel.vq || femodel.vg || femodel.vh || femodel.vr
    [femodel.Q,femodel.G,femodel.H,femodel.R]=femodel.thePde.assembleBoundary(uFull,t);
end

if ~(femodel.vh || femodel.vr)
    K = femodel.Kr;
    FF=femodel.F +femodel.G;
    FFr = femodel.Phi.'*femodel.B'*FF;
    F=[zeros(nu,1); FFr];
else
    dt=femodel.tspan*eps^(1/3);
    t1=t+dt;
    dt=t1-t;
    tm1=t-dt;
    
    FF = femodel.F+femodel.G;
    HH=femodel.H*femodel.Or;
    ud=femodel.Or*(HH\femodel.R);
    [~,~,~,R]=femodel.thePde.assembleBoundary(uFull,t1);
    ud1=femodel.Or*(HH\R);
    [~,~,~,R]=femodel.thePde.assembleBoundary(uFull,tm1);
    udm1=femodel.Or*(HH\R);
    udDot = (ud1 - udm1)/(2*dt);
    
    K = femodel.Kr;

    dampForce = zeros(size(femodel.Cr,1),1);
    if(any(femodel.Cr))
        dampForce = femodel.Cr*(udDot.'*femodel.Mass.'*femodel.B*femodel.Phi).';
    end
    udDotDot = (ud1-2*ud+udm1)/(dt^2);
    
    FFfull1 = full(FF- femodel.K*ud - femodel.Mass*udDotDot) ;
    FFr = femodel.Phi'*femodel.B'*FFfull1 - dampForce;
    F=[zeros(nu,1); FFr];
end

f=-K*u+F;

end

