clear all
clc
cd('~/Documents/NTNU/Project/matlab/')

%% FYI
% y   = [FN2 PW pS1 TS4 TS5 TS6]'                               (6x1)
% u   = [FN2 PW]'                                               (2x1)
% d   = [Tin(H1) ... Tin(C2) FCp(H1) ... FCp(C2)]'             (10x1)
% ny  = [FN2 PW pS1 TS4 TS5 TS6]'                               (6x1)
% F   = [dFN2/dd ; dPW/dd]'                                    (10x2)
% Wd  = diag(d_values)                                        (10x10)
% wny = diag(ny_values)                                         (2x2)
% Gy  = [dy/du]                                                 (2x6)

%% Misc data
y = ones(6,1);                % y   = [FN2 PW pS1 TS4 TS5 TS6]' (6x1)
d = ones(10,1);%d = [Tin(H1) ... Tin(C2) FCp(H1) ... FCp(C2)]' (10x1)
Tin = [298 265 195 220 255]';                % Inlet temperatures [K]
FCp = [3 4 2 3 3.5]';               % Inlet flow heat capacity [kW/K]
TS  = [2.657347e+2 1.514299e+2 2.939971e+2]';   % State temperatures,
                                             % TS = [TS4 TS5 TS6] [K]
pS1  = 7.25345316;                       % State pressure at S1 [bar]

%% Disturbance and measurement error data
d_T   = 10;                    % Absolute temperature disturbance [K]
d_FCp = 0.05;        % Relative flow heat capacity disturbance [kW/K]
n_F = 1e-3;                  % Abolute flow measurment error [kmol/s]
n_w = 0;                  % Absolute pump work measurement error [kW]
n_T = 1;                 % Absolute Temperature measurement error [K]
n_p = 0.01;               % Absolute pressure measurement error [bar]

Wd  = diag([ones(length(Tin),1)*d_T; FCp*d_FCp]);
Wny = diag([n_F n_w n_p ones(1,length(TS))*n_T]');

% Hessian of costfunction. Values given from mhex.py
Juu = [-3.6800349537e+12, 2.0833333334e+06;
      2.0833333334e+06, -1.1666666596e+04];
% E = eig(Juu);
Gy = [1.0000000000e+00, 0.0000000000e+00, -4.1566666668e+01, ...
      2.5000000000e+04, 1.4666666667e+04, -2.6666666666e+04;
      0.0000000000e+00, 1.0000000000e+00, 7.3684210526e+00, ...
      5.8479532183e+01, 0.0000000000e+00, -5.8479532149e+01]';
  % NOTE: transposed

F =  [9.0950000000e-05, 2.2595000000e-04, 1.4895000000e-04, ...
      2.2295000000e-04, 4.8395000000e-04, 2.5130000000e-03, ...
      6.3297500000e-03, 3.3495000000e-03, -1.8603333333e-03, ...
      -1.2517142857e-03 ;
      5.2394000000e-04, 1.3089400000e-03, 8.6194000000e-04, ...
      1.2929400000e-03, 2.8019400000e-03, 1.4566266667e-02, ...
      3.6634700000e-02, 1.9399400000e-02, -1.0773733333e-02, ...
      -7.2517714286e-03 ;
      -1.5999999992e-07, -1.5999999992e-07, -1.5999999992e-07, ...
      -1.5999999992e-07, -1.5999999992e-07, -1.0666666661e-06, ...
      -7.9999999958e-07, -1.5999999992e-06, -1.0666666661e-06, ...
      -9.1428571380e-07 ;
      2.3571710000e+00, 2.2899580000e+00, -1.8894300000e-01, ...
      -2.8268600000e-01, -6.0713200000e-01, 3.4663793333e+01, ...
      -7.7379850000e+00, -4.2242600000e+00, 2.3954533333e+00, ...
      -2.0616057143e+01 ;
      1.3432440000e+00, 1.3049430000e+00, -1.0766800000e-01, ...
      -1.6108800000e-01, -3.4597500000e-01, 1.9753300000e+01, ...
      -4.4095100000e+00, -2.4071900000e+00, 1.3650733333e+00, ...
      -1.1748125714e+01 ;
      9.9995600000e-01, -4.5000000000e-05, -4.2000000008e-05, ...
      -4.2000000008e-05, -4.2000000008e-05, -2.8000000005e-04, ...
      -2.1000000004e-04, -4.2000000008e-04, -2.8000000005e-04, ...
      -2.4000000004e-04]';
 % NOTE: transposed
 
%% Calculations Exact Local Method

Y = [F'*Wd Wny] ;
H = Gy'*(Y*Y')^-1 ;
%H = ((Y*Y')^(-1)*Gy)' ;
P = (Juu^-0.5*Gy'*(Y*Y')^-1*Gy*Juu^-0.5)^(-1);

%% Loss calulations

L = (1/(6*(length(y)+length(d))))*...
    (norm(Juu^0.5*(H*Gy)^(-1)*H*Y,'fro'))^2;
L_avg = (1/(6*(length(y)+length(d))))*trace(P);
L_w = 0.5*(svds(Juu^0.5*(H*Gy)^(-1)*H*Y,1,'L'))^2;

%% Exporting as tex
H_n = H/norm(H);    % Making H readable. Doesn't affect solution

cd('~/Documents/NTNU/Project/tex/matlab/')
matrix2latexmatrix(F,   'F_val.tex')
matrix2latexmatrix(Gy,  'Gy_val.tex')
matrix2latexmatrix(Juu, 'Juu_val.tex')
matrix2latexmatrix(H_n, 'H_val.tex')
matrix2latexmatrix(L,   'L_val.tex')
matrix2latexmatrix(L_avg,'Lavg_val.tex')
matrix2latexmatrix(L_w, 'Lw_val.tex')

%% Null Space Method
%H_null = null(F');
%L_null = (1/(6*length(y)*length(d)))*...
%(norm(Juu^0.5*(H_null*Gy)^(-1)*H_null*Y,'fro'))^2;

%% Testing
H2   = H/norm(H,'fro') ;
L_w2 = 0.5*(svds(Juu^0.5*(H2*Gy)^(-1)*H2*Y,1,'L'))^2 ;
H3   = H/norm(H) ;
L_w3 = 0.5*(svds(Juu^0.5*(H3*Gy)^(-1)*H3*Y,1,'L'))^2 ;