% 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  = false;
dec.save  = true;
dec.plot  = true;
dec.Htype = 'In_Out'; % Norm_2, Norm_1, Norm_inf, Max_Gain, In_Out
dec.QP    = true;

% Definition of the chosen H and the measured disturbances
H_var = [1];          % Calculated and considered H matrices
dist_var = [1 2 3 4 5 6];       % 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 number of measurements in the QP approach
n.y_used = 2;
n.y_var  = 11;

% 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
    if dec.QP
        savename   = [savename '_QP_n' num2str(n.y_used)];
        savename_H = [savename_H '_QP_n' num2str(n.y_used)];
    else
        savename   = [savename '_' dec.Htype];
        savename_H = [savename_H '_' dec.Htype];
    end
    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]);
if n.y_var == 11
    ind.y = ind.T;
elseif n.y_var == 22
    ind.y = [ind.T(1:11) ind.w(1:11) ...
             ind.T(12:22) ind.w(12:22) ...
             ind.T(23:33) ind.w(23:33)];
elseif dec.Qp
    error('The measurements are not possible.')
end
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');
fun.Jdu = jacobian(rf.jacobian('d','J'),'u');

% 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));
Jdu = full(fun.Jdu(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));

% j = 1;
% for k = 3:3:n.x+n.u
% 
%     % Calculation of the reduced variables
%     ind.y_used = ind.y([1:k/n.u 12:11+k/n.u 23:22+k/n.u]);
%     F_red  = F(ind.y_used,dist_var);
%     Gy_red = Gy(ind.y_used,:);
% 
%     % Definition of the scaling
%     noise.meas = ones(1,numel(ind.y_used))*.01;
%     Wn = diag(noise.meas);
% 
%     % SOC using the Exact local method for all inputs
%     Y  = [F_red*Wd Wn];
%     H{j} = ((Y*Y')\Gy_red)';
%     M{j} = sqrtm(Juu)*(H{j}*Gy_red)\(H{j}*Y);
%     Lwc(j) = 0.5*norm(M{j},2)^2;
%     Lav(j) = 0.5*norm(M{j},'fro')^2;
%     
%     % Scaling of H and calculation of the steady state RGA
%     H{j}(1,:) = H{j}(1,:)/max(abs(H{j}(1,:)));
%     H{j}(2,:) = H{j}(2,:)/max(abs(H{j}(2,:)));
%     H{j}(3,:) = H{j}(3,:)/max(abs(H{j}(3,:)));
%     RGA.H{j} = (H{j}*Gy_red).*pinv(H{j}*Gy_red)';
%     j = j+1;
% end
if dec.QP
    sqJuu = sqrtm(Juu);   % Square root of Juu
    noise.meas = ones(1,n.y_var)*1e-7;
%     noise.meas = [ones(1,n.y_var/2)*1e-3 ones(1,n.y_var/2)*1e-5];
    Wn = diag(noise.meas);
    m = 100;
    for k = 1:3        
        ind.y_used{k} = ind.y(1+(k-1)*n.y_var:k*n.y_var);
        ind.y_red{k}  = 1+(k-1)*n.y_var:k*n.y_var;
        n.y = numel(ind.y_red{k});
        Y_del     = [F(ind.y_used{k},dist_var)*Wd Wn];
        sqJuu_del = sqJuu(k,k);
        Gy_del    = Gy(ind.y_used{k},k);
        F_del     = Y_del*Y_del';
        
        % Setup of Gurobi
        clear model
        model.Q   = sparse(blkdiag(F_del,zeros(n.y)));
        model.obj = zeros(1,2*n.y);
        model.A   = sparse([Gy_del'         zeros(1,n.y);...
                            zeros(1,n.y)    ones(1,n.y);...
                            eye(n.y)       -m*eye(n.y);...
                           -eye(n.y)       -m*eye(n.y)]);
        model.rhs = [sqJuu_del; n.y_used; zeros(2*n.y,1)];
        model.sense = ['='; '=';...
                       char(60*ones(n.y_var*2,1))];...      '<'
        model.vtype = [char(67*ones(n.y_var,1));...      	'C'
                       char(66*ones(n.y_var,1))];...        'B'
        model.lb = -10*ones(2*n.y,1);
        gurobi_write(model, 'qp.lp');
        results  = gurobi(model);
        
        % Rearranging the solution vector
        H_red{k} = results.x(abs(results.x(1:n.y_var)) >1e-8)';
        ind.y_used{k} = ind.y_used{k}(results.x(n.y_var+1:2*n.y_var) > 1e-1);
        ind.y_red{k}  = ind.y_red{k} (results.x(n.y_var+1:2*n.y_var) > 1e-1);
                
        % Scaling of H
        H_red{k} = H_red{k}/max(abs(H_red{k}));
    end
    
    
else
    switch dec.Htype
        case 'Norm_2'
            % Calculation of the matrix norm for the disturbances
            for k = 1:33
                nor.full(k) = norm(F(ind.T(k),dist_var)*Wd,2);
            end
        case 'Norm_1'
            % Calculation of the matrix norm for the disturbances
            for k = 1:33
                nor.full(k) = norm(F(ind.T(k),dist_var)*Wd,1);
            end
        case 'Norm_inf'
            % Calculation of the matrix norm for the disturbances
            for k = 1:33
                nor.full(k) = norm(F(ind.T(k),dist_var)*Wd,Inf);
            end
        case 'Max_Gain'
            nor.full(1:11)  = -abs(Gy(ind.T(1:11),1));
            nor.full(12:22) = -abs(Gy(ind.T(12:22),2));
            nor.full(23:33) = -abs(Gy(ind.T(23:33),3));
        case 'In_Out'
            nor.full(1:11)  = [0 ones(1,9) 0];
            nor.full(12:22) = [0 ones(1,9) 0];
            nor.full(23:33) = [0 ones(1,9) 0];

    end
    [~,ind.sort{1}] = sort(nor.full(1:11));
    [~,ind.sort{2}] = sort(nor.full(12:22));
    [~,ind.sort{3}] = sort(nor.full(23:33));
    ind.r = [ind.sort{1}(1:2);...
             ind.sort{2}(1:2);...
             ind.sort{3}(1:2)];

    for k = 1:n.u

        % Calculation of the reduced variables
        ind.y_used{k} = ind.y([ind.r(k,1)+(k-1)*11 ind.r(k,2)+(k-1)*11]);
        ind.y_red{k}  = [ind.r(k,1)+(k-1)*11 ind.r(k,2)+(k-1)*11];
        F_red  = F(ind.y_used{k},dist_var);
        Gy_red = Gy(ind.y_used{k},k);

        % Definition of the scaling
        noise.meas = ones(1,numel(ind.y_used{k}))*1e-3;
        Wn = diag(noise.meas);

        % SOC using the Exact local method for all inputs
        Y  = [F_red*Wd Wn];
        H_red{k} = ((Y*Y')\Gy_red)';
        lol{k} = H_red{k};

        % Scaling of H
        H_red{k} = H_red{k}/max(abs(H_red{k}));
    end
end
H{1} = blkdiag(H_red{:});
ind.y_used = cell2mat(ind.y_used);
ind.y_red  = cell2mat(ind.y_red);

% Calculation of the steady state RGA
RGA.H = (H{1}*Gy(ind.y_used,:)).*pinv(H{1}*Gy(ind.y_used,:))';
    
% Redefinition of the H matrices
H_var = 1:numel(H);

% Definition of the number of different H matrices
n.H = numel(H);

return
% Save of the results
if dec.save
    save(['Local Data\' savename_H],'H','noise','ind')
end

%% Part e) Root finding for the new system

if dec.loss

% Definition of the variation
Var.n(1) = 76;
Var.n(2) = 41;
Var.n(3) = 41;
Var.n(4) = 31;
Var.n(5) = 61;
Var.n(6) = 61;
Var.lub = [-7.5 -20 -20 -0.015 -0.3 -0.4;...
            7.5  20  20  0.015  0.3  0.2];

% Combination of the variables
w   = [dae.x; dae.z; dae.p(1:n.u)];
w0  = full(nom_opt.comb.x);

% Predefinition of variables and load of the optimal values
SS_SOC = cell(n.d,n.H);
load([dirmain '\Data\SS_opt_6dist_rspace.mat'])

% Clearing of the auxiliary variable
clear aux

for l = H_var
            
% Combination of the equality constraints to keep the setpoint constant
g_org = vertcat(dae.ode,dae.alg,H{l}*w(ind.y_used)-H{l}*nom_opt.y(ind.y_red));

for j = 1:n.d
    in.SOC{j,1} = linspace(Var.lub(1,j),Var.lub(2,j),Var.n(j));
    time.in.loss(j,l) = tic;
    for k = 1:Var.n(j)
        % Substitution of the inlet values
        in.dist = nom_opt.d;
        in.dist(j) = in.dist(j)+in.SOC{j}(k);
        g = substitute(g_org,d,in.dist);

        % Definition and solving of the system
        g_fun = Function('g_fun',{w},{g});
        G = rootfinder('G','newton',g_fun);
        aux.comb = G('i0',w0);
        aux.x(:,k) = full(aux.comb.o0(ind.x));
        aux.y(:,k) = full(aux.comb.o0(ind.y));
        aux.z(:,k) = full(aux.comb.o0(ind.z));
        aux.u(:,k) = full(aux.comb.o0(ind.u));
        aux.f(k)   = -(aux.z(N,k)-(in.dist(4)+in.nom(4)))*(in.nom(1)+in.dist(1));
        w0 = full(aux.comb.o0);
    end
    w0  = full(nom_opt.comb.x);
    
    % Combination into a structure
    SS_SOC{j,l} = struct('Var',in.SOC{j}+in.nom(j),'z',aux.z,...
                         'u',aux.u,'x',aux.x,'y',aux.y,'f',aux.f);
                     
    % Calculation of the loss
    if numel(SS_opt{j}.Var) == numel(SS_SOC{j,l}.Var)
    if sum(abs(diff([SS_opt{j}.Var;SS_SOC{j,l}.Var]))) < 1e-6
        SS_SOC{j,l}.Var = SS_opt{j}.Var;
    end
    end
    SS_SOC{j,l}.loss = (SS_opt{j}.f(end,ismember(SS_opt{j}.Var,SS_SOC{j,l}.Var))-SS_SOC{j,l}.f(end,:))'...
                        ./SS_opt{j}.f(end,ismember(SS_opt{j}.Var,SS_SOC{j,l}.Var))'*100;
%     SS_SOC{j,l}.loss = (SS_opt{j}.f(end,:)-SS_SOC{j,l}.f(end,:))'...
%                         ./SS_opt{j}.f(end,:)'*100;

    % Sampling of the time
    time.loss(j,l) = toc(time.in.loss(j,l));
    clear aux
    fprintf('The steady-state simulation for the disturbance %1i and the H matrix %1i is finished. \n',j,l)
end
end

% Save of the results
if dec.save
    save(['Local Data\' savename],'SS_SOC')
end
end


%% Part f) Plotting of the results

if dec.plot

    % Calculation of the loss for each of the cases
    if ~isfield(SS_SOC{1,1},'loss')
    for j = 1:n.d
        for l = H_var
            if numel(SS_opt{j}.Var) == numel(SS_SOC{j,l}.Var)
                if sum(abs(diff([SS_opt{j}.Var;SS_SOC{j,l}.Var]))) < 1e-6
                    SS_SOC{j,l}.Var = SS_opt{j}.Var;
                end
            end
            SS_SOC{j,l}.loss = (SS_opt{j}.f(end,ismember(SS_opt{j}.Var,SS_SOC{j,l}.Var))-SS_SOC{j,l}.f(end,:))'...
                                ./SS_opt{j}.f(end,ismember(SS_opt{j}.Var,SS_SOC{j,l}.Var))'*100;
        end
    end
    end

    % Definition of integer values
    itg.fig = 1;
    itg.lab = 1;
    itg.arr = 1;
    itg.sub = 1;

    % Definition of figure and values
    fig.xlab = {'Flowrate [kg/s]','Pressure [bar]','Temperature [C]',...
                'Mass fraction [-]','Ratio H$_2$/N$_2$ [-]','Catalyst activity [-]'};
    fig.xlim = [];
    fig.d     = [5 25 25 0.02 0.1 0.2];
    for k = 1:n.d
        fig.xlim = [fig.xlim;
                    floor(round(SS_opt{k}.Var(1)/fig.d(k)))*fig.d(k) ...
                    fig.d(k) ...
                    ceil(round(SS_opt{k}.Var(end)/fig.d(k)))*fig.d(k)];
    end
    fig.dimw  = 12;
    fig.dimh  = 7;
    fig.label = [];
    fig.Font  = [10 8];
    fig.ann   = [];
    fig.sub   = [];

    % Definition of the colours
    rk = {'k','k--','r','r--'};
    H_cell = {'T','NSM','ELM$_1$','ELM$_2$'};

    % Initialization of the figure
    fig.in(itg.fig) = figure(itg.fig);
    set(fig.in(itg.fig),'units','centimeters');
    set(gcf, 'position', [5+itg.fig,5+itg.fig, fig.dimw, fig.dimh]);

    % Plotting of the loss with flowrate change
    for k = 1:n.d
        fig.sub(k) = subplot(2,ceil(n.d/2),k);
        fig.axes(k) = gca;
        xlim(fig.xlim(k,[1 3]))
%         ylim([0 3])
        hold on
        for l = 1:H_var
            plot(SS_SOC{k,l}.Var,SS_SOC{k,l}.loss)%,rk{l})
        end
        fig.axes(k).XTick = fig.xlim(k,1):fig.xlim(k,2):fig.xlim(k,3);
%         fig.axes(k).YTick = 0:1:3;
        fig.label(itg.lab) = xlabel(fig.xlab{k}); itg.lab = itg.lab+1;
        fig.axes(k).LineWidth = 1;
        if mod(k-1,n.d/2) == 0
            fig.label(itg.lab) = ylabel('Loss [\%]'); itg.lab = itg.lab+1;
        end
        switch k
            case 1
%                 fig.label(itg.lab) = legend(H_cell{H_var},'Location','best'); itg.lab = itg.lab+1;
%                 legend BOXOFF 
            case 4
                fig.axes(k).XTick = [0.04 0.06 0.08 0.10 0.12];
                fig.axes(k).XTickLabel = {'0.04', '0.06', '0.08', '0.10', '0.12'};
            case 5
%                 ylim([0 0.02])
%                 fig.axes(k).YTick = 0:0.005:0.02;
%                 fig.axes(k).YTickLabel = {'0.000','0.005','0.010','0.015','0.020'};
        end
    end

    % Modificiation of the plotting properties
    set(fig.ann, 'Interpreter', 'Latex');
    set(fig.ann, 'LineStyle', 'none');
    set(fig.label, 'Interpreter', 'Latex');
    set(fig.axes,'TickLabelInterpreter', 'Latex');
    set(fig.axes,'XMinorTick', 'on');
    set(fig.ann, 'Fontsize', fig.Font(1));
    set(fig.label, 'Fontsize', fig.Font(1));
    set(fig.label(3), 'Fontsize', fig.Font(2));
    set(fig.axes,'Fontsize', fig.Font(2));
end