function g = geom2RAPTOR(rhoin,iR,iR2,dpsi,dpsi2,dpsi2oR2,dpsidV,F,epsilon,Rgeom,kappa,delta,Phib,geom)
% function g = geom2RAPTOR(rhoin,iR,iR2,dpsi,dpsi2,dpsi2oR2,dpsidV,F,epsilon,Rgeom,kappa,delta,Phib,geom)
% maps geometric profiles from grid of source code to RAPTOR g vector
% uses spline matrics in geom structure, which is calculated by prep_geom.m

%#codegen

%% init
nsp = size(geom.LamG,2);
nspr = size(geom.LamGr,2);
nbt = size(iR2,2);
nrho = numel(rhoin);

coeff_H = zeros(nsp,nbt);
coeff_gg0 = zeros(nsp,nbt);
coeff_gg1 = zeros(nsp,nbt);
coeff_gg2 = zeros(nsp,nbt);
coeff_gg3 = zeros(nsp,nbt);
coeff_Vp = zeros(nsp,nbt);
coeff_F = zeros(nsp,nbt);
coeff_epsilon = zeros(nspr,nbt);
coeff_delta = zeros(nspr,nbt);
coeff_kappa = zeros(nspr,nbt);
coeff_Rgeom = zeros(nspr,nbt);

g = zeros(geom.ng,nbt);

%% compute quantities to store in the g vector.
% these are carefully chosen to avoid infinity for all cases
irin = 1./rhoin;
H = dpsidV;
gg0 = dpsi; % = <|\grad \psi|>
gg1 = bsxfun(@times,dpsi2,irin); % = <(\grad \psi).^2>/rhotor
gg2 = bsxfun(@times,dpsi2oR2,irin.^2); % = <(\grad \psi).^2/R^2>/rhotor.^2
gg3 = iR2; % = <1/R^2>
k = bsxfun(@times,rhoin,1./(F.*iR2));
Vp = bsxfun(@times,4.*pi*Phib,k); % = dV/drho = 4pi*Phib*rho/(F*<1/R^2>)

diverted = (abs(H(end,:)) < 10*eps); % flag if plasma is diverted, then ggpsV2 = 0

% special cases
if rhoin(1) == 0.;
  gg0(1,:) = 0.;
  gg1(1,:) = 0.;
end

%% Compute spline coefficients
% impose boundary conditions directly via spline coefficients.
% boundary condition at edge depend on whether the plasma is diverted or
% not.

% varying rhoin grid, recompute spline matrix every time.
[S] = splineval(rhoin,geom.knots,geom.ord);
L = S'*geom.Tf; % spline fit matrix

tLpp = geom.tLpp*nrho; % pre-calculated to save time
% multiply by nrho to get same relative tension independent of number of
% data points used for error;
zz = zeros(size(geom.rho)); % we want 2nd derivative to be close to zero

% choose which data points to include
iin = true(nrho,1); % data points to include
if diverted
  iin(end) = false;
end
% ignore internal points
%iin(rhoin<0.1) = false;

% Optionally ignore internal or last points
%iin(rhoin<0.1) = false;%
%iin(rhoin==1) = false;
%% Quantities that go like ~rho near the axis
% cases w special behaviour when diverted

% <|grad psi|> goes like ~rho near axis
ifix = false(nsp,1); % spline coefficients to manually fix
ifix([1,3]) = true; % value and second derivative are zero
if diverted
  %    ifix(end) = true; % end point is zero
end

A = [L(iin,~ifix);tLpp(:,~ifix)];
b = [gg0(iin);zz];
iA = A\eye(size(A,1)); % solve once

coeff_gg0(~ifix,:) = iA*b;
coeff_gg0(ifix,:) = 0; % g1(0),g1'(0) = 0;

% <gradPsi^2> ~ <(Bp/R)^2> goes like rho^2 near axis, hence <(Bp/R)^2>/rho goes like rho
A = [L(iin,~ifix);tLpp(:,~ifix)];
b = [gg1(iin);zz];
iA = A\eye(size(A,1)); % solve once

coeff_gg1(~ifix,:) = iA*b;
coeff_gg1(ifix,:) = 0; % g1(0),g1'(0) = 0;

A = [L(iin,~ifix);tLpp(:,~ifix)];
b = [Vp(iin);zz];
iA = A\eye(size(A,1)); % solve once

coeff_Vp(~ifix,:) = iA*b;
%% quantities with zero first derivative, that go like ~cst near the axis

% special case, this one may also go to 0 at the edge for diverted plasmas
% (dpsi/dV) goes like ~c
ifix = false(nsp,1);
ifix(end) = false;
ifix(2) = true; % zero first derivative at 0
if diverted
  %ifix(end) = true;
end
A = [L(iin,~ifix);tLpp(:,~ifix)];
b = [H(iin);zz];
coeff_H(~ifix,:) = A\b;
coeff_H(ifix,:) = 0;

% <Bp^2> goes like rho^2 near axis, hence <Bp^2>/rho^2 goes like ~cst
ifix = false(nsp,1);
ifix(2) = true; % 1st derivative is 0
if diverted
  %    ifix(end) = true; % end point is zero
end

iin_gg2 = iin; % Need to find the analytic value for the gg2 at the axis. At the moment the value is skipped
iin_gg2(1) = 0;
A = [L(iin_gg2,~ifix);tLpp(:,~ifix)];
b = [gg2(iin_gg2);zz];
coeff_gg2(~ifix,:) = A\b;
coeff_gg2(ifix,:) = 0;


%%% other cases without special limit at rho=edge
% g3 = <1/R^2> goes like c near the axis in a quadratic psi approximation
ifix = false(nsp,1);
ifix(2) = true; % constant
A = [L(iin,~ifix);tLpp(:,~ifix)];
iA = A\eye(size(A,1)); % solve once

b = [gg3(iin);zz];
coeff_gg3(~ifix,:) = iA*b;
coeff_gg3(ifix,:) = 0; % no constraints

% F = RBphi
ifix = false(nsp,1);
ifix(2) = true;
%since dF/drho = dF/dpsi dpsi/drho = 0
b = [F(iin);zz];
coeff_F(~ifix,:) = iA*b;
coeff_F(ifix,:) = 0;


%% plasma shaping quantities, stored on 'reduced' spline basis to save space
% epsilon goes like ~rho near the axis
ifixr = false(nspr,1); ifixr(1) = true; % value and second derivative 0 at 0;
[Sr] = splineval(rhoin,geom.knotsr,geom.ord);
L = Sr'*geom.Tfr; % spline fit matrix
A = L(iin,~ifixr); iA = A\eye(size(A,1));
b = epsilon(iin);
coeff_epsilon(~ifixr,:) = iA*b;
coeff_epsilon(ifixr,:)  = 0;

% delta goes like ~rho near the axis
% re-use same A
b = delta(iin);
coeff_delta(~ifixr,:) = iA*b;
coeff_delta(ifixr,:)  = 0;

% Rgeom goes like ~c near the axis
ifixr = false(nspr,1);  ifixr(2) = true; % constant
A = L(iin,~ifixr); iA = A\eye(size(A,1));
b = Rgeom(iin);
coeff_Rgeom(~ifixr,:) = iA*b;
coeff_Rgeom(ifixr,:)  = 0;

% kappa goes like ~c near axis
b = kappa(iin);
coeff_kappa(~ifixr,:) = iA*b;
coeff_kappa(ifixr,:)  = 0;

%% assign to g
g(geom.ind_H,:)  = coeff_H;
g(geom.ind_gg0,:)     = coeff_gg0;
g(geom.ind_gg1,:)     = coeff_gg1;
g(geom.ind_gg2,:)     = coeff_gg2;
g(geom.ind_gg3,:)     = coeff_gg3;
g(geom.ind_F,:)       = coeff_F;
g(geom.ind_Vp,:)      = coeff_Vp;
g(geom.ind_epsilon,:) = coeff_epsilon;
g(geom.ind_Rgeom,:)   = coeff_Rgeom;
g(geom.ind_kappa,:)   = coeff_kappa;
g(geom.ind_delta,:)   = coeff_delta;
g(geom.ind_Phib,:)    = Phib;


%% check fits
% Plot debug
if geom.debug
  
  % Choose what to display
  disp_G = 0;
  disp_gauss = 1;
  disp_derivative = 1;
  disp_spline_knots = 1;
  
  %% Generate the figure and the subplot handles, if not already present
  [hax] = debug_figure_init;
  ii = 0;
  %%
  rhogauss = geom.rhogauss;
  rho = geom.rho;
  
  % bo input
  % r- interpolated in geometry spline
  % k. evaluation in rhogauss
  % m  derivative if the geometrical splines
  
  ii = ii+1;
  ax = hax(ii);
  cla(ax)
  plot(ax, rhoin,gg1,'bo')
  if disp_G
    hold(ax,'on');
    plot(ax,rho,geom.LamG*coeff_gg1,'r.-');
  end
  if disp_gauss
    hold(ax,'on');
    plot(ax,rhogauss,geom.LamGgauss*coeff_gg1,'k.-');
  end
  if disp_derivative
    hold(ax,'on');
    plot(ax,rho,geom.LamGp*coeff_gg1,'m');
  end
  if disp_spline_knots
    hold(ax, 'on')
    tmp = ylim(ax);
    plot(ax, geom.knots , tmp(1), 'g+')
  end
  xlabel(ax, 'rhotorN')
  title(ax, 'gg1 (\rho_{tor})');
  
  %--------------------------------------------------------
  ii = ii+1;
  ax = hax(ii);
  cla(ax)
  plot(ax, rhoin, gg2, 'bo')
  if disp_G
    hold(ax,'on');
    plot(ax,rho,geom.LamG*coeff_gg2,'r.-');
  end
  if disp_gauss
    hold(ax,'on');
    plot(ax,rhogauss,geom.LamGgauss*coeff_gg2,'k.-');
  end
  if disp_derivative
    hold(ax,'on');
    plot(ax,rho,geom.LamGp*coeff_gg2,'m');
  end
  if disp_spline_knots
    hold(ax, 'on')
    tmp = ylim(ax);
    plot(ax, geom.knots , tmp(1), 'g+')
  end
  xlabel(ax,'rhotorN')
  title(ax, 'gg2 = < (\nabla \psi)^2/R^2>/rhotor^2')
  
  %-----------------------------------------------------------
  ii = ii+1;
  ax = hax(ii);
  cla(ax)
  plot(ax, rhoin, gg3,'bo');     hold(ax,'on');
  if disp_G
    plot(ax, rho,geom.LamG*coeff_gg3,'r.-');
  end
  if disp_gauss
    plot(ax, rhogauss,geom.LamGgauss*coeff_gg3,'k.-');
  end
  if disp_derivative
    plot(ax,rho,geom.LamGp*coeff_gg3,'m');
  end
  if disp_spline_knots
    plot(ax, geom.knots , 0, 'g+')
  end
  xlabel(ax,'rhotorN')
  title(ax, 'gg3 = <1/R^2>')
  
  %----------------------------------------------------
  ii = ii+1;
  ax = hax(ii);
  cla(ax)
  plot(ax, rhoin, H,'bo');     hold(ax,'on');
  
  if disp_G
    plot(ax,rho,geom.LamG*coeff_H,'r.-');
  end
  if disp_gauss
    plot(ax,rhogauss,geom.LamGgauss*coeff_H,'k.-');
  end
  if disp_derivative
    plot(ax,rho,geom.LamGp*coeff_H,'m');
  end
  if disp_spline_knots
    plot(ax, geom.knots , 0, 'g+')
  end
  xlabel(ax,'rhotorN')
  title(ax, 'H = d \psi / dV')
  
  
  %-------------------------------------------------
  ii = ii+1;
  ax = hax(ii);
  cla(ax)
  plot(ax,rhoin, F, 'bo'); hold(ax,'on');
  
  if disp_G
    plot(ax,rho,geom.LamG*coeff_F,'r.-');
  end
  if disp_gauss
    plot(ax,rhogauss,geom.LamGgauss*coeff_F,'k.-');
  end
  if disp_derivative
    plot(ax,rho,geom.LamGp*coeff_F,'m');
  end
  if disp_spline_knots
    plot(ax, geom.knots , 0, 'g+')
  end
  xlabel(ax,'rhotorN')
  title(ax, 'F')
  
  %----------------------------------------------------------
  ii = ii+1;
  ax = hax(ii);
  cla(ax)
  plot(ax,rhoin, Vp, 'bo') ; hold(ax,'on');
  if disp_G
    plot(ax,rho,geom.LamG*coeff_Vp,'r.-');
  end
  if disp_gauss
    plot(ax,rhogauss,geom.LamGgauss*coeff_Vp,'k.-');
  end
  if disp_derivative
    plot(ax,rho,geom.LamGp*coeff_Vp,'m');
  end
  if disp_spline_knots
    plot(ax, geom.knots , 0, 'g+')
  end
  xlabel(ax,'rhotorN')
  title(ax, 'Vp = 4 \pi \Phi_b rhotorN/F 1/g3')
  
  %-----------------------------------------------------------
  ii = ii+1;
  ax = hax(ii);
  cla(ax)
  plot(ax,rhoin, gg0, 'bo'); hold(ax,'on');
  if disp_G
    plot(ax,rho,geom.LamG*coeff_gg0,'r.-');
  end
  if disp_gauss
    plot(ax,rhogauss,geom.LamGgauss*coeff_gg0,'k.-');
  end
  if disp_derivative
    plot(ax,rho,geom.LamGp*coeff_gg0,'m');
  end
  if disp_spline_knots
    plot(ax, geom.knots , 0, 'g+')
  end
  xlabel(ax,'rhotorN')
  title(ax, 'gg0 = <\nabla \psi>')
  
  
  %-----------------------------------------------------
  ii = ii+1;
  ax = hax(ii);
  cla(ax)
  h = zeros(4,1);
  plot(ax,NaN, NaN, 'bo', 'displayname', 'inputs'); hold(ax,'on')
  
  if disp_G
    plot(ax,NaN, NaN, 'r.-', 'displayname', 'rho gid');
  end
  if disp_gauss
    plot(ax,NaN, NaN,  'k.-', 'displayname', 'gauss');
  end
  if disp_derivative
    plot(ax,NaN, NaN, 'm', 'displayname', 'derivative');
  end
  if disp_spline_knots
    plot(ax,NaN, NaN,  'g+', 'displayname', 'Spline Knots');
  end
  legend(ax, 'show');
  drawnow
  
  
  %     subplot(427);
  %     plot(rhoin,epsilon,'bo',rho,geom.LamGr*coeff_epsilon,'r.-',...
  %         rhogauss,geom.LamGrgauss*coeff_epsilon,'k.');
  %     hold on;
  %     plot(rho,geom.LamGpr*coeff_epsilon,'m')
  %     title('epsilon')
  %
  %     subplot(427);
  %     plot(rhoin,Rgeom,'bo',rho,geom.LamGr*coeff_Rgeom,'r.-',...
  %         rhogauss,geom.LamGrgauss*coeff_Rgeom,'k.');
  %     hold on;
  %     plot(rho,geom.LamGpr*coeff_Rgeom,'m')
  %     title('R_{geom}')
  %
  %     subplot(427);
  %     plot(rhoin,delta,'bo',rho,geom.LamGr*coeff_delta,'r.-',...
  %         rhogauss,geom.LamGrgauss*coeff_delta,'k.');
  %     hold on;
  %     plot(rho,geom.LamGpr*coeff_delta,'m')
  %
  %     subplot(427);
  %     plot(rhoin,kappa,'bo',rho,geom.LamGr*coeff_kappa,'r.-',...
  %         rhogauss,geom.LamGrgauss*coeff_kappa,'k.');
  %     hold on;
  %     plot(rho,geom.LamGpr*coeff_kappa,'m')
  %     title('epsilon')
  %
  %     title('\epsilon, \kappa, \delta')
end
end

function [hax] = debug_figure_init
figname = 'geometry debugging';
hf = findobj('Name',figname);
if isempty(hf) || numel(hf)>1 || ~ishandle(hf)
  delete(hf);
  hf = figure('name',figname);
end

hax = findobj(hf,'type','axes');
if isempty(hax) || ~all(ishandle(hax));
  nrow = 4; ncolumn = 2;
  hax = zeros(1,nrow*ncolumn); % init
  for ii=1:nrow*ncolumn
    hax(ii) = subplot(nrow,ncolumn,ii, 'parent', hf);
  end
end
end
