function  [out1] = beta_controller(varargin)
% beta_controller
%


%%% feedback contoller parameters

controller_params = struct(......
    'name', 'PI beta controller', ...
    'Kp', 1e3, ... % proportional feedback gain
    'Ki', 1e3, ...  % integral feedback gain
    't_fb', 0, ... % time of feedback loop closure
    'iterplot', 1, ... % RT-plots of progress every N time steps
    'fname',''... % optional file name to store controller data
);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Input processing

if nargin == 0,
    out1 = controller_params;
    return %empty call, probably to get default structures
elseif nargin==6;
    [stap,geop,Uin,it,model,params] = deal(varargin{:});
    controller_params_in = model.controllers.(mfilename).params;
    controller_params = local_procinput( controller_params, {controller_params_in} ); %convert list to structure, replace default by user-defined values.
else
    error('must call with 0 or 6 inputs');
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Do the actual work
persistent integrator_state
persistent controller_data
persistent hax

ff = 0*200; % feedforward

% evaluate beta for this plasma
ne    = stap.ne;
te    = stap.te;
ni    = stap.ni;
ti    = stap.ti;

Volume = int_Vtot(ones(size(model.rgrid.rhogauss)),geop,model);
Wth = int_Vtot(3/2*1.6e-19*(te.*ne+ ti.*ni),geop,model); % total plasma thermal energy
pav = 2/3*Wth ./ Volume; % volume averaged pressure
beta = pav/(model.equi.B0.^2 / (2*4e-7*pi))*100; % percent

%%
% error
reference = Uin(:,it) * 100; % target this beta 
err = reference-beta; % error

if params.tgrid(it) >= controller_params.t_fb
    act = 1; % feedback activation switch
else
    act = 0; 
end
nt = numel(params.tgrid);
if it == 1 % init
    integrator_state = 0;
    controller_data = zeros(6,nt);
    if controller_params.iterplot>0 && ~any(ishandle(hax))
        clf;
        hax=multiaxes(1,2);
    end
else
    integrator_state = integrator_state+act*err*diff(params.tgrid(it-1:it)); % avoid integrator build up if act=0; (off)
end

fb = err*controller_params.Kp +...
    controller_params.Ki*integrator_state;

P_aux = act*fb+ff;

% avoid asking for negative power 
P_aux = max(0,P_aux);

controller_data(:,it) = [reference,beta,err,fb,ff,P_aux]';
%disp(controller_data(:,it))

if controller_params.iterplot>0 && (rem(it,controller_params.iterplot)==0)
    plot(hax(1),params.tgrid(1:it),controller_data(1:3,1:it)' );
    plot(hax(2),params.tgrid(1:it),controller_data(4:6,1:it)' );
    set(hax,'xlim',[params.tgrid(1),params.tgrid(end)]);
    legend(hax(1),'ref','beta','error')
    legend(hax(2),'fb','ff','total');
    ylabel(hax(1),'\beta [%]');
    ylabel(hax(2),'P_{aux} [W]');
    xlabel(hax(2),'t [s]');
    drawnow;
end

if it==nt % store data to file
    if ~isempty(controller_params.fname)
        fname = fullfile(controller_params.fname);
        save(fname,'controller_data','controller_params');
        disp(['saved controller data to ',fname]);
    end
end

out1 = P_aux;
