%% SIMULATE AND PLOT ONE PI CONTROLLER with anti-windup

clear;
close all;

%% Simulation parameters
t_sim = 100; %simulation time

%% MODEL
%To work with physical variables, we are using an interpreted Matlab
%function called SimpleTank_ODE  dh/dt=(qin-qout)/A. 
% This could be substituted with an integrator (1/s). It is left as it is
% in case future changes are required.

%Nominal values
qout0= 0.5;
qin0= 0.5;
h0=0.5;
h_set = 0.5; %Set-point for PI controller
p=1; %Area of the tank
theta=0; %Time delay in the tank
kprime=-1/p;

%% DISTURBANCES
% The disturbance is the inflow

sin_ON =1;             % add sinusoidal to nominal inflow
d1amplitude=0.05;
d1bias=0;
d1freq=1; %rad/s
d1phase=0; %rad

noise_ON=0;    % add noise
noise_power=1e-5;
ratelimit=10000; %to limit P controllers

d2= 0.4; %step disturbance
td2=30;%20;  

d3= -0.8;
td3=200;%40; 

d4=0.05;
td4=350;%60;

d5=-0.9;
td5=300;%75;

%% SATURATION AND ANTI-WINDUP
Umax=1;   % minimum output (q_out,max)
Umin=0;   % maximum output (q_out,min)

%Saturation of input given u0
Uminsat=Umin-qout0;
Umaxsat=Umax-qout0;
%% SIMULATIONS

% Tight control
tau_c = 0.5; 
Kc=(1/kprime)*(1/(tau_c+theta));
tauI=4*(tau_c+theta);
sim('TankPIctrl',t_sim) 
hPI_t=h;
q_inPI=q_in;
q_outOnePI_t=q_out;

% Slow control
tau_c = 3; 
Kc=(1/kprime)*(1/(tau_c+theta));
tauI=4*(tau_c+theta);
sim('TankPIctrl',t_sim) 
hPI=h;
q_outOnePI=q_out;


%% PLOT

hmin=[0.1,0.1];
hmax=[0.9,0.9];
hminmin=[0,0];
hmaxmax=[1,1];
hsp=[0.5,0.5];
tim=[0,t_sim];
Linewidth=1.3;

figure(1);
subplot(211)
plot(hPI.time,hPI.signals.values,'LineWidth',Linewidth)
%title('One PI controller with anti-windup')
hold on
plot(hPI_t.time,hPI_t.signals.values,'Color',[50/255 205/255 50/255],'LineWidth',Linewidth); %'Color',[0 165/255 115/255]) %'Color',[139/255 0/255 139/255]); [0/255 128/255 0/255]
plot(tim,hmin,'--','Color',[255/255 165/255 0/255])
plot(tim,hmax,'--','Color',[255/255 165/255 0/255])
plot(tim,hminmin,'--r')
plot(tim,hmaxmax,'--r')
%plot(tim,hsp,'--k','LineWidth',0.5)%'Color',[0 165/255 115/255])
hold off
legend('h, \tau_c=3s','h,\tau_c=0.5s','Location','southeast')
ylim([-2 2]); grid on; grid minor; ylabel('Level [m]')
subplot(212)
plot(q_out.time,q_out.signals.values,'LineWidth',Linewidth)
hold on
plot(q_outOnePI_t.time,q_outOnePI_t.signals.values,'Color',[50/255 205/255 50/255],'LineWidth',Linewidth)
plot(q_in.time,q_in.signals.values,'--r','LineWidth',Linewidth)
legend('q_{out}\tau_c=3s','q_{out},\tau_c=0.5s', 'q_{in}', 'Location','southeast'); ylim([-0.1 1.1]); grid on; grid minor
xlabel('time [s]');ylabel('Flow [m^3/s]')
hold off

%saveas (figure (1), 'figures/OnlyPID','eps2c');