clear

%% 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]
V = 2*pi^2*p.kappa*p.R_0*p.a^2;

%% Define time span, input signal and initial condition
t_span = [0 200]; % time span [s]
P_aux = [zeros(1,400) 20E6*ones(1,401)]; % input power time trace
time = linspace(t_span(1),t_span(end),numel(P_aux)); % time grid
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);


%% Define the linearized system
T_0=1000;        % fill in operating point temperature here
P_0=10^6;        % fill in operating point auxiliary power here
[G,KT,KP]=linearise_model(T_0,P_0,p);


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



%% Design linear controller
s = tf('s');
C=1/s  % define controller
warning('Fill in controller model')



%% Simulate linear controller on nonlinear plant model

% 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 closed loop system which takes the reference temperature T_0 as
% the controller input
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;





%% Design controller based on neutron rate and simulate closed loop system

% Define the operating point
P_neutron0=10^6;        % fill in operating point neutron power here
P_0=10^6;               % fill in operating point auxiliary power here

% design controller
% (based on the transfer function between input power and neutron power)
C=1/s  % define controller
warning('Fill in controller model')

% 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.P_0 = P_0; C_ss.P_neutron = P_neutron0;

% Simulate closed loop system which takes the reference neutron rate
% P_neutron0 as an the controller input
t_span = [0 10]; % 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_neutral_power(t,x,time,C_ss,p),time,[T_init zeros(1,size(A_c,1))]);
T = x(:,1);
z = x(:,2:end);
Salpha = sources(abs(T),0,p);   % reconstruct neutron power, note that the alpha power doesn't depend on the auxiliary power, so I put 0
e = C_ss.P_neutron - 4*Salpha.S_alpha*V; % control error: setpoint is P_neutral
P_aux = (C_ss.C*x(:,2:end)')' + C_ss.D*(e) + C_ss.P_0;