% Calculate F matrix
clear all; close all; clc;

global coldata dyndata hydraulic termodata UU alfa

% COLUMN DATA %%
%Pass the options for optimization
% Number of stages in each section (C11,C12,C21,C22,C23,C24)
coldata.Nt = [12 12 12 12 12 12];
% Initial holdup in reboiler and condenser
coldata.M0 = [1 1 1 1 1 1 1 1 1 1 1 1];
%Boiling points of ethanol, propanol and n-butanol
coldata.Tboil = [78.37 97 117.4];
%Hydraulics
hydraulic.k1 = 1/0.063;
% Relative volatility of Ethanol, Propanol and n-Butanol with respect to the
%heaviest component n-Butanol
alfa=[4.215374389 2.102194688 1];

% Optimization 
% Re-optimize for disturbances (F, zA, zB, V, and RV)
% Load output temperature at the nominal values Y0
load Y0
% All linear equality and inequality constraints given in specs, the
% Disturbances can be directly set in specs file
%Specs
% Initial values for optimization made global
load Nominal_final.mat  
dyndata.x_nom = x_nom;
dyndata.u_nom = u_nom;
dyndata.temp_nom = temp_nom;
options = optimset('fmincon');
options = optimset(options,'TolFun',1e-10,'TolCon',1e-13,'MaxFunEvals',500000,'Display','iter',...
'Algorithm','active-set');

%
Specs
%disturbance in feed flow rate dd1
dd1=1e-5;                        % perturbation in feed flow rate
beq(1)=dyndata.u_nom(6)+dd1;

%Run the optimization with disturbance in feed flow rate dd1 while
%keeping other values same, run the loop to plot profile for disturbance in
%feed
[x_opt,fval,exitflag,output,lambda,Grad,Hessian] = fmincon(@(x) Objective(x),[dyndata.x_nom;dyndata.u_nom],Ain,bin,Aeq,beq,[],[],@(x) Petlyuk_nonlcon(x),options);
x_nom=x_opt(1:im_mS1);u_nom=x_opt(im_mS1+1:end);
x_nom=x_opt(1:225);u_nom=x_opt(226:end);     %It is same as above

dxdt = norm(Petlyuk_NLmodel(0,x_nom, u_nom));
[~, Temp]=Petlyuk_NLmodel(0,x_nom,u_nom);
ydd1= [Temp];

%disturbance in compositions zF1

beq(1)=dyndata.u_nom(6);     %Nominal feed same as before F0=1
dd2 = 1e-5;
beq(2)=dyndata.u_nom(7)+dd2; 

%Run the optimization with the disturbance in feed compositions
[x_opt,fval,exitflag,output,lambda,Grad,Hessian] = fmincon(@(x) Objective(x),[dyndata.x_nom;dyndata.u_nom],Ain,bin,Aeq,beq,[],[],@(x) Petlyuk_nonlcon(x),options);

x_nom=x_opt(1:225);u_nom=x_opt(226:end);

dxdt = norm(Petlyuk_Nlmodel(0,x_nom, u_nom));
[~, Temp]=Petlyuk_NLmodel(0,x_nom,u_nom);
ydd2= [Temp];

%disturbance in compositions zF2, B in feed
dd3= 1e-5;
beq(2)=dyndata.u_nom(7);     %Nominal composition A 
beq(3)=dyndata.u_nom(8)+dd3;  

%Run the optimization with the disturbance in feed compositions
[x_opt,fval,exitflag,output,lambda,Grad,Hessian] = fmincon(@(x) Objective(x),[dyndata.x_nom;dyndata.u_nom],Ain,bin,Aeq,beq,[],[],@(x) Petlyuk_nonlcon(x),options);

x_nom=x_opt(1:225);u_nom=x_opt(226:end);

dxdt = norm(Petlyuk_NLmodel(0,x_nom, u_nom));
[~, Temp]=Petlyuk_NLmodel(0,x_nom,u_nom);
ydd3= [Temp];
%Give disturbance in the Vmax (boilup) and make the last disturbance
%to the nominal
dd4=1e-5;
beq(3)=dyndata.u_nom(8);
beq(5)= dyndata.u_nom(2)+dd4;

%Run the optimization with disturbance in the boilup but keeping other
%disturbances to the nominal
[x_opt,fval,exitflag,output,lambda,Grad,Hessian] = fmincon(@(x) Objective(x),[dyndata.x_nom;dyndata.u_nom],Ain,bin,Aeq,beq,[],[],@(x) Petlyuk_nonlcon(x),options);

x_nom=x_opt(1:225);u_nom=x_opt(226:end);

dxdt = norm(Petlyuk_NLmodel(0,x_nom, u_nom));
[~, Temp]=Petlyuk_NLmodel(0,x_nom,u_nom);
ydd4= [Temp];

% Change in RV
dd5 = 1e-5;
beq(5)=dyndata.u_nom(2);
beq(6)=dyndata.u_nom(4)+dd5;

%Run the optimization
[x_opt,fval,exitflag,output,lambda,Grad,Hessian] = fmincon(@(x) Objective(x),[dyndata.x_nom;dyndata.u_nom],Ain,bin,Aeq,beq,[],[],@(x) Petlyuk_nonlcon(x),options);

%x_nom=x_opt(1:225);u_nom=x_opt(226:end);

dxdt = norm(Petlyuk_NLmodel(0,x_nom, u_nom));
[sys Temp]=Petlyuk_NLmodel(0,x_nom,u_nom);
ydd5= [Temp];


% Calculate the sensitivity matrix F and store the value, 
F1=[(ydd1-Y0)/dd1 (ydd2-Y0)/dd2 (ydd3-Y0)/dd3 (ydd4-Y0)/dd4 (ydd5-Y0)/dd5];