close all
clc
clear all
%Try to optimize distillation column "A" for different disturbances!
%%
load('x0_F15_p007c.mat') %Initial values at F=1.5 and pV=0.07
%%
DeltaxDmin=-0.01;
set(0,'DefaultTextFontName','Times',...
'DefaultTextFontSize',18,...
'DefaultAxesFontName','Times',...
'DefaultAxesFontSize',18,...
'DefaultLineLineWidth',2,...
'DefaultLineMarkerSize',7.75)
%%
Jcol;
D0  = D;
B0  = B;
xD0 = xD;
xB0 = xB;
x0  = x; 
LT0 = u_opt(1);
VB0 = u_opt(2);
F0  = F;
pV0 = pV;
% Define nominal inputs!

% Nominal inputs from optimization file
% LT0=2.70629;                          % Reflux
% VB0= 3.20629;                          % Boilup
%  D0=0.5;                               % Distillate
%  B0=0.5;                               % Bottoms
% F0=1.0;                               % Feedrate
% x0 = 0.5*ones(1,82)' ; %This is X in cola_lv (this one is given in col_constr)

%Constants
zF=0.5;                              % Feed composition
qF=1.0;                              % Feed liquid fraction
Uinit = [LT0 VB0 D0 B0 F0 zF qF]';


%global NT d
NT = 41 ;

% Inputs
u0 = Uinit(1:2) ; %L, V

%Prices
pf = 1 ;  pB = 2 ; pD = 1 ;

%% Optimal values
% J1 =  -0.5058; % at F=1.5 and Pv 0.07   -0.5571
% J2 =  -0.5548; %-0.6117; % at F=1.65 and Pv 0.07     -0.6118
% J3 = -0.3157; %-0.4230;  % at F=1.65 and Pv 0.13  -0.4230


%% Setpoints and min values

xBsp    = 0.95;
xDmin   = 0.95; 

%Saturations
Vmax    = 4; 
VmaxPI  = Vmax-VB0;
Lmax    = 10;
LmaxPI  = Lmax-LT0;

Fmax=1.6825;  %Here it becomes infeasible
% Bias for LT for RIII (from simulation)
Fbias= Fmax; %1;%  2.5221; %4.0085-1.4864;

%% Tuning
TuningCol

TuningSRC

%% Matrices to extract values in simulation
%xprime is of size 41*2+1=83
%Stage 1 is the reboiler
Extract_J       =zeros(83,1)';
Extract_J(end)  =1;

Extract_x       =ones(83,1)';
Extract_x(end)  =0;

%Now we have 82 states. 
Extract_xB      =zeros(82,1)'; %horizontal
Extract_xB(1)   =1;

Extract_xD      =zeros(82,1)'; %horizontal
Extract_xD(NT)  = 1; 



%% Steps for finding open loop tf
% LTstep=  0*LT0*.2;
% LTstep_t = 10; %time
% 
% VBstep=  VB0*.1;
% VBstep_t = 10; %time
% 
% F0step=  1*F0*.1;
% F0step_t = 10; %time

%% Steps for simulation

F0step= 0.15; %0.15; 
F0step_t = 10; %time

F0step2= +0.1; % 0*(0.15-F0step); % To get to 1.65 in RII
F0step2_t = 80; %time

F0step3= 0; %+0.15; % 0*(0.15-F0step); % To get to 1.65 in RII
F0step3_t = 180; %time

pVstep=  +0.06; %(0.13-pV0);
pVstept= 50;

pVstep2 =  0; %+0.12; %-0*(0.13-pV0);
pVstep2t = 140;

% F0step= -0.1; %0.15; 
% F0step_t = 10; %time
% 
% F0step2= +0.25; % 0*(0.15-F0step); % To get to 1.65 in RII
% F0step2_t = 80; %time
% 
% F0step3= +0.15; % 0*(0.15-F0step); % To get to 1.65 in RII
% F0step3_t = 180; %time
% 
% pVstep=  -0.05; %(0.13-pV0);
% pVstept= 40;
% 
% pVstep2 =  +0.12; %-0*(0.13-pV0);
% pVstep2t = 140;
%% Simulate
tend=150;
a = sim('OneColA_R2b','SimulationMode','normal'); 
%%
xmin=0.95*ones(length(a.time),1);
Vmax_plot=4*ones(length(a.time),1);
Lmax_plot=10*ones(length(a.time),1);
PVtrans=0.1145;  % pV from R1 to R3
PVtransplot=PVtrans*ones(length(a.time),1);
Fmaxplot=Fmax*ones(length(a.time),1);
%% Generate J

%% Optimal values
J1 =  -0.5058; % at F=1.5 and Pv 0.07   -0.5571
J2 =  -0.5548; %-0.6117; % at F=1.65 and Pv 0.07     -0.6118
J3 = -0.3157; %-0.4230;  % at F=1.65 and Pv 0.13  -0.4230

Jplot=zeros(length(a.time),1);
Simsize = length(Jplot);
i=1;
for i=1:Simsize
    if a.time(i)<=10 %a.Fin(i) == 1.5 && a.pV(i)==0.07
        Jplot(i) = J1;
    elseif   a.time(i)>10 && a.time(i)<=50 % a.Fin(i) == 1.65 && a.pV(i)==0.07
        Jplot(i) = J2; 
    elseif a.time(i)>50 && a.time(i)<=80  %a.Fin(i) == 1.65 && a.pV(i)==0.13
        Jplot(i) = J3; 
    else
        Jplot(i) = -0.3212; % -0.52; %-0.4313;
    end
end

r1x=[10,10];
r1y=[-1 10];
r2x=[50 50];
r2y=r1y;
r3x=[80,80];
r3y=r1y;

%% Plot
figure
subplot(4,1,1)
p1=plot(a.time,a.xB);
hold on
%plot(a.t,a.xB)
p2=plot(a.time, xmin,'--','Color',[  0.9290    0.6940    0.1250]);
%xlabel('Time (s)')
%separate regions
plot(r1x,r1y, 'k--', 'Linewidth',0.5, 'Color',[0.5 0.5 0.5])
plot(r2x,r2y, 'k--', 'Linewidth',0.5, 'Color',[0.5 0.5 0.5])
plot(r3x,r3y, 'k--', 'Linewidth',0.5, 'Color',[0.5 0.5 0.5])
%
ylabel('x_B, (mol/mol)')
ylim([0.94 0.96])
title('$y_1 = x_B$', 'Interpreter','latex')
legend([p1,p2],'$x_B$','$x_B^{sp}$', 'Interpreter','latex','Orientation','horizontal')
legend boxoff
ytickformat('%.2f')


subplot(4,1,2)
p1=plot(a.time,a.xD);
hold on
p2= plot(a.time, a.xDsp,'k--');
p3=plot(a.time, xmin,'--','Color',[  0.9290    0.6940    0.1250]);
%xlabel('Time (s)')
ylabel('x_D, (mol/mol)')
title('$y_2 = x_D$', 'Interpreter','latex')
ylim([0.94 1])
%separate regions
plot(r1x,r1y, 'k--', 'Linewidth',0.5, 'Color',[0.5 0.5 0.5])
plot(r2x,r2y, 'k--', 'Linewidth',0.5, 'Color',[0.5 0.5 0.5])
plot(r3x,r3y, 'k--', 'Linewidth',0.5, 'Color',[0.5 0.5 0.5])
%
legend([p1,p2,p3],'$x_D$','$x_D^{sp}$','$x_D^{min}$', 'Interpreter','latex','Orientation','horizontal')
legend boxoff
ytickformat('%.2f')

subplot(4,1,3)
p1=plot(a.time, a.LT);
hold on
p2=plot(a.time, a.LTxD,'--');
p3=plot(a.time, a.LTSR,'--','Color', [0.4660    0.6740    0.1880]);
title('$u_1 = L$', 'Interpreter','latex')
%separate regions
plot(r1x,r1y, 'k--', 'Linewidth',0.5, 'Color',[0.5 0.5 0.5])
plot(r2x,r2y, 'k--', 'Linewidth',0.5, 'Color',[0.5 0.5 0.5])
plot(r3x,r3y, 'k--', 'Linewidth',0.5, 'Color',[0.5 0.5 0.5])
%
legend([p1,p2,p3],'$L$', '$L_{x_D}$','$L_{SRC}$','Interpreter','latex','Orientation','horizontal', 'Location','Southeast')
ylabel(' L (mol/s)')
ylim([2.5 3.5])
legend boxoff 
ytickformat('%.1f')

subplot(4,1,4)
p1=plot(a.time,a.VB);
hold on
p2=plot(a.time, Vmax_plot,'--','Color',[  0.9290    0.6940    0.1250]);
%separate regions
plot(r1x,r1y, 'k--', 'Linewidth',0.5, 'Color',[0.5 0.5 0.5])
plot(r2x,r2y, 'k--', 'Linewidth',0.5, 'Color',[0.5 0.5 0.5])
plot(r3x,r3y, 'k--', 'Linewidth',0.5, 'Color',[0.5 0.5 0.5])
%
%xlabel('Time (s)')
ylabel(' V (mol/s)')
title('$u_2 = V$', 'Interpreter','latex')
ylim([3.5 4.1])
legend([p1,p2],'$V$', '$V^{max}$' ,'Interpreter','latex','Orientation','horizontal', 'Location','Southeast')
legend boxoff
ytickformat('%.1f')
xlabel('Time (min)')
% plot(a.time,a.LT)
% hold on
% %plot(a.time, Lmax_plot,'--','Color',[  0.9290    0.6940    0.1250])
% %xlabel('Time (s)')
% ylabel('Reflux, L (mol/s)')
% ylim([2.5 3.5])
%legend('L', 'L^{max}')
%legend boxoff

%%
figure
subplot(3,1,1)
p1=plot(a.time, a.Fin);

hold on
p2=plot(a.time, a.Fin_xD);
p3=plot(a.time, a.Fin_sp,'--','Color',[   0.9290    0.6940    0.1250])
p4=plot(a.time, Fmaxplot,'--', 'Color', [0.3010    0.7450    0.9330], 'Linewidth',1)
%separate regions
plot(r1x,r1y, 'k--', 'Linewidth',0.5, 'Color',[0.5 0.5 0.5])
plot(r2x,r2y, 'k--', 'Linewidth',0.5, 'Color',[0.5 0.5 0.5])
plot(r3x,r3y, 'k--', 'Linewidth',0.5, 'Color',[0.5 0.5 0.5])
%
title('$d_1 = F^{sp}$', 'Interpreter','latex')
%xlabel('Time (s)')
ylabel('F, (mol/s)')
legend([p1,p2,p3,p4],'F','$F_{x_D}$',  '$F^{sp}$','$F^{max}$','Interpreter','latex','Orientation','horizontal')
legend boxoff 
ylim([1.4 2])
ytickformat('%.1f')
% plot(a.time, a.Fin)
% ylabel('F (mol/s)')

subplot(3,1,2)
p1=plot(a.time, a.pV);
hold on
p2=plot(a.time, PVtransplot, '--','Color',[0.9290    0.6940    0.1250]);
ylim([0.01 0.15])
%separate regions
plot(r1x,r1y, 'k--', 'Linewidth',0.5, 'Color',[0.5 0.5 0.5])
plot(r2x,r2y, 'k--', 'Linewidth',0.5, 'Color',[0.5 0.5 0.5])
plot(r3x,r3y, 'k--', 'Linewidth',0.5, 'Color',[0.5 0.5 0.5])
%
ylabel('p_V ($/mol)')
title('$d_2 = p_V$', 'Interpreter','latex')
legend([p1,p2],'$p_V$', 'RI $\rightarrow$ RIII','Interpreter','latex','Orientation','horizontal')
legend boxoff 
ytickformat('%.2f')

subplot(3,1,3)
Jactual=a.Jreal; %-0.05;
p1=plot(a.time,Jactual);
hold on
%plot(a.time,a.Jreal)
p2=plot(a.time, Jplot,'--');
%separate regions
plot(r1x,r1y, 'k--', 'Linewidth',0.5, 'Color',[0.5 0.5 0.5])
plot(r2x,r2y, 'k--', 'Linewidth',0.5, 'Color',[0.5 0.5 0.5])
plot(r3x,r3y, 'k--', 'Linewidth',0.5, 'Color',[0.5 0.5 0.5])

legend([p1,p2],'$J$', '$J^{opt}$','Interpreter','latex','Orientation','horizontal')
xlabel('Time (s)')
title('Cost ($J$)', 'Interpreter','latex')
ylabel('J ($/s)')
legend boxoff 
ylim([-0.9 0])
ytickformat('%.1f')
xlabel('Time (min)')

%%
figure
subplot(2,1,1)
p1=plot(a.time, a.Fin);
hold on
p2=plot(a.time, a.Fin_xD);
p3=plot(a.time, a.Fin_sp,'--','Color',[   0.9290    0.6940    0.1250])
p4=plot(a.time, Fmaxplot,'--', 'Color', [0.3010    0.7450    0.9330], 'Linewidth',1)
%separate regions
plot(r1x,r1y, 'k--', 'Linewidth',0.5, 'Color',[0.5 0.5 0.5])
plot(r2x,r2y, 'k--', 'Linewidth',0.5, 'Color',[0.5 0.5 0.5])
plot(r3x,r3y, 'k--', 'Linewidth',0.5, 'Color',[0.5 0.5 0.5])
%
title('$d_1 = F^{sp}$', 'Interpreter','latex')
%xlabel('Time (s)')
ylabel('F, (mol/s)')
legend([p1,p2,p3,p4],'F','$F_{x_D}$',  '$F^{sp}$','$F^{max}$','Interpreter','latex','Orientation','horizontal')
legend boxoff 
ylim([1.4 2])
ytickformat('%.1f')
% plot(a.time, a.Fin)
% ylabel('F (mol/s)')

subplot(2,1,2)
p1=plot(a.time, a.pV);
hold on
p2=plot(a.time, PVtransplot, '--','Color',[0.9290    0.6940    0.1250]);
ylim([0.01 0.15])
%separate regions
plot(r1x,r1y, 'k--', 'Linewidth',0.5, 'Color',[0.5 0.5 0.5])
plot(r2x,r2y, 'k--', 'Linewidth',0.5, 'Color',[0.5 0.5 0.5])
plot(r3x,r3y, 'k--', 'Linewidth',0.5, 'Color',[0.5 0.5 0.5])
%
ylabel('p_V ($/mol)')
title('$d_2 = p_V$', 'Interpreter','latex')
legend([p1,p2],'$p_V$', 'RI $\rightarrow$ RIII','Interpreter','latex','Orientation','horizontal')
legend boxoff 
ytickformat('%.2f')
xlabel('Time (min)')
%%
figure
Jactual=a.Jreal; %-0.05;
p1=plot(a.time,Jactual);
hold on
%plot(a.time,a.Jreal)
p2=plot(a.time, Jplot,'--');
%separate regions
plot(r1x,r1y, 'k--', 'Linewidth',0.5, 'Color',[0.5 0.5 0.5])
plot(r2x,r2y, 'k--', 'Linewidth',0.5, 'Color',[0.5 0.5 0.5])
plot(r3x,r3y, 'k--', 'Linewidth',0.5, 'Color',[0.5 0.5 0.5])

legend([p1,p2],'$J$', '$J^{opt}$','Interpreter','latex','Orientation','horizontal')
xlabel('Time (s)')
title('Cost function ($J$)', 'Interpreter','latex')
ylabel('J ($/s)')
legend boxoff 
ylim([-0.9 0])
ytickformat('%.1f')
xlabel('Time (min)')

text(3,-0.8,'R I')
text(30,-0.8,'R II')
text(60,-0.8,'R III')
text(85,-0.8,'R IV')
%%
%% Plot
figure
subplot(5,1,1)
p1=plot(a.time,a.xB);
hold on
%plot(a.t,a.xB)
p2=plot(a.time, xmin,'k--'); %,'Color',[  0.9290    0.6940    0.1250]);
%xlabel('Time (s)')
%separate regions
plot(r1x,r1y, 'k--', 'Linewidth',0.5, 'Color',[0.5 0.5 0.5])
plot(r2x,r2y, 'k--', 'Linewidth',0.5, 'Color',[0.5 0.5 0.5])
plot(r3x,r3y, 'k--', 'Linewidth',0.5, 'Color',[0.5 0.5 0.5])
%
ylabel('x_B, (mol/mol)')
ylim([0.94 0.96])
title('$y_1 = x_B$', 'Interpreter','latex')
legend([p1,p2],'$x_B$','$x_B^{sp}$', 'Interpreter','latex','Orientation','horizontal')
legend boxoff
ytickformat('%.2f')
text(3,0.955,'R I')
text(30,0.955,'R II')
text(65,0.955,'R III')
text(85,0.955,'R IV')

subplot(5,1,2)
p1=plot(a.time,a.xD);
hold on
p2= plot(a.time, a.xDsp,'k--');
%p3=plot(a.time, xmin,'--','Color',[  0.9290    0.6940    0.1250]);
%xlabel('Time (s)')
ylabel('x_D, (mol/mol)')
title('$y_2 = x_D$', 'Interpreter','latex')
ylim([0.94 1])
%separate regions
plot(r1x,r1y, 'k--', 'Linewidth',0.5, 'Color',[0.5 0.5 0.5])
plot(r2x,r2y, 'k--', 'Linewidth',0.5, 'Color',[0.5 0.5 0.5])
plot(r3x,r3y, 'k--', 'Linewidth',0.5, 'Color',[0.5 0.5 0.5])
%
%legend([p1,p2,p3],'$x_D$','$x_D^{sp}$','$x_D^{min}$', 'Interpreter','latex','Orientation','horizontal')
legend([p1,p2],'$x_D$','$x_D^{sp}$', 'Interpreter','latex','Orientation','horizontal')
legend boxoff
ytickformat('%.2f')

subplot(5,1,3)

p1=plot(a.time, a.Fin, 'Color', [0.3010    0.7450    0.9330]);
hold on
p2=plot(a.time, a.Fin_xD1,'--');
p3=plot(a.time, a.Fin_sp,'k--'); %,'Color',[   0.9290    0.6940    0.1250])
%p4=plot(a.time, Fmaxplot,'--','Color',[   0.9290    0.6940    0.1250]); %, 'Color', [0.3010    0.7450    0.9330], 'Linewidth',1)
%separate regions
plot(r1x,r1y, 'k--', 'Linewidth',0.5, 'Color',[0.5 0.5 0.5])
plot(r2x,r2y, 'k--', 'Linewidth',0.5, 'Color',[0.5 0.5 0.5])
plot(r3x,r3y, 'k--', 'Linewidth',0.5, 'Color',[0.5 0.5 0.5])
%
title('$y_3 = F$', 'Interpreter','latex')
%xlabel('Time (s)')
ylabel('F, (mol/s)')
%legend([p1,p2,p3,p4],'F',  '$F_{x_D}$','$F^{sp}$','$F^{max}$','Interpreter','latex','Orientation','horizontal')
legend([p1,p2,p3],'F',  '$F_{x_D}$','$F^{sp}$','Interpreter','latex','Orientation','horizontal')
%legend([p1,p3,p4],'F',  '$F^{sp}$','$F^{max}$','Interpreter','latex','Orientation','horizontal')
legend boxoff 
ylim([1.4 2])
ytickformat('%.1f')
%

subplot(5,1,4)
p1=plot(a.time, a.LT);
hold on
p2=plot(a.time, a.LTxD,'--');
p3=plot(a.time, a.LTSR,'--','Color', [0.4660    0.6740    0.1880]);
title('$u_1 = L$', 'Interpreter','latex')
%separate regions
plot(r1x,r1y, 'k--', 'Linewidth',0.5, 'Color',[0.5 0.5 0.5])
plot(r2x,r2y, 'k--', 'Linewidth',0.5, 'Color',[0.5 0.5 0.5])
plot(r3x,r3y, 'k--', 'Linewidth',0.5, 'Color',[0.5 0.5 0.5])
%
legend([p1,p2,p3],'$L$', '$L_{x_D}$','$L_{x_B}$','Interpreter','latex','Orientation','horizontal', 'Location','Southeast')
ylabel('L (mol/s)')
ylim([2.5 3.5])
legend boxoff 
ytickformat('%.1f')

subplot(5,1,5)
p1=plot(a.time,a.VB);
hold on
p2=plot(a.time, Vmax_plot,'--','Color',[  0.9290    0.6940    0.1250]);
%separate regions
plot(r1x,r1y, 'k--', 'Linewidth',0.5, 'Color',[0.5 0.5 0.5])
plot(r2x,r2y, 'k--', 'Linewidth',0.5, 'Color',[0.5 0.5 0.5])
plot(r3x,r3y, 'k--', 'Linewidth',0.5, 'Color',[0.5 0.5 0.5])
%
%xlabel('Time (s)')
ylabel('V (mol/s)')
title('$u_2 = V$', 'Interpreter','latex')
ylim([3.5 4.1])
legend([p1,p2],'$V$', '$V^{max}$' ,'Interpreter','latex','Orientation','horizontal', 'Location','Southeast')
legend boxoff
ytickformat('%.1f')
xlabel('Time (min)')

%%
% figure
% subplot(2,1,1)
% plot(a.time, a.LT)
% hold on
% plot(a.time, a.LTxD,'--')
% plot(a.time, a.LTSR,'--')
% legend('L', 'L_{xD}','L_{SRC}')
% xlabel('Time (s)')
% ylabel('Reflux, (mol/s)')
% 
% subplot(2,1,2)
% plot(a.time, a.error_xD)
%%
%figure
% plot(a.time, a.Fin_xD)
% hold on
% plot(a.time, a.Fin_sp)
% plot(a.time, Fmaxplot,'--')
% plot(a.time, a.Fin,'--')
% xlabel('Time (s)')
% ylabel('F, (mol/s)')
% legend('F_{xD}',  'F^{sp}','Fmax','F')

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