% Function to derive ICRH power
function [varargout] = ichcd_colli(varargin)
%#codegen
%Input processing
if all(nargin ~= [0,6])
    error('must call with 0 or 6 inputs');
elseif nargin == 0,
    %%%% DEFAULT PARAMETERS
    ichcd_params_default = struct(...
        'active',false, ...
        'uindices',uint32(2), ... % indices of input vector u for this module
        'check',false, ...
        'P_lossTau',0, ...  % % ripple loss energy
        'f',55e6, ...   % frequency, small w, big R
        'fz',0.4, ...   % fraction of the vertical chord
        'n',25, ...     % dominant toroidal number
        'p',1,...          % cyclotron harmonic
        'upshift',1,...    % Poloidal upshift effect
        'Nu',1e3,...       % size of u normalized velocity from 0:30
        'Nz',1e2,...       % size of rhogauss
        'Zm',1,...         % charge of minority
        'ZM',1,...         % charge of Majority
        'Am',1,...         % number of mass (proton+neutron) of minority
        'AM',2,...         % number of mass (proton+neutron) of Majority 
        'rate_ni', 0.12, ...% fraction of power to electrons
        'tau_jic', 0.01 ...% fraction of current
        );
    
    varargout{1} = ichcd_params_default;
    return %empty call, probably to get default structures
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
stap = varargin{1};
trap = varargin{2};
u = varargin{4};
model  = varargin{5}; 
ichcd_params = varargin{6}; % distribute inputs

%% init varargout by default
ngauss = numel(model.rgrid.rhogauss);
Pic = zeros(ngauss,1);
dPic_dx = zeros(ngauss,numel(x));
dPic_du = zeros(ngauss,numel(u));
jic = zeros(ngauss,1);
djic_dx = zeros(ngauss,numel(x));
djic_du = zeros(ngauss,numel(u));

if ichcd_params.active &&  u(ichcd_params.uindices) > 1e2% if not active, return zeros
    %% Get some model parameters from RAPTOR observer

q = stap.q;
dq_dx = stap.dq_dx;
te = stap.te;
dte_dx = stap.dte_dx;
ne = stap.ne;
dne_dx = stap.dne_dx;
ti = stap.ti;
ni = stap.ni;
    
    %%%% from METIS
%     te = ichcd_params.te_prof;
%     ti = ichcd_params.ti_prof;
%     ne = ichcd_params.ne_prof;
%     ni = ichcd_params.ni_prof;
%     q  = ichcd_params.q_prof;
%     
    
    % Call function icrh
    U = u(ichcd_params.uindices);
    [jic,Pic] = icrh_TV(q,te,ne,ti,ni,U,model,ichcd_params,geop); % only Pe is considered!
    dPic_dx = (diag(Pic./q)*dq_dx + diag(Pic./te)*dte_dx + diag(Pic./ne)*dne_dx); %
    dPic_du(:,ichcd_params.uindices)= Pic/U; % uindices scalar
    djic_dx = (diag(jic./q)*dq_dx + diag(jic./te)*dte_dx + diag(jic./ne)*dne_dx); %
    djic_du(:,ichcd_params.uindices)= jic/U;
    % %%%%%%%%%%%%%%%% NEW CAL for Derivatives %%%%%%%%%%%%%%%%%%%%%
    % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end

%% Output definition
varargout{1} = Pic;
varargout{2} = jic;
varargout{3} = dPic_dx;
varargout{4} = djic_dx;
varargout{5} = dPic_du;
varargout{6} = djic_du;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%% Define function icrh
function [jic,Pe,PM,pabs] = icrh_TV(q,te,ne0,ti,ni0,U,model,ichcd_params,geop)
%#codegen 
persistent te_prev 
persistent pabs_prev 
persistent Pe_prev 
persistent PM_prev 
persistent U_prev

nrho = length(model.geom.rhogauss);

if isempty(pabs_prev) % first iteration
    te_prev = zeros(nrho,1);
    pabs_prev = zeros(nrho,1);
    Pe_prev = zeros(nrho,1);
    PM_prev = zeros(nrho,1);
    U_prev = 0;
end

if abs(mean(te-te_prev))< 10 || abs(U-U_prev) <1e2
    tau = U/max(1,U_prev); % although U > 6e5 in main file!
    Pe = tau*Pe_prev;
    PM = tau*PM_prev;
    pabs = tau*pabs_prev;
else
U_prev = U;
te_prev = te;

phys = icrh_phys;
ipa = ichcd_params;
R0 = model.equi.R0;
B0 = model.equi.B0;
a  = R0*model.equi.epsilon;
eps1 = model.equi.kappa;

%%% ATTENTION 
% m for minority spicies (heated test ion)
% M for majority species

tie0 = ti*phys.e; % while ti [eV]
tee0 = te*phys.e;
tim0 = tie0; tiM0 = tie0; % Suppose tim = tiM = ti; temperatures are equal
% nM0 = ni0/(1+ipa.rate_ni);  nm0 = ni0-nM0; % Density of minority, majority

nm0 = ni0*ipa.rate_ni;
nM0 = ni0 - nm0;

mi = ipa.Am*phys.mp;
% Given heating Power
P_RF = U; % U(ipa.uindices);
P_loss = ipa.P_lossTau*P_RF;

%% Cal Process %%%%%%%%%%%%%%%%%%%%%
%%%%Thermal velocity % sqrt(2*T/m)
vthi0 =  sqrt(2*tim0/(phys.mp*ipa.Am)); % ion ti(eV) for ion+1
vthiM0 = sqrt(2*tiM0/(phys.mp*ipa.AM));
vthe0 =  sqrt(2*tee0/phys.me);
lnA_ii = 30 - log(ipa.ZM^3*sqrt(ni0)./ti.^(3/2));
G_iM = (nM0*ipa.Zm^2*ipa.ZM^2.*(phys.e)^4.*lnA_ii)./(4*pi*phys.eps0^2*mi.^2); % 4.68
nu_i = G_iM./vthi0.^3; % 4.63

% if min(nu_i) < 0;
%     return
% end

%% cal pabs
% Using g_ geometry to calculate pabs!!!
Nz = model.rgrid.nrhogauss; %ipa.Nz;  % point nb = 2Nz
r_abs = R0+(-1:1/(Nz-1):1)'*a;

% Determine R, the resonance position;  when neglect the B_poloidal part
% B0 = Bt(R0); Bt(z) ~ 1/R
btor = B0*R0./r_abs;
bpol1 = btor*a/4/R0.*interp1(R0+a*(-1:(1/(nrho-1)):1)',(1./[flipud(q); q(2:end)]),r_abs,'linear'); % TRANG
Bt   = sqrt( btor .^2  + bpol1.^2);
wci = ipa.Zm * phys.e./ mi .* Bt;
w = 2*pi*ipa.f;
% figure; plot(r_abs,[wci, ones(2*Nz-1,1)*w])

%%% Poloidal upshift effect
vthi_Nz = vthi0;% Nz = nrhogauss! interp1(model.geom.rhogauss,vthi0,(r_abs(Nz:end)-R0)/a,'linear','extrap');
if ipa.upshift ==1
    res = ipa.p*wci + ipa.n./r_abs.*[flipud(vthi_Nz(2:end)); vthi_Nz(1:end)]; % (C.4)
else
    res = ipa.p*wci;
end
% Detect bad icrh frequency w
if w > max(res) || w < min(res)
    fprintf('Cant find the Resonance location. Need to change the ICHCD frequency. \n')
    return
end

tmp= abs(res- w); [~,idxabs] = min(tmp); % while w == res
R = r_abs(idxabs);
R_right = (idxabs>Nz);
idx = abs(idxabs-(Nz)); % ti(z)
delR = ipa.n/w*vthi0(idx); % C.5)
delZ = 2*a*eps1*ipa.fz; % C.6

%% Profile of pabs with 2 gaussians
z= (-1:1/(Nz-1):1)'*a*eps1;
r = R0+(-1:1/(Nz-1):1)'*a;
%%% profile for each axis
Pz = exp(-(z).^2/(delZ^2/4)); % 3sigma = delZ central resonance (1/2 Gaussian)
% % Pr = exp(-(r-r(idxabs)).^2/(delR^2/9)); % 3sigma = delR
Pr = exp(-(r-R).^2/((delR)^2/4*4/2)); % 2sigma = delR

% Pmean = (P_RF - P_loss)/(pi^2*R*delR*delZ);% = =A in note
Pmean = 1/(pi^2*R*delR*delZ);% for P= 1W
P2D = Pmean*Pz*Pr'*sqrt(2); % P2D(z,r)
%P_RF - 2*pi*trapz(r,r.*trapz(z,P2D,1)'); % Test

% In pratice, we base on the eq.15: P1D = dP/dV
% integrate from the center R=0 <=> rho=0
Prho = zeros(Nz,1); %%% P(rho)
for i = 1:Nz-1
    intnum = Nz-i:Nz+i;
    Prho(i+1) = 2*pi*trapz(r(intnum),r(intnum).*trapz(z(intnum),P2D(intnum,intnum),1)');
    %     Prho(i+1) = 2*pi*trapz(r(intnum),trapz(z(intnum),P2D(intnum,intnum),1)');
end
%%% Prho(end) ==1
rhoNz = linspace(0,1,Nz)';
Vrho = 2*pi^2*R*a^2*eps1*rhoNz.^2;

P1D = diff(Prho)./diff(Vrho);
P1D = interp1(rhoNz(1:end-1),P1D,rhoNz,'linear','extrap');
% P1D = P1D*0.96; % adjust from METIS!!!
% figure(1); plot(model.geom.rhogauss,[ipa.picrh pabs],rhoNz,P1D,'Linewidth',2)
% 1 - 4*pi^2*R*a^2*eps1*trapz(model.geom.rhogauss,model.geom.rhogauss.*pabs)

Ptot = int_V_TV(P1D,geop,model,model.rgrid.nrho); % normalize to unit total power
pabs = P1D.*(1./Ptot).* abs(P_RF- P_loss);

%% Cal from 4.65-4.71 with the species are electron and i_M (except i_n which is heated)
rho = R0-(-1)^R_right*model.geom.rhogauss*a; 
%%% u grid for integral
% u0 = (1e-3:1/ipa.Nu:1)*phys.c/vthi(idx); % from 0 to inf
u0 = logspace(-5,log10(phys.c/vthi0(idx)),ipa.Nu); % from 0 to inf
Bt0 = interp1(r_abs,Bt,rho,'linear','extrap');

%%%%%% repmat for all variables
u = repmat(u0,nrho,1); % same u for each value of v_thermal
vthi = repmat(vthi0,1,ipa.Nu);
vthiM = repmat(vthiM0,1,ipa.Nu);
vthe = repmat(vthe0,1,ipa.Nu);
tiM = repmat(tiM0,1,ipa.Nu); 
tim = repmat(tim0,1,ipa.Nu); 
tee = repmat(tee0,1,ipa.Nu);
nM = repmat(nM0,1,ipa.Nu); 
nm = repmat(nm0,1,ipa.Nu); 
ne = repmat(ne0,1,ipa.Nu);
pabs_u = repmat(pabs,1,ipa.Nu);
Bt =  repmat(Bt0,1,ipa.Nu);

%% cal Dw
up = u;
if ipa.p == 1
    % 	Dp = pabs_u/4./(nm.*tim);%  C.15 for i_minor
    % keep the first Bessel
    kpar = ipa.n/(R);
    %     kpar = w/vthe(idx); % from Stix
    npar = kpar * phys.c/w;
    
    %%%%% NEW cal from Stix62 %%%%%%%%%%%%%%%%%%%%%%%
    % %     Pulsation (eq III.3)
    wpm = sqrt(nm.*ipa.Zm^2*phys.e^2/(phys.eps0*ipa.Am*phys.mp));
    wpM = sqrt(nM.*ipa.ZM*phys.e^2/(phys.eps0*ipa.AM*phys.mp));
    %     wpi2 = (wpm.^2+wpM.^2);
    wpe = sqrt(ne.*phys.e^2/(phys.eps0*phys.me));
    
    % Cyclotronique pulsation (eq III.4)
    wci = Bt*ipa.Zm*phys.e/(ipa.Am*phys.mp);
    wcM = Bt*ipa.ZM*phys.e/(ipa.AM*phys.mp);
    wce = -Bt*phys.e/(phys.me);
    
    %     wci = B0*ipa.Zm*phys.e/(ipa.Am*phys.mp);
    %     wcM = B0*ipa.ZM*phys.e/(ipa.AM*phys.mp);
    %     wce = -B0*phys.e/(phys.me);
    
    % Dielectric tensor elements
    e_pert = 1- (wpm.^2./(w.^2-wci.^2)+ wpM.^2./(w.^2-wcM.^2)+ wpe.^2./(w.^2-wce.^2));
    e_xy = wpm.^2.*wci./w./(w.^2-wci.^2) + wpM.^2.*wcM./w./(w.^2-wcM.^2)+ wpe.^2.*wce./w./(w.^2-wce.^2);
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
    RS = e_pert + e_xy;
    LS = e_pert - e_xy;
    SS = 1/2*(RS+LS); % S in Stix
    
    k_bot2 = -w^2/phys.c^2*(npar^2-RS).*(npar^2-LS)./(npar^2-SS); % (C.10)
    k_bot = sqrt(abs(k_bot2)); %%%
    v_bot = u.*vthi; %%
    bess0 = (besselj(ipa.p -1,k_bot.*v_bot./wci));
    bess2 = (besselj(ipa.p +1,k_bot.*v_bot./wci));
    EE = abs((npar^2-LS)./(npar^2-RS));% (C.11)
    EE = EE*0;  % Add EE => Dw is not "good"
    
    int_Dp = up.^3.*abs(bess0 + EE.*bess2).^2.*exp(-up.^2); % C.14
    res_u = repmat((trapz(u0,int_Dp'))',1,ipa.Nu);
    Dp = pabs_u/8./(nm.*tim)./res_u;
    %     Dp1 = pabs_u/4./(nm.*tim); %~ Dp Quite good!!!!
    Dw0 = Dp.*abs(bess0 + EE.*bess2).^2;
    
else %if ipa.p == 2
    % later C.16
end

% mesh(Dw0)

%% Cal alpha, beta, gamma from Dumont's note
% cal u_beta (4.72)
u_iM = vthi./vthiM.*u;  % for Majority ions
u_e = vthi./vthe.*u;    % for electron

% %% cal vi/beta_vi (4.71)
viM_vi = 1; % for beta species ==M
vie_vi = ne./(nM*ipa.ZM^2); %for electron

Fu_M = - viM_vi.*tim./tiM.*Psi_func(u_iM); % 4.58
Fu_e = - vie_vi.*tim./tee.*Psi_func(u_e);
% Fu = Fu_M + Fu_e;

Duu_M = 1./2./u.*(-Fu_M.*tiM./tim);  % 4.57
Duu_e = 1./2./u.*(-Fu_e.*tee./tim);
Duu = Duu_M + Duu_e;

gam_M = 1./(2*u).*viM_vi.*Theta(u_iM); % 4.59
gam_e = 1./(2*u).*vie_vi.*Theta(u_e);
gam_c = gam_M + gam_e;

%%%%% Cal alpha, beta, gamma (C.31)
du_Duu_M  = -1./u.*Duu_M + 1/2./u.*viM_vi.*(vthi./vthiM).*(-2./u_iM.*Psi_func(u_iM)+ 4*exp(-u_iM.^2)/pi^0.5);
du_Duu_e  = -1./u.*Duu_e + 1/2./u.*vie_vi.*(vthi./vthe) .*(-2./u_e .*Psi_func(u_e) + 4*exp(-u_e.^2)/pi^0.5);
du_Duu = du_Duu_M + du_Duu_e;
% du_test =  diff(Duu')./diff(u');
% du_test1 = [du_test',du_test(end,:)'];

alp_M = Fu_M + 1./u.^2.* (2*u.*Duu_M + u.^2.*du_Duu_M); % du(u^2*Duu); % D.31
alp_e = Fu_e +1./u.^2.* (2*u.*Duu_e + u.^2.*du_Duu_e );
alp = alp_M + alp_e;

beta = 2*Duu; beta_M = 2*Duu_M; beta_e = 2*Duu_e;
gam = 4*gam_c; gam_M = 4*gam_M; gam_e = 4*gam_e; 

 %% 2016 Sept 26: Try iteration loop for coefficient \Xi = Plin /pabs; or Dw_up == \Xi* Dw
% %% Cal Fbot C.33 the distribution func
% % suppose u == u_bot = up (perpendicular)
% Dw_up = Dw;
%  clear Xi
 count = 1;
 Xi = ipa.Xi_init *ones(size(pabs));
 pabs_loop = pabs;
 differ = ipa.eps_Xi*max(pabs)+1;
%% Iterative loop to find CF coefficient
while   differ>= ipa.eps_Xi*max(pabs)
    Xi = sqrt(pabs./pabs_loop).* Xi;
    Dw_up = repmat(Xi,1,ipa.Nu).*Dw0;
%     Xi(count) =(max(pabs)/max(pabs_loop)).^Xi(count-1)* Xi(count-1);
%     Dw_up = Dw0.*repmat((pabs./pabs_loop).^Xi(count),1,ipa.Nu);
%     Dw_up = Dw0.*Xi(count);

    fbot = ( -4.*alp.*up + gam + 2*beta + 2*up*2.*du_Duu)./(2*up.*beta + 4*up.*Dw_up);
    Fbot_norm = exp(-int_u(u,fbot)); % C.33 without CF
%     figure(110);semilogy(u0,Fbot_norm)
%     set(gca,'ylim',[1e-10,10])

    CF = nm0./(2*pi.*vthi0.^3.*(trapz(u0,up.*Fbot_norm,2))); %%???/100
%     CF(11:end) = CF(11);

%     du_dF = diff(Fbot')./diff(up');
    du_dF = diff(Fbot_norm')./diff(up');
    duFbot = [du_dF',du_dF(end,:)'];
    pabs_int = up.^2.*Dw_up.*duFbot; 
    pabs_loop = -4*pi*nu_i.*vthi0.^3.*tim0.*CF.*(trapz(u0,pabs_int,2));
    % pabs_loop == plin
    
    figure(500);plot([pabs pabs_loop])
    differ = max(abs(((pabs-pabs_loop))));

    count = count +1;
    if count > 20
        fprintf('count_over for CF')
        break
    end
end
%% %% Cal power profiles
% comp1 = up.^2.*Dw_up.*duFbot;
% comp2 = up.*(alp.*up -gam/4 +beta/2).*Fbot_norm;
% comp2 = up.*Fbot_norm.*(alp.*up -gam/4 - beta/2 - up.*du_Duu) -1/2*up.^2.*beta.*duFbot;
% figure; mesh(comp1); figure; mesh(comp2); %mesh(comp1/4-comp2)
% Fbot = repmat(CF,1,ipa.Nu).*Fbot_norm; % C.33 CF == B0
% semilogy(u0,Fbot)

P_toInt = up.*Fbot_norm.*(alp.*up -gam/4 - beta/2 - up.*du_Duu) -1/2*up.^2.*beta.*duFbot; % No CF
Pcoll   = 4*pi*nu_i.*vthi(:,idx).^3.*tim(:,idx).*CF.*(trapz(u0,abs(P_toInt')))'; % C.39
% plot([Pcoll pabs])
% W_toInt = ipa.Am*phys.mp*vthi.^2/2.*up.*Fbot;
% Wpar   = 2*pi.*vthi(:,idx).^3.*abs(trapz(u0,W_toInt'))'; 

%%% Cal pcoll_e and pcoll_i
PM_toInt = up.*Fbot_norm.*(alp_M.*up-gam_M/4 - beta_M/2 -up.*du_Duu_M)-1/2*up.^2.*beta_M.*duFbot;
Pe_toInt = up.*Fbot_norm.*(alp_e.*up-gam_e/4 - beta_e/2 -up.*du_Duu_e)-1/2*up.^2.*beta_e.*duFbot;

PM = 4*pi*nu_i.*vthi(:,idx).^3.*tim(:,idx) .*CF.*(trapz(u0,abs(PM_toInt')))';
Pe = 4*pi*nu_i.*vthi(:,idx).^3.*tim(:,idx).*CF.*(trapz(u0,abs(Pe_toInt')))';

% Pcoll = PM + Pe;
% jic = (Pm+PM-Pe)*ipa.tau_jic;

% plot([PM Pe Pcoll pabs])
% plot([-Pcoll pabs])

end

Pe_prev = Pe; 
PM_prev = PM; 
pabs_prev = pabs; 
jic = Pe*ichcd_params.tau_jic;
%%% Avoid Pm <0  
% Pe(Pe<0)=0;  

return

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function phys = icrh_phys
% Constants
	phys.c = 2.99792458e8;  % vitesse de la lumiere dans le vide (m/s) 
    phys.mp = 1.67e-27;   % kg %938e6/ phys.c^2;% [eV] % % Deuterium
	phys.me = 9.109e-31;    % 511e3/ phys.c^2;% [eV] % 
	phys.e = 1.6e-19;
    phys.eps0 = 8.854187817e-12;
    phys.mu = 4*pi*1e-7;
return

function out = Psi_func(x) % \Psi (4.69)
	erfprime = 2/sqrt(pi).*exp(-x.^2);
	out = (erf(x) - x.*erfprime)./x.^2;
return

function out = Theta(x) % (4.70)
	erfprime = 2/sqrt(pi).*exp(-x.^2);
	out = 1./x.^2.*((x.^2 -1/2).*erf(x) + x/2.*erfprime);
return

function out = int_u(u,fu)
	out = zeros(size(u));
	x = (u(1,:));
    for i = 2: length(u)
		out(:,i) = trapz(x(1:i),(fu(:,1:i)),2);
    end
return

function [integr] = int_V_TV(quantity,geop,model,varargin)
%function [integral] = int_V(quantity,geop,model,indices)
% integrate quantity over plasma volume
% int(quantity dV/drho) drho
% quantity: quantity on rhogauss
% integr: integral on rhotor
% indices: optional choice of indices, if none given, all are returned

% #codegen

if numel(varargin)>0
    indices = varargin{1};
    assert(all(indices>0),'index must be >1')
    assert(all(indices<=model.rgrid.nrho),'indices can not be larger than nrho')
else
    indices = 1:model.rgrid.nrho; % return all
end
%%
Vpgauss = geop.Vp;
if numel(indices) == 1 && (indices == model.rgrid.nrho)
%     integr = sum(bsxfun(@times,model.rgrid.wgauss.*Vpgauss,quantity));
    integr = sum(quantity.*model.rgrid.wgauss.*Vpgauss,1);
else
%     cumsumall = cumsum(bsxfun(@times,model.rgrid.wgauss.*Vpgauss,quantity));
    cumsumall = cumsum(quantity.*model.rgrid.wgauss.*Vpgauss,1);
    integrall = [0;cumsumall(model.rgrid.ngauss:model.rgrid.ngauss:end)];
    integr = integrall(indices);
end
%%
return

