%% RAPTOR tutorial: Feedback controllers
% In this tutorial we define feedback controllers for some plasma
% quantities. This means some inputs are not known a-priori, but are
% determined on-line by the controllers. Instead, the inputs represent
% _references_ to the system.
% 
% In this example we will control the central heating power to obtain a
% reference $\beta$. The controller implemented in |beta_controller.m| 
% for this purpose is a standard PI controller. Inspect this file for more
% details, and use it as a template for your own controller.
%
% Since some other actuators (for example Ip) are NOT controlled in
% feedback, we also need to define an "open-loop" controller, that will
% pass the inputs directly to the actuator command.

%% 
% Load and modify default parameters
close all hidden;
clear; run ../RAPTOR_path.m;

[config] = RAPTOR_config;
config.grid.tgrid = [0:0.001:0.1];
config.grid.rhogrid = linspace(0,1,21);
config.debug.iterdisp = 10; % plot every 10;
% actuators
config.echcd = RAPTORmodule('echcd_gaussian'); % use default with 2 EC sources

config.nbhcd = RAPTORmodule('none'); % no NBH

%% Define controllers
% We need two controllers: 
% Controllers are defined very similarly to heating&current drive
% actuators. First, call RAPTOR_contollers with controller function handles
% as inputs:
config.controllers = RAPTOR_controllers({@openloop_controller,@beta_controller});
%% 
% This loads the default parameters for two controllers. We now modify the
% controller parameters.
config.controllers.beta_controller.params.Ki = 2e9; % i gain
config.controllers.beta_controller.params.Kp = 2e6; % p gain
config.controllers.beta_controller.params.t_fb = 0.05;  % switch-on time of feedback controller
config.controllers.beta_controller.params.iterplot = 10;
config.controllers.beta_controller.params.fname = ... % file to store internal controller data.
    fullfile(config.envopts.RAPTOR_path,'personal','beta_controller_data'); 

%% 
% Now define *mapping* between various controllers and actuators
% For each controller, specify a 2x1 cell which contains the indices of
% commanded actuators respectively inputs (u) received by that controller.
% For example:
%%
% Open-loop controller:
% command actuators [1,3] (Ip and ECCD power) in open-loop from inputs [1,2]
map_openloop = {[1 3],[1 2]}; % 
%%
% Beta feedback controller
% command actuator 2 (ECH power) by input 3 (the _reference_ beta)
map_betafb = {2,3};
%% 
% Combine them both into the |controllers.mapping| parameter
config.controllers.mapping = {map_openloop,map_betafb}; 
%% 
% Now inspect the |controllers| structure
disp(config.controllers)

%%
% Prepare the model
[model,params,init,g,v,U] = build_RAPTOR_model(config);

% actuators
params.echcd.active = true;

%%
% recall that this gives two EC sources, one central ECH and one off-axis
% ECCD.
disp(params.echcd.active) 

% note that the EC actuator indices are [2,3] respectively (1=Ip by
% default).

%%

% Define inputs
% The inputs are now defined, following the above controller mapping, as
% |u=[Ip,P_{ECCD},|$\beta_{ref}$|]|

% Ip (feedforward)
U(1,:) = 250e3*ones(size(params.tgrid)); % input Ip trace: ramp from 80kA to 250kA
U(1,params.tgrid<0.02) = linspace(80e3,250e3,sum(params.tgrid<0.02));

% Off-axis power (feedforward)
U(2,:) = 1e6*ones(size(params.tgrid)); % off-axis power
U(2,params.tgrid<0.05) = linspace(0,1e6,sum(params.tgrid<0.05)); % ramp

% Beta reference: 1% (feedback)
U(3,:) = 0.01*ones(size(params.tgrid)); % beta reference (%)

% Initial conditions
init.Ip0 = U(1,1);
x0 = RAPTOR_initial_conditions(model,init,g(:,1),v(:,1));

% Run simulation
simres = RAPTOR_predictive(x0,g,v,U,model,params);

%% Plot results
out = RAPTOR_out(simres,model,params);
clf;
subplot(321); plot(out.time,out.U(1,:)); title('Ip');
subplot(322); plot(out.time,out.U([2:3],:)); title('P_{aux}')
subplot(323); plot(out.time,[out.beta;U(3,:)]); title('\beta and \beta_{ref}')
subplot(324); plot(out.time,out.f_ss); title('f_{ss} (steady stateness)')
subplot(325); [cs,h] = contour(out.time,out.rho,out.q,[1 2 3 4],'color','k'); clabel(cs,h,'labelspacing',72); title('rational q locations');
xlabel('t [s]'); ylabel('\rho');
subplot(326); [cs,h] = contour(out.time,out.rho,out.te/1e3,[0.5 1 2 4 8],'color','k'); clabel(cs,h,'labelspacing',72); title('T_e contours [keV]')
xlabel('t [s]'); ylabel('\rho');

%%
% end of tutorial

