% Clearing of the workspace and closing of all windows
clc; clear; close all;

% Definition of the current path folder
FileName = mfilename('fullpath');
[directory,~,~] = fileparts(FileName);
[dirmain,~,~] = fileparts(directory);

% Import of CASADI
addpath([dirmain '\Util1. Casadi3.1.0'])
addpath([dirmain '\Util2. Functions'])
import casadi.*

% Definition of decision variables
dec.slave = false;
dec.loss  = true;
dec.save  = true;
dec.plot  = true;
dec.Htype = 'In_Out'; % Norm_2, Norm_1, Norm_inf, Max_Gain, In_Out
dec.QP    = false;

% Definition of the chosen H and the measured disturbances
H_var = [1];          % Calculated and considered H matrices
dist_var = [1 2 3 4 5];       % Considered disturbances for SOC
if numel(dist_var) > 4
    H_var = H_var(H_var ~= 2);
end

if ~dec.loss
    dec.plot = false;
end

% Load of the model
[func,dae,d,lb,ub,n,Cineq] = AmmonReac_NewDist(1);

% Definition of the save name
if dec.save
    savename = 'SS_opt_SOC_dist';
    savename_H = 'H_dist';
    for k = 1:6
        if any(dist_var == k)
            savename = [savename num2str(k)];
            savename_H = [savename_H num2str(k)];
        end
    end
    savename   = [savename '_' dec.Htype];
    savename_H = [savename_H '_' dec.Htype];
    savename   = [savename '.mat'];
    savename_H = [savename_H '.mat'];
end    

% Definition of feed variables
in.nom = zeros(4,1);
in.nom(1) = 252/3.6;        % Feed flow (kg/s)
in.nom(2) = 200;            % Pressure [bar]
in.nom(3) = 250;            % Temperature [C]
in.nom(4) = 0.08;           % Mass fraction ammonia [-]
in.nom(5) = 3;              % Hydrogen/nitrogen molar ratio [-]
in.nom(6) = 1;              % Catalyst activity \
nom_opt.d = zeros(n.d,1);

% Definition of the initial guesses and redefinition into x and z
load([dirmain '\Data\Init_In.mat'])

%% Part a) Root finding for the modified system

% Combination of the variables
w_root = [dae.x;dae.z];
w0     = [x0;z0];

% Combination of the equality constraints and substitution of inlet values
g = vertcat(dae.ode,dae.alg);
g_root = substitute(g,d,nom_opt.d);
g_root = substitute(g_root,dae.p(1:n.u),p0(1:n.u));

% Definition and solving of the system
g_fun = Function('g_fun',{w_root},{g_root});
G = rootfinder('G','newton',g_fun);
[root.comb] = G('i0',w0);

% Calculation of the number of CSTRs in total
N = size(dae.x,1);

% Definition of indices used in the solution
ind.T = [3*N+1 1:N/3       3*N+2 N/3+1:2*N/3   3*N+3  2*N/3+1:N]; % 3*N+4 
ind.w = [3*N+5 N+1:4*N/3   3*N+6 4*N/3+1:5*N/3 3*N+7  5*N/3+1:2*N];
ind.R = [3*N+8 2*N+1:7*N/3 3*N+9 7*N/3+1:8*N/3 3*N+10 8*N/3+1:3*N];

ind.x = 1:N;
ind.y = [3*N+4 3*N+1 N/3 3*N+2 N*2/3 3*N+3 N];
ind.z = (N+1):(3*N+10);

ind.u = (3*N+11):(3*N+13);

% ind.y = [ind.T ind.w];
% ind.y = ind.T([1:5 12:17 23:28]);
ind.y = ind.T;
n.y = length(ind.y);

% Reassignment of variables
root.x = full(root.comb.o0(ind.x));
root.z = full(root.comb.o0(ind.z));
root.T = full(root.comb.o0(ind.T));
root.w = full(root.comb.o0(ind.w));
root.R = full(root.comb.o0(ind.R));

x0 = root.x;
z0 = root.z;

fprintf('\n\n\nThe root-finding is completed. \n\n\n')

%% Part b) Steady state optimization of the system

% Combination of the variables
w_opt = [dae.x; dae.z; dae.p(1:n.u)];
lbw   = [lb.x;  lb.z;  lb.p(1:n.u)];
ubw   = [ub.x;  ub.z;  ub.p(1:n.u)];
w0    = [x0;    z0;    p0(1:n.u)];

% Combination of the equality and inequality constraints
gadd = [g; Cineq];
lbg  = [zeros(size(gadd,1)-1,1); -1];
ubg  = [zeros(size(gadd))];

% Substitution of the inlet values
g = substitute(gadd    ,d ,nom_opt.d);
J = substitute(dae.quad,d ,nom_opt.d);

% Definition of the NLP function
nlp = struct('x',w_opt,'f',J,'g',g);
solver = nlpsol('solver','ipopt',nlp);
nom_opt.comb = solver('x0',w0,'lbx',lbw,'ubx',ubw,'lbg',lbg,'ubg',ubg);

% Reassignment of variables
nom_opt.x = full(nom_opt.comb.x(ind.x));
nom_opt.z = full(nom_opt.comb.x(ind.z));
nom_opt.y = full(nom_opt.comb.x(ind.y));

nom_opt.T = full(nom_opt.comb.x(ind.T));
nom_opt.w = full(nom_opt.comb.x(ind.w));
nom_opt.R = full(nom_opt.comb.x(ind.R));

nom_opt.u = full(nom_opt.comb.x(ind.u));
nom_opt.p = [full(nom_opt.u); zeros(4,1)];
nom_opt.f = full(nom_opt.comb.f);

fprintf('\n\n\nThe steady-state optimization is completed. \n\n\n')

%% Part c) Calculation of the plant gains and sensitivity matrix
% Redefinition of the constraints, cost function and variables
g = [dae.ode; dae.alg];
J = dae.quad;
u = dae.p(1:n.u);
z = [dae.x; dae.z];

% Definition of the rootfinder function
rf_function = casadi.Function('rf_function',{z,u,d},...
                    {g,J,z},{'z','u','d'},{'g','J','z'});
rf  = casadi.rootfinder('rf','newton',rf_function);

% Definition of the casadi functions
fun.Gy  = rf.jacobian('u','z');
fun.Gd  = rf.jacobian('d','z');
fun.Juu = rf.hessian('u','J');
fun.Jud = jacobian(rf.jacobian('u','J'),'d');

% Calculation of the matrices
nom_opt.w = [nom_opt.x; nom_opt.z];
Gy  = full(fun.Gy( nom_opt.w, nom_opt.u, nom_opt.d));
Gd  = full(fun.Gd( nom_opt.w, nom_opt.u, nom_opt.d));
Juu = full(fun.Juu(nom_opt.w, nom_opt.u, nom_opt.d));
Jud = full(fun.Jud(nom_opt.w, nom_opt.u, nom_opt.d));
F   = Gd-Gy/Juu*Jud;

% Calculation of the relative gain array
RGA.nom = Gy.*pinv(Gy)';

%% Part d) Application of the exact local method on the system

% Definition of the scaling matrix for the disturbances
noise.dist = [10.5 15 15 0.01 0.2 0.05];
Wd = diag(noise.dist(dist_var));

% Calculation of the norms
for k = 1:33
    nor{1}(k) = norm(F(ind.T(k),dist_var)*Wd,1);
    nor{2}(k) = norm(F(ind.T(k),dist_var)*Wd,2);
    nor{3}(k) = norm(F(ind.T(k),dist_var)*Wd,Inf);
end
% Calculation of the gain
nor{4}(1:11)  = abs(Gy(ind.T(1:11),1));
nor{4}(12:22) = abs(Gy(ind.T(12:22),2));
nor{4}(23:33) = abs(Gy(ind.T(23:33),3));

%% Part e) Plot of the norm and the gain
close all
% Definition of integer values
itg.fig = 1;
itg.lab = 1;
itg.fda = 1;
itg.axe = 1;
itg.ann = 1;

% Definition of figure and values
fig.dimw = 8.6;
fig.dimh = 6;
fig.Font = [10 8];
fig.label = [];
fig.ann   = [];

% Plot of the norm of the sensitivity matrix and the absolute gain
fig.in(itg.fig) = figure(itg.fig);
set(fig.in(itg.fig),'units','centimeters','position', [5+itg.fig,5+itg.fig, fig.dimw, fig.dimh]);
fig.axes(itg.axe) = gca;
hold on

yyaxis left
fig.axes(itg.axe) = gca;
fig.axes(itg.axe).YAxis(1).Color = 'k';
fig.axes(itg.axe).YAxis(2).Color = 'r';

% fig.dat(itg.fda) = plot(nor{1},'k'); itg.fda = itg.fda+1;
% fig.dat(itg.fda) = plot(nor{2},'k--'); itg.fda = itg.fda+1;
fig.dat(itg.fda) = plot(nor{3},'k'); itg.fda = itg.fda+1;
fig.dat(itg.fda) = plot([11.5 11.5],[0 12],'k--','LineWidth',0.25); itg.fda = itg.fda+1;
fig.dat(itg.fda) = plot([22.5 22.5],[0 12],'k--','LineWidth',0.25); itg.fda = itg.fda+1;
                        
fig.label(itg.lab) = xlabel('Temperature Coordinate $k$'); itg.lab = itg.lab+1;
% fig.label(itg.lab) = ylabel('Norm $\left|\mathbf{F}_{y_k}\mathbf{W}_d\right|_j$'); itg.lab = itg.lab+1;
fig.label(itg.lab) = ylabel('Norm $\left|\mathbf{F}_{y_k}\mathbf{W}_d\right|_\infty$'); itg.lab = itg.lab+1;
                                 

yyaxis right
fig.dat(itg.fda) = plot(nor{4},'r'); itg.fda = itg.fda+1;
fig.label(itg.lab) = ylabel('Absolute Gain $\left|G_{y,kj}\right|$'); itg.lab = itg.lab+1;
% legend(fig.dat(itg.fda-4:itg.fda-2),{'$j = 1$',...
%                                      '$j = 2$',...
%                                      '$j = \infty$'})
% fig.axes(itg.axe).XTickLabel = {'2.8'; '3.0'; '3.2'; '3.4'; '3.6'; '3.8'; '4.0'};
set(fig.axes(itg.axe).Legend,'Location','Best','FontSize',fig.Font(2),'Interpreter','latex','Box','off')
fig.axes(itg.axe).XLim = [0 33];
fig.ann(itg.ann) = annotation('textbox',[.19 .15 .10 .10],'String','Bed 1'); itg.ann = itg.ann+1;
fig.ann(itg.ann) = annotation('textbox',[.435 .15 .10 .10],'String','Bed 2'); itg.ann = itg.ann+1;
fig.ann(itg.ann) = annotation('textbox',[.675 .15 .10 .10],'String','Bed 3'); itg.ann = itg.ann+1;
fig.axes(itg.axe).Position = [0.13 0.15 0.73 0.80];
fig.axes(itg.axe).XTick = [1 6 11  17 22  28 33];
fig.axes(itg.axe).XTickLabel = {'$in,1$'; '$5$';  '$10$';...  % '$T_{in,3}$'; 
                                '$15$'; '$20$';...
                                '$25$'; '$30$'};
itg.axe = itg.axe+1;
itg.fig = itg.fig+1;

% Plot of the absolute gain
fig.in(itg.fig) = figure(itg.fig);
set(fig.in(itg.fig),'units','centimeters','position', [5+itg.fig,5+itg.fig, fig.dimw, fig.dimh]);
fig.axes(itg.axe) = gca;
hold on
fig.dat(itg.fda) = plot(nor{4},'k'); itg.fda = itg.fda+1;
fig.dat(itg.fda) = plot([11.5 11.5],[0 3000],'k--','LineWidth',0.25); itg.fda = itg.fda+1;
fig.dat(itg.fda) = plot([22.5 22.5],[0 3000],'k--','LineWidth',0.25); itg.fda = itg.fda+1;
fig.label(itg.lab) = xlabel('Temperature Coordinate $k$'); itg.lab = itg.lab+1;        
fig.label(itg.lab) = ylabel('Absolute Gain $\left|G_{y,kj}\right|$'); itg.lab = itg.lab+1;
set(fig.axes(itg.axe).Legend,'Location','Best','FontSize',fig.Font(2),'Interpreter','latex','Box','off')
fig.axes(itg.axe).XLim = [0 33];
fig.ann(itg.ann) = annotation('textbox',[.19 .15 .10 .10],'String','Bed 1'); itg.ann = itg.ann+1;
fig.ann(itg.ann) = annotation('textbox',[.435 .15 .10 .10],'String','Bed 2'); itg.ann = itg.ann+1;
fig.ann(itg.ann) = annotation('textbox',[.675 .15 .10 .10],'String','Bed 3'); itg.ann = itg.ann+1;
% fig.axes(itg.axe).Position = [0.13 0.15 0.73 0.80];
itg.axe = itg.axe+1;
itg.fig = itg.fig+1;

set(fig.ann,   'Interpreter',   'Latex');
set(fig.ann,   'Fontsize',      fig.Font(2));
set(fig.ann,   'LineStyle',     'none');
set(fig.label, 'Fontsize',      fig.Font(1));
set(fig.label, 'Interpreter',   'Latex');
set(fig.axes,  'FontSize',      fig.Font(2));
set(fig.axes,  'Box',           'on');
set(fig.axes,  'TickLabelInterpreter', 'Latex');
set(fig.axes,  'LineWidth',     .75);
set(fig.axes,  'XMinorTick',    'on');
set(fig.axes,  'YMinorTick',    'on');
set(fig.dat,   'LineWidth',     .75)