function [dFdIy,dFdag,dFdIp,dFdIe,dFdCo] = fgsFlin(L,LY)
% function [dFdIy,dFdag,dFdc,dFdY] = fgsFlin(L,LY)
% Returns linearizations of fgsF operator around equilibrium LY
% Done numerically - later to be computed analytically if possible
%
% [+MEQ MatlabEQuilibrium Toolbox+] Swiss Plasma Center EPFL Lausanne 2022. All rights reserved.

%% perturb w.r.t. x=[Iy,ag]
assert(numel(LY.t)==1,'call fgsFlin for only a single time slice');
iy = L.ind.iy; ig = L.ind.ig; ip = L.ind.ip;

assert(numel(ig)==numel(LY.ag),'size of L.ind.ig (%d) does not match LY.ag (%d)',numel(ig),numel(LY.ag))
assert(numel(iy)==numel(LY.Iy),'size of L.ind.iy (%d) does not match LY.Iy (%d)',numel(iy),numel(LY.Iy))

if any(L.icde), Ip = LY.IpD(L.icde); else, Ip=[]; end
x([iy,ig,ip],1) = [LY.Iy(:);LY.ag(:);Ip];

nxs = L.ny+L.ng+L.np; % size of FGS-only states

dFdIy = zeros(nxs,L.ny); % init
dFdag = zeros(nxs,L.ng); % init
dFdIp = zeros(nxs,L.np); % init

d = 1e-3;
%% Perturb w.r.t. Iy
dd = d*max(abs(LY.Iy(:)));
for ii = 1:L.ny
  dx = zeros(nxs,1);
  dx(iy(ii)) = dd;
  dFdIy(:,ii) = (fgsF(x+dx,L,LY,LY)-fgsF(x-dx,L,LY,LY))/(2*dd);
end

%% Perturb w.r.t. ag
dd = d;
for ii = 1:L.ng
  dx = zeros(nxs,1);
  dx(ig(ii)) = dd;
  dFdag(:,ii) = (fgsF(x+dx,L,LY,LY)-fgsF(x-dx,L,LY,LY))/(2*dd);
end

%% Perturb w.r.t. Ip
dd = d;
for ii=1:L.np
  dx = zeros(nxs,1);
  dx(ip(ii)) = dd;
  dFdIp(:,ii) = (fgsF(x+dx,L,LY,LY)-fgsF(x-dx,L,LY,LY))/(2*dd);
end

%% Optional further outputs
if nargout>3
  %perturb w.r.t. LY.{Ia,Iu,agcon}
  dFdIa = zeros(nxs,L.G.na);
  dd=1e-4*abs(LY.Ip);
  for ia = 1:L.G.na
    dIa = zeros(L.G.na,1);
    dIa(ia) = dd;
    LYp = LY; LYp.Ia = LYp.Ia+dIa;
    LYm = LY; LYm.Ia = LYm.Ia-dIa;
    dFdIa(:,ia) = (fgsF(x,L,LYp,LYp)-fgsF(x,L,LYm,LYm))/(2*dd);
  end
  
  dFdIu = zeros(nxs,L.G.nu);
  for iu = 1:L.G.nu
    dIu = zeros(L.G.nu,1);
    dd = 1;
    dIu(iu) = dd;
    
    LYp = LY; LYp.Iu = LYp.Iu+dIu;
    LYm = LY; LYm.Iu = LYm.Iu-dIu;
    dFdIu(:,iu) = (fgsF(x,L,LYp,LYp)-fgsF(x,L,LYm,LYm))/(2*dd);
  end
  dFdIe = [dFdIa,dFdIu];
  
  % Derivatives w.r.t. quantities in LY that define ag constraints (via meqagcon)
  dFdCo = zeros(nxs,L.nC); % init to worst-case size
  dd = d;
  for iC = 1:L.nC
    % loop over agcon entries
    iD = L.agconc{iC,2}; ii = L.agconc{iC,4}; 
    myfield = L.agconc{iC,3}; % field and index for this constraint
    if L.icde(iD) && contains(myfield,'Ip') 
      if L.P.debug>1; fprintf('fgslin: skip icon=%d: %s(%d) in CDE case\n',iC,myfield,iD); end
      continue; 
    end % skip Ip constraints in CDE case
    
    if L.P.debug>1, fprintf('fgslin: icon=%d: %s(%d)\n',iC,myfield,ii); end
    LYp = LY; LYp.(myfield)(ii) = LYp.(myfield)(ii) + dd;
    LYm = LY; LYm.(myfield)(ii) = LYm.(myfield)(ii) - dd;
    dFdCo(:,iC) = (fgsF(x,L,LYp,LYp)-fgsF(x,L,LYm,LYm))/(2*dd);
  end
end
end
