function fun = meqagconfun(h)
% container for ag constraint functions
% Function signature:
% [A,b] = myconstraint(L,LX,LY,F0,F1,rA,dr2FA,dz2FA,drzFA,ag,Fx,Opy,fPg,fTg,TpDg,ITpDg,iD)
% then the residual is res = A*ag + b;
% In some cases, the constraint can not be expressed linearly in ag, in that case A=zeros(1,L.ng) and res = -b
%
% [+MEQ MatlabEQuilibrium Toolbox+] Swiss Plasma Center EPFL Lausanne 2022. All rights reserved.

fun=localfunctions;
if nargin==0, return; end

if ischar(h) && isequal(h,'help')
  help(mfilename);
  fprintf('\nCatalog of available %s constraints\n',mfilename)
  for ii=1:numel(fun)
    myfun = func2str(fun{ii});
    fprintf('%s:\n',myfun)
    help(sprintf('%s>%s',mfilename,myfun));
  end
end
end

function [A,b] = ag(L,LX,LY,F0,F1,rA,dr2FA,dz2FA,drzFA,IpD,ag,Fx,Opy,fPg,fTg,TpDg,ITpDg,ii)
% Directly specify ag = LX.ag
%res = ag(ii)-LX.ag(ii);

b = LX.ag(ii);
A = zeros(1,L.ng);
A(:,ii) = 1;
end

function [A,b] = li(L,LX,LY,F0,F1,rA,dr2FA,dz2FA,drzFA,IpD,ag,Fx,Opy,fPg,fTg,TpDg,ITpDg,iD) %#ok<*INUSD,*DEFNU,*INUSL>
% constrain plasma li to LX.li (LIUQE li definition with fixed r0)
assert(numel(iD)==1,'only one domain supported for li constraint')
[Wp,~,~] = vizrmex(Fx,Opy==iD,L.ry,L.iry,LX.rBt,L.drx,L.dzx);
Ip = TpDg(iD,:)*ag;
WN = 1e-7*pi*L.P.r0*Ip^2;

% li = Wp/WN;
res = 1e-3*(Wp - LX.li*WN);

A = zeros(1,L.ng);
b = -res;
end

function [A,b] = Ip(L,LX,LY,F0,F1,rA,dr2FA,dz2FA,drzFA,IpD,ag,Fx,Opy,fPg,fTg,TpDg,ITpDg,iD)
% Impose Ip = IpD(iD), which comes either from data or from CDE
Ip  = IpD(iD);  

%res = 1e-6 * (TpDg(iD,:)*ag) - Ip);
scal = 1e-6;
A = scal*TpDg(iD,:);
b = scal*Ip;
end

function [A,b] = bp(L,LX,LY,F0,F1,rA,dr2FA,dz2FA,drzFA,IpD,ag,Fx,Opy,fPg,fTg,TpDg,ITpDg,iD)
% Impose bpD(iD) = LX.bpD(iD) (beta poloidal per domain) or total bp
% Uses LIUQE bp equation involving r0
if isequal(iD,1:L.nD), bp = LX.bp;
else,                  bp = LX.bpD(iD);
end

Ip = sum(TpDg(iD,:)*ag,1);
WN = 1e-7*pi*L.P.r0*Ip^2;
%res = 1e-3*(ITpDg(iD,:).*fPg*a) - bp*WN);

A = 1e-3*sum(ITpDg(iD,:),1).*fPg.';
b = 1e-3*bp*WN;
end

function [A,b] = bt(L,LX,LY,F0,F1,rA,dr2FA,dz2FA,drzFA,IpD,ag,Fx,Opy,fPg,fTg,TpDg,ITpDg,iD)
% Impose bt = LX.bt (toroidal beta toroidal)
if isequal(iD,1:L.nD), bt = LX.bt;
else,                  bt = LX.btD(iD);
end

[~,Ft0,~] = vizrmex(Fx,Opy==iD,L.ry,L.iry,LX.rBt,L.drx,L.dzx);
Wt0 = 2.5e6*LX.rBt*Ft0;

%Wk = 1.5*ITpDg(iD,:).*fPg.'*ag;
%res = 1e-3*(2/3*Wk-bt*Wt0); % scale to order 1
A = 1e-3*sum(ITpDg(iD,1),1).*fPg.';
b = 1e-3*bt*Wt0;
end

function [A,b] = qA(L,LX,LY,F0,F1,rA,dr2FA,dz2FA,drzFA,IpD,ag,Fx,Opy,fPg,fTg,TpDg,ITpDg,iD)
% Impose qA = LX.qA
% With qA calculated from axis quadratic expansion, as LIUQE paper eq.88
% with small diamagnetic approximation
assert(numel(iD)==1 && iD<=numel(rA),'can impose qA constraint only on one axis domain')
[gAg,IgAg] = L.bfct(5,L.bfp,[],F0,F1); IgAg = 2e-7*L.idsx*IgAg;
TyAg = gAg(iD,:).*(rA(iD)*fPg+fTg/rA(iD)).'; % TyAg: Axis current (jphi.dR.dZ) contribution from each basis function
gA  = sqrt(dr2FA(iD)*dz2FA(iD)-drzFA(iD)^2)/(abs(dr2FA(iD)+dz2FA(iD))); % sqrt(det(H))/abs(tr(H))
cqA = 4e-7*pi*rA(iD)^2*LX.rBt*gA/L.dsx;

%res = (LX.rBt.^2 + (IgAg.*fTg.')*ag) - LX.qA(iD)*(cqA*Ty1g*ag);
A = (IgAg(iD,:).*fTg.') - LX.qA(iD)*cqA*TyAg;
b = -LX.rBt.^2;

end
