function [varargout] = lhhcd_TV(varargin)
%# codegen

%% LHHCD module by T. Vu, status unknown.

%%% Defaut parameters
lhcd_params_default = struct(...
    'active',true,... % activation switch
    'uindices',uint32(2), ... % indices of input vector u for this module
    'check',false,...
    'der',0, ... % activate analytic derivative cal
    'eff',1e-3,...
    'flh',3.7e9,...
    'npar0',2,...
    'width',32*11e-3,...
    'upshift',0,... %zeros(size(model.geom.rho)); %%case '1/q'  'Bpol' 'x^2' 'sqrt(x)' 'null'  'newmodel'  'step@edge'
    'upshiftmode',1 ...
    );
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Input processing

if all(nargin ~= [0,5])
    error('must call with 0 or 5 inputs');
elseif nargin == 0,
    varargout{1} = lhcd_params_default;
    return % empty call, probably to get default structures
end

stap = varargin{1};
geop = varargin{2};
u = varargin{3};
model  = varargin{4};
lhhcd_params = varargin{5};

%% Cal LHCD from METIS with AutoDerivative tool
persistent plh_prev
persistent te_prev %ne_prev
persistent dPlh_dupow jlhprof Plh jlh

% get profiles from structure
nep = stap.ne; %% FF: do you want ne or nep = dne/drho??
dne_dx = stap.dne_dx;
qp = stap.q;
dq_dx = stap.dq_dx;
tep = stap.te; %% FF: do you want te or tep=dte/drho??
dTe_dx = stap.dte_dx;

% init to right size
ngauss = numel(model.rgrid.rhogauss);

nunits = numel(lhhcd_params.uindices);  % number of units
iupow = lhhcd_params.uindices(1:nunits); % indices of u for POWER
plh = u(iupow);
if isempty(plh_prev)
    plh_prev = plh*0;
    te_prev = zeros(ngauss,1);
    %     ne_prev = nep;
    Plh = zeros(ngauss,1);
    jlh = zeros(ngauss,1);
end

dPlh_dx = zeros(ngauss,model.dims.nx);
djlh_dx = zeros(ngauss,model.dims.nx);
dPlh_du = zeros(ngauss,model.dims.nu);
djlh_du = zeros(ngauss,model.dims.nu);
jlhperunit= jlh;

if  sum(plh,1) > 0 && ((max(abs(plh-plh_prev))>1e3) || max(abs(tep-te_prev))>0.5e2)
    
    %%% correct te profile
    % te_prof = interp1(model.rgrid.rho,lhhcd_params.te_prof,model.rgrid.rhogauss,'spline');
    % tep = te_prof*tep(1); % TV: NEW
    
    [dPlh_dupow,jlhprof,Plh,jlh,jlhperunit] = lhcd_AD_TV(qp,tep,nep,geop,plh,model,lhhcd_params);
    
    dPlh_du(:,lhhcd_params.uindices) = dPlh_dupow;%*ones(1,2);
    djlh_du(:,lhhcd_params.uindices) = jlhprof;%*ones(1,2);
    
    dPlh_dx = diag(Plh./qp)*dq_dx + diag(Plh./tep)*dTe_dx + diag(Plh./nep)*dne_dx;
    djlh_dx = diag(jlh./qp)*dq_dx + diag(jlh./tep)*dTe_dx + diag(jlh./nep)*dne_dx;
end

plh_prev = max(0,plh);
te_prev = tep;
% ne_prev = nep;

% %%%%%%%%%%%%%%%% NEW CAL for Derivatives %%%%%%%%%%%%%%%%%%%%%
% if max(plh) > 0 && lhhcd_params.der
% % %     %%%%%%%%%%%%%%%%%%%%
% % %     %%% TV AutoDe
%     xAD = varDer(qp); % Define variable with its derivative
%     [outAD,outAD1] =  lhcd_AD_TV(xAD,tep,nep,g,u,model,lhhcd_params);
%     dPlh_dq = outAD.der;
%     djlh_dq = outAD1.der;
%     dPlh_dx = plh*(dPlh_dq*dq_dx);
%     djlh_dx = plh*(djlh_dq*dq_dx);
%
% %     xAD = varDer(nep);
% %     [outAD,outAD1] =  lhcd_AD_TV(qp,tep,xAD,g,u,model,lhhcd_params);
% %     dPlh_dne = outAD.der;
% %     djlh_dne = outAD1.der;
% % %
% %     xAD = varDer(tep);
% %     [outAD,outAD1] =  lhcd_AD_TV(qp,xAD,nep,g,u,model,lhhcd_params);
% %     dPlh_dTe = outAD.der;
% %     djlh_dTe = outAD1.der;
% %
% %     dPlh_dx = plh*(dPlh_dq*dq_dx + dPlh_dTe*dTe_dx + dPlh_dne*dne_dx);
% %     djlh_dx = plh*(djlh_dq*dq_dx + djlh_dTe*dTe_dx + djlh_dne*dne_dx);
%
% end
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% Output definition
varargout{1} = Plh;
varargout{2} = jlh;
varargout{3} = dPlh_dx;
varargout{4} = djlh_dx;
varargout{5} = dPlh_du;
varargout{6} = djlh_du;
varargout{7} = jlhperunit;

function [dPlh_dupow,jlhprof,Plh,jlh,jlhperunit] = lhcd_AD_TV(qp,tep,nep,geop,plh,model,lhhcd_params)
phys = lhcd_phys;

flh = lhhcd_params.flh;
npar0 = max(1,lhhcd_params.npar0); % security
width = lhhcd_params.width;
agaz = model.atom.Ai;
zgaz = model.atom.Zi;

% upshift = lhhcd_params.upshift;
% if lhhcd_params.upshiftmode
%     upshiftmode = 'newmodel';%lhcd_params.upshiftmode;
% else
%     upshiftmode = 'oldmodel';
% end

xz = model.geom.rhogauss;
Raxe = model.equi.R0;
rmx = 1;
Bt = model.equi.B0;

landau = 6.5./( sqrt(max(30,tep) .*1e-3));%%% Te min > 30
dn0  = phys.c ./flh ./width; % depend on grill
dlhabs = (dn0  + npar0 .* width ./ Raxe) .* (1 + landau .*(1/ npar0)); % delta_npar
% Power profile
Plhprofile_pure = exp(0-(landau - (npar0 + dlhabs .*0.5)) .^ 2  .* (1./dlhabs) .^ 2);

%%
ve = ones(size(xz)); % size vector
% vt = ones(size(xz',1),1);
vt = ones(size(tep',1),1);

%%%%% Complex computation

% champ total
btor      = Bt * ve;
bpol      = sign(vt.* xz).* rmx.* btor./Raxe .*(1./qp);
btot      = sqrt( btor .^2  + bpol.^2);

% calcul du up-shift
% switch upshiftmode
%     case 'newmodel'
upshift   = 0;%zeros(size(qp));

% frequence
w    = 2 .* pi .* flh;
wpe  = sqrt(nep .* (phys.e .^ 2 ./ phys.me ./ phys.epsi0));
wpi  = sqrt(nep .*(1./ (zgaz *ve) .* phys.e .^ 2 ./ phys.mp ./ (agaz * ve) ./ phys.epsi0));
wce  = phys.e ./ phys.me .* btot;

% SDP
S = 1 + wpe .^2.* (1./ wce .^2) - wpi .^ 2.* (1/ w^ 2);
P = 1 - wpe.^2.*(1./w.^2) - wpi.^2.*(1./w^2);

% courbes
denum =  rmx ./ Raxe.* (1./qp)  .* sqrt(max(eps,0 - P.*(1./S)));
lc     = (npar0 + upshift) .* (1./(1 + denum));
hc     = (npar0 + upshift) .* (1./(1 - denum));
hc(hc<lc) = 6.5./sqrt(0.03);
% % % %         indbad  = find((hc <lc) | (hc > (6.5./sqrt(0.03))));
% % % %         hc(indbad) = (6.5./sqrt(0.03));
hc = min(6.5./sqrt(0.03),hc);
% % security
%         hc(find(imag(hc))) = npar0;
%         hc(find(~isfinite(hc))) = npar0;

%%%% TV change for AutoDer
acc    = wpe.* (1./wce) + sqrt(1 + wpe .^ 2 .* (1./wce .^ 2) - wpi .^2 .* (1./w .^2));
% %         acc(imag(acc) ~= 0)    =  1000;  %%% acc is real number
factacc = min(1,exp((landau -  (acc + dn0)).*(1./dn0)));
factlc  = min(1,exp((landau - (lc + dlhabs .* 0.5)) .*(1./dlhabs)));
facthc  = min(1,exp(((hc -dlhabs .* 0.5 ) - landau) .*(1./dlhabs)));

% probability of absorbtion choice 1
Plhprofile = Plhprofile_pure.*  factacc .* factlc .* facthc;
%%% Normalize with gaussian grid
Ptot = int_V_TV(Plhprofile,geop,model,model.rgrid.nrho); % normalize to unit total power
dPlh_dupow = Plhprofile.*(1./Ptot);
Plhperunit = dPlh_dupow.*plh';
Plh = sum(Plhperunit,2); % Power profile

%%%%%%%%%%%%%%%%%%
% debut efficacite
% d'apres les simulations completes ALOHA/C3PO/LUKE
w1     = min(1./ npar0 - eps,(1./landau));
w2     = 1 ./ npar0;
% lineaire /quasi lineaire (cf. publication LUKE)
Dpar   = 0.32 .* ((sum(plh)  ./ 1e6) * ve) ./ max(width,rmx) ./ Raxe .* sqrt(max(0,tep) .*(1./ 1e3)) .* (1./(nep .* 1e-19)) .^ (3/2) .* ...
    (1./sqrt(S)) .*  landau .^ 2 .*(1./dlhabs);
quasi  = 1 + 0.5 .* (1 + tanh(log(max(eps,Dpar)) - log(0.1)));

lnl = 14.9 - 0.5 .* log(nep.*(1./1e20)) + log(max(30,tep).*(1./1e3));
f1 = 31e20.* (1./lnl);
% accessibility
effacc  = min(1,exp((npar0 -  acc(end)).*(1./dn0)));
efficiency = (effacc .* ve)  .* max(0,f1 .* quasi .*( (w2 .^ 2 - w1 .^ 2).* (1./(max(eps,log(w2 .* (1./w1)))))));
efficiency = efficiency .* lhhcd_params.eff;
jlhprof    =  dPlh_dupow .* efficiency .* (1./(max(1,nep)));

%%%% j integrates on V
jlhperunit = jlhprof.*plh';
jlh = sum(jlhperunit,2);

%%%%
%         dPlh_dq = 0; dPlh_dne = 0;
%         dPlh_dTe = (landau - (npar0 + dlhabs .*0.5)).* inv_TV(dlhabs.^2.*tep).*landau.*Plh;

%%%%    Manual calculation
%         ngauss = numel(model.rgrid.rhogauss);
%             bpol_dq = - diag(bpol./qp);
%             btot_dq = diag(1/2./btot.*2.*bpol).*bpol_dq;
%             wce_dq = phys.e/phys.me * btot_dq;
%             S_dq =  diag(-2*wpe .^2 ./ wce .^3).*wce_dq;
% %             yy = (-2*wpe .^2 ./ wce .^3); S_dq = yy(:,ones(1,ngauss)).*wce_dq;
% %             denum_dq =  1/2*denum./(max(eps,- P ./ S)).*P./S.^2;
%             denum_dq = diag(-denum./qp) + diag(-1/2*denum./S) .*S_dq;
% %             denum_dq = - (denum(:,ones(1,ngauss))./qp(:,ones(1,ngauss))).*eye(ngauss) + denum_dq(:,ones(1,ngauss)) .*S_dq;
%
%             lc_dq = bsxfun(@times,-lc./(1 + denum),denum_dq);
%             hc_dq = bsxfun(@times,-hc./(1 - denum),-denum_dq);
% %
%             if (1 + wpe .^ 2 ./ wce .^ 2 - wpi .^2 ./ w .^2) > 0
%                 acc_dq = diag(-wpe ./ wce.^2).*wce_dq + diag(1/2/sqrt(S)).*S_dq;
%             else
%                 acc_dq = zeros(ngauss);
%             end
%
%             idx = (factacc == 1) + 1;
%             factacc_dq = diag(-1./dn0.*((landau -  (acc+ dn0))./dn0).*exp((landau -  (acc+ dn0))./dn0)) .*acc_dq;
%             factacc_dq(idx==2,:) = 0;
%
%             idx = (factlc == 1) + 1;
%             factlc_dq = diag(-1./dlhabs.*((landau - (lc + dlhabs ./ 2)) ./ dlhabs).*exp((landau - (lc + dlhabs ./ 2)) ./ dlhabs)).*lc_dq;
%             factlc_dq(idx==2,:) = 0;
%
%             idx = (facthc == 1) + 1;
%             facthc_dq = diag(-1./dlhabs.*(((hc -dlhabs ./ 2 ) - landau) ./ dlhabs).*exp(((hc -dlhabs ./ 2 ) - landau) ./ dlhabs)).*hc_dq;
%             facthc_dq(idx==2,:) = 0;
%
%             tmp = factacc_dq.*diag(factlc.* facthc)+ diag(factacc.* facthc).*factlc_dq+ diag(factacc.*factlc).* facthc_dq;
%             Plhprofile_dq = Plhprofile_pur(:,ones(1,ngauss)).*tmp;
%             Plh_dq = (Plh./Plhprofile);
%             Plh_dq = Plh_dq(:,ones(1,ngauss)).*Plhprofile_dq;
%
%             Dpar_dq = -1/2*Dpar./S;
%             Dpar_dq = Dpar_dq(:,ones(1,ngauss)).*S_dq;
%             quasi_dq = 1/2./(cosh(log(max(eps,10.*Dpar)))).^2./Dpar; % problem?
%             quasi_dq = quasi_dq(:,ones(1,ngauss)).*Dpar_dq;
%
%             effacc_dq = zeros(ngauss);
%             effacc_dq(end,:) = (effacc ~= 1).*(-1/dn0.*((npar0 -(acc(end)))./dn0).*exp((npar0 - (acc(end)))./dn0)).*(acc_dq(end,:));
%
%             effi_dq = diag(efficiency./effacc).*effacc_dq + diag(efficiency./quasi).*quasi_dq;
%             jlh_dq = diag(jlh./efficiency).* effi_dq + diag(jlh./Plhprofile).*Plhprofile_dq;

%     otherwise
%         upshift   = (upshift * ve)  .* (1 - rmx ./ (rmx(:,end) * ve));
%%%% other calculus by Artaud
% end
return

function [integr] = int_V_TV(quantity,geop,model,varargin)
%%% T. Vu if you read this: why do you need your own int_V instead of the
%%% default one? Please let me know, cheers Federico


%function [integral] = int_V(quantity,g,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

function phys = lhcd_phys
%%% Constants
phys.c           =   2.99792458e8;             % vitesse de la lumiere dans le vide (m/s)  (definition)
phys.h           =   6.62606876e-34;           % constante de Planck (J*s) (+/- 0.0000052e-34)
phys.e           =   1.602176462e-19;          % charge de l'electron (C)   (+/- 0.000000063e-19)
phys.mu0         =   4*pi*1e-7;                % permeabilite du vide (H/m) (definition)
phys.epsi0       =   1./phys.c.^2./phys.mu0;   % permitivite du vide (F/m)  (definition)
phys.g           =   6.673e-11;                % constante de la gravitation (N*m^2/kg^2) (+/- 0.010e-11)
phys.k           =   1.3806503e-23;            % constante de Boltzmann (J/K)  (+/- 0.0000024e-23)
phys.alpha       =   7.297352533e-3 ;          % constante de structure fine (+/- 0.000000027e-3 )
phys.me          =   9.10938188e-31;           % masse au repos de l'electron (kg) (+/- 0.00000079e-31)
phys.mp          =   1.6726485e-27;            % masse au repos du proton (kg)
phys.ua          =   1.66053873e-27;           % 1 unite atomique (kg) (+/- 1.00000013e-27)
phys.avo         =   6.02214199e23;            % nombre d'avogadro (mol^-1) (+/- 0.00000047e23)
phys.sigma       =   5.670400e-8;              % constante de stephan ( W*m^-2*K^-4) (+/- 0.000040e-8)
return