function L = liuc(P,G,varargin)
%LIUC LIUQE ancillary data
% L = LIUC(P,G) returns a structure with LIUQE ancillary data. P contains
% the configuration parameters as obtained by LIUP<TOK>. G contains the
% geometry as obtained by LIUG<TOK>. See also LIUP,LIUG.
%
% [+MEQ MatlabEQuilibrium Toolbox+] Swiss Plasma Center EPFL Lausanne 2022. All rights reserved.

%% Input arguments
for k = 1:2:numel(varargin), P.(varargin{k}) = varargin{k+1}; end

%% Generic MEQ ancillary data
L = meqc(P,G);

%% Checks
assert(all(size(P.wreg) == [L.nq,1]) || numel(P.wreg)==1   ,'wreg must have size 1 or [L.nq,1] = [%d,1]',L.nq)
assert(P.Ipmeas || ~isempty(G.Ipm),'if P.Ipmeas=0, G.Ipm needs to be non-empty');

%% Code
L.code = 'liu';

%% Update meqpdom parameters for rt
if P.itert, L.FN = -1000; L.dimw={20}; L.raN = -1000; end

%% Plasma current estimator: Ip = Ipm*Bm + Ipa*Ia + Ipu*Iu
L.Ipa = -G.Ipm*G.Bma;
L.Ipu = -G.Ipm*G.Bmu;
L.Ips = -G.Ipm*G.Bms;

%% Measurement and constraint indices
[L.kdf,L.kdm,L.kda,L.kdu,L.kdt,L.kdp,L.nd] = n2k(G.nf,G.nm,G.na,G.nu,1,1);
[                        L.kit,L.kip     ] = n2k(                    1,1);
L.kdr = [L.kdf L.kdm]; L.kde = [L.kda L.kdu]; L.kdi = [L.kdt L.kdp];
L.nr  = numel(L.kdr);  L.ne  = numel(L.kde);  L.ni  = numel(L.kdi);
 
%% Measurement weighting
% Pad as necessary
wFf = P.wFf(:); wFf(end+1:G.nf,1) = 1;
wBm = P.wBm(:); wBm(end+1:G.nm,1) = 1;
wIa = P.wIa(:); wIa(end+1:G.na,1) = 1;
wIu = P.wIu(:); wIu(end+1:G.nu,1) = 1;

% overwrite wIu if no vessel current measurements
if ~P.ivesm, wIu(:) = 0; end

Wf = wFf(1:G.nf)/P.Fferr; Wm = wBm(1:G.nm)/P.Bmerr;
Wa = wIa(1:G.na)/P.Iaerr; Wu = wIu(1:G.nu)/P.Iuerr;
L.Wp = P.wIp/P.Iperr; L.Wt = P.idml/P.Fterr;
L.Wr = [Wf ; Wm]; L.We = [Wa ; Wu]; L.Wi = [L.Wt ; L.Wp];
L.Wd = [L.Wr ; L.We ; L.Wi];
L.Wq = P.wreg.*ones(L.nq,1);

%% Response matrices
L.Mry = [G.Mfx(:,L.lxy);G.Bmx(:,L.lxy)];
L.Mre = [G.Mfa G.Mfu;G.Bma G.Bmu];
L.Wre = L.Wr.*L.Mre;
L.Wee = diag(L.We);
if isfield(G,'Mtx'), L.Mty = reshape(G.Mtx(L.lxy),1,[]); end

%% Finite elements
rip = ifc(isnan(P.rip),min(G.rl),P.rip);
rop = ifc(isnan(P.rop),max(G.rl),P.rop);
zlp = ifc(isnan(P.zlp),min(G.zl),P.zlp);
zup = ifc(isnan(P.zup),max(G.zl),P.zup);

nrh = min(P.nelem,2);
nzh = floor(P.nelem/nrh);
% Corners of FE domain bounding box
L.bh = [max(rip,G.rx(1)) max(zlp,G.zx(1)) min(rop,G.rx(end)) min(zup,G.zx(end))];

% Finite Elements for initial plasma current guess
if strcmp(P.fetype,'tri')
  [L.Tyh,L.rh,L.zh] = fetr(L); % triangular finite elements
  L.nh = size(L.Tyh,2);
else
  % pyramidal or bilinear elements - share same rh,zh
  rh = linspace(L.bh(1),L.bh(3),nrh+2); drh = rh(2)-rh(1); rh([1 end]) = [];
  zh = linspace(L.bh(2),L.bh(4),nzh+2); dzh = zh(2)-zh(1); zh([1 end]) = [];
  L.nh = nrh*nzh;
  [L.rh,L.zh] = meshgrid(rh,zh); L.rh = L.rh(:); L.zh = L.zh(:);
  hr = max(1-abs(reshape(L.ry,[L.nry,1])-reshape(rh,[1,nrh]))/drh,0);
  hz = max(1-abs(reshape(L.zy,[L.nzy,1])-reshape(zh,[1,nzh]))/dzh,0);
  
  if strcmp(P.fetype,'pyr')
    L.Tyh = reshape(min(reshape(hz,[L.nzy,1,nzh,1]), reshape(hr,[1,L.nry,1,nrh])),L.ny,L.nh) .* L.Oly(:);
  elseif strcmp(P.fetype,'bil')
    L.Tyh = reshape(    reshape(hz,[L.nzy,1,nzh,1]).*reshape(hr,[1,L.nry,1,nrh]) ,L.ny,L.nh) .* L.Oly(:);
  else
    error('LIUQE:liuc:fetype','unknown P.fetype');
  end
end
L.Tyh = L.Tyh./sum(L.Tyh);

%% Vertical position feedback
L.Iph  = sum(                                    L.Tyh,1);
L.Ipzh = sum(repmat(reshape(L.zy,[],1),L.nry,1).*L.Tyh,1); % L.Ipzh = repmat(zh,1,nrh).*sum(L.Tyh,1);
L.ndz  = (P.stabz ~= 0)*L.nD;

%% FE fit
% response matrices
Wrh = L.Wr.*L.Mry*L.Tyh;
Wih = [zeros(1,L.nh) ; L.Wp*L.Iph];
L.Wde([L.kdr L.kde L.kdi],:) = [L.Wre ; L.Wee            ; zeros(L.ni,L.ne)];
L.Wdh([L.kdr L.kde L.kdi],:) = [  Wrh ; zeros(L.ne,L.nh) ; Wih             ];
if P.iterh % ipm
  Ahh = [Wrh ; Wih]'*[Wrh ; Wih];
  L.uAhh = Ahh(triu(true(size(Ahh))));
  L.Ahd = zeros(L.nh,L.nd);
  L.Ahd(:,[L.kdr L.kdi]) = -[Wrh' Wih'];
  L.Aed = zeros(L.ne,L.nd);
  L.Aed(:,[L.kdr L.kde]) = (L.Wre'*L.Wre + L.Wee'*L.Wee) \ [L.Wre' L.Wee'];
  L.Ahe = Wrh'*L.Wre;
  L.Aeh = -L.Aed(:,L.kdr)*Wrh;
  L.Ahh = L.Ahe*L.Aeh;
else % lsq
  WdH   = [L.Wde L.Wdh];
  AHd   = (WdH'*WdH) \ WdH';
  L.Aed = AHd(     1:L.ne,:);
  L.Ahd = AHd(L.ne+1:end ,:);
end

%% Edge condition
L.Mbh = G.Mby * L.Tyh;

%% Fitting base function amplitudes
% fixed parts
Wree = [L.Wre ; L.Wee]; iAee = (Wree'*Wree)\eye(L.ne); Aere = iAee*Wree';
L.Ae1d(:,[L.kdr L.kde L.kdi]) = [Aere zeros(L.ne,L.ni)];
L.Mdzry = [G.dzMfx(:,L.lxy) ; G.dzBmx(:,L.lxy)]*P.fdz; % dz is rescaled in the LLS problem
L.dzMbe = (-2*pi)*L.rb.*[G.Brxa(~L.lxy,:),G.Brxu(~L.lxy,:)];
if P.gsxe == 2
  L.dzMboe = (-2*pi)*L.rb.*[G.Brboa,G.Brbou];
end
% for bslv
Aee  = L.Wre'*L.Wre + L.Wee'*L.Wee;
L.Aer  = Aee\L.Wre';
L.Aee  = Aee\L.Wee';

%% Fitting variables
% Pad as necessary
wag = P.wag(:); wag(end+1:L.ng ,1) = 0;
wdz = P.wdz(:); wdz(end+1:L.ndz,1) = 0;

Wg  = wag(1:L.ng)./P.agerr;
Wdz = wdz(1:L.ndz)/(P.fdz*P.dzerr);
Wj  = [Wg ; Wdz];
L.nj  = L.ng + L.ndz;
L.kj  = ~isinf(Wj); % Approximate constraints selector
L.nja = sum(L.kj); % Number of approximate constraints
L.nje = L.nj - L.nja; % Number of exact constraints
% Approximate constraints will enter the LLS problem as Wjj*aj = Yj,
% lines corresponding to exact constraints are replaced with 0s, including
% for Yj to avoid artificially increasing chi
Wj(~L.kj) = 0;
L.Wj  = Wj;
L.Wjj = diag(Wj);
% Group meqsurements/constraints linked to aj only
[L.kki,L.kkq,L.kkj,L.nk] = n2k(L.ni,L.nq,L.nj); 

%% p and T2/2 profile, RT only
[L.TQg,L.ITQg] = L.bfct(2,L.bfp,L.pQ(:).^2,zeros(L.nD,1),ones(L.nD,1));

%% Miscellaneous
L.liurtemu = P.itert && ~P.slx; % Flag for when liut is used to emulate Simulink/rt runs

end

function varargout = n2k(varargin)
varargout = cell(1,nargin+1);
varargout{end} = 0;
for k = 1:nargin
  varargout{k} = varargout{end}+(1:varargin{k});
  varargout{end} = varargout{end} + varargin{k};
end
end
