clear all;
clc;

%%   NAMĚŘENÁ DATA:

%     [ F1      x1,1    x1,2
%       F2      x2,1    x2,2
%       F3      x3,1    x3,2]

%   Průtoky a hmotnostní zlomky
%		PRŮTOK	HZS 1   HZS 2       HZS = Hmotnostní Zlomek Složky
dat = [	1095.47 0.4822  0.5170;		% PROUD 1 (Nástřik)
        478.40	0.9410  0.0501;		% PROUD 2 (Destilát)
		488.23	0.0197  0.9748];	% PROUD 3 (Zbytek)

%   Směrodatné odchylky:
%			PRŮTOK	HZS 1   HZS 2
std_raw = [	54.8	0.0048	0.0052;		% PROUD 1
			23.9	0.0094	0.0005;		% PROUD 2
			24.4	0.0002	0.0097];	% PROUD 3
        
%%  VYROVNÁVÁNÍ

F = dat(:,1);       %průtoky proudů 1, 2, 3 = F1; F2; F3 (složek 1+2 zároveň)
x = dat(:,2:3);     %koncentrace složek 1 a 2 v proudech F1, F2, F3

F1 = F.*x(:,1);     %průtoky složky 1 = F1*x1,1; F2*x2,1; F3*x3,1
F2 = F.*x(:,2);     %průtoky složky 2 = F2*x1,2; F2*x2,2; F3*x3,2
y = [F; F1; F2];    %průtoky: celkový, složky 1, skložky 2 = F1; F2; F3; F1,1; F2,1; F3,1; F1,2; F2,2; F3,2

vF1 = x(:,1).^2.*std_raw(:,1).^2 + F.^2.*std_raw(:,2).^2;   %např. rozptyl F1,1 = (x1,1)^2*(std_F1)^2 + (F1)^2*(std_x1,1)^2
vF2 = x(:,2).^2.*std_raw(:,1).^2 + F.^2.*std_raw(:,3).^2;
V = diag([std_raw(:,1).^2; vF1; vF2]);                      %kovarianční matice

% MASS/MATERIAL BALANCES: A - incidenční matice
%    F1 F2 F3 F1,1 F2,1 F3,1 F1,2 F2,2 F3,2
A = [eye(3),  -eye(3),       -eye(3);               % COMPOSITON 
     0  0  0  1    -1   -1   0    0    0;			% MATERIAL X1
	 0  0  0  0    0    0    1    -1   -1 ];		% MATERIAL X2
%   podmínky:
%   F1 - F1,1 - F1,2 = 0
%   F2 - F2,1 - F2,2 = 0
%   F3 - F3,1 - F3,2 = 0
%   F1,1 - F2,1 - F3,1 = 0
%   F1,2 - F2,2 - F3,2 = 0

%kdybych přidal ještě jednu rovnici, pak by nebyla splněna podmínka h(A) = K < L. Byli by tam lineárně závislé řádky
A_aa = [eye(3),  -eye(3),       -eye(3);
     0  0  0  1    -1   -1   0    0    0;
	 0  0  0  0    0    0    1    -1   -1;
     1  -1 -1 0    0    0    0    0    0];  %   F1 - F2 - F3 = 0
h_A_aa = rank(A_aa);
[K, L] = size(A_aa);

% RECONCILIATION: W, y_hat, V_hat, std_hat
W = eye(length(y)) - V*A'*inv(A*V*A')*A;
y_hat = W*y;
V_hat = W*V*W';
std_hat = sqrt(diag(V_hat));

%opravy
v = V*A'*inv(A*V*A')*A*y;

% RECOVER FRACTIONS
y_hat = reshape(y_hat, size(dat));
F_hat = y_hat(:,1);
x_hat = y_hat(:,2:3)./([F_hat F_hat]);

% VARIANCE
std_hat = reshape(std_hat, size(dat));
std_F = std_hat(:,1);
std_f = std_hat(:,2:3);
std_x = sqrt((std_f.^2 - x_hat.^2.*[std_F std_F].^2)./[F_hat F_hat].^2);

% Kritérium minimalizace oprav
Q = v'*inv(V)*v;

%%  Detekce hrubých chyb

%vektor reziduí
r = A*y;
V_r = A*V*A';
std_V_r = sqrt(diag(V_r));

%   Global Test (GT)
gama = r'*inv(V_r)*r;               %Hodnota statistiky GT je rovna hodnotě minima objektivní funkce u vyrovnávání dat.
%pearsrnd  %možná funkce

%   Constraint nebo Nodal Test (NT)
z_NT = abs(r)./sqrt(diag(V_r));          % z_r(i) = |r(i)|/sqrt(V_r(ii))
%gausswin

%   Measurement Test (MT)
v_MT = V*A'*inv(V_r)*r;             % v = y - y_hat
%V_v = V*A'*inv(V_r)*V;              %z kapitoly 7 z Data Reconciliation and Gross Error detection
V_v = V*A'*inv(V_r)*A;              %!!! upravil jsem, ale vychází moc velká čísla - špatně
z_MT = abs(v_MT)./sqrt(diag(V_v));
d = inv(V)*v_MT;
%V_d = A'*inv(A*V*A')*A';       %z kapitoly 7 z Data Reconciliation and Gross Error detection
V_d = A'*inv(A*V*A')*A;         % !!! kdybych to ale upravil takto
z_MT_d = abs(d)./sqrt(diag(V_d));

%   Pro alfa = 5% je ''kritická'' hodnota 1,96 - musím to přepsat podle správné terminologie!

%%  NELINEÁRNÍ OPTIMALIZACE S PODMÍNKAMI

%%  
y;      %již zadaná výše
V;      %již zadaná výše
y_0 = y.*1.05;      %navýšení hodnot o 5%   Nezáleží na tom momentálně.
A;      %již zadaná výše
b = [0;0;0;0;0];

%   účelová funkce
f = @(y_vyr) (y-y_vyr)'*inv(V)*(y-y_vyr);
[y_vyr,fval] = fmincon(f,y_0,[],[],A,b,[],[],[],[]);
disp(y_vyr); disp(fval);
[y_vyrc,fvalc] = fmincon(f,y_0,[],[],[],[],[],[],@confuneqy,['active-set']);    %stejná fce jako předchozí ale za použití ceq
disp(y_vyrc); disp(fvalc);

x_vyr = y_vyr(4:9)./([y_vyr(1:3); y_vyr(1:3)]);
%%  kdyby jsme vyrovnávali pouze průtoky F1, F2, F3
F;      %již zadaná výše
V_F = diag(std_raw(:,1).^2);
F_0 = F.*1.05;
A_F = [1 -1 -1];    %F1 - F2 - F3 = 0
b_F = [0];

f_F = @(F_vyr) (F-F_vyr)'*inv(V_F)*(F-F_vyr);
[F_vyr,fvalF] = fmincon(f_F,F_0,[],[],A_F,b_F,[],[],[],[]);
disp(F_vyr); disp(fvalF);
%%  kdyby jsme vyrovnávali pouze hmotnostní zlomky
X = [dat(:,2); dat(:,3)];
V_X = diag([std_raw(:,2).^2; std_raw(:,3).^2]);
X_0 = X.*1.05;
A_X = [1 0 0 1 0 0;
       0 1 0 0 1 0;
       0 0 1 0 0 1];
b_X = [1;1;1];

f_X = @(X_vyr) (X-X_vyr)'*inv(V_X)*(X-X_vyr);
[X_vyr,fvalX] = fmincon(f_X,X_0,[],[],A_X,b_X,[],[],[],[]);
disp(X_vyr); disp(fvalX);
%%  kdyby jsme vyrovnávali pouze průtoky F1; F1,1; F1,2
F1;     %již zadaná výše
vF1_v = x(:,2).^2.*std_raw(:,1).^2 + F1.^2.*std_raw(:,2).^2;
V_F1 = diag(vF1_v);
F1_0 = F1.*1.05;
A_F1 = [1 -1 -1];
b_F1 = [0];

f_F1 = @(F1_vyr) (F1-F1_vyr)'*inv(V_F1)*(F1-F1_vyr);
[F1_vyr,fvalF1] = fmincon(f_F1,F1_0,[],[],A_F1,b_F1,[],[],[],['active-set']);
disp(F1_vyr); disp(fvalF1);
[F1_vyrc,fvalF1c] = fmincon(f_F1,F1_0,[],[],[],[],[],[],@confuneqF1,['active-set']);    %stejná fce jako předchozí ale za použití ceq
disp(F1_vyrc); disp(fvalF1c);
%%  
z = [dat(1,:), dat(2,:), dat(3,:)]';  %NAMĚŘENÁ DATA - vektor
Vz = diag([std_raw(1,:).^2, std_raw(2,:).^2, std_raw(3,:).^2]); %kovarianční matice
z_0  = z.*0.95;             %nástřel hodnot
%z_0  = [1.09547; 0.4784; 0.48823; 0.4822; 0.517; 0.941; 0.0197; 0.05; 0.98];

fz = @(z_vyr) (z-z_vyr)'*inv(Vz)*(z-z_vyr);      %účelová funkce
% Start with the default options
options = optimoptions('fmincon');
% Modify options setting
options = optimoptions(options,'Display', 'iter-detailed');
%options = optimoptions(options,'Display', 'final-detailed');
options = optimoptions(options,'Algorithm', 'active-set');
[z_vyr,fvalz,exitflag,output] = fmincon(fz,z_0,[],[],[],[],[],[],@confuneqz,options);   %nelineární podmínky zadané pomocí funkce confuneqz.m
%disp(z_vyr); disp(fvalz); disp(exitflag); disp(output);

%%  Výpis výsledků

%vysledky = ['metoda'; 'y'; 'y_hat'; 'y_vyr'; 'y_vyrc'; 'z'];


