% Task  : Transformed MVs. Mixing example
% Date  : 22 June 2021
% Author: CZ
% TODO  : -
% Var   : MVs=[q1;q2]; CVs=[q;T]; DVs=[T1;T2].

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
V   = 100;                                                %[m3] tank holdup
q0  = 10;                                       %[m3/s] nominal outlet flow
q10 = 5;                                             %[m3/s] inlet flow 1
q20 = 5;                                             %[m3/s] inlet flow 2
T0  = 35;                                %[degC] nominal outlet temperature
T10 = 20;                          %[degC] nominal inlet flow 1 temperature
T20 = 50;                          %[degC] nominal inlet flow 2 temperature
v10 = q0;                                              %transf input 1 bias
u0  = [q10;q20];                                               % input bias
d0  = [T10;T20];

q1max = 10;
q2max = 10;
qmin = 0;
%% Set up simulation
tsim    = 1000;                                         %[s] simulation time

% Controller parameters
tauCu1 = 1;               % desired closed loop time constant for flow loop
tauCu2 = 2;               % desired closed loop time constant for temp loop
tauTs = 0.1; % tracking time constant

pairing = 1;
switch pairing
     case 1              % u1-v1; u2-v2
          ku1 = 1;       % gain from u1 to v1
          ku2 = 1/0.05;  % gain from u2 to v2
          Kparu1 = [1 0]; Kparu2 = [0 1];      % select pairing in Simulink
     case 2              % u1-v2; u2-v1
          ku1 = -1/0.05; % gain from u1 to v2
          ku2 = 1;       % gain from u2 to v1
          Kparu1 = [0 1]; Kparu2 = [1 0];      % select pairing in Simulink
     otherwise
          disp('incorrect pairing')
end

Ku1 = 1/ku1*1/(tauCu1);              % integral slave controller to find u1
Ku2 = 1/ku2*1/(tauCu2);              % integral slave controller to find u2

%% Simulation
tv1 = 50;                          %[s] step time for flow setpoint changes
tv2 = 50;                   %[s] step time for temperature setpoint changes
td1  = 100;                                %[s] step time for disturbance 1
td2  = 150;                                %[s] step time for disturbance 2

v1f  = q0;                              % step change for flow setpoint
T1f   = T10-5;                      % step change for inflow 1 temperatures
T2f   = T20;                     % step change for inflow 2 temperatures
T1fm  = T10;    T2fm = T20;                         % measured disturbances

q1f=q10*1; q2f=q20*1; % steps in u for tuning slave ctrl. cascade implem.
filename = {'simTransformedMVs','simTransformedMVs_cascade','simTransformedMVs'};

A = [-q0/V; -q0/V*2; -q0/V/2; 0];              %calc block; cascade; fb lin
B = [ q0/V;  q0/V*2;  q0/V/2; 1];              %calc block; cascade; fb lin
f20 = 0;                                   % energy balance at steady-state
%trf = {'trf','cascasde','fblin'};
trf  = {'org','fast','slow','fblin'};
for i = 1:4
     
     p   = [V;A(i);B(i)];                                      % parameters
     v20 = B(i)^(-1)*(f20-A(i)*T0);                           % bias for v2
     v2f = v20;                                % step change for v setpoint
     %sim(filename{i})
     sim('simTransformedMVs')
     file = strcat('Shower_Dist_',trf{i});     % calc block & fb lin: unmeasured disturbances
     
     save(file)
     %% Plot results
     figure(1)
     subplot(3,2,1)
     plot(time, q)
     hold on
     plot([0 time(end)],[q0 q0])
     legend('y_1=q','q^s')
     xlabel('Time')
     ylabel('Flow q')
     
     subplot(3,2,2)
     plot(time, T)
     hold on
     plot([0 time(end)], [T0 T0])
     legend('y_2=T','T^s', 'Location','best')
     %ylim([29 31])
     xlabel('Time')
     ylabel('Temperature T')
     
     subplot(3,2,3)
     plot(time,v(:,1))
     hold on
     plot(time,v1s)
     legend('v_1','v_1^s')
     xlabel('Time')
     ylabel('Tranformed input ')
     
     subplot(3,2,4)
     plot(time,v(:,2))
     hold on
     plot(time,v2s)
     legend('v_2','v_2^s')
     xlabel('Time')
     ylabel('Tranformed input ')
     
     subplot(3,2,5)
     plot(time,uplant(:,1))
     hold on
     plot([0 time(end)],[qmin qmin], 'k--')
     plot([0 time(end)],[q1max q1max],'k--')
     legend('u_1=q_1')
     xlabel('Time')
     ylabel('Flow q_1')
     
     subplot(3,2,6)
     plot(time,uplant(:,2))
     hold on
     plot([0 time(end)],[qmin qmin], 'k--')
     plot([0 time(end)],[q2max q2max],'k--')
     legend('u_2=q_2')
     xlabel('Time')
     ylabel('Flow q_2')
end
