function varargout = Hmode_feedback(varargin)
% #codegen
coder.extrinsic('check_gradients');

% Hmode module

%--------------------------------------------------------------
if nargin == 0,
    %% DEFAULT PARAMETERS
    
    default_params = struct( ...
        'active', false,...          % activation switch
        'rhoped' ,  0.9 ,   ...      % rho of chie reduction
        'Teped'  ,  4e3 ,   ...      % height of desired pedestal
        'KTeped' ,  1   ,   ...      % Strength of Tped control gain
        'chiefact', 1   ,   ...      % scaling factor for overall core chie
        'check'  , false    ...      % optional check of derivatives
        );
    
    varargout{1} = default_params;
    return %empty call, probably to get default structures
elseif nargin==5; % change this depending on number of inputs to module
    stap = varargin{1};
    geop = varargin{2};
    model = varargin{3};
    params = varargin{4}; % distribute inputs
    species = varargin{5}; % choose ti or te
    chi0 = varargin{6};
    dchi0_dx = varargin{7};
else
    error('must call with 0 or 6 inputs');
end

switch species
    case 'e'
        dte_dx = stap.dte_dx;
        te     = stap.te;
    case 'i'
        dte_dx = stap.dti_dx;
        te     = stap.ti;
    otherwise
        error('invalid species')
end

%%
if ~isempty(model.hmode.vind) && stap.lamH > 1e-3; % tolerance for filtered v
    inHmode = true;
    lam = stap.lamH; % H mode indicator (0 to 1)
else
    inHmode = false;
    lam = 0;
end

nosaturate = true; frho = 0; % default init
iped    = model.rgrid.rhogauss>=params.rhoped;

if inHmode
    dTeped_dx = dte_dx(iped,:);
    tepedestal = te(iped); % entire pedestal
    Teped =     tepedestal(1); % at top of pedestal
    
    % proportional feedback on pedestal temperature
    
    % core scaling: scale linearly from 1 to params.chiefact
    hmodefact = ( 1 + lam*(params.chiefact-1)) * ones(model.rgrid.nrhogauss,1);
    
    % reference pedestal temperature: 
    % scale linearly from current Tped to reference Tped
    Teped_ref = Teped * (1-lam) + params.Teped * (lam); %

    % factor used to scale the pedestal by (1-pedfact)
    pedfact0     =  0.9 + params.KTeped*(Teped_ref-Teped)/params.Teped;
    % limit the factor for stability
    pedfact = min(max(0,pedfact0),0.98); % max: maximum reduction, min: minimum 
    if pedfact==pedfact0;
        nosaturate=true;
    else
        nosaturate=false;
    end % flag that the control has not saturated
    
    frho = tanh(1000*(model.rgrid.rhogauss(iped)-params.rhoped));
    hmodefact(iped) =  params.chiefact*(1-(pedfact)*frho);
    
    % pedestal control factor, scale chie to get closer to desired pedestal
%    disp([pedfact, Teped/1e3,params.Teped/1e3]); %% debugging display
%    plot(model.rgrid.rhogauss,hmodefact); drawnow

    chi = chi0.*hmodefact;
else % no Hmode
    chi = chi0;
end

varargout{1} = chi;

%%
if nargout>1
    if inHmode && nosaturate
            dhmodefact_dx  = zeros(numel(model.rgrid.rhogauss),model.dims.nx);
            dpedfact_dTeped = (-lam)*params.KTeped/params.Teped;
            dhmodefact_dTeped = params.chiefact*(-frho.*dpedfact_dTeped);
            dhmodefact_dx(iped,:) = dhmodefact_dTeped*dTeped_dx(1,:);
            
            dchi_dx = bsxfun(@times,Hmodefact,dchi0_dx) + ...
              bsxfun(@times,chi0,dhmodefact_dx);
    else
    dchi_dx = dchi0_dx;
    end
    varargout{2} =   dchi_dx;
end

return
