%MEQC MEQ ancillary data
% L = MEQC(P,G,'P1',...) returns a structure with MEQ ancillary data. P
% contains the configuration parameters as obtained by LIUP<TOK> or
% FBTP<TOK>. G contains the geometry as obtained by LIUG<TOK> or FBTG<TOK>.
% It defines the mesh data by calling MEQMESH and the Poisson solver data
% by calling GSZRC, plus .Oly  Limiter domain. See also
% LIUP,LIUG,FBTP,FBTG,MEQMESH,GSZRC.
%
% [+MEQ MatlabEQuilibrium Toolbox+] Swiss Plasma Center EPFL Lausanne 2022. All rights reserved.
function L = meqc(P,G,varargin)
 
 L.P = P; L.G = G;
  
 %% Mesh related
 L = meqmesh(L,G.rx,G.zx,G.Mbc,varargin{:});

 %% Cubic spline interpolation
 if P.icsint
   L.csnthreads = 2;
   [L.Mr,L.taur] = csdec(L.G.rx,L.G.rx,'a');
   [L.Mz,L.tauz] = csdec(L.G.zx,L.G.zx,'a');
 end

 %% Domains
 if isnan(P.ndom)
  % set maximum number of axes and domains based on idoublet
  if P.idoublet
   L.nD = 3;
  else
   L.nD = 1;
  end
 else
  assert(~P.idoublet || P.ndom>1,'Must select ndom>1 if idoublet==true')
  L.nD = P.ndom;
 end
 
 %% Solving Poisson equation
 [L.cx,L.cq,L.cr,L.cs,L.ci,L.co] = gszrc(L.ry,L.drx,L.dzx,L.nry,L.nzy);
 
 %% Limiter domain
 assert(all(G.rl<=G.rx(end)) && all(G.zl<=G.zx(end)) && all(G.rl>=G.rx(1)) && all(G.zl>=G.zx(1)),'Limiter outside computational mesh')
 L.Oly = inpolygon(repmat(reshape(L.ry,1,[]),L.nzy,1),repmat(reshape(L.zy,[],1),1,L.nry),G.rl,G.zl);
 
 %% Interpolate flux on limiter
 [L.kxl,L.clx]   = bintc(G.rl ,G.zl ,G.rx,G.zx);
 if P.ilim == 2
   [L.kxlh,L.clhx] = bintc(G.rlh,G.zlh,G.rx,G.zx); % points between rl,zl for interpolation
 elseif P.ilim == 3
   assert(P.icsint,'P.icsint must be true if P.ilim==3');
   switch lower(L.P.tokamak)
     case 'tcv'
       meqmdsopen(P.shot,'tcv_shot');
       ppl = mkpp(mdsvalue('static("pp_t:breaks")'),mdsvalue('static("pp_t:coefs")'),2);
       [L.Ml,L.taul] = pp2bsp(ppl);
     otherwise
       [M,L.taul] = csdec((0:L.G.nl)/L.G.nl,(0:L.G.nl)/L.G.nl,'p');
       L.Ml = M*[L.G.rl,L.G.zl;L.G.rl(1),L.G.zl(1)];
   end
   L.taul = reshape(L.taul,[],1);
 end

 %% Critical-point search domain == L.Oly plus neighbours or whole grid when P.lasy is true
 L.Oasx = false(L.nzx,L.nrx);
 L.Oasx(2:end-1,2:end-1) = L.Oly | L.P.lasy;
 L.Oasx(1:end-2,2:end-1) = L.Oasx(1:end-2,2:end-1) | L.Oly;
 L.Oasx(3:end  ,2:end-1) = L.Oasx(3:end  ,2:end-1) | L.Oly;
 L.Oasx(2:end-1,1:end-2) = L.Oasx(2:end-1,1:end-2) | L.Oly;
 L.Oasx(2:end-1,3:end  ) = L.Oasx(2:end-1,3:end  ) | L.Oly;
 
 %% meqpdom parameters
 % FN: marker value for non-extremum limiter points
 % dimw: working space for extrema/saddle points. Emulate Simulink if itert=1.
 L.FN = -Inf; L.dimw = {}; % defaults for meqpdom
 
 %% theta-rho mesh
 % rho mesh - pq=rho(psiN);
 if ~isempty(P.pq)
  % take user-defined pq
  assert(numel(P.pq)>1,'pq must be an array')
  L.pq = [reshape(P.pq(P.pq > 0 & P.pq < 1),1,[]) 1];
 else
  % build array of npq points
  L.pq = [linspace(P.pql,sqrt(2)-sqrt(1-P.pqu^2),P.npq-1) sqrt(2)];
  k = L.pq>sqrt(0.5); 
  L.pq(k)= sqrt(1-(sqrt(2)-L.pq(k)).^2);
 end
 L.pQ = [0,L.pq];
 L.npq = numel(L.pq);
 L.pinit = 0.8;
 L.nQ = L.npq+1;
 % theta mesh
 L.noq = P.noq;
 L.doq = 2*pi/L.noq; L.oq = L.doq*(0:L.noq-1)';
 L.fq = 1-L.pq.^2; % normalized psiN with 0 at LCFS and 1 on axis
 L.crq  = -cos(L.oq);
 L.czq  =  sin(L.oq);
 L.cdrq = -cos(L.oq)*L.idrx;
 L.cdzq =  sin(L.oq)*L.idzx;
 L.nQ = L.npq+1;
 
 L.nS = numel(P.raS);
 L.nR = numel(P.iqR);
 assert(any(L.crq==1),'need noq to be even to have an outboard theta direction for computing r/a')
 
 msgid = 'MEQ:ParameterError';

 %% Fixed base function parameters
 % Handle default cases
 if ~isempty(P.bfct)
  % bf related checks
  bfp = bfpck(P.bfct,P.bfp);
  if isequal(func2str(P.bfct),'bffbt')
   % handle bfp multi-time case
   bfp1 = bfp(:,1);
  else
   bfp1 = bfp;
  end
  % Use generic multidomain bfgenD if needed
  [~,~,TDg] = P.bfct(0,bfp1);
  if size(TDg,1) == 1 && L.nD > 1
   L.bfct = @bfgenD;
   L.bfp = repmat({P.bfct,bfp ,zeros(L.nD,1)},L.nD,1);
   bfp1  = repmat({P.bfct,bfp1,zeros(L.nD,1)},L.nD,1);
   for iD = 1:L.nD, L.bfp{iD,3}(iD) = 1; bfp1{iD,3}(iD) = 1; end
  else
   L.bfct = P.bfct;
   L.bfp  =   bfp;
  end
  % Get ng/nq/nc
  [L.fPg,L.fTg,L.TDg] = L.bfct(0,bfp1);
  assert(~any(L.fPg.*L.fTg),msgid,'One basis function cannot be assigned to both p'' and TT'', all elements of fPg.*fTg must be 0');
  assert(size(L.TDg,1) == L.nD,msgid,'bfct=''%s'' can support only %d domains but L.nD=%d',func2str(P.bfct),size(L.TDg,1),L.nD);
  L.ng = numel(L.fPg); % #base functions
  o = ones(L.nD,1); z = zeros(L.nD,1);
  L.nq = size(L.bfct(6,bfp1,[],o,z,o,o,1),1); % #regularisations
  L.nc = size(L.bfct(7,bfp1,[],o,z      ),1); % #inequality constraints
 else
  L.fPg = []; L.fTg = []; L.TDg = ones(L.nD,0); L.ng = 0; L.nq = 0; L.nc = 0;
 end
 assert(L.ng > 0,'MEQ requires that the number of basis functions be strictly positive');
 
 %% linear interpolation indices for q95
 rho95 = sqrt(0.95);
 ii = find(L.pQ>=rho95,1,'first');
 if ~isempty(ii) && ii>2
  L.i95  = [ii-1,ii];
  L.c95  = 1-[rho95-L.pQ(ii-1), L.pQ(ii)-rho95]/diff(L.pQ(L.i95));
 else
  error(msgid,'can not compute q95 for this pq mesh')
 end
 
 %% Matrix for preparing interpolation
 if ~isempty(P.infct)
  % e.g. infct = qi5p.m becomes cfun=qi5pc.m
  cfun = str2func([strrep(func2str(P.infct),'mex',''),'c']);
  L.inM = cfun(P.inp,L.drx,L.dzx);
  L.nn = numel(P.rn);
 else
  L.nn = 0;
 end
 
 %% FS integrals
 if P.iterq || (isfield(P,'itert') && P.itert)
  % pre-calculate metrics for flux surface integrals
  % 1. rho-derivative
  [L.M1q,taup] = csdec(L.pQ,L.pQ,'w'); % cubic spline with dy/dp(0)=0 and not-a-knot bc at 1
  L.M1q = bspsum(taup,L.M1q,L.pq,1)./L.pq(:); % dy/dp/p
  %    The next line removes the need to have a point at pq=0 by assuming y(0)=0
  L.M1q = L.M1q(:,2:end).'; % y*L.M1q = dy/dp/p assuming y(0)=dy/dp(0)=0
  % 2. theta-derivative
  [L.M2q,tauo] = csdec([L.oq;2*pi],[L.oq;2*pi],'p'); % cubic spline with periodic bc
  L.M2q = bspsum(tauo,L.M2q,L.oq,1) / 2; % dy/do/2
  %    The next 2 lines remove the need to have a point at oq=2*pi by assuming y(2*pi)=y(0)
  L.M2q(:,1) = L.M2q(:,1) + L.M2q(:,end);
  L.M2q = L.M2q(:,1:end-1); % L.M2q*y = dy/do/2 assuming y 2pi periodic
 end
 
 %% Gaps
 if G.nW
  L.erW  = (G.rW-G.rx(1))*L.idrx;
  L.ezW  = (G.zW-G.zx(1))*L.idzx;
  L.cdrW = -cos(G.oW)*L.idrx;
  L.cdzW =  sin(G.oW)*L.idzx;
  % Flux interpolation at origin of gaps (similar to limiter interpolation)
  [L.kxW,L.cWx] = bintc(G.rW,G.zW,G.rx,G.zx);
 end
L.nW = G.nW*P.nFW;

 %% Conductors
 L.ne  = G.na + G.nu;

 %% Treatment of external currents inside the computational grid
 switch L.P.gsxe
  case 1 % Plasma external current in computational grid, Iy(kyW) = TWe*Ie(kew)
   rw = [G.rw ; G.rv]; % windings from coils and vessel
   zw = [G.zw ; G.zv];
   
   % windings within computational domain
   kw = find(rw > G.rx(1) & rw < G.rx(end) & ...
             zw > G.zx(1) & zw < G.zx(end));
   ky = sub2ind([L.nzy L.nry],...
     min(max(round((zw(kw)-L.zy(1))/L.dzx)+1,1),L.nzy),...
     min(max(round((rw(kw)-L.ry(1))/L.drx)+1,1),L.nry));
   assert(all(~L.Oly(ky)),'Some external current filaments are within the limiter domain')
   
   T = sparse(ky,kw,ones(size(ky)),L.ny,numel(rw)) * blkdiag(G.Twa, G.Tvu);
   L.kew = find(any(T,1));
   L.kyw = find(any(T,2));
   L.Tww = T(L.kyw,L.kew);
  case 2 % Use mutual for currents in computational grid
   iia = any(G.Mxia);iiu = any(G.Mxiu);
   ioa = any(G.Mboa);iou = any(G.Mbou);
   L.iie  = [iia,iiu];
   L.ioe  = [ioa,iou];
   L.Mxie = [G.Mxia(:,iia),G.Mxiu(:,iiu)];
   L.Mboe = [G.Mboa(:,ioa),G.Mbou(:,iou)];
 end
 
 %% Edge condition
 L.Mbe = [G.Mxa(~L.lxy,:),G.Mxu(~L.lxy,:)];
 
 %% Approximations
 L.smalldia  = isfield(P,'itert') && P.itert;

 %% Post-processing
 L.raN = 1; % default value for rational surface finder
 L.liurtemu = false; % Flag for when liut is used to emulate Simulink/rt runs
 
end

%BFPCK  Check bf parameter validity and assign default values
function bfp = bfpck(bfct,bfp)

  msgid = 'MEQ:ParameterError';

  switch func2str(bfct)
   case 'bfabmex'
     if isempty(bfp), bfp = [1 2]; % default for this bf if not defined
     else,  assert(numel(bfp)==2 && isnumeric(bfp),msgid,'bfp should be a vector with 2 elements for bfct=''bfabmex''');
     end
     assert(bfp(1)  <=   3           ,msgid,'BFCT=BFAB and BFP(1)=%g',bfp(1)   )
     assert(bfp(2)  <=   3           ,msgid,'BFCT=BFAB and BFP(2)=%g',bfp(2)   )
   case 'bfefmex'
     if isempty(bfp), bfp = [1 2]; % default for this bf if not defined
     else, assert(numel(bfp)==2 && isnumeric(bfp),msgid,'bfp should be a vector with 2 elements for bfct=''bfefmex''');
     end
     assert(bfp(1)  <=   4           ,msgid,'BFCT=BFEF and BFP(1)=%g',bfp(1)   )
     assert(bfp(2)  <=   4           ,msgid,'BFCT=BFEF and BFP(2)=%g',bfp(2)   )
   case 'bf3pmex'
     if isempty(bfp)||isequal(bfp,0), bfp = false; % default for this bf if not defined or user sets 0
     else, assert(islogical(bfp) && isscalar(bfp),msgid,'bfp must be scalar logical for bf3pmex')
     end
   case 'bfgenD'
     % Check individual function handles
     assert(all(cellfun(@(x) isa(x,'function_handle') ,bfp(:,1),'UniformOutput',true)),...
       msgid,'If bfct=@bfgenD all elements in the first column of bfp must be function handles');
     % Check individual parameters
     for ii = 1:size(bfp,1)
       bfp{ii,2} = bfpck(bfp{ii,1},bfp{ii,2});
     end
     % Check domain assignment vectors
     assert(all(cellfun(@(x) isequal(size(x),[L.nD,1]),bfp(:,3),'UniformOutput',true)),...
       msgid,'If bfct=@bfgenD all elements in the third column of bfp must be vectors of size [L.nD,1]');
  end
end

