%%%% Unstable with the static calculation block, due to RHP zero
% Task  : Transformed MVs. Tank. Study static, linear, int transf.
% Date  : August 2020
% Author: CZ
% TODO  :
% Var   : x=[T1;T2]; y = T2; w=T1; u=q; ...
%         d=[T0; TD; qD; Q]; v=[v0 or vL or vL0 or vF]

clc;clear;close all; warning off
%% Plotting options
set(0,'DefaultTextFontName','Times',...
     'DefaultTextFontSize',14,...
     'DefaultAxesFontName','Times',...
     'DefaultAxesFontSize',14,...
     'DefaultLineLineWidth',4,...
     'DefaultLineMarkerSize',7.75,...
     'DefaultStairLineWidth',2)

%% Parameters and nominal conditions
V1    = 10;                                            % [m3] tank 1 volume
V2    = 10;                                            % [m3] tank 2 volume
rho   = 1000;                                             % [kg/m3] density
cp    = 4.18;                                   % [kJ/kg/deg] specific heat
qnom  = 1e-2;                                    % [m3/s] nominal flow rate
qDnom = 2*qnom;                  % [m3/s] nominal flow rate added to tank 2
T0nom = 10;                                      % [degC] inlet temperature
TDnom = 20;                  % [degC] % temperature of flow added to tank 2
T1nom = 50;                             % [degC] tank 1 nominal temperature
Qnom  = qnom*(T1nom-T0nom)*rho*cp;                 % [kW] nominal heat rate
T2nom = 30;                             % [degC] tank 2 nominal temperature
Tnom = [T1nom;T2nom];                     % [degC] nominal temperature cond
%% Set up simulation
tsim     = 2000;                                     % [s], simulation time

% Disturbances step change
dnom  = [T0nom; TDnom; qDnom; Qnom];
dstep = [T0nom*0; TDnom*0;qDnom*0; Qnom];        % steps for first 3 disturbances
t0    = [10;20;30;40];     % time when the step starts for each disturbance
tf    = t0+[1 1 1 1];       % time when the step stops for each disturbance
nd    = numel(dstep);
for i = 1:nd                        % construct timeseries for disturbances
     t         = [0 t0(i) t0(i) tf(i)];
     d         = [0 0 dstep(i) dstep(i)];
     dist {i}  = timeseries(d',t);
end

% Setpoint step change
t0sp = 0; tfsp = t0sp+0;               % time when the step starts and ends
t    = [0 t0sp t0sp tfsp];                % time vector for setpoint change
sp   = [0 0 T2nom*0 T2nom*0];                          % change in setpoint
T2sp = timeseries(sp',t);                     % setpoint step as timeseries


%% Select simulation case

lgd = {'v0', 'vL','vL0','vI','Setpoint'};          % legend text
ylb = {'Inflow, q (m3/s)';'Heat, Q (kW)'};          % yaxis label for input
lintype = {'-',':','--',':'};


tauC  = [100;100];                  % [s] desired closed loop time constant
theta = 0;                                                 % [s] time delay

tau    = [V1/qnom;V2/(qnom+qDnom)];     % = -1/A; A =(df/dT)*. tuning param for general linear
k      = tau;
A1     = -qnom/V1;      % tuning parameter for general linear transformation
A2     = -(qnom+qDnom)/V2;
tau01  = -1/A1*1;                          % [s] tuning param for linear IFAC
tau02  = -1/A2*1; 

tauq = tau;
tauQ = tau;

filename = {'sim_uFlow'; 'sim_uHeat'};           % filenames for simulation
for i =1
     for j =1
          MV     = i;                                % MV=1=>u=q; MV=2=>u=Q
          transf = j; % 0=> static; =1=>linear, vL; =2=>linear,vL0;3=> integr
          
          switch MV
               case 1 % u=q
                    d2nom = Qnom;
                    umax = 3*qnom;
                    switch transf             % select PI controller tuning
                         case 0                     % static transformation
                              Kc    = -tauq./(tauC+theta);
                              tauI  = min(tauq,4*(tauC+theta));
                              vbias = Tnom;
                         case 1                     % linear transformation
                              Kc    = -1./k.*tau./(tauC+theta);
                              tauI  = min(tau,4*(tauC+theta));
                              vbias = [-A1*T1nom; -A2*T2nom];
                         case 2                                  % IFAC vL0
                              Kc    = -tau./(tauC+theta);
                              tauI  = min(tau,4*(tauC+theta));
                              vbias = Tnom;
                         case 3                % integrating transformation
                              Kc    = -1./(tauC+theta);
                              tauI  = 4.*(tauC+theta);
                              vbias = [0;0];
                         otherwise
                    end
                    
               case 2 % u=Q
                    d2nom = qnom;
                    umax = 3*Qnom;
                    switch transf             % select PI controller tuning
                         case 0                % static transformation (v0)
                              Kc    = tauQ./(tauC+theta);
                              tauI  = min(tauQ,4*(tauC+theta));
                              vbias = Tnom;
                         case 1                  % linear transformation vL
                              Kc    = 1./k.*tau./(tauC+theta); 
                              tauI  = min(tau,4*(tauC+theta));
                              vbias = [-A1*T1nom;-A2*T2nom];
                         case 2                                  % IFAC vL0
                              Kc    = tau./(tauC+theta);
                              tauI  = min(tau,4*(tauC+theta));
                              vbias = Tnom;
                         case 3                % integrating transformation
                              Kc    = 1./(tauC+theta); 
                              tauI  = 4.*(tauC+theta);
                              vbias = [0;0];
                         otherwise
                    end
                    
               otherwise
          end      
              
          p = [V1;V2;rho;cp;A1;A2;tau01; tau02;transf];        % parameters
                   
          sim(filename{i})                                       % simulate
          
          figure(i)
          subplot(2,1,1)
          plot(time, T(:,2),lintype{j+1})
          hold on
          xlabel('Time, (s)')
          ylabel('Temperature, (degC)')
          
          subplot(2,1,2)
          plot(time, u,lintype{j+1})
          hold on
          xlabel('Time, (s)')
          ylabel(ylb{i})
          
     end
end

