%% RAPTOR tutorial: Control vector parametrization.
% Here we explore how to parametrize the input (control) trajectory with a
% set of parameters. This is useful for optimization purposes, so you can
% parametrize a whole time-trajectory with just a few numbers.

%%
% The actuator trajectories are written as a sum of
% basis functions in time
% 
% $u_i(t) = \sum_{j=1}^{n_{kts}} P_{ij}(t) p_{ij}$
%
% where $i$ is the actuator index and $j$ is the basis function index.
% In the end, collect all $p_{ij}$ together in a single vector.
% 
% We start by using use the same example as tutorial 3 but specify the
% inputs differently.

%% Set up for time-varying H&CD actuators
% This is identical to the set-up of tutorial 3
close all hidden;
clear; run('../RAPTOR_path.m');

[config] = RAPTOR_config; % load default config
config.grid.tgrid = [0:0.001:0.1]; % time grid
config.grid.rhogrid = linspace(0,1,21); % spatial grid
config.debug.iterplot = 0; %10; % plot profiles every N iterations
config.debug.iterdisp = 5; % display progress every N iterations

% heating & current drive
% set ECH/ECCD
config.echcd.Configuration.n_rdep_external = 2;

% set NBI
config.nbhcd.Configuration.n_units = 1;

% rerun the config file with the new config structure definition
[model,params,init,g,v,U] = build_RAPTOR_model(config);

% heating & current drive parameters
% set ECH/ECCD parameters
params.echcd.active = true;
params.echcd.rdep = [-1 -1]; % -1 to indicate it is commanded extrernally
params.echcd.wdep = [0.3, 0.35]; % deposition width
params.echcd.cd_eff = [0 1]; % CD efficiency

% set NBI parameters
params.nbhcd.active = true;
params.nbhcd.rdep = [0]; % -1 to indicate commanded extrernally
params.nbhcd.wdep = [0.8]; % broad heating
params.nbhcd.cd_eff = [0]; % no current drive
params.nbhcd.uindices = uint32([6]); % index in input vector

%% Define inputs via control vector parametrization:

%%
% Define number of knots in time grid.
params.cvp.tknots = 3; % 3 knots in time (start,middle,end)
%%
% Define a scaling factor for each input such that the numbers are "human".
params.cvp.pscale = [1e6 1e6 1e6 1 1 1e6]; % [MA, MW, MW, [0,1], [0.1], MW] scales

%%
% Define the order for the polynomial bases of each vector.
params.cvp.psporder = [1 0 0 1 1 0]; % piecew linear Ip, pw constant Paux, pwl rdep

%% Generate input vector from parameters
% specify parameter vector in order of appearance. Note the 'piecewise
% linear' (usporder=1) require 3 numbers, while 'piecewise constant'
% (usporder=0) require 2 numbers. In general $np_i$ = tknots+usporder-1
p0 = [ [0.08,0.25,0.25], ... % Ip ramp to 250kA
    [0.0  0.5],... % Power step to 0.5MW
    [0.2  0.5],... % Power step to 0.5MW
    [0 0.4 0] ,... % Position sweep from 0 to 0.4 and back
    [0.2 0 0.2],...% Position sweep from 0.2 to 0 and back
    [0,1]]'; % 1MW NBI power.
[U,dU_dp,dU_dt] = RAPTOR_cvp(p0,params);

% inspect the results
plot(params.tgrid,diag(1./params.cvp.pscale)*U); 
legend('Ip [MA]','P_{EC1} [MW]','P_{EC2} [MW]','r_{dep,EC1}','r_{dep,EC2}','P_{NBI} [MW]')

%%
% initial condition
init.Ip0 = U(1,1);
x0 = RAPTOR_initial_conditions(model,init,g(:,1),v(:,1));
% Run RAPTOR
simres = RAPTOR_predictive(x0,g,v,U,model,params);

%% 
% *plot results*
out = RAPTOR_out(simres,model,params);
subplot(321); plot(out.time,out.U(1,:)); title('Ip');
subplot(322); plot(out.time,out.U([2:3,6],:)); title('P_{aux}')
subplot(323); plot(out.time,out.U([4:5],:)); title('\rho_{dep}')
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');

%% Set different time knots for each grid
params.cvp.psporder = [1 1 1 1 1 1]; % all piecewise linear

tknots1 = linspace(params.tgrid(1),params.tgrid(end),3)';
tknots2 = linspace(params.tgrid(1),params.tgrid(end),4)';
tknots3 = params.tgrid(1) + [0,0.45,0.5,1]'*params.tgrid(end);
params.cvp.tknots = {tknots1,tknots2,tknots2,tknots2,tknots2,tknots3}; % define as cell array, one cell for each actuator

p0 = [ [0.08,0.25,0.25], ... % Ip ramp to 250kA
    [0.0  0.2  0.2 0.5],... % Power step to 0.5MW
    [0.2  0.2  0.5 0.5],... % Power step to 0.5MW
    [0 0.4 0.4 0] ,... % Position sweep from 0 to 0.4 and back
    [0.2 0 0 0.2],...% Position sweep from 0.2 to 0 and back
    [0,0,1,1]]'; % 1MW NBI power.
[U,dU_dp,dU_dt] = RAPTOR_cvp(p0,params);

% *plot results*
figure
subplot(411); plot(params.tgrid,U(1,:));     title('Ip');
subplot(412); plot(params.tgrid,U([2:3],:)); title('P_{aux}')
subplot(413); plot(params.tgrid,U(6,:));     title('P_{aux}')
subplot(414); plot(params.tgrid,U([4:5],:)); title('\rho_{dep}')
xlabel('t[s]');

% end of tutorial