%====================================
%====Simulates disturbed case
close all
clear all
load('nullspace_sim_parameters.mat')
p_real=parameters_real;
p_nom=parameters_real;

DistCase=2;

switch DistCase
    %nominal case
    case 0
        p_real=p_nom;
    
    case 1
        %==case 1===
        p_real.k1=0.0424;
        p_real.k2=0.1024;
    case 2
        %==case 2===
        p_real.k1=0.0424;
         p_real.k2=0.1536;

    case 3
        %==case 3===
        p_real.k1=0.0636;
        p_real.k2=0.1024;
    case 4
        %==case 4===
        p_real.k1=0.0636;
        p_real.k2=0.1536;
        
    otherwise
        
end

[solution_re,Jr]=propt(p_real,x0);

uopt.time= solution_re.time;
% Knom.time=Knom.time';
uopt.signals.values=solution_re.u;
% Knom.signals.dimensions=2;
uopt.signals.dimensions=[1];


%% PID tuning
Kc=3;
tauI=30;
tauD=3;




sim('self_opt_batch')

%data for "disturbed condition"

%Self-optimizing
ca_self=concentrations_self(1,1,:);
cb_self=concentrations_self(1,2,:);
V_self=concentrations_self(1,5,:);
u_self=inputs_self(:,1,1);
ca_self=ca_self(:);
cb_self=cb_self(:);
V_self=V_self(:);
nc=p_real.ca0*p_real.V0-ca_self(:).*V_self(:);
nd=(1/2)*(p_real.cb0*p_real.V0+p_real.cb_in.*(V_self(:)-p_real.V0)-cb_self(:).*V_self(:)-p_real.ca0*p_real.V0+ca_self(:).*V_self(:));
cost_self=-(nd-nc);

%Nominal solution
ca_nom=concentrations_nom(1,1,:);
cb_nom=concentrations_nom(1,2,:);
V_nom=concentrations_nom(1,5,:);
ca_nom=ca_nom(:);
cb_nom=cb_nom(:);
V_nom=V_nom(:);

u_nom=inputs_nom(:,1,1);
nc2=p_real.ca0*p_real.V0-ca_nom(:).*V_nom(:);
nd2=(1/2)*(p_real.cb0*p_real.V0+p_real.cb_in.*(V_nom(:)-p_real.V0)-cb_nom(:).*V_nom(:)-p_real.ca0*p_real.V0+ca_nom(:).*V_nom(:));
cost_nom_input=-(nd2-nc2);

%Static output feedback
ca_static=concentrations_static(1,1,:);
cb_static=concentrations_static(1,2,:);
V_static=concentrations_static(1,5,:);
ca_static=ca_static(:);
cb_static=cb_static(:);
V_static=V_static(:);

u_static=inputs_static(:,1,1);
ncs=p_real.ca0*p_real.V0-ca_static(:).*V_static(:);
nds=(1/2)*(p_real.cb0*p_real.V0+p_real.cb_in.*(V_static(:)-p_real.V0)-cb_static(:).*V_static(:)-p_real.ca0*p_real.V0+ca_static(:).*V_static(:));
cost_static=-(nds-ncs);


%Reoptimized
ca_opt=concentrations_opt(1,1,:);
cb_opt=concentrations_opt(1,2,:);
V_opt=concentrations_opt(1,5,:);
ca_opt=ca_opt(:);
cb_opt=cb_opt(:);
V_opt=V_opt(:);

u_opt=inputs_opt(:,1,1);
ncs=p_real.ca0*p_real.V0-ca_opt(:).*V_opt(:);
nds=(1/2)*(p_real.cb0*p_real.V0+p_real.cb_in.*(V_opt(:)-p_real.V0)-cb_opt(:).*V_opt(:)-p_real.ca0*p_real.V0+ca_opt(:).*V_opt(:));
cost_opt=-(nds-ncs);

cost_nom=cost_nom_input;

span=(p_real.u_max-p_real.u_min)/100;
u_opt=(u_opt-p_real.u_min)/span;
u_nom=(u_nom-p_real.u_min)/span;
u_self=(u_self-p_real.u_min)/span;
H1=H_sim(:,1);
H2=H_sim(:,2);
H3=H_sim(:,3);
T=table(time,H1,H2,H3,c_sp,c_self,cost_opt,u_opt,ca_opt,cb_opt,V_opt,cost_self, u_self,ca_self,cb_self,V_self,cost_nom,u_nom,ca_nom,cb_nom,V_nom);
path='C:\Users\Vinicius\Dropbox\phd_related\my papers\NTNU\DYCOPS2016\pics\';
writetable(T,[path 'TableCase' num2str(DistCase) '.dat'],'Delimiter','\t');





%====================================
%========Ploting results
close all
figure(1)
plot(time,u_opt,'r',time,u_static,'b',time,u_nom,'k-.',time,u_self, 'linewidth',1.2 )
set(gca,'FontSize',12)
ylabel('$u, l/min$','interpreter','latex','FontSize',14)
xlabel('$time, min$','interpreter','latex','FontSize',14)
axis tight
legend('Optimal','Proposed','Openloop','Location','Southeast')
legend boxoff;

set(gcf, 'PaperPosition', [0 0 10 6]); %Position plot at left hand corner with width 5 and height 5.
set(gcf, 'PaperSize', [10 6]); %Set the paper to have width 5 and height 5.
% saveas(gcf, 'ucstr', 'pdf') %Save figure

figure(5)
subplot(211)
plot(time,ca_opt,'r',time,ca_static,'b',time,ca_nom,'k-.', 'linewidth',1.2 )
set(gca,'FontSize',12)
ylabel('$c_A, mol/l$','interpreter','latex','FontSize',14)
xlabel('$time, min$','interpreter','latex','FontSize',14)
axis tight
legend('Optimal','Proposed','Openloop','Location','Northeast')
legend boxoff;

subplot(212)
plot(time,cb_opt,'r',time,cb_static,'b',time,cb_nom,'k-.', 'linewidth',1.2 )
set(gca,'FontSize',12)
ylabel('$c_B, mol/l$','interpreter','latex','FontSize',14)
xlabel('$time, min$','interpreter','latex','FontSize',14)
axis tight

set(gcf, 'PaperPosition', [0 0 7 5]); %Position plot at left hand corner with width 5 and height 5.
set(gcf, 'PaperSize', [7 5]); %Set the paper to have width 5 and height 5.
% saveas(gcf, 'meas_cstr', 'pdf') %Save figure




figure(2)
subplot(211);
plot(time,ca_self,'b-.',time,ca_nom,'k-',time,ca_static,'g', solution_re.time,solution_re.x1,'r.','Linewidth',1.)
hold on
ylabel('ca [mol/L]','FontSize',14);
ha=gca; set(ha,'FontSize',14);
subplot(212);
plot(time,cb_self,'b-.',time,cb_nom,'k-',time,cb_static,'g', solution_re.time,solution_re.x2,'r.','Linewidth',1.)
hold on

xlabel('Time [min]','FontSize',14)
ylabel('cb [mol/L]','FontSize',14);
ha=gca; set(ha,'FontSize',14);
legend('self-opt', 'nominal','static output','reoptimized')

figure(3)
subplot(211);
plot(time,V_self,'b-.',time,V_nom,'k-',time,V_static,'g',solution_re.time,solution_re.x3,'r.','Linewidth',1.)
hold on
ylabel('V [L]','FontSize',14);
ha=gca; set(ha,'FontSize',14);
subplot(212);
plot(time,u_self,'b-.',time,u_nom,'k-',time,u_static,'g', solution_re.time,solution_re.u,'r.','Linewidth',1.)
hold on
xlabel('Time [min]','FontSize',14)
ylabel('u [L/min]','FontSize',14);
ha=gca; set(ha,'FontSize',14);
legend('self-opt', 'nominal','static output','reoptimized')
figure(4)
plot(time,cost_opt-cost_self,'b-.',time,cost_opt-cost_nom_input,'k-.',time,cost_opt-cost_static,'g')
ha=gca; set(ha,'FontSize',14);
xlabel('Time [min]','FontSize',14)
ylabel('Loss [mol]','FontSize',14);
legend('self-opt', 'nominal','static output')
% % c_s=zeros(size(c));
% % figure(4)
% % plot(time,c,time,c_s,'r-.')
% % ha=gca; set(ha,'FontSize',14);
% % ylabel('c=H\Delta y','FontSize',14);
% % xlabel('Time [min]','FontSize',14)

figure
plot(time,c_sp,time,c_self,time,c_static,time,c_opt)
legend('c_sp','nullspace','static','opt')

% legend('self-opt', 'input-nom','feed-nom','reoptimized')
tilefigs

Jre=cost_opt(end)
Jself=cost_self(end)
Jnom=cost_nom_input(end)
Jstatic=cost_static(end)


loss_self=Jre-Jself
loss_nom=Jre-Jnom
loss_static=Jre-Jstatic
%%
%%
