%% RAPTOR tutorial : Local linearization
% In this tutorial we first simulate the time-evolution of the plasma
% ramp-up using a RAPTOR predictive run. From this, we extract a local linear 
% model describing the (linear) response of the state to a small input
% perturbation, and compare this response to the nonlinear response of
% the original model.

%% Nominal (non-perturbed) ramp-up simulation
close all hidden;
clear; run('../RAPTOR_path.m');

[config] = RAPTOR_config; % load default config

config.echcd = RAPTORmodule('echcd_gaussian'); % add ECHCD
config.echcd.Configuration.n_units = 1;
config.echcd.Configuration.n_rdep_external = 0;

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

% Other modifications
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 = 10; % display progress every N iterations

% Create RAPTOR model with new parameters
[model,params,init,g,v,U] = build_RAPTOR_model(config);

params.echcd.active = true;
params.echcd.rdep = [0.4]; % one ECCD source at rho=0.4;
params.echcd.wdep = [0.35];  % wdep =0.35 
params.echcd.cd_eff = [2];  % double current drive efficiency to see effect better.

% inputs
% plasma current
U(1,:)= 200e3*ones(size(params.tgrid)); % input Ip trace: ramp from 80kA to 200kA
U(1,params.tgrid<0.02) = linspace(80e3,200e3,sum(params.tgrid<0.02));
% EC actuator power: off-axis current drive, 1MW start at 40ms
U(2,:) = zeros(size(params.tgrid)); U(2,params.tgrid>0.04) = 1e6;

% initial conditions
% Plasma current
init.Ip0 = U(1,1);
% Initial condition for state
x0 = RAPTOR_initial_conditions(model,init,g(:,1),v(:,1)); 
% run RAPTOR
simres = RAPTOR_predictive(x0,g,v,U,model,params);
% get outputs
out = RAPTOR_out(simres,model,params);

%% Generate discrete-time, linear-time-varying state-space model
% To generate a linear-time varying state space model, we linearize the
% nonlinear state equation around the nominal trajectory $(x_k^o,u_k^o)$
%
% $0 = f(x^o_{k},x^o_{k-1},u^o_k) + \frac{\partial f}{\partial x_{k}}\delta x_{k} + \frac{\partial f}{\partial x_{k-1}}\delta x_{k-1} + \frac{\partial f}{\partial u_{k}}\delta u$
%
% or
%
% $x_{k} = A_kx_{k-1} + B_ku_k$ 
%
% with 
% 
% $A_k = (\frac{\partial f}{\partial x_{k}})^{\textrm{--}1}\frac{\partial f}{\partial x_{k-1}}$ and 
% $B_k = (\frac{\partial f}{\partial x_{k}})^{\textrm{--}1}\frac{\partial f}{\partial u_{k}}$

nt = numel(params.tgrid);
Ad = cell(1,nt); Bd = Ad; % allocate empty cells
for ik = 1:nt
    % apply scalings for numerical conditioning of inverse (same scalings
    % as in RAPTOR_predictive_step.m
   iA = out.df_dxk{ik};
   Ad{ik} = -iA \ out.df_dxkm1{ik};
   Bd{ik} = -iA \ out.df_du{ik};
end
Cd = eye(size(Ad{1})); % all states as outputs
Dd = 0; % no direct feedthrough

%% Check response of lin vs. nonlin system: perturb ECCD power 

% Perturb input
Upert = U; % copy old U
Upert(1,:) = U(1,:)+1e4*randn(size(U(2,:))); % perturb Ip with noise
Upert(2,:) = U(2,:)+1e5*rand(size(U(2,:)));  % perturb P_EC with noise

% Plot perturbed and nominal inputs
clf; subplot(211)
plot(params.tgrid,U(1,:)/1e3,'b',params.tgrid,Upert(1,:)/1e3,'k--');
title('I_p'); ylabel('[kA]'); xlabel('time [s]')

subplot(212);
plot(params.tgrid,U(2,:)/1e6,'b',params.tgrid,Upert(2,:)/1e6,'k--');
title('ECCD at \rho=0.4'); ylabel('[MW]'); xlabel('time [s]')
legend('nominal','perturbed','location','southeast');

%% 
% *Perturbed Nonlinear Simulation*

% new nonlinear simulation
simrespert = RAPTOR_predictive(x0,g,v,Upert,model,params);
outpert=RAPTOR_out(simrespert,model,params);

%% 
% *Linear simulation*
%
% This takes the perturbation $\delta u_k = u_k \textrm{--} u_k^o$ and
% returns the state perturbation $\delta x_k = x_k \textrm{--} x_k^o$

% linear model
dU = Upert-U; % delta U
dX = zeros(size(Ad{1},2),nt); % allocate
for ik = 1:(nt-1) % run simulation
    dX(:,ik+1) = Ad{ik}*dX(:,ik) + Bd{ik}*dU(:,ik);
end
Xpertlin = Cd*dX + simres.X; % linear prediction of state (X+\delta X)


% use internal RAPTOR evaluation functions to get the correponding te and iota
iotalin = eval_iota(Xpertlin,simres.G,simres.V,model,false);
telin   = eval_te(Xpertlin,simres.G,simres.V,model,false);
%%
% *Plots comparing linear and nonlinear models*
%

clf; set(gcf,'position',[50 50 700 600])
subplot(211)
contour(out.time,out.rho,out.te/1e3,[0.2,0.5 1,2,3,4],'b'); hold on;
contour(outpert.time,outpert.rho,outpert.te/1e3,[0.2,0.5,1,2,3,4],'k','linewidth',2); 
hold on;
[cs,h] = contour(out.time,out.rho,telin/1e3,[0.2,0.5,1,2,3,4],'r:','linewidth',2);
clabel(cs,h);
title('T_e profile contours'); xlabel('t'); ylabel('\rho');

hleg=legend('nominal','perturbed nonlinear','peturbed linear');
set(hleg,'location','Southeast','orient','hor')

subplot(212)
contour(out.time,out.rho,1./out.iota,[1,6/5,4/3,3/2,2,3,4],'b'); hold on;
contour(outpert.time,outpert.rho,1./outpert.iota,[1,6/5,4/3,3/2,2,3,4],'k','linewidth',2); 
[cs,h] = contour(out.time,out.rho,1./iotalin,[1,6/5,4/3,3/2,2,3,4],'r-','linewidth',1);
clabel(cs,h);
title('q profile contours'); xlabel('t'); ylabel('\rho');

% end of tutorial