% Task  : Tune SRC for tanks in series with moving bottlnecks
% Date  : April 2021


addpath('\\home.ansatt.ntnu.no\cristinz\Documents\phd\Split range control\Functions')

clc;close all; clear

%% Set-up for all tanks
N       = 2;                                                % number of MVs
opening = [0;100];
vmin    = 0;
vmax    = 100;
umin    = zeros(N,1);
umax    = ones(N,1)*100;
price   = [1;100];                                          % Inlet, outlet
s       = tf('s');                                         % Laplace transf

hmax = [2.3; 2.8; 3.2];              % [m] max L=[2.3; 2.8; 3.2]. from h=2d
hH   = 0.9;                                     % level high setpoint = 90%
hL   = 0.1;                                     % level low setpoint = 10 %
%% Different Cv for different TPMs
for TPM = 0:3                                   % 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
     
     %% Model prameters for all tanks
     q0    = 1;                                     % nominal flowrates [m3/min]
     A     = [1; 1.5; 2];                        % [m3] tanks cross section area
     rho   = 1000;                                       % [kg/m3] water density
     DP    = [0.5; 0.6; 0.7; 1]*1e5;                                       %[Pa]
     Cv    = q0./(z0.*sqrt(DP/rho));                  % linear valve coefficient
     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
     
     %% Tune SRC
     j=1;
     tauC = ones(6,1)*0.5;  % [min] closed loop time constant. res time~=[2;4;6]
     for i = [1,3,5] %1=tank1;3=tank2;5=tank3
          G      = [k(i)/s k(i+1)/s];
          G      = set(zpk(G), 'DisplayFormat','time constant');
          n      = [tauC(i); tauC(i+1)];                                 % =tauC
          param  = [vmin; vmax; umin; umax; n; price;opening];  % SRC parameters
          [Kc(j),tauI(j),alpha(i:i+1),MVorder(i:i+1)] = SRC(G,param);
          j=j+1;
     end
     Kc=double(Kc); tauI=double(tauI); alpha=double(alpha);
     dv     = 100./abs(alpha);
     vs     = dv([1 3 5]);
     u0     = -100.*alpha([2 4 6]);                  % SR bias for outlet valve
     alpha1 = alpha(1:2); alpha2 = alpha(3:4); alpha3 = alpha(5:6); % slopes SR
     u01    = [0 u0(1)];  u02    = [0 u0(2)]; u03    = [0 u0(3)]; % bias in SR
     
     
     switch TPM % calculate controller bias.depends on the loops direction
          case 0 % TPM = z0.               
               v0(1:3) = (z0(2:4)'*100-u0)./(alpha([2 4 6]));    %MV=outlet
          case 1 % TPM = z1
               v0(1)   = z0(1)*100/abs(alpha(1));                 %MV=inlet
               v0(2:3) = (z0(3:4)'*100-u0(2:3))./(alpha([4 6])); %MV=outlet
          case 2 % TPM = z2
               v0(1:2) = z0(1:2)'*100./abs(alpha([1 3]));         %MV=inlet
               v0(3)   = (z0(4)'*100-u0(3))./(alpha(6));         %MV=outlet   
          case 3 % TPM = z3.
               v0(1:3) = z0(1:3)'*100./abs(alpha([1 3 5]));       %MV=inlet
          otherwise
               disp('Incorrect TPM')
     end
     
     filename = strcat('SRCparamTPMz',num2str(TPM));
     save(filename,'Kc','tauI','v0','alpha1','alpha2','alpha3','u01','u02','u03','vs');
     
end


