%Plot together

clc
clear all
close all

num=5; %case

%% Load data
namePI=strcat('PI',num2str(num));
nameMPC=strcat('MPC',num2str(num),'_newsim');
load(namePI,'-mat')

load(namePI,'-mat')
load(nameMPC,'-mat')

%% Parameters
LinewidthPI=1.3;
LinewidthMPC=1.5;
hinit = 0.5;
uinit = 0.5;
par.tf=0.1;
par.simtime=200;
%% Plot
figure(1);

subplot(3,1,1)
plot(q_in.time,q_in.signals.values,'k','LineWidth',LinewidthPI)
ylim([-0.1 1.1]); grid on; grid minor
ylabel('Inflow, q_{in} [m^3/s]')
hold off

subplot(3,1,2)
plot(0:par.tf:par.simtime,[vertcat(hinit,sim.x(:,1))],'r','LineWidth',LinewidthMPC)
hold on
plot(h.time,h.signals.values,'b','LineWidth',LinewidthPI)
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,'--','Color',[0 165/255 115/255])
hold off
ylim([-0.1 1.1]); grid on; grid minor; ylabel('Level, h [m]')


subplot(3,1,3)
% plot(q_outmax.time,q_outmax.signals.values,'r','LineWidth',Linewidth)  %,'Color',[105/255 56/255 27/255])
% hold on;
% plot(q_outmid.time,q_outmid.signals.values,'Color', [50/255 205/255 50/255],'LineWidth',2) 
% hold on;
% plot(q_outmin.time,q_outmin.signals.values,'Color',[255/255 165/255 0/255],'LineWidth',Linewidth)
% hold on;
stairs(0:par.tf:par.simtime,[horzcat(uinit,sim.u)],'r','LineWidth',LinewidthMPC)
hold on;
plot(q_out.time,q_out.signals.values,'b','LineWidth',LinewidthPI)
legend('MPC','P(I)','Location','southeast')
ylim([-0.1 1.1]); grid on; grid minor
%xlabel('time [s]');
ylabel('Outflow, q_{out} [m^3/s]');


fig = figure (1);
fig.PaperUnits = 'inches';
fig.PaperPosition = [0 0 8 7.5];
namefile=strcat('figures/Case',num2str(num));
saveas (figure (1), namefile,'eps2c'); 

%Calculate integral(abs(delta qout/delta y))
integralMPC=0;
integralPI=0;
deltat=0.1;
for j=1:(size(sim.x(:,1))-1)
%     delta=sim.u(j+1,1)-sim.u(j,1);
%     deltaydeltat=abs(delta/deltat);
%     integralMPC=integralMPC+deltaydeltat;  
    delta2=q_out.signals.values(j+1)-q_out.signals.values(j);
    deltaydeltat2=abs(delta2);
    integralPI=integralPI+deltaydeltat2;
end 
for k=1:length(sim.u)-1
    deltaMPC(k)=abs(sim.u(k+1)-sim.u(k));
    % deltaydeltat=abs(delta);
   % integralMPC=integralMPC+deltaydeltat;
end 
integralMPC=sum(deltaMPC)/deltat

%integralMPC
integralPI=integralPI/deltat


