function u = solveStationaryNonlinearS2S(self, coefstruct,u0)
% solveStationaryNonlinearS2S - Internal function for solving surface-to-surface thermal radiation problem
%
% Similar to usual solveStationaryNonlinear, but with updates specific to
% the radiosity based surface-to-surface thermal radiation problem.


% Copyright 2023 The MathWorks, Inc.

[p,~,t] = self.Mesh.meshToPet();
N = self.PDESystemSize;
np = size(p,2);
getResidual=@(u)compResidual(u,self,p,t,coefstruct,np*N);
exactjac_fh=@(u)getJacobian(u,self,p,t,coefstruct,np*N);
% Initial residual and update u0 and q0 radiosity with Lagrange multiplier dofs
s2sData = self.PackedS2SRadiationData;
[~,~,~,~,Jvec0] = pde.internal.FESolverUtilities.computeRadiosity(self.ParentModel.ThermalRadiation,s2sData.FacetEmissivity,s2sData.StefanBoltzmann,s2sData.AmbientTemperature,s2sData.AmbientFactor,u0);
u0 = [u0; Jvec0];
[r,pdenonK,pdenonF,u]=getResidual(u0);

[warningMsg,warningID] = lastwarn;
cleanupLastwarn = {@lastwarn,warningMsg,warningID};
originalStateSingularity(1) = warning('off','MATLAB:nearlySingularMatrix');
originalStateSingularity(2) = warning('off','MATLAB:singularMatrix');
cleanupOriginalStateSingularity = {@warning,originalStateSingularity};
oC = onCleanup(@() cellfun(@(f)feval(f{:}),{cleanupOriginalStateSingularity,cleanupLastwarn}));

if all(u0==0)
    % if the initial guess is all zeros, use the stiffness
    % matrix as the initial Jacobian to improve the guess
    u=pdenonK\pdenonF;
    [r,pdenonK]=getResidual(u);
end

if ~all(all(isfinite(u)))
    error(message('pde:pdenonlin:InvalidInitGuess'))
end

Knorm=pdenonK;
nrm=resNorm(r,self,Knorm);
nr2=nrm^2;

if strcmp(self.SolverOptions.ReportStatistics,'on')
    disp('Iteration     Residual     Step size  Jacobian: Full')
    fprintf('%4i%20.4e\n',0,nrm);
end

iter=0;
alfaPrev = 1;
while (nrm > self.SolverOptions.ResidualTolerance)  % Gauss-Newton iteration loop

    iter=iter+1;
    if iter>self.SolverOptions.MaxIterations
        error(message('pde:pdenonlin:TooManyIter'));
    end

    J = exactjac_fh(u);
 
    % Any near singular warning before this point is ignored, potentially
    % from getResidual. It is unlikely that getResidual will warn while
    % solving for Dirichlet DoF, since H is constant in thermal analysis.
    % Even if such warning is thrown, it would also result in
    % singular Jacobian, which is caught next.

    lastwarn('')

    cor=-J\r;
    [~,wid] = lastwarn;
    if (strcmp(wid,'MATLAB:nearlySingularMatrix')) || ...
                (strcmp(wid,'MATLAB:singularMatrix'))
        error(message('pde:pdenonlin:SingularJacobian'))
    end

    % Bisect until squared residual norms decrease sufficiently
    alfa=1;
    if (alfaPrev < 0.5)
        alfa = 2*alfaPrev;
    end
    notdone=1;
    while(notdone)
        if alfa<self.SolverOptions.MinStep, error(message('pde:pdenonlin:SmallStepSize')), end
        tmp=u+alfa*cor;
        [rr,pdenonK]=getResidual(tmp);
        nrm=resNorm(rr,self,Knorm); % Keep energy norm K constant during loop
        nrr=nrm^2;

        notdone=(nr2-nrr < alfa/2*nr2);
        if notdone
            alfa=alfa/2;
        end
    end
    u=tmp;
    r=rr;
    nr2=nrr;
    Knorm=pdenonK;
    alfaPrev = alfa;
    if strcmp(self.SolverOptions.ReportStatistics,'on')
        fprintf('%4i%20.4e%12.7f\n',iter,nrm,alfa);
    end
end % END  - Gauss-Newton iteration loop

u=full(u(1:np*N));
end

function J=getJacobian(u, pdenonTempPde, pdenonp, pdenont, pdenoncoef, pdenonn)
u=full(u);
[K, M, ~, Q, ~, H, ~, A, D, B] = assemblePdeMatrices(pdenonTempPde, pdenonp, pdenont, pdenoncoef, u(1:pdenonn));
Kcond = K+M+Q;
zHB = zeros(size(H,1),size(B,2));
zDH = zeros(size(D,1),size(H,1));
if any(any(H))
    J = [Kcond, H' B; H, zeros(size(H,1)), zHB;-4*D,zDH, A];
else
    J = [Kcond, B; -4*D, A];
end

end

function [r, pdenonK, pdenonF, u]=compResidual(u, pdenonTempPde, pdenonp, pdenont, pdenoncoef, pdenonn)
%compResidual Residual for nonlinear solver

u=full(u);
[K, M, F, Q, G, H, R, A, D, B, Qamb] = assemblePdeMatrices(pdenonTempPde, pdenonp, pdenont, pdenoncoef, u(1:pdenonn));

% Conduction
Kcond = K+M+Q;
Fcond = F+G;
zeroPad = zeros(size(B,2),1);

if any(any(H))
    if (size(u,1)-size(B,2))<(size(Kcond,2)+size(H,1))
        uCond= u(1:pdenonn);
        qRad = u(pdenonn+1:pdenonn+size(B,2));
        u=[uCond;H'\(Fcond-Kcond*u(1:pdenonn));qRad];
    end
    Kcond=[Kcond H';H zeros(size(H,1))];
    Fcond=[Fcond;R];
    zHB = zeros(size(H,1),size(B,2));
    zDH = zeros(size(D,1),size(H,1));
    Ks2s = [Kcond, [B; zHB] ; -D, zDH, A];
    Fs2s = [Fcond; Qamb];
else
    Ks2s = [Kcond, B;-D, A];
    Fs2s = [Fcond; Qamb];
end

pdenonK=Ks2s;
pdenonF=Fs2s;
r=pdenonK*u-pdenonF;
end


function nrm = resNorm(r, self, K)
if ischar(self.SolverOptions.ResidualNorm) && ...% energy norm
        strcmp(self.SolverOptions.ResidualNorm, 'energy')
    nrm=sqrt(abs(r'*K*r));
else
    nrm=norm(r,self.SolverOptions.ResidualNorm)/length(r)^(1/self.SolverOptions.ResidualNorm);
end
end


function [K, M, F, Q, G, H, R, A, D, B,Qamb] = assemblePdeMatrices(thePde, p, t, coefstruct, u)
femat =  thePde.assembleSelectedFEMatrices(p, t, coefstruct, u, [], 'KFAQGHR', false);
s2sData = thePde.PackedS2SRadiationData;
[A,D,B,Qamb] = pde.internal.FESolverUtilities.computeRadiosity(thePde.ParentModel.ThermalRadiation,s2sData.FacetEmissivity,s2sData.StefanBoltzmann,s2sData.AmbientTemperature,s2sData.AmbientFactor,u);
K = femat.K;
M = femat.A;
F = femat.F;
Q = femat.Q;
G = femat.G;
H = femat.H;
R = femat.R;
end