% FGEF Physics operator for FGE
% [res, LY] = fgeF(xnl,L,LXt,LYt,doplot)
% Computes F(x) operator for FGE, x s.t. F(x) = 0 is the solution
% Calls fgsF() for residuals of plasma - related quantities
%
% xnl is the nonlinear state vector [Iy(:);ag(:);Ie(:)] or [Iy(:);ag(:);Ip(:);Ie(:)] 
%
% L is the ancilliary data structure
% LXt contains input data
% LYt contains the previous equilibrium.
%
% [+MEQ MatlabEQuilibrium Toolbox+] Swiss Plasma Center EPFL Lausanne 2022. All rights reserved.
function [res, LY ] = fgeF(xnl,L,LXt,LYt,varargin)

if numel(varargin)>0
  doplot = varargin{1};
else
  doplot = false;
end

postprocess = (nargout>1 || doplot);

%% Extract controller actions
Ve   = zeros(L.ne,1); Ve(1:L.G.na) = LXt.Va;

%% Extract NL unknowns from xnl
Iy = reshape( xnl(L.ind.iy) , L.nzy, L.nry);
Ia = xnl(L.ind.ia);
Iu = xnl(L.ind.iu);

%% Plasma-related equation (GS, Constraints, CDE)
LXt.Iu = Iu; LXt.Ia = Ia; % pass Ia, Iu

if postprocess
  [respla(:,1),LY] = fgsF(xnl([L.ind.iy,L.ind.ig,L.ind.ip]),L,LXt,LYt);
else
  respla(:,1) = fgsF(xnl([L.ind.iy,L.ind.ig,L.ind.ip]),L,LXt,LYt);
end

%% Conductor equation residuals
Ie = [Ia;Iu];
Fedot = (L.Mee*(Ie-[LYt.Ia;LYt.Iu]) + L.P.mey*L.Mey*(Iy(:)-LYt.Iy(:)))/LXt.dt;
rese = (Fedot + L.Re.*Ie - Ve); % residual for conductor equation

%% Collect and scale residuals
rese = L.resscal([L.ind.ia,L.ind.iu]).*rese; % scale
res = [respla; rese];

%% Add ouputs
if postprocess
  LY.rese = rese;
end

%% plot
if doplot
  clf;
  subplot(121)
  
  FFB = repmat(LY.FB,1,max(3-numel(LY.FB),1)); % fix me
  contourf(L.rx,L.zx,LY.Fx-LY.FB(1),21); hold on;
  contour( L.rx,L.zx,LY.Fx,FFB,'linewidth',2,'color','k');
  plot(LY.rA,LY.zA,'oc',LY.rX,LY.zX,'xc');
  axis equal tight;
  
  title(sprintf('Ip=%2.3f',LY.Ip));
  
  subplot(422)
  plot(res,'.'); title(sprintf('max(|res|)=%4.2e',max(abs(res))));
  
  subplot(424)
  resy = norm(res(L.ind.iy));
  resa = norm(res(L.ind.ia));
  resu = norm(res(L.ind.iu));
  resg = norm(res(L.ind.ig));
  
  bar([resy;resa;resu;resg]); title(sprintf('res y,a,u,ag'));
  
  subplot(426)
  plot([[LYt.Ia;LYt.Iu],Ie]); title('Ie');
  
  drawnow;
end
end
