%% RZIP vs Free-boundary simulation with controller
% This example shows how to perform a closed-loop simulation
% for magnetic position and current (R,Z,Ip) control.
% As well as control of coil currents orthogonal to those used for R,Z,Ip
% control.
%
% We perform the simulation both with rzp (rigid body model),
% with fge (free boundary model), and with fgel (linearized model of fge)
% and plot the differencess
%
% [+MEQ MatlabEQuilibrium Toolbox+] Swiss Plasma Center EPFL Lausanne 2022. All rights reserved.

% Set some basic parameters
dt = 1e-4; % step time time for simulation
tok = 'ana'; % anamak tokamak
shot = 2;  % anamak shot 2 is elongated
t0 = 0;    % start time
debuglevel = 0; % debugging level - set to 0 for publication

% concatenate parameters, add convergence tolerance and CDE
params = {tok,shot,t0,'izgrid',true,...
  'tol',1e-4,'tolF',1e-4,... % low tolerance both for fbt and fge
  'cde','OhmTor_rigid',...
  'ilim',3,'icsint',true}; % spline interpolation for limiter and axis finding

% Compute FGE and RZP models
disp('Computing FGE linearization')
[L,LX] = fge(params{:});

disp('Computing RZP linearization')
Lz  = rzp(params{:});
Lz  = rzpl(Lz,LX); % linearization for this equilibrium

%% This equilibrium is unstable, look at the eigenmode
figure(1); 
subplot(121);fgeploteig(L) ; title(['FGEL',getfield(get(gca,'Title'),'String')])
subplot(122);fgeploteig(Lz); title(['RZP ',getfield(get(gca,'Title'),'String')])
% FGE and RZIP show similar growth rates and eigenmode structures

%% Compute the time-varying Va and Iadot vector to sustain plasma current
% Since we model the resistive plasma current decay, a loop voltage
% is needed to maintain the plasma current.
% This loop voltage is generated by varying the poloidal field currents 
% in time along a particular vector.
% Also a time-varying Va (coil voltage) is needed to 
% sustain these time-varying voltages.
% 
t  = t0+(0:dt:0.1); % time grid
LX = fgex([],t,L,LX);

%% Compute controller parameters
% Use |meqctrl| function that implements a simple R,Z,Ip controller

help meqctrl

debugdesign = debuglevel; % optional debugging of controller design, set to 1 to see bode diagrams

% Inspect meqctrl for details of what this function does.
[~,~,ctrlpar] = meqctrl('setup',[],[],L,LX,'debug',debugdesign,'dt',dt);

%% Check controller closed-loop behaviour
ctrlpar.Kmain = 1;
[cstate,V,ctrlpar,ctdebug] = meqctrl('init',[],ctrlpar);
KKd = ctdebug.KKd;

z=tf('z',dt);
KKd=KKd/z; % add unit delay

% Linear system
Ts=dt; % continuous time
sysd = fgess(Lz,Ts,{'Ia','zIp','rIp','Ip'});
sysd = sysd(:,'Va'); % Va only

syscl=feedback(sysd*KKd,eye(size(sysd,1)));

% Plot transfer function for z,r,Ip closed-loop step-responses
izrp = [sysd.OutputGroup.zIp;sysd.OutputGroup.rIp;sysd.OutputGroup.Ip];
step(syscl(izrp,izrp),0.2)

%% Define targets for closed-loop simulation
ctrlpar.debug = debuglevel;
% perturb Z/R ref
ctrlpar.zQref = timeseries((LX.zIp/LX.Ip(:,1) + interp1([0 0.005 0.01 0.02],+[0 0 0.02 0.02],LX.t,'linear','extrap'))',...
    LX.t);
ctrlpar.rQref = timeseries((LX.rIp/LX.Ip(:,1) + interp1([0 0.02 0.05 0.1],+[0 0.00 0.02 0.02],LX.t,'linear','extrap'))',...
    LX.t);
ctrlpar.IQref = timeseries((LX.Ip + interp1([0 0.06 0.07 0.1],-[0 0.00 20e3 20e3],LX.t,'linear','extrap'))',...
    LX.t);

% perturb bp as a disturbance
LX.bp = LX.bp(1) + interp1([0 0.07 0.09 0.1],[0 0 1 1],LX.t,'linear','extrap');

% Set control function handle and parameter
L.P.ctrlfct  = @meqctrl;
L.P.ctrlpar = ctrlpar;

% show debugging info during sim
L.P.debug=debuglevel;

%% Run FGE simulation
LY = fget(L,LX,'debug',1);

%% Run RZP simulation
Lz.P.ctrlfct = @meqctrl;
Lz.P.ctrlpar = ctrlpar;
Lz.P.debug=0;
LYz = rzpt(Lz,LX);

%% Run FGEL simulation
% just switch L.P.lin to true
Ll=L;
Ll.P.lin = true;
LYl = fget(Ll,LX);

%% Throw an error if simulation diverges
assert(max(abs(LYl.zIp./LYl.Ip - ctrlpar.zQref.Data'))<1e-2,'too large z error');

%% Compare time evolutions
clf;
meqplotevo(L,LY,LYl,LYz,'argplot',{'Ia','Va','Ip','zIp','rIp','Bm','bp','li','qA'});
legend('FGE','FGEL','RZP');
shg

%% Post-simulation movie from FGE simulation
if debuglevel
  %%
  clf;
  meqmovie(L,LY,'decimate',5,'dosave',false)
end