%FGET  Forward Grad-shafranov Evolution solver
% LY = FGET(L,LX) solves the Grad-Shafranov equation and the active and passive coil evolution
%
% FGE-specific outputs (on top of FGS-specific ones): 
%  .Ini   Non-inductive plasma current                             (t)       [A]
%
% [+MEQ MatlabEQuilibrium Toolbox+] Swiss Plasma Center EPFL Lausanne 2022. All rights reserved.
function [LY] = fget(L,LX,varargin)

% optional parameterss
for k = 1:2:length(varargin), L.P.(varargin{k}) = varargin{k+1}; end

%% Display info
if L.P.debug
  meqinfo(L,LX)
  tstart=tic;
end

%% Check linearization is present
assert(isfield(L,'lin') || ~isequal(L.P.algoNL,'all-nl'), ... 
  'L.lin is necessary for all fget calls with algoNL=all-nl')

%% Time step cycle
nt = numel(LX.t);
assert(norm(diff(LX.t)-meqdt(LX.t,1))<1000*eps,'dt must be constant for fget.m')
dt = meqdt(LX.t,1);

%% Initial state computed using fgs
LXt = meqxk(LX,1); % get only first time index
LY0 = rmfield(LXt,{'Vadot0','Iadot0','SQ'}); % initial LY

%% Pack linearized system into state space representation, with discrete time
if L.P.lin
  [sys_tmp,L.lin.ind,~,L.lin.yo] = fgess(L,dt);
  % store ABCD in matrices since much faster access
  L.lin.sysA = sys_tmp.A;  L.lin.sysB = sys_tmp.B;
  L.lin.sysC = sys_tmp.C;  L.lin.sysD = sys_tmp.D;
  L.lin.iut = [sys_tmp.InputGroup.Co,sys_tmp.InputGroup.dCodt,sys_tmp.InputGroup.Va];
  if L.icde
    L.lin.iut = [L.lin.iut,sys_tmp.InputGroup.Ini];
  end
  L.lin.OutputGroupString = sys_tmp.OutputGroup;
end

%% Init controller
if ~isempty(L.P.ctrlfct)
  fbcontrol = true;
  [cstate,Vctr] = L.P.ctrlfct('init',[],L.P.ctrlpar,L,LY0);
  assert(L.P.ctrlpar.dt>=dt,...
    'controller time step %3.3e is smaller than simulation time step %3.3e',L.P.ctrlpar.dt,dt)
else
  fbcontrol = false;
  Vctr = zeros(L.G.na,1);
end

%% Init power supply
[psstate,~] = meqps(L.G,[],dt,0,Vctr);

%% Init protection limits
[Tstate,LY0,~] = meqlim(L,LY0,dt,0);

%% Add missing parts of output structure
LY0.Ffdot  = zeros(L.G.nf,1); % Voltage measurements of magnetic probes
LY0.Bmdot  = zeros(L.G.nm,1); % Voltage measurements of flux probes
LY0.res = 0;

%% NL state vector initialization 
if any(L.icde), IpD=LY0.IpD(L.icde); else, IpD = []; end
switch lower(L.P.algoNL)
  case 'all-nl'
    % for all-nl [y,g,a,u,p];
    xnl([L.ind.iy,L.ind.ig,L.ind.ia,L.ind.iu,L.ind.ip],1) = ...
      [LY0.Iy(:);LY0.ag(:);LY0.Ia;LY0.Iu;IpD(:)]./L.xscal;
  case 'picard'
    %for Picard: [y,g,p];
    xnl([L.ind.iy,L.ind.ig,L.ind.ip],1) = ...
      [LY0.rA(:);LY0.zA(:);LY0.ag(:);IpD(:)]./L.xscal;
end

%% Allocate LY
LY = repmat(LY0,nt,1);

%% Index initialization
itstart = 2; itend = nt; 
itc = 1; itctr = 1;
dobreak = false; nnoc = 0;

%% Preconditioner initialization
if ~all(LX.Ip==0) && L.P.usepreconditioner && itend>1 && ~L.P.lin && ~isequal(lower(L.P.algoNL),'picard')
  % function for pre-conditioner
  LXt.Va = LX.Va(:,1);
  LXt.dt = LX.t(2)-LX.t(1);
  Prec = fgepre(L,LXt.dt);
else
  Prec = 1;
end

%% Time loop
for it = itstart:itend
  %% Power supply
  [psstate,Vact] = meqps(L.G,psstate,dt,it,Vctr);
  
  % data for this step
  LXt = meqxk(LX,it);
  LXt.dt = LX.t(it) - LX.t(it-1);

  if fbcontrol
    LXt.Va = Vact; % control action (assume FF is included)
  end
  
  if L.P.lin
    % Euler implicit dt step L
    LYt = fgetkl(L,LXt,LY(it-1));
  else
    % Euler Implicit dt step NL
    [xnl,LYt] = fgetk_implicit(xnl,L,LXt,LY(it-1), Prec);
  end
  % copy common things
  LYt.t       = LXt.t;
  LYt.shot    = LXt.shot;
  LYt.tokamak = LXt.tokamak;
  
  %% Derivatives
  LYt = meqLYdot(LYt,LY(it-1));
  
  % Check coils limits and add normalized distance from limits to outputs
  [Tstate,LYt,alarm] = meqlim(L,LYt,dt,it,Tstate); % limits
  
  LY(it) = LYt; % Assign next time step
  
  if alarm && L.P.stoponalarm
    fprintf('stop on protection alarm'); dobreak = true;
  end
  
  if ~L.P.lin
    if ~LYt.isconverged
      nnoc = nnoc+1; % increment counter for non-convergent time steps
      if nnoc > L.P.nnoc
       fprintf('stopped due to non-convergence\n'); dobreak=true;
      end
    else
      nnoc = 0; % reset counter
    end
  end
  
  %% Controller Step
  if fbcontrol
    if mod(LY(it).t,L.P.ctrlpar.dt)==0
      % controller will get simulation info from previous controller time step
      itcontrol = it-round(L.P.ctrlpar.dt/dt)+1-itc;
      if itcontrol>=1 % else, not enough simulation times to call controller yet
        % call controller step
        [cstate,Vctr,~,ctdebugt] = L.P.ctrlfct('step',cstate,L.P.ctrlpar,L,LY(itcontrol));
        if ~isempty(ctdebugt), ctdebug(itctr) = ctdebugt; end
        itctr = itctr+1; % controller counter
      end
    end
  end
  
  %% Debugging display/outputs
  if L.P.debug==0 && (it==itstart || it==itend || ~rem(it, ceil(itend/10)))
    meqprogress(it,itend,it==itstart);
  elseif L.P.debug
    if ~rem(it-itstart,10)
      nbytes = fprintf(  '\n  %7s | %3s | %7s | %6s | %6s | %6s | %6s', ...
          't','it', 'Ip [kA]' ,'zA[m]','rA[m]','bp','qA');
      if ~L.P.lin
        nbytes = nbytes + fprintf(  ' | %5s | %6s | %4s | %8s', ...
          'niter','nfeval','conv', 'res');
      end
      fprintf('\n  %s\n',repmat('=',nbytes-3,1));
    end
    fprintf(  '  %7.5f | %3d | %7.2f | %6.3f | %6.3f | %6.3f | %6.3f',...
        LY(it).t,it,LY(it).Ip/1e3,LY(it).zA(1),LY(it).rA(1),LY(it).bp,LY(it).qA(1));
    if ~L.P.lin
      fprintf(  ' | %5d | %6d | %4d | %8.2e',...
        LY(it).niter,LY(it).nfeval,LY(it).isconverged, LY(it).res);
    end
    fprintf('\n');
  end
  if dobreak, break; end
end % time loop

if dobreak && ~isempty(it), LY = LY(1:it); end % chuck the rest if stopped prematurely

if L.P.debug, fprintf('Time elapsed: %2.2f[s]\n',toc(tstart)); end

LY = meqlpack(LY, {'FW'}); % pack into structure of arrays 
if isfield(LX,'shot'), LY.shot = LX.shot; end
if ~isempty(L.P.ctrlfct) && exist('ctdebug','var')
  % append controller debug info to LY if desired
  LY.ctdebug = meqlpack(ctdebug);
end

end
