function u = solveStationaryNonlinear(self, coefstruct,u0)
% solveStationaryNonlinear - Internal function for solving nonlinear stationary PDE
%

% Copyright 2015-2019 The MathWorks, Inc.

[p,~,t] = self.Mesh.meshToPet();
N = self.PDESystemSize;
np = size(p,2);

pseudocoefstruct = coefstruct;
for i = 1:numel(pseudocoefstruct.Coefficients.c)
    pseudocoefstruct.Coefficients.c{i} = 1;
    pseudocoefstruct.Coefficients.a{i} = ones(N*N,1);
    pseudocoefstruct.Coefficients.f{i} = zeros(N,1);
end

dummyResidual=@(u)compResidual(u,self,p,t,pseudocoefstruct,np*N);
getResidual=@(u)compResidual(u,self,p,t,coefstruct,np*N);
numjac_fh=@(~,u)compResidual(u,self,p,t,coefstruct,np*N);

% Initial residual and update u0 with Lagrange multiplier dofs
[r,pdenonK,pdenonF,u]=getResidual(u0);
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

% For initial numjac call
fac=[];
G=[];
iter=0;
% Pad out S
[~,pseudoK]=dummyResidual(ones(size(u)));
S=(pseudoK~=0);

while (nrm > self.SolverOptions.ResidualTolerance)  % Gauss-Newton iteration loop

    iter=iter+1;
    if iter>self.SolverOptions.MaxIterations
        error(message('pde:pdenonlin:TooManyIter')) % *** TODO Change message ID
    end
    
    thresh = calcThresh(u, np*N);
    S=S|(pdenonK~=0);
    [J,fac,G]=numjac(numjac_fh,0,u,r,thresh,fac,0,S,G);
    
    lastwarn('','');
    warnState = warning('off', 'MATLAB:singularMatrix');
 
    cor=-J\r;
    [~,wid] = lastwarn;
    if(strcmp(wid,'MATLAB:singularMatrix'))
        warning(warnState);
        error(message('pde:pdenonlin:SingularJacobian'))
    end
    warning(warnState);
    
    % Bisect until squared residual norms decrease sufficiently
    alfa=1;
    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;
    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 [K, M, F, Q, G, H, R] = assemblePdeMatrices(thePde, ndims, p, t, coefstruct, u)

tempPde = pde.PDEModel(thePde.PDESystemSize);

if(ndims==2)
    [K, F] = formGlobalKF2D(tempPde, p, t, coefstruct,u,[]);
    M = formGlobalM2D(tempPde, p, t, coefstruct,u,[],'a');
elseif(ndims==3)
    [K, F] = formGlobalKF3D(tempPde, p, t, coefstruct,u,[]);
    M = formGlobalM3D(tempPde, p, t, coefstruct,u,[],'a');
end
% Impose BCs
[Q,G,H,R]=thePde.assembleBoundary(u);

end


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

%       Copyright 2015 The MathWorks, Inc.

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

pdenonK=K+M+Q;
pdenonF=F+G;
if any(any(H))
    if size(u,1)<(size(pdenonK,2)+size(H,1))
        u=[u;H'\(pdenonF-pdenonK*u)];
    end
    pdenonK=[pdenonK H';H zeros(size(H,1))];
    pdenonF=[pdenonF;R];
end
r=pdenonK*u-pdenonF;

end


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


function thresh = calcThresh(u, numDofs)
nu = size(u,1);
thresh=zeros(nu,1);
threshFact = 1e-6;
if all(u(1:numDofs)==0)
    thresh(1:numDofs)=sqrt(eps);
else
    thresh(1:numDofs)=threshFact*max(abs(u(1:numDofs)));
end
if numDofs<nu
    if all(u(numDofs+1:nu)==0)
        thresh(numDofs+1:nu)=sqrt(eps);
    else
        thresh(numDofs+1:nu)=threshFact*max(abs(u(numDofs+1:nu)));
    end
end
end