function [dFdIy,dFdag,dFdIp,dFdIe,dFdCo] = rzpFlin(L,LY)
%% [dFdIy,dFdag,dFdIp,dFdIe,dFdCo] = rzpFlin(L,LY)
% Returns linearizations of rzp Force operator around equilibrium LY. 
%
% LY contains the equilibrium time slice and L is the associated parameter structure.
%
% The outputs are the linearisation matrices with respect to the states of the
% system: F = dFdIy*dIy + dFdag*dag + dFdIe*dIe + dFdCo*dCo
%
% The residual is F=[Fy;Fp;Fr;Fz]. 
%
% Fr and Fz are the residuals of the radial and vertical force balance as in
% M. Walker and D. Humphreys, Automatica, vol. 45, no. 3, pp. 665–674,
% 2009:
%
% Fr = \sum_i(2*pi*Ri*Ixi*Bzvac,i) + (mu0*Ip^2/2)*(ln(8*Rc/a*sqrt(k)) + betap + li/2 -3/2)
% Fz = -\sum_i(2*pi*Ri*Ixi*Brvac,i)
%
% where Brvac amd Bzvac are the vacuum (only coil + vessel) radial and vertical
% magnetic field
%
% Fy is the expansion of Iy along with its parameters ag (Ip,Rc,Zc):
%
% Fy = dIy - (dIy/dRc)*dRc - (dIy/dZc)*dZc - (Iy0/Ip0)*dIp
% 
% where dIy/dRc = -dIy/dr and dIy/dZc = -dIy/dz are computed using finite difference method 
%
% The last equation Fp is:
%
% Fp = dIp - dIp'
%
% where dIp is the expansion parameter ag and dIp' the constraint Co. Physically,
% they are the same quantity, but this trick enables to make use of the more general 
% variables ag rather than Rc and Zc. The variables adopted are therefore
% the same for fge (Iy, ag, Ie and Co) and for any general forward model
% for the magnetic equilibrium calculation. 
%
% We introduce one more empty equation in case of cde to fit with the number 
% of equations in fgsFlin. In case of deployment of the cde from fgs to fge, 
% the addition row must be removed.
%
% The linearisation variables are Iy, ag = [Ip;Rc;Zc] (the expansion
% parameters for Iy), Ie = [Ia;Iu] and Co = [Ip;bp;qa/li] (the constraints, only li for the moment)
%
% Note that inserting the two trivial equations the numbers of equations
% and of the variables Iy and ag is the same.
%
% [+MEQ MatlabEQuilibrium Toolbox+] Swiss Plasma Center EPFL Lausanne 2022. All rights reserved.

assert(numel(LY.t)==1,'call fgsFlin for only a single time slice, use meqxk() to slice');
assert(all(contains(L.agconc(:,3),{'Ip','bp','li'})),'rzpl works only with constraints (Ip,bp,li)')
assert(L.ny==numel(LY.Iy),'L.ny (%d) does not match the numers of elements in LY.Iy (%d)',L.ny,numel(LY.Iy))

%% Compute fast version of the linearisation
% Compute only dFdag (splitted in dFydag and
% dFprzdag=[dFpdag;dFrdag;dFzdag]), dFprzdIe=[dFpdIe;dFrdIe;dFzdIe],
% dFpdCo.
% For more information, check rzpFlinfast
[dFydag,dFprzdag,~,dFprzdIe,dFprzdIp] = rzpFlinfast(L,LY);

nxs = L.ny+L.ng+L.np; % rzpF x size: Iy-Iy', Ip=Ip and the two force balances Fr and Fz

%% Compute linearization w.r.t Iy
dFdIy = [eye(L.ny); zeros(L.ng+L.np,L.ny)];

%% Compute complete linearization w.r.t. ag
dFdag = [dFydag;
         dFprzdag;
         zeros(L.np,L.ng)];
       
%% Compute complete linearisation w.r.t Ie
dFdIe = [zeros( L.ny, L.ne );
         dFprzdIe;
         zeros( L.np, L.ne )];
         
%% Compute complete linearization w.r.t. Co
%for now, constraints are Ip, betap and li
dFydCo = zeros(L.ny,L.nC);
dFrdbp = mu0*(LY.Ip.^2)/2;
dFrdli = mu0*(LY.Ip.^2)/4;
dFprzdCo = [dFprzdIp, [0, 0; dFrdbp, dFrdli; 0, 0]];

dFdCo = [dFydCo;
         dFprzdCo;
         zeros(L.np,L.nC)];

dFdIp = zeros(nxs,L.np);

%in case of cde, populate dFdIp with the value in dFdCo(:,Ip)
if L.icde
  iagconIp          = contains(L.agconc(:,3),'Ip');
  dFdIp             = dFdCo(:,iagconIp);
  dFdCo(:,iagconIp) = 0; % replace with a 0 in correspondance of Ip
end

%% Order the force operator F according to L.ind and L.agconc
iF = [L.ind.iy,L.ind.ig,L.ind.ip]; % index into F
%ordering of the constraints according to L.agconc
indCo = [find(strcmp(L.agconc(:,3),'Ip')), find(strcmp(L.agconc(:,3),'bp')), find(strcmp(L.agconc(:,3),'li'))];
dFdIy(iF,:)     = dFdIy; 
dFdag(iF,:)     = dFdag; 
dFdIe(iF,:)     = dFdIe;
dFdCo(iF,indCo) = dFdCo; 
dFdIp(iF,:)     = dFdIp; 

end