
clc
close all
clear all
%====================================
%====Simulates nominal case



p_real=parameters_real;
p_nom=parameters_real;
sim('optimal_batch')

%Recovering data
time_simu=concentrations.time;
%data for "nominal condition"
ca1=concentrations.signals(1).values(1,1,:);
cb1=concentrations.signals(1).values(1,2,:);
V1=concentrations.signals(1).values(1,5,:);
u1=inputs.signals.values(:,1,1);

ca1=ca1(:);
cb1=cb1(:);
V1=V1(:);

%Initial guess
x0=[ca1 cb1 V1 u1 ];

[solution_n,J0]=propt(p_real,x0);

yopt_nom=[solution_n.x1_p solution_n.x2_p solution_n.x3_p]';

unom.time= time_simu;
% Knom.time=Knom.time';
unom.signals.values=u1;
% Knom.signals.dimensions=2;
unom.signals.dimensions=[1];



ynom.time= solution_n.time;
% Knom.time=Knom.time';
ynom.signals.values=[solution_n.x1 solution_n.x2 solution_n.x3];
% Knom.signals.dimensions=2;
ynom.signals.dimensions=[3];



%====================================
%====Simulates disturbed case
%Disturbance on k1 only
%====================================

p_real=parameters_real;
 p_real.k1=0.0424;
%  p_real.k1=p_real.k1*0.95;
%  p_real.k2=0.1536;
[solution_d1,J1]=propt(p_real,x0);


yopt_dist1=[solution_d1.x1_p solution_d1.x2_p solution_d1.x3_p]';

% [sol_gain1,~]=propt_gain(p_real,x0,solution_n);
% y_d1=[sol_gain1.x1_p sol_gain1.x2_p  sol_gain1.x3_p]';


%====================================
%Disturbance on k2 only
%====================================
p_real=parameters_real;
%   p_real.k2=p_real.k2*1.05;
 p_real.k2=0.1536;


[solution_d2,J2]=propt(p_real,x0);
yopt_dist2=[solution_d2.x1_p solution_d2.x2_p solution_d2.x3_p]';


% [sol_gain2,~]=propt_gain(p_real,x0,solution_n);
% y_d2=[sol_gain2.x1_p sol_gain2.x2_p  sol_gain2.x3_p]';




hold on
%%

% p_real.k1=0.0424;

%====================================
%========Computes optimal sensitivity F(t)


Hv=[];
Fv=[];
Fdv=[];
Kv=[];
ni=2;
% 
%  d1=p_nom.k1*0.95-p_nom.k1;
% 
%  d2=p_nom.k2*1.05-p_nom.k2;
% 
 d1=0.0424-p_nom.k1;
 d2=0.1536-p_nom.k2;

copt_ref=[];
copt_refd1=[];
copt_refd2=[];
for t=ni:length(solution_n.T_p)
    
    
    
    %     yopt_nom=[ca1(t); cb1(t); V1(t)];
    %
    %     yopt_dist1=[solution_d1.x1_p(t); solution_d1.x2_p(t); solution_d1.x3_p(t)];
    %
    %     yopt_dist2=[solution_d2.x1_p(t); solution_d2.x2_p(t); solution_d2.x3_p(t)];
    %
    
    
    
    
    F=[(yopt_dist1(1:3,t)-yopt_nom(1:3,t))/d1   (yopt_dist2(1:3,t)-yopt_nom(1:3,t))/d2    ];
    Fy=[(yopt_dist1(1:2,t)-yopt_nom(1:2,t))/d1   (yopt_dist2(1:2,t)-yopt_nom(1:2,t))/d2    ];
    
%     Fd=[(y_d1(1:3,t)-yopt_nom(1:3,t))/d1   (y_d2(1:3,t)-yopt_nom(1:3,t))/d2    ];
    
    
    
    Fu=[(solution_d1.T_p(t)-solution_n.T_p(t))/d1   (solution_d2.T_p(t)-solution_n.T_p(t))/d2 ];
    
    
    
    Fin=pinv(F);
    K=Fu*Fin;
    Kv=[Kv ; K];
    
    Fv=[Fv F];
    %     Fdv=[Fdv Fd];
    
    H=null(F')';
    Hv=[ Hv ;   H ];
    
    copt_ref=[copt_ref H*yopt_nom(1:3,t)];
    copt_refd1=[copt_refd1 H*yopt_dist1(1:3,t)];
    copt_refd2=[copt_refd2 H*yopt_dist2(1:3,t)];
    
end
% t=time(ni:end);
% plot(t,copt_ref,t,copt_refd1,t,copt_refd2)
% hold on


% time=linspace(0,250,length(solution_n.T_p));
% time=solution_d1.time;
% time=time';


time=solution_d1.time;
Hsim.time= time(ni:end);
% Knom.time=Knom.time';
Hsim.signals.values=Hv;
% Knom.signals.dimensions=2;
Hsim.signals.dimensions=[3];



Ksim.time= time(ni:end);
% Knom.time=Knom.time';
Ksim.signals.values=Kv;
% Knom.signals.dimensions=2;
Ksim.signals.dimensions=[3];


cset.time= time(ni:end);
% Knom.time=Knom.time';
cset.signals.values=copt_ref';
% Knom.signals.dimensions=2;
cset.signals.dimensions=[1];


save('nullspace_sim_parameters')

%%


%%
% figure(2)
% subplot(311);
% plot(time_sim(3:end),Hv(:,1),'Linewidth',1.)
% axis tight
% hold on
% ylabel('h_1','FontSize',14);
% ha=gca; set(ha,'FontSize',14);
% subplot(312);
% plot(time_sim(3:end),Hv(:,2),'Linewidth',1.)
% axis tight
% ylabel('h_1','FontSize',14);
% ha=gca; set(ha,'FontSize',14);
% hold on
% subplot(313);
% plot(time_sim(3:end),Hv(:,3),'Linewidth',1.)
% ylabel('h_3','FontSize',14)
% ha=gca; set(ha,'FontSize',14);
% hold on
% xlabel('Time [min]','FontSize',14)
% axis tight
% %%
