%% Exercise 5.2 - Control of plasma $\beta$
clear
close all

%%
% Define constants and wrap in struct
p.n = 1E20; % density [#/m^3]
p.R_0 = 8;
p.a = 2;
p.kappa = 2;
p.B_0 = 7;
p.A = 2;
p.Z_eff = 1.5;
p.f_DT = 0;
p.I_p = 15; % plasma current [MA]

% Draw plot of dT/dt vs T
T_plot = linspace(1e3,10e3,1001);
dTdt = thermal_energy_balance(0,T_plot,[0 1],[25E6 25E6],p);
figure; 
plot(T_plot,dTdt); grid on
title('P_{aux} = 25 MW')
xlabel('Temperature [eV]'); ylabel('time derivative of temperature [eV/s]')

%%
% Define the operating point
P_0 = 25E6; % [W]
T_0 = 2763; % [eV] read out from phase plot at P_aux = 25MW
T = T_0;
P_aux = P_0;


%%
% Define the linearized system
[G,KT,KP]=linearise_model(T_0,P_0,p);


%%
% Design linear controller
close all

% not per se the optimal controller design, but fulfills requirements
s = tf('s');
k=3500;
Ti=10;
C=k*(1+s*Ti)/s; % PI feedback block

% compute some transfer functions
OL = G*C;
S = 1/(1+OL);
DR = G/(1+OL);  % disturbance rejection
CL=feedback(OL,tf(1));    % CL feedback system
figure
subplot(411)
bode(OL,{1E-2,1E2}); grid on
title('Open loop Bode diagram')
subplot(412)
bode(S,{1E-2,1E2}); grid on
title('Sensitivity Bode diagram')
subplot(413)
bode(DR,{1E-2,1E2}); grid on
title('Disturbance rejection Bode diagram')
subplot(414)
bode(CL,{1E-2,1E2}); grid on
title('Closed loop Bode diagram')
display(['dcgain equals ' num2str(dcgain(CL))])

% Convert controller to state space form for simulation
[num,den] = tfdata(C);
[A_c,B_c,C_c,D_c] = tf2ss(cell2mat(num),cell2mat(den));
C_ss.A = A_c; C_ss.B = B_c; C_ss.C = C_c; C_ss.D = D_c;
C_ss.T_0 = T_0; C_ss.P_0 = P_0;

%%
% Simulate linear controller on nonlinear plant model
t_span = [0 30]; % time span [s]
time = linspace(t_span(1),t_span(end),500); % time grid for input power
T_init = 0.5*T_0;
[~,x] = ode45(@(t,x) CL_thermal_energy_balance(t,x,time,C_ss,p),time,[T_init zeros(1,size(A_c,1))]);
T = x(:,1);
z = x(:,2);
e = C_ss.T_0 - T; % control error: setpoint is T_0
P_aux = (C_ss.C*x(:,2:end)' + C_ss.D*e')' + C_ss.P_0;

%%
% Plot temperature and controller state
fig2 = figure;
subplot(311)
hold on
plot(time,T/1E3); 
plot([time(1) time(end)],[T_0 T_0]/1E3,'b--'); grid on
grid on; xlabel('Time [s]'); ylabel('Temperature [keV]')
subplot(312)
plot(time,e); 
grid on; xlabel('Time [s]'); ylabel('Control error [eV]')
subplot(313)
plot(time,P_aux/1E6); 
grid on; xlabel('Time [s]'); ylabel('Total aux power [MW]')
set(fig2,'PaperPositionMode','auto');
print -depsc exercise_5_2_simulation