%Transformed Input MVs
%Two tanks in series example (extension)
%Case 1 - System 2
%Callum Kingstree

warning off
clc 
clear all
close all

set(0,'DefaultTextFontName','Times',...
'DefaultTextFontSize',14,...
'DefaultAxesFontName','Times',...
'DefaultAxesFontSize',14,...
'DefaultLineLineWidth',4,...
'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 = 3.8;                            %Nominal flow out of tank 1 [m3/min]
h_1nom = 5.7;                                   %Nominal height of tank 1 [m]
F_2nom =4;                           %Nominal flow out of tank 2 [m3/min]
h_2nom =0.067;                                 %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 = 0.8;                 %nominal value flow of inlet to tank 1 [m3/min]
F_1dnom = 3; %nominal value flow of disturbance to tank 2 [m3/min]
F_2dnom = 0.2;         %nominal value flow of disturbance to tank 2 [m3/min]

T_1dnom = 70; %nominal value temperature of disturbance to tank 1 [degC]
T_2dnom = 40;     %nominal value temperature of disturbance to tank 2 [degC]
T_1nom = 70;
T_2nom = 68.5;

res_t_1_nom = (A_1*h_1nom)/(F_0nom+F_1dnom);%residence time of tank 1 [min]
res_t_2_nom = (A_2*h_2nom)/(F_1nom+F_2dnom);%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_1dnom*(T_1nom-T_1dnom)+...
       %F_2dnom*T_2dnom-F_2dnom*T_2nom-A*A_2*h_2nom*T_2nom)/(A_2*h_2nom);               %nominal v value[degC min-1]

v_0 = (T_0nom*F_0nom+T_1dnom*F_1dnom+T_2dnom*F_2dnom-T_2nom*F_2nom...
    -A*A_2*h_2nom*T_2nom)/(A_2*h_2nom);

%Number of variables and model parameters
Nx = 4;                                                     %number of ODEs
Nu = 1;                                                      %number of MVs
Nd = 5;                                             %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 = 68.5;                            %initial temperature of tank 2 [degC]
h1_0 = 5.7;                                      %initial level of tank 1 [m]
h2_0 = 0.067;                                    %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 = 20;                                              %tuning parameter
theta_2 = 0;                                                         %delay
tau_2= 5.95;                                    %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)

%Level 1 Controller

k_L1 = 1/A_1;
theta_L1 = 0;
tau_cL1= res_t_1_nom/4;
Kc_L1 = 1/k_L1 * 1/(tau_cL1+theta_L1);

%Level 2 Controller

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

%% 3. Simulation

F_0tstep = 10;
F_1dtstep = 10;
T_1dtstep = 10;
F_2dtstep = 10;
T_2dtstep = 10;

F_0step =  0; %0.1;
F_1dstep =0; % 0.2;
T_1dstep =  0; %2;
F_2dstep = 0.1;
T_2dstep =  0; %2;

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

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

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

tsim = 100;                                          %simulation time [min]

sim('Model_W1');                               %execute simulation

n = numel(ans.iae);
iae = ans.iae(n)

%% 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;
v=ans.v;
ysp=ans.ysp;
t=ans.t;
T1approx_1 = ans.T1_approx;

plot(t,T2,'blue')
xlabel('time[min]')
ylabel('temperature [\circC]')
hold on
plot(t,ysp,'k--')
plot(t,T0,'c')
lgd=legend('y=T_2','ysp=T_2sp','u=T_0','Location','northwest');
lgd.NumColumns=1;

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