%% Calculation of WCE of outputs (full order controller and low order controller)
clc
clear all
close all

for Nsim =  10;
nfull = 16;
nred  = 16;
Nfull = 8;
% Nred = 5;   % Big Example 1
% Nred = 6;   % Big Example 2
Nred = 7;   % Big Example 3
d01 = [];
d02 = [];
d0n = [];
    for i = 1:length(Nred)
        [objval(i) ur(:,i) uf(:,i) xr xf Nsim d0] = yalmip_N_WCE(nfull,nred,Nsim,Nfull,Nred(i))
        d01 = [d01; d0(1,1)];
        d02 = [d02; d0(2,1)];
        d0n = [d0n; d0];
    end

d01 = double(d01);
d02 = double(d02);

%% plot of WCE for different nred in last step of Nsim 
figure;
plot(Nred,objval)
xlabel('Nred')
ylabel('worst case error (outputs)')
% XLIM([1 16]);
title(['Nsim' num2str(Nsim)])

%% save Nsim.mat nred objval
save(['P_Nsim' num2str(Nsim) '.mat'])
end

%% calculation and plot sum of WCE using MPC simulation and information from the end of Nsim

%calculate objsum of WCE for this Nsim and plot it 
figure;
Nsum = 10;  % lenght Nsum
for Nplot = Nsim
load(['P_Nsim' num2str(Nplot) '.mat'])
[Pobjvalsum] = sum_N_WCE(d01,d02,nfull,nred,Nsim,Nsum,Nfull,Nred); 


plot(Nred,Pobjvalsum)               %using nonscaling obj. fun
grid on;
hold on
end

% plotting (labels, title, legend..)
xlabel('Nred')
ylabel('worst case error (outputs)')
title(['Nsim sum' num2str(Nsim)])
% XLIM([1 16]);

%% just plot plot sum of WCE for Nsim

% for Nplot = Nsim
% 
% load(['Objsum_N_Nsim' num2str(Nplot) '.mat'])
% plot(Nred,Pobjvalsum,'LineWidth',2)               %using nonscaling obj. fun
% grid on
% hold on
% end
% 
% % plotting (labels, title, legend..)
% 
% xlabel('Nred')
% ylabel('sum of worst case error (outputs)')
% title('');
% % XLIM([1 16]);

%% save data
% save PHWCE_E1
% save PHWCE_E2
save PHWCE_E3
