% Main script for simulating the room temperature control using SRC
% September 2018/May2019

%There are 4 available MVs
% 1: Refrigerated water
% 2: Cooling water
% 3: Hot water
% 4: Electric heating

clc
clear
close all

% set(0,'DefaultTextFontName','Times',...
% 'DefaultTextFontSize',24,...
% 'DefaultAxesFontName','Times',...
% 'DefaultAxesFontSize',24,...
% 'DefaultLineLineWidth',2.5,...
% 'DefaultLineMarkerSize',12)

%u: valves, inputs to the process
%v: internal signals from controllers to SR switching block


%load('STDdel_')  %standard with delay


%% Simulation parameters
tend=400;           % Simulation time

%% Nominal values
Tr0=   18;           % Room temperature
Tamb0= 18;           % Ambient temperature
Tsp0=  18;           % Set point temperature

% All valves are nominally closed
u10=0;              % v1 opening
u20=0;  
u30=0;
u40=0;

v0=0;               % Controller output; %from the SRC solution, CW crosses 0 at 43.7500

%% Disturbances

% Set point
Tsp1= 0; %5;            % First set point
Tsp2= 0; %10;           % Second change of set point
Tsp3= 0; %-Tsp1-Tsp2-2; % Third change of set point
Tsp4= 0; %-9; 


tTsp1= 10;          % First set point, time
tTsp2= tTsp1+40;    % Second change of set point, time
tTsp3= tTsp2+40;    % Third change of set point, time
tTsp4= tTsp3+40; 

% Ambient temperature
Tamb1=0;
Tamb2=0;
Tamb3=0;
Tamb4=0;

tTamb1=tTsp1;
tTamb2=80;
tTamb3=140;
tTamb4=180;
tTamb5=280;
tTamb6=350;


%% Upper and lower limits

u1max=1; 
u1min=0;
u2max=1;
u2min=0;
u3max=1;
u3min=0;
u4max=1;
u4min=0;



v1max=1; 
v1min=0;
v2max=1;
v2min=0;
v3max=1;
v3min=0;
v4max=1;
v4min=0;

v10 = v1max;
v20 = v2min;
v30 = v3min;
v40 = v4min;

u1nom=0; u2nom=0; u3nom=0; u4nom=0; % if Tsp=Tamb, all MVs are closed..
v1nom=0; v2nom=0; v3nom=0; v4nom=0; % if Tsp=Tamb, all MVs are closed.

%% Transfer functions from valves to room temperature
% Same from DYCOPS 2019 paper

%Refrigerated water
K_rw        =-5;   %gain must be negative! more RW, lower temperature
tau_rw      =8;
theta_rw    =2;

%Cooling water
K_cw        =-10;    %gain must be negative! more CW, lower temperature
tau_cw      =15;
theta_cw    =3;

%Heating water
K_hw        =12;
tau_hw      =10;
theta_hw    =3;

%Electric heating
K_eh        =8;
tau_eh      =5;
theta_eh    =1;

%Ambient temperature (Disturbance)
K_amb       =1;
tau_amb     =15;
theta_amb   =6;

taup=[tau_rw,tau_cw,tau_hw,tau_eh];

%% Controller tunning for SRC
%Kp=7/20; %Kuy; %Kuy;   %Still to be defined
%taup_tune=min(taup);
theta       =0; 


%Kc =(1/Kp)*(taup_tune/(tauc+theta));
%tauI = min(taup_tune,4*(tauc+theta));
tauC1       =1*theta_rw; %2;
tauC2       =1*theta_cw; %3;
tauC3       =1*theta_hw; %3;
tauC4       =1*theta_eh; %3;% 15;

tauC        = 3;
% Controller tunings
Kc_RW       = 1/K_rw*tau_rw/(tauC1+theta_rw);
Kc_CW       = 1/K_cw*tau_cw/(tauC2+theta_cw);
Kc_HW       = 1/K_hw*tau_hw/(tauC3+theta_hw);
Kc_EH       = 1/K_eh*tau_eh/(tauC4+theta_eh); %This tuning might be improved

tau_I_RW    = min(tau_rw,4*(tauC1+theta_rw));
tau_I_CW    = min(tau_cw,4*(tauC2+theta_cw));
tau_I_HW    = min(tau_hw,4*(tauC3+theta_hw));
tau_I_EH    = min(tau_eh,4*(tauC4+theta_eh));

KI_RW       = Kc_RW/tau_I_RW;
KI_HW       = Kc_HW/tau_I_HW;
KI_CW       = Kc_CW/tau_I_CW;
KI_EH       = Kc_EH/tau_I_EH;

Ki=1/(4*tauC);
%Trick to have the previous values of the MVs
delay_switch=0.1;
%initial value of previous state of variables
initial_switch=zeros(10,1);


% %% Standard SRC
% Kc  = 0.0482;
% tauI = 9.5; 
% 
% % We should consider offsets
% off1=12.5000;
% off2=0;
% off3=0;
% off4= -18.7500;
% Ks1 = 0.0320;
% Ks2 =0.0800;
% Ks3 = 0.0533;
% Ks4 =   0.0267;
% 
% split =[ -43.7500  -12.5000         0   18.7500   56.2500];
% 
% RWlow= split(1);
% RWup= split(2);
% CWlow= split(2);
% CWup= split(3);
% HWlow= split(3);
% HWup= split(4);
% EHlow=split(4);
% EHup= split(5);
% 
% % Gain correction
% K1=-1; %sign(Kvy(1));
% K2=-1; %sign(Kvy(2));
% K3=1; %sign(Kvy(3));
% K4=1; %sign(Kvy(4));

%% Simulations with changes in ambient temperature

Tsp1= 0;            % First set point
Tsp2=0;             % Second change of set point
Tsp3=0;             % Third change of set point
Tsp4=0; 

% Ambient temperature
Tamb1=2; %0.2;
Tamb2=9; %0.10;
Tamb3=-5; %-0.12;
Tamb4=-25; %-0.15;
Tamb5=18;
Tamb6=5;

%sim('SRC_Room_simple')
%SRC4 works fine
% In SRC5 I am trying to implement Sigurd's idea
%sim('SRC_Room');%,'simulationTime', [0 tend])    
Tracking=5;  %Tracking coefficient

Tt_RW = 1; %(tau_I_RW)*0.9;
Tt_CW = 1;
Tt_HW = 1;
Tt_EH = 1;
sim('SRC_Room_independent_C_V8')
% V4 - decent. could work on tracking scheme
%V5- conditional integration
%V6, self-made back calculation. Works fine
timelength=length(t);
errorplot=zeros(timelength);
maxsatplot=ones(timelength);

%% Integrators
% figure
% plot(t,int_RW,t,int_CW,t,int_HW,t,int_EH)
% legend('AC','CW','HW', 'EH')

figure
subplot(2,1,1)
plot(t,T,'r');
hold on
plot(t,Tsp,'k')
title('Changes in ambient temperature')
plot(t,Tamb,'Color',[0 0.5 0])
legend('T','T^{sp}','T^{amb}', 'Location','best','Orientation','horizontal')
legend boxoff 

subplot(2,1,2)
plot(t,u1,'Color',[0.3010    0.7450    0.9330])
hold on
plot(t,u2,'Color',[0    0.4470    0.7410])
plot(t,u3,'Color', [0.9290    0.6940    0.1250])
plot(t,u4,'r')
legend('AC','CW','HW', 'EH','Location','best','Orientation','horizontal')
legend boxoff 
%%
% figure
% subplot(2,1,1)
% plot(t,T_error)
% hold on
% plot(t,errorplot,'k--')
% title('Error')
% 
% subplot(2,1,2)
% plot(t,deltav1,'Color',[0.3010    0.7450    0.9330])
% hold on
% plot(t,deltav2,'Color',[0    0.4470    0.7410])
% plot(t,deltav3,'Color', [0.9290    0.6940    0.1250])
% plot(t,deltav4,'r')
% plot(t,maxsatplot,'k--')
% plot(t,errorplot,'k--')
% legend('AC','CW','HW', 'EH','max','min','Location','best','Orientation','horizontal')
% legend boxoff 
% title ('Suggested values')

%%
% figure
% subplot(3,1,1)
% plot(t,Tamb,'Color',[0 0.5 0])
% hold on
% plot(t,Tsp,'k--')
% title('Ambient temperature')
% ylabel('Temperature (°C)')
% legend('$T^{amb}$', 'Location','best','Orientation','horizontal','Interpreter','latex')
% legend boxoff 
% 
% subplot(3,1,2)
% plot(t,T,'Color',[ 0.6350    0.0780    0.1840])
% hold on
% plot(STDdel_t,STDdel_Troom,'--', 'Color',[0.4660    0.6740    0.1880]);%[ 138/255,43/255,226/255])
% plot(t,Tsp,'k--')
% title('Room temperature')
% ylabel('Temperature (°C)')
% legend('Generalized','Standard', '$T^{sp}$', 'Location','best','Orientation','horizontal','Interpreter','latex')
% legend boxoff 
% 
% subplot(3,1,3)
% plot(t,u1,'Color',[0.3010    0.7450    0.9330])
% hold on
% plot(t,u2,'Color',[0    0.4470    0.7410])
% plot(t,u3,'Color', [0.9290    0.6940    0.1250])
% plot(t,u4,'r')
% plot(t,maxsatplot,'k--')
% plot(t,errorplot,'k--')
% %Suggested
% % plot(t,deltav1,'--','Color',[0.3010    0.7450    0.9330])
% % plot(t,deltav2,'--','Color',[0    0.4470    0.7410])
% % plot(t,deltav3,'--','Color', [0.9290    0.6940    0.1250])
% % plot(t,deltav4,'r--')
% title('Manipulated variables')
% legend('$u_{AC}$','$u_{CW}$','$u_{HW}$', '$u_{EH}$','Location','best','Orientation','horizontal','Interpreter','latex')
% legend boxoff 
% ylabel('u_i (-)')
% xlabel('Time (min)')
% %%
% figure
% plot(t,u1,'Color',[0.3010    0.7450    0.9330])
% hold on
% plot(t,u2,'Color',[0    0.4470    0.7410])
% plot(t,u3,'Color', [0.9290    0.6940    0.1250])
% plot(t,u4,'r')
% plot(t,maxsatplot,'k--')
% plot(t,errorplot,'k--')
% %Suggested
% title('Manipulated variables')
% legend('AC','CW','HW', 'EH','Location','best','Orientation','horizontal')
% legend boxoff 
% xlabel('Time (s)')
% 
% figure
% plot(t,u1,'Color',[0.3010    0.7450    0.9330])
% hold on
% plot(t,u2,'Color',[0    0.4470    0.7410])
% plot(t,u3,'Color', [0.9290    0.6940    0.1250])
% plot(t,u4,'r')
% plot(t,maxsatplot,'k--')
% plot(t,errorplot,'k--')
% %Suggested
% % plot(t,deltav1,'--','Color',[0.3010    0.7450    0.9330])
% % plot(t,deltav2,'--','Color',[0    0.4470    0.7410])
% % plot(t,deltav3,'--','Color', [0.9290    0.6940    0.1250])
% % plot(t,deltav4,'r--')
% title('Manipulated variables')
% legend('AC','CW','HW', 'EH','Location','best','Orientation','horizontal')
% legend boxoff 
% xlabel('Time (s)')
% %%
% figure
% 
% subplot(2,1,1)
% plot(t,T_error)
% hold on
% plot(t,errorplot,'k--')
% title('Error')
% 
% 
% subplot(2,1,2)
% plot(t,activeMV)
% title('activeMV')
% %% Standard
% figure  %Only without lead lag
% subplot(2,1,1)
% plot(STDdel_t,STDdel_Troom,'r');%'Color',[ 138/255,43/255,226/255])
% hold on
% %plot(t_ll,Troom_ll,'k'); %'Color',[ 138/255,43/255,226/255])
% plot(STDdel_t,STDdel_Tsp,'k--')
% plot(STDdel_t,STDdel_Tamb,'Color',[34/255,139/255,34/255])
% %grid on
% %grid minor
% ax = gca;
% ax.FontSize = 16;
% %xlabel('Time,s')
% %ylim([0 40])
% ylabel('Temperature, T (°C)','Interpreter','Latex')
% legend('T','T^{ref}','T^{amb}','Location','Northeast','Orientation','horizontal')
% title('Standard SRC','Interpreter','Latex')
% hold off
% %legend boxoff
% 
% subplot(2,1,2)
% plot(STDdel_t,STDdel_v1,'Color',[25/255    25/255    112/255])
% hold on
% plot(STDdel_t,STDdel_v2,'Color',[0.3010    0.7450    0.9330])
% plot(STDdel_t,STDdel_v3,'Color', [0.9290    0.6940    0.1250])
% plot(STDdel_t,STDdel_v4,'r')
% ax = gca;
% ax.FontSize = 16;
% xlabel('Time (min)')
% ylim([-0.05 1.05])
% ylabel('Valve opening, $u_i$ (-)','Interpreter','Latex')
% legend('u_{AC}','u_{CW}','u_{HW}','u_{EH}', 'Location','Northeast','Orientation','horizontal')
% 
% hold off
%%
figure
subplot(3,1,1)
plot(t,intRW,'Color',[0.3010    0.7450    0.9330])
hold on
plot(t,intCW,'Color',[0    0.4470    0.7410])
plot(t,intHW,'Color', [0.9290    0.6940    0.1250])
plot(t,intEH,'r')
plot([0 tend], [0 0],'k--')
title ('Integrators')
legend('Int_{AC}','Int_{CW}','Int_{HW}','Int_{EH}', 'Location','Northeast','Orientation','horizontal')

subplot(3,1,2)
plot(t,resetInt1,'Color',[0.3010    0.7450    0.9330])
hold on
plot(t,resetInt2,'Color',[0    0.4470    0.7410])
plot(t,resetInt3,'Color', [0.9290    0.6940    0.1250])
plot(t,resetInt4,'r')
legend('Int_{AC}','Int_{CW}','Int_{HW}','Int_{EH}', 'Location','Northeast','Orientation','horizontal')
title ('Reset integration')

subplot(3,1,3)
plot(t,activeMV)
title('Active MV')
%%
% subplot(4,1,4)
% plot(t,T_error)
% hold on
% plot(t,errorplot,'k--')
% title('Error')

% %% Common integrator
% sim('SRC_Room');%,'simulationTime', [0 tend])                
% timelength=length(t);
% errorplot=zeros(timelength);
% 
% figure
% subplot(2,1,1)
% plot(t,T,'r');
% hold on
% plot(t,Tsp,'k')
% title('Changes in ambient temperature')
% plot(t,Tamb,'Color',[0 0.5 0])
% legend('T','T^{sp}','T^{amb}', 'Location','best','Orientation','horizontal')
% legend boxoff 
% 
% subplot(2,1,2)
% plot(t,u1,'Color',[0.3010    0.7450    0.9330])
% hold on
% plot(t,u2,'Color',[0    0.4470    0.7410])
% plot(t,u3,'Color', [0.9290    0.6940    0.1250])
% plot(t,u4,'r')
% legend('AC','CW','HW', 'EH','Location','best','Orientation','horizontal')
% legend boxoff 
% 
% figure
% plot(t,error)
% plot(t,errorplot,'k--')
% title('Common integrators')

%% 
% filename = 'GenDCS1theta.mat';
% save(filename)
% x = load('GenDCS1theta.mat');
% 
% names = fieldnames(x);
% for iname = 1:length(names)
%   x.(['GenDCS1theta_', names{iname}]) = x.(names{iname});
%   x = rmfield(x, names{iname});
% end
% %save('newmymat.mat', '-struct', 's');
% save('GenDCS1theta_.mat', '-struct', 'x');