%% Master project, continuation of my Specialization project (simulation of three tanks in series)
% Student: D. Alexander L. Skaug, Main supervisor: Sigurd Skogestad,
%                                 co-supervisor: Cristina Zotica 
clear; clc; close all

% System specifications
rho = 1000; % kg/m^3
A1 = 5; A2 = 5; A3 = 5;
% Parameter sent to function SysThreeTanks in Simulink
p = [rho, A1, A2, A3];

%% Nominal point
% Nominal height of tanks is used in the integrator after the MATLAB
% function block with function "SysThreeTanks" in Simulink.
z0_nom = 0.4;
z1_nom = 0.5;
z2_nom = 0.7;
z3_nom = 0.6;
H1_nom = 1;
H2_nom = 1.2;
H3_nom = 0.7;

h1nom = [1 ];
h1_steptime = [0 ];
h2nom = [1.2 ];
h2_steptime = [0 ];
h3nom = [0.7 ] ;
h3_steptime = [0 ];

%% Limts for figures
lim_levels = [0. 100]; % Levels [%]
lim_valves = [0. 1]; % Valve positions [-]

%%
tsim = 600; % Simulation time [min] %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Nominal manual valve position as first value, extend with each step
Z0_Setpoint = [z0_nom ]; %[-], nominal TPM
Z0_steptime = [0 ];
Z1_Setpoint = [1 ];
Z1_steptime = [0 ];
Z2_Setpoint = [1 ];
Z2_steptime = [0 ];
Z3_Setpoint = [1 ];
Z3_steptime = [0 ];

% Internal value v at nominal point when z0 is TPM
v1_0 = 22.2036;
v2_0 = 29.1970;
v3_0 = 32.2665;

% Initial conditions for time delays in algebraic loops
T1_z0 = 1; T1_z1 = 0;
T2_z1 = 1; T2_z2 = 0;
T3_z2 = 1; T3_z3 = 0;


% Disturbances (additional inflows and outflows of the tanks)
F4 = [0 ]; % In tank 1
F4_steptime = [0 ];
F5 = [0 ]; % Out tank 1
F5_steptime = [0 ];
F6 = [0 ]; % In tank 2
F6_steptime = [0 ];
F7 = [0 0.12 0]; % Out tank 2
F7_steptime = [0 20 140];
F8 = [0 ]; % In tank 3
F8_steptime = [0 ];
F9 = [0 ]; % Out tank 3
F9_steptime = [0 ];


%% System bounds
z0max=1; z0min=0; z1max=1; z1min=0; z2max=1; z2min=0; z3max=1; z3min=0;

%% Initial conditions, implemented in integrator for function SysThreeTanks
% in Simulink.
x0 = [H1_nom; H2_nom; H3_nom];

%% Tuning parameters
k_1 =  0.0801;
k_2 = -0.0640;
k_3 =  0.0640;
k_4 = -0.0458;
k_5 =  0.0458;
k_6 = -0.0533;
theta = 0;
% tau1_1 = 62.82; % [min]
% tau1_2 = 64.21; % [min]
% tau1_3 = 49.23; % [min]
% tau1_4 = 31.41; % [min]
% tau1_5 = 79.26; % [min]
% tau1_6 = 36.80; % [min]

tauc_1 = 30;
tauc_2 = 30;
tauc_3 = 30;
tauc_4 = 30;
tauc_5 = 30;
tauc_6 = 30;

Kc_1 = 1/k_1/(tauc_1 + theta);
TauI_1 = 4*(tauc_1 + theta);

Kc_2 = 1/k_2/(tauc_2 + theta);
TauI_2 = 4*(tauc_2 + theta);

Kc_3 = 1/k_3/(tauc_3 + theta);
TauI_3 = 4*(tauc_3 + theta);

Kc_4 = 1/k_4/(tauc_4 + theta);
TauI_4 = 4*(tauc_4 + theta);

Kc_5 = 1/k_5/(tauc_5 + theta);
TauI_5 = 4*(tauc_5 + theta);

Kc_6 = 1/k_6/(tauc_6 + theta);
TauI_6 = 4*(tauc_6 + theta);

% For the common split range controllers
    % Tank 1
    Kc_common_1 = -(100-0)*( 1/(Kc_1) + 1/abs(Kc_2) )^(-1);
    TauI_common_1 = (TauI_1+TauI_2)/2; % [min]

    % Tank 2
    Kc_common_2 = -(100-0)*( 1/(Kc_3) + 1/abs(Kc_4) )^(-1);
    TauI_common_2 = (TauI_3+TauI_4)/2;  % [min]
       
    % Tank 3
    Kc_common_3 = -(100-0)*( 1/(Kc_5) + 1/abs(Kc_6) )^(-1);
    TauI_common_3 = (TauI_5+TauI_6)/2;  % [min]
    
%% Gains to filter out the states for each level controller
K_h1 = [1 0 0]; K_h2 = [0 1 0]; K_h3 = [0 0 1];

%% Bias for level controllers, updated "when necessary"
global bias_v1
bias_v1 = 0;
global v1_time_before_next_update_is_allowed
v1_time_before_next_update_is_allowed = 0;
global bias_v2
bias_v2 = 0;
global v2_time_before_next_update_is_allowed
v2_time_before_next_update_is_allowed = 0;
global bias_v3
bias_v3 = 0;
global v3_time_before_next_update_is_allowed
v3_time_before_next_update_is_allowed = 0;

Delay_update = 0.00001;
%% Simulating
sim('Project_three_tanks_SRC')
% sim('Project_three_tanks_3101')

%% Plotting
set(0,'DefaultTextFontName','Times',...
'DefaultTextFontSize',18,...
'DefaultAxesFontName','Times',...
'DefaultAxesFontSize',18,...
'DefaultLineLineWidth',2,...
'DefaultLineMarkerSize',7.75)

plot_inputs_min_selectors = 0; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Normalizing levels to 0-100 [%]
norm_h1 = 100/2.0;
norm_h2 = 100/2.4;
norm_h3 = 100/1.4;

h1 = h1*norm_h1;
h2 = h2*norm_h2;
h3 = h3*norm_h3;

h1_setpoint = h1_setpoint*norm_h1;
h2_setpoint = h2_setpoint*norm_h2;
h3_setpoint = h3_setpoint*norm_h3;


figure
% subplot(211)
hold on
plot(time, h1,'r')
plot(time, h2,'g')
plot(time, h3,'b')
plot(time, h1_setpoint,'r--')
plot(time, h2_setpoint,'g--')
plot(time, h3_setpoint,'b--')
ylabel('Level [%]'); xlabel('Time [min]'); grid
% title('Tank levels')
lgd1 = legend({'tank 1', 'tank 2', 'tank 3','tank 1 sp','tank 2 sp', ...
    'tank 3 sp'},'Location','eastoutside','Orientation','vertical','FontSize',22);
% legend({'1st tank', '2nd tank', '3rd tank'},'Location','northeast','FontSize',11)
ylim(lim_levels)
set(gcf,'Position',[100 100 750 500]) 
% Adding text

figure
% subplot(212)
hold on
plot(time,z0,'k'); 
plot(time,z1,'r'); 
plot(time,z2,'g'); 
plot(time,z3,'b'); 
ylabel('Valve position [-]'); xlabel('Time [min]'); grid
% title('Valve positions')
ylim(lim_valves)
lgd2 = legend({'Z0','Z1', 'Z2', 'Z3'},'Location','eastoutside','Orientation','vertical','FontSize',22);
set(gcf,'Position',[100 100 750 500]) 
% Adding text

if plot_inputs_min_selectors == 1

% Plotting the valve positions        %%%%%%%%%%%%%%%%%%%%%%%
figure; hold on
title('Z0, inputs to min selector')
plot(time, Z0_man,'r')
plot(time, Z0_C1,'--b')
% plot(time, Z0_minsel,'b')
% legend({'Z0 manual','Z0 tank 1','Z0 min selector'},'Location','northeast','FontSize',10)
legend({'Z0 manual','Z0 tank 1'},'Location','northeast','FontSize',10)
ylabel('Valve position [-]'); xlabel('Time [min]'); grid
%%%%% Adding text


set(gcf,'Position',[100 100 750 500]) 

figure; hold on
title('Z1, inputs to min selector')
plot(time, Z1_man,'r')
plot(time, Z1_C2,'--g')
plot(time, Z1_C3,'.b')
% plot(time, Z1_minsel,'k')
% legend({'Z1 manual','Z1 tank 1','Z1 tank 2','Z1 min selector'},'Location','northeast','FontSize',10)
legend({'Z1 manual','Z1 tank 1','Z1 tank 2'},'Location','northeast','FontSize',10)
ylabel('Valve position [-]'); xlabel('Time [min]'); grid
%%%%% Adding text

set(gcf,'Position',[100 100 750 500]) 

figure; hold on
title('Z2, inputs to min selector')
plot(time, Z2_man,'r')
plot(time, Z2_C4,'--g')
plot(time, Z2_C5,'.b')
% plot(time, z2_minsel,'k')
% legend({'Z2 manual','Z2 tank 2','Z2 tank 3','Z2 min selector'},'Location','northeast','FontSize',10)
legend({'Z2 manual','Z2 tank 2','Z2 tank 3'},'Location','northeast','FontSize',18)
ylabel('Valve position [-]'); xlabel('Time [min]'); grid
%%%%% Adding text

set(gcf,'Position',[100 100 750 500])

figure; hold on
title('Z3, inputs to min selector')
plot(time, Z3_man,'r')
plot(time, Z3_C6,'.g')
% plot(time, Z3_minsel,'b')
% legend({'Z3 manual','Z3 tank 3','Z3 min selector'},'Location','northeast','FontSize',10)
legend({'Z3 manual','Z3 tank 3'},'Location','northeast','FontSize',18)
ylabel('Valve position [-]'); xlabel('Time [min]'); grid
%%%%% Adding text


set(gcf,'Position',[100 100 750 500])

end

figure; hold on; grid; title('Internal value v')
plot(time, v1)
plot(time, v2)
plot(time, v3)
legend('v1','v2','v3')

set(gcf,'Position',[100 100 750 500])
