
% 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'])
addpath(genpath([dirmain '\Util4. MATLAB2tikz']))
import casadi.*

% Definition of decision variables
dec.int  = true;
dec.plot = true;
dec.scan = false;
dec.root = false;
dec.red  = false;
dec.save = true*dec.scan;

% Definition of the save name
if dec.red
    savename = 'SS_opt_6dist_red.mat';
else
    savename = 'SS_opt_6dist_rspace.mat';
end

% Definition of nominal disturbance variables
in.nom = zeros(6,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 
        
% Load of the model
[func,dae,d,lb,ub,n,Cineq,ToutFun] = AmmonReac_vanHeerden(1);

% Definition of the integrations step-changes
in_d = [0; -2.2; -2.2; 0; 0; 0];
% in_d = [0;  -8;    0;   0; 0; 0];

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

%% Part a) Root finding for the modified system

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

% 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);

% Initialization of the variables
nom_opt.TvH = 250+in_d(3):1:550;
n.samp      = numel(nom_opt.TvH);

nom_opt.a = zeros(numel(w0),n.samp);
nom_opt.x = zeros(n.x,n.samp);
nom_opt.y = zeros(n.y,n.samp);
nom_opt.z = zeros(n.z,n.samp);

nom_opt.T = zeros(N+3,n.samp);
nom_opt.w = zeros(N+3,n.samp);
nom_opt.R = zeros(N+3,n.samp);

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

% Startup of the waitbar
h = waitbar(0,'Solving the system');
time.in.sim = tic;
for k = 1:n.samp
    g_root = substitute(g_ro,dae.p(end),nom_opt.TvH(k));

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

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

    % Reassignment of variables
    nom_opt.a(:,k) = full(res.o0);
    
    nom_opt.x(:,k) = nom_opt.a(ind.x,k);
    nom_opt.y(:,k) = nom_opt.a(ind.y,k);
    nom_opt.z(:,k) = nom_opt.a(ind.z,k);
    
    nom_opt.T(:,k) = nom_opt.a(ind.T,k);
    nom_opt.w(:,k) = nom_opt.a(ind.w,k);
    nom_opt.R(:,k) = nom_opt.a(ind.R,k);
    
    w0 = nom_opt.a(:,k);
    
    % Updating the waitbar
    if mod(k,20) == 0
        waitbar(k/(n.samp))
    end
end

% Close of the waitbar
close(h)

% Load of the model
% [func,dae,d,lb,ub,n,Cineq,ToutFun] = AmmonReac_NewDist(t.int);
% g = vertcat(dae.ode,dae.alg);
% g_ro = substitute(g,d,zeros(n.d,1));
% g_ro = substitute(g_ro,dae.p(1:n.u),p0(1:n.u));


% Print of the found results
fprintf('\n\n\nThe van-Heerden is completed. \n\n\n')


%% Part c) Calculation and plotting of the results

% Definition of integer values
itg.fig = 1;
itg.lab = 1;
itg.fda = 1;
itg.axe = 1;

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

if dec.plot
    
    % Plot of the van Heerden analysis
    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]);
    hold on
    fig.dat(itg.fda) = plot(nom_opt.TvH,nom_opt.y(1,:),'k'); itg.fda = itg.fda+1;
    fig.axes(itg.axe) = gca;
    hold on
    fig.dat(itg.fda) = plot(nom_opt.TvH,nom_opt.TvH,'r'); itg.fda = itg.fda+1;
    fig.label(itg.lab) = xlabel('$T_{HEx,def}$ [C]'); itg.lab = itg.lab+1;
    fig.label(itg.lab) = ylabel('$T_{HEx,cal}$ [C]'); itg.lab = itg.lab+1;
    itg.axe = itg.axe+1;
    
    % Redefinition of the labels
    set(fig.label, 'Interpreter', 'Latex');
    set(fig.label, 'Fontsize', fig.Font(1));
    % set(fig.label(itg.lab-1), 'Fontsize', fig.Font(2));
    % set(fig.label(7), 'Fontsize', fig.Font(2));
    set(fig.axes,'Box',           'on');
    set(fig.axes,'TickLabelInterpreter', 'Latex');
    set(fig.axes,'Fontsize', fig.Font(2));
    set(fig.axes,'LineWidth',     .75);
    set(fig.axes,'XMinorTick',    'on');
    set(fig.axes,'YMinorTick',    'on');
    set(fig.dat, 'LineWidth',     .75)
end