%% Exercise 6.3 - Control of a burning plasma
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 = 1;
p.I_p = 15; % plasma current [MA]
V = 2*pi^2*p.kappa*p.R_0*p.a^2;


%% Simulate response to input power ramp
% Define time span, input signal and initial condition
t_span = [0 200]; % time span [s]
P_aux = [10E6*ones(1,800) 25E6*ones(1,800) 50E6*ones(1,801)]; % apply 20 MW input power
time = linspace(t_span(1),t_span(end),size(P_aux,2)); % time grid for input power
T_0 = 1E3; % initial temperature [eV]

% Simulate thermal energy balance and compute power density sources and pressure
[~,T] = ode45(@(t,T) thermal_energy_balance(t,T,time,P_aux,p),time,T_0);
S = sources(T,P_aux',p);

% Plot temperature evolution
fig = figure;
subplot(2,2,1)
plot(time,T)
grid on; xlabel('Time [s]'); ylabel('Temperature [eV]');
subplot(2,2,3)
plot(time,S.pressure)
grid on; xlabel('Time [s]'); ylabel('Pressure');
subplot(2,2,[2 4])
hold on
plot(time,S.S_aux,'b')
plot(time,S.S_ohm,'k')
plot(time,S.S_alpha,'m')
plot(time,S.S_rad,'g')
plot(time,S.S_cond,'r')
legend('S_{aux}','S_{ohm}','S_{alpha}','S_{rad}','S_{cond}','Location','Northwest')
grid on; xlabel('Time [s]'); ylabel('Power density [W/m^3]');
set(fig,'PaperPositionMode','auto');
print -depsc exercise_5_1_simulation


%% Linearize around each stationary point

clear T_0
% Define operating point power
P_0 = 25E6; % [W]

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

% These are the equilibrium temperatures, from low to high
T_0{1} = 3038; % [eV] read out from phase plot (P_aux = 25MW)
T_0{2} = 6800; % [eV] read out from phase plot (P_aux = 25MW)
T_0{3} = 1.335E4; % [eV] read out from phase plot (P_aux = 25MW)

% Linearize the output relation at each stationary point
for i = 1:length(T_0)
    [G{i},A{i},B{i},H{i},S]=linearise_model(T_0{i},P_0,p);
    P_neutron{i} = 4*S.S_alpha*V;
end

%% Plot bode plots of the linearized system in each stationary point

figure
subplot(121)
bode(H{1}*G{1},H{2}*G{2},H{3}*G{3},{1E-2,1E2}); grid on
legend('1','2','3')
subplot(122)
pzmap(H{1}*G{1},H{2}*G{2},H{3}*G{3})
legend('1','2','3')

%% Design linear controllers for the system in the vicinity of each stationary point
% As the controller is now to take the the reference neutron power as an
% input signal (instead of the temperature before), the plant model is
% actually dPneutron/dPaux = dPneutron/dT dT/dPaux = H G

close all
clear S

% not per se the optimal controller designs, but fulfill requirements
s = tf('s');
C{1}=0.12*(1+s*10)/s;
C{2}=0.03*(1+s*10)/s; % note that stability has to be ensured here
C{3}=0.02*(1+s*10)/s;

for i = 1:3
    OL{i} = H{i}*G{i}*C{i};
    S{i} = 1/(1+OL{i});
    DR{i} = H{i}*G{i}/S{i};  % disturbance rejection
    CL{i}=feedback(OL{i},tf(1));
end
figure
subplot(411)
bode(OL{1},OL{2},OL{3},{1E-2,1E2}); grid on
title('Open loop Bode diagram')
subplot(412)
bode(S{1},S{2},S{3},{1E-2,1E2}); grid on
title('Sensitivity Bode diagram')
subplot(413)
bode(DR{1},DR{2},DR{3},{1E-2,1E2}); grid on
title('Disturbance rejection Bode diagram')
subplot(414)
bode(CL{1},CL{2},CL{3},{1E-2,1E2}); grid on
title('Closed loop Bode diagram')
legend('1','2','3')
figure
nyquist(OL{1},OL{2},OL{3}); 
legend('1','2','3')
display(['dcgain operating point 1 equals ' num2str(dcgain(CL{1}))])
display(['dcgain operating point 2 equals ' num2str(dcgain(CL{2}))])
display(['dcgain operating point 3 equals ' num2str(dcgain(CL{3}))])


%% Simulate closed-loop system with three controllers close to each stationary point
clear T e z P_aux
for i = 1:3
    [num,den] = tfdata(C{i});
    [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.P_0 = P_0; C_ss.P_neutron = P_neutron{i};
    
    %% Simulate linear controller on nonlinear plant model
    t_span = [0 20]; % time span [s]
    time = linspace(t_span(1),t_span(end),500); % time grid for input power
    T_init = 0.5*T_0{i};
    [~,x] = ode45(@(t,x) CL_neutral_power(t,x,time,C_ss,p),time,[T_init zeros(1,size(A_c,1))]);
    T{i} = x(:,1);
    z{i} = x(:,2:end);
    Salpha = sources(abs(T{i}),0,p);   % reconstruct neutron power, note that the alpha power doesn't depend on the auxiliary power, so I put 0
    e{i} = C_ss.P_neutron - 4*Salpha.S_alpha*V; % control error: setpoint is P_neutral;
    dP_aux{i} = (C_ss.C*x(:,2:end)')' + C_ss.D*(e{i});
    P_aux{i} = dP_aux{i}+C_ss.P_0;
end

%% Plot closed-loop performance
fig=figure;
subplot(311)
hold on
plot(time,T{1}); grid on
plot(time,T{2});
plot(time,T{3});
xlabel('Time [s]'); ylabel('Temperature [eV]')
title(strcat('Closed-loop trajectories for equilibrium point index',num2str(i)));
subplot(312)
hold on
plot(time,e{1}); grid on
plot(time,e{2});
plot(time,e{3});
xlabel('Time [s]'); ylabel('Control error [W]')
subplot(313)
hold on
plot(time,P_aux{1}/1E6);
plot(time,P_aux{2}/1E6);
plot(time,P_aux{3}/1E6);
legend('1','2','3')
grid on; xlabel('Time [s]'); ylabel('Total aux power [MW]')
set(fig,'PaperPositionMode','auto');