function L = fgsc(P,G)
% FGSC Parameter consolidation for fgs
% L = fgsc(P,G)
%
% [+MEQ MatlabEQuilibrium Toolbox+] Swiss Plasma Center EPFL Lausanne 2022. All rights reserved.

L = meqc(P,G,'rrx','zzx');

%% Code name
L.code = 'fgs';

%% Constraint functions
L.agconc = meqagconc(L);     % consolidate constraint equations
L.nC     = size(L.agconc,1); % number of constraint equations

%% Indices into x
%% Set scaling for residuals of non-linear operator
Ip0 = P.b0/P.r0/(4e-7*pi);
Iy0 = Ip0/L.ny; % per point on y grid

% scale ag by typical value to get integrated Ip
ri = L.G.rx(1); ro = L.G.rx(end); zl = L.G.zx(1); zu = L.G.zx(end);
Fx = (L.rrx-ri).*(L.rrx-ro).*(L.zzx-zl).*(L.zzx-zu).*16/(ro-ri)^2/(zu-zl)^2; % 1/0 at the center/boundary
FA = ones(L.nD,1); FB = zeros(L.nD,1);
[~,TpDg,~] = L.bfct(1,L.bfp,Fx,FA,FB,ones(L.nzy,L.nry,'int8'),L.ry,L.iry);
ag0 = Ip0./sum(TpDg,1);

L.icde   = meqcdec(L);
L.np     = sum(L.icde);                   % number of plasma current equations

switch lower(P.algoNL)
  case 'picard'
    assert(L.np==0,'CDE not supported yet for Picard method');
    assert(L.nD==1,'Doublets not supported yet for Picard method');
    L.ind.iy = 1:2; % plasma parametrized by only 2 numbers: axis locations
    L.ind.ig = L.ind.iy(end) + (1:L.ng); % bf coefficients
    L.ind.ip = L.ind.ig(end) + (1:L.np); % Ip coefficients
    L.nN     = 2 + L.ng + L.np;          % total number of nonlinear states
    
    L.resscal = zeros(L.nN,1);

    % Typical value of FR,FZ in fgsFpicard
    % FR ~ h * r0 / 2*r0 = h/2
    % FZ ~ h * r0
    % where h is the typical value of the flux Hessian
    % at the axis. using tr(H) = -2*pi*mu0*rA*jphiA (Moret 2015 paper eq.88)
    % assuming H = diag([h,h]), we get tr(H) = 2h and
    % 2*h ~ -2*pi*mu*rA*jphiA
    h = pi*4e-7*pi*L.P.r0*L.idsx*Iy0; % typical value of H assuming jphiA = Iy0/dsx
    
    L.resscal(L.ind.iy) = 1./[h/2;h*L.P.r0];
    L.resscal(L.ind.ig) = 1; % residuals are already scaled by meqagcon

    xscal = zeros(L.nN,1);
    xscal(L.ind.iy) = L.P.r0;
    xscal(L.ind.ig) = ag0;
  case 'all-nl'
    L.ind.iy = 0             + (1:L.ny); % plasma current
    L.ind.ig = L.ind.iy(end) + (1:L.ng); % bf coefficients
    L.ind.ip = L.ind.ig(end) + (1:L.np); % Ip coefficients
    L.nN     = L.ny + L.ng + L.np;       % total number of nonlinear states
    
    L.resscal = zeros(L.nN,1);
    L.resscal(L.ind.iy) = 1./Iy0;
    L.resscal(L.ind.ig) = 1; % residuals are already scaled by meqagcon
    L.resscal(L.ind.ip) = 1./1;
    
    xscal = ones(L.nN,1);
  otherwise
    error('Unknown numerical method %s',P.algoNL);
end

L.xscal = xscal;

end

function icde = meqcdec(L)
% consolidate CDE equation indices
icde = false(1,L.nD);
if ~isfield(L.P,'cde') || isempty(L.P.cde); return; end

assert(~isempty(contains(L.agconc{1,3},'Ip')),...
  'first constraint must be for Ip/IpD when solving CDE')

assert(L.nD ==1 || ~ismember(L.P.cde, {'OhmTor','OhmTor_rigid'}), 'cde type %s not ready for multiple domains',L.P.cde)
 
% solve CDE for all domains with Ip constraints
for iC=1:L.nC
  if contains(L.agconc{iC,3},'Ip')
    iD = L.agconc{iC,2}; % domain index for this constraint
    icde(iD) = true; % solve CDE for this domain
  end
end
end
