%Transformed Input MVs
%Two tanks in series example

%Cascade Control of w (T1)

warning off
clc 
clear all
close all

set(0,'DefaultTextFontName','Times',...
'DefaultTextFontSize',14,...
'DefaultAxesFontName','Times',...
'DefaultAxesFontSize',14,...
'DefaultLineLineWidth',1,...
'DefaultLineMarkerSize',7.75,...
'DefaultStairLineWidth',2)

%% 1. Initialisation

%Model Parameters

A_1 = 4;                                               %Area of tank 1 [m2]
A_2 = 6;                                               %Area of tank 2 [m2]
c_p = 4.2;                      %specific heat capacity of fluid [kJ/(kgK)]
rho = 1000;                                       %density of fluid [kg/m3]
g = 9.81*3600;                        %gravitational acceleration [m/min^2]
F_1nom = 1;                            %Nominal flow out of tank 1 [m3/min]
h_1nom = 1;                                   %Nominal height of tank 1 [m]
F_2nom =1.5;                           %Nominal flow out of tank 2 [m3/min]
h_2nom = 0.5;                                 %Nominal height of tank 2 [m]

p = [A_1;A_2;c_p;rho;g];   %vertical vector with model parameters

T_0nom = 70;           %nominal value temperature of inlet to tank 1 [degC]
F_0nom = 1;                 %nominal value flow of inlet to tank 1 [m3/min]
F_dnom = 0.5;         %nominal value flow of disturbance to tank 2 [m3/min]
T_dnom = 40;     %nominal value temperature of disturbance to tank 2 [degC]
T_1nom = 70;
T_2nom = 60;

res_t_1_nom = (A_1*h_1nom)/(F_0nom);        %residence time of tank 1 [min]
res_t_2_nom = (A_2*h_2nom)/(F_1nom+F_dnom); %residence time of tank 2 [min]

A = -1/res_t_2_nom;  %Parameter used for transformed input (=df/dy*)[min-1]

v_0 = (F_1nom*T_1nom-F_1nom*T_2nom+F_dnom*T_dnom-F_dnom*T_2nom-A*A_2*...
    h_2nom*T_2nom)/(A_2*h_2nom);               %nominal v value[degC min-1]


%Number of variables and model parameters
Nx = 4;                                                     %number of ODEs
Nu = 1;                                                      %number of MVs
Nd = 3;                                             %number of disturbances
Np = numel(p);                                  %number of input parameters


%Initial Conditions for states (x)

T1_0 = 70;                            %initial temperature of tank 1 [degC]
T2_0 = 60;                            %initial temperature of tank 2 [degC]
h1_0 = 1;                                      %initial level of tank 1 [m]
h2_0 = 0.5;                                    %initial level of tank 2 [m]
x0=[T1_0; T2_0; h1_0; h2_0];             %vertical vector of initial states


%% 2. Controller Parameters

%Main Controller (2) (y error to v signal)

k_2 = -1/A;                                              %gain of y(s)/v(s) 
tauc_2 = 6.5;                                              %tuning parameter
theta_2 = 0;                                                         %delay
tau_2= 4.9;                                    %time constant of y(s)/v(s)

Kc_2 = (1/k_2)*(tau_2/(tauc_2+theta_2));         %Main controller Kc (SIMC)
tauI_2 = min(tau_2,4*(tauc_2+theta_2));       %Main controller tau_I (SIMC)

%Slave Controller (1) (X2 error to u signal)

k_1 = 1;                                                %gain of x2(s)/u(s)
tauc_1 = 3;                                               %tuning parameter
theta_1 = 0;                                                         %delay
tau_1= res_t_1_nom;                            %time constant of x2(s)/u(s)

Kc_1 = (1/k_1)*(tau_1/(tauc_1+theta_1));        %Slave controller Kc (SIMC)
tauI_1 = min(tau_1,4*(tauc_1+theta_1));      %Slave controller tau_I (SIMC)

%Level 1 Controller

k_L1 = 1/A_1;
theta_L1 = 0;
tau_cL1= 0.5; %res_t_1_nom/2;
Kc_L1 = 1/k_L1 * 1/(tau_cL1+theta_L1);

%Level 2 Controller

k_L2= 1/A_2;
theta_L2 = 0;
tau_cL2= 1;  %res_t_2_nom/2;
Kc_L2= 1/k_L2*1/(tau_cL2+theta_L2);

%% 3. Simulation

F_0tstep = 50;
F_dtstep = 30;
T_dtstep = 10;

F_0step = 0.2;
F_dstep = 0.2;
T_dstep = 4;

y_sp = 60;                             %nominal set-point for u (T2) [degC]
y_sp_t= 70;                                 %Time of set-point change [min]
y_newsp= 65;                                          %New set-point [degC]

h1_sp= 1;                                     %nominal set-point for h1 [m]
h1_sp_t= 10;                                %time of set-point change [min]
h1_newsp= 1;                                             %new set-point [m]

h2_sp= 0.5;                                    %nominal set-point for h2[m]
h2_sp_t= 10;                                %time of set-point change [min]
h2_newsp= 0.5;                                           %new set-point [m]

tsim = 100;                                          %simulation time [min]


D_gain = 1;

sim('Model_GLT.slx');                               %execute simulation

%% 4. Plots

T1=ans.out(:,1);
T2=ans.out(:,2);
h1 = ans.out(:,3);
h2 = ans.out(:,4);
T0 = ans.u(:,1);
u=ans.u;
ysp=ans.ysp;
t=ans.t;
iae_n = numel(ans.iae);
iae = ans.iae(iae_n)


plot(t,T2,'blue')
xlabel('$t$[min]','Interpreter','latex')
ylabel('$T$ [\textsuperscript{o}C]','Interpreter','latex')
ylim([50, 90])
set(gca,'TickLabelInterpreter','latex')
hold on
plot(t,ysp,'k--')
plot(t,T0,'c')
lgd=legend('$T_2 \; (y)$','$T_{2sp} \;(y_{sp})$','$T_0 (u)$',...
    'Location','northwest','Interpreter','latex');
lgd.NumColumns=1;


% filename = 'general';
% save(filename,'T0','T1','T2','u','h1','h2','ysp','t')