% Task: controllers with different setpoints for tanks
%       in series with moving bottlneck
% Date: 15.04

clc; clear; close all

%% Initial conditions
q0   = 1;                                      % nominal flowrates [m3/min]
zOP0 = [1;1;1;1];             % initial operator setpoints for the 4 valves
hmax = [2.3; 2.8; 3.2];              % [m] max L=[2.3; 2.8; 3.2]. from h=2d
hH   = 0.90;                                     % level high setpoint = 90%
hL   = 0.1;                                     % level low setpoint = 10 %
d0   = 0;                                                    % disturbances
%% Select TPM

TPM = 2;                                    % select where to place the TPM
switch TPM
     case 0 % TPM = z0
          z0 = [1;   0.8; 0.7; 0.6];                   % operator set flows
          x0 = [hL; hL; hL].*hmax;                     % initial tank level          
     case 1 % TPM = z1
          z0 = [0.8; 1;   0.7; 0.6];                  % operator set valves
          x0 = [hH; hL; hL].*hmax;                     % initial tank level         
     case 2 % TPM = z2
          z0 = [0.8; 0.7; 1; 0.6];                    % operator set valves
          x0 = [hH; hH; hL].*hmax;                     % initial tank level          
     case 3
          z0 = [0.6; 0.7; 0.8; 1];                    % operator set valves
          x0 = [hH; hH; hH].*hmax;                     % initial tank level
     otherwise
          disp('Incorrect TPM')
end

%% Parameters
A     = [1; 1.5; 2];                        % [m3] tanks cross section area
rho   = 1000;                                       % [kg/m3] water density
N     = 3;                                                % number of tanks
DP    = [0.5; 0.6; 0.7; 1]*1e5;                                       %[Pa]
Cv    = q0./(z0.*sqrt(DP/rho));                  % linear valve coefficient
param = [A; Cv; DP; rho; N];% vector of parameters passed to the ode system
kv    = Cv.*sqrt(DP./rho);
%% Controller tuning. Integrating process
tauC = [0.5; 0.5; 0.5; 0.5; 0.5; 0.5]; % [min] closed loop time constant. res time~=[2;4;6]
k    = zeros(6,1);
k(1) =  1/A(1)*Cv(1)*sqrt(DP(1)/rho);                        % k': z0 to h1
k(2) = -1/A(1)*Cv(2)*sqrt(DP(2)/rho);                        % k': z1 to h1
k(3) =  1/A(2)*Cv(2)*sqrt(DP(2)/rho);                        % k': z1 to h2
k(4) = -1/A(2)*Cv(3)*sqrt(DP(3)/rho);                        % k': z2 to h2
k(5) =  1/A(3)*Cv(3)*sqrt(DP(3)/rho);                        % k': z2 to h3
k(6) = -1/A(3)*Cv(4)*sqrt(DP(4)/rho);                        % k': z3 to h3

theta= [0; 0; 0; 0; 0; 0];                                    % [min] delay
Kc   = 1./k.*1./(tauC+theta);     % proportional gain for all 6 controllers
%Kc(1) = 0; Kc(3)=0; Kc(5)=0;
tauI = 4.*(tauC+theta);               % integral time for all 6 controllers
KI   = Kc./tauI;                                            % integral gain
tauT = tauI./2;                                    % tracking time constant
u0   = [z0(1); z0(2); z0(2); z0(3); z0(3); z0(4)];       % controllers bias

%setpoints
LC1max = hH*hmax(1); LC1min = hL*hmax(1);        % [m] setpoints for tank 1
LC2max = hH*hmax(2); LC2min = hL*hmax(2);        % [m] setpoints for tank 2
LC3max = hH*hmax(3); LC3min = hL*hmax(3);        % [m] setpoints for tank 3

%% %% Set-up simulation
nx     = length(x0); nu = length(z0); nd = length(d0); np = length(param);
td     = 0;           % [min] disturbances step time. given for consistency
df     = 0;                                               % no disturbances
tsim   = 120;                                       % [min] simulation time
tdelay = 1;

%% Change in TPM
%zOP      = zOP0-[0;0;0.6;0];     % change in valve position from zmax=1         
zOP      = z0.*[0.5; 1; 1; 1];     % change in valve position from zmax=1         


%duration = 'short';                                       % short bottlneck
duration = 'long';                                   % prolonged bottlneck
switch duration
     case 'short'
          tBend = 150;
     case 'long'
          tBend = 29;
     otherwise
          disp('error duration TPM')
end
tz0OP = [10; tBend];  % [min] time for operator decrease and increase in z0
tz1OP = [10; tBend];  % [min] time for operator decrease and increase in z1
tz2OP = [10; tBend];  % [min] time for operator decrease and increase in z2
tz3OP = [10; tBend];  % [min] time for operator decrease and increase in z3

%% Simulate controllers with different setpoints
th   = ones(6,1);            % [min] step time for level setpoint. not used
h0   = [LC1max ; LC1min; LC2max; LC2min; LC3max;LC3min ]; %[m] nom setpoint
hf   = h0;                           % [m] step in level setpoint. not used
sim('sim2C');                                                     %simulate

%% Save results 
idx         = find((zOP-z0)~=0); % find which valve was changed
filename2C  = strcat('2C','_TPM_z',num2str(TPM),'_',duration,'_z',num2str(idx-1));    % create filename
save(filename2C,'t2C','u2C','y2C','h0','zOP','z0','u02C','u12C','u22C','u32C');
%% Plot results

figure('Name','Controllers with different setpoints')
subplot(2,1,1)
plot(t2C,y2C./hmax'*100)
legend('h1', 'h2', 'h3')
xlabel('Time [min]')
ylabel('Level [%]')

subplot(2,1,2)
plot(t2C,u2C'*100)
legend('z0','z1', 'z2', 'z3')
xlabel('Time [min]')
ylabel('Valve opening [%]')


