clear all, close all, clc, warning off

v=get(0,'ScreenSize');
H=800; %Altezza in pixel grafici
W=1200; %Larghezza in pixel grafici

load('ModelloIdent.mat');

handle1=figure;
set(handle1,'Position',[round(v(3)/2-W/2) round(v(4)/2-H/2) W H]);

N=10; %N° totale di sensori
K=5;  %N° di sensori da selezionare 

n=size(A,1);  %N° di stati del sistema;
PermutazioneScelta=randperm(size(C,1));
PermutazioneScelta=sort(PermutazioneScelta(1:N))
C=C(PermutazioneScelta,:);

Combinazioni=nchoosek(1:N,K);
NComb=size(Combinazioni,1);
VettTraccePrior=zeros(NComb,1);
VettTraccePost=zeros(NComb,1);
VettTracceMis=zeros(NComb,1);
for i=1:NComb
   C_i=C(Combinazioni(i,:),:);
   R=VarianzaErr(PermutazioneScelta(Combinazioni(i,:)),PermutazioneScelta(Combinazioni(i,:)));
   S=K*VarianzaErr(:,PermutazioneScelta(Combinazioni(i,:)));
   Q=K*VarianzaErr*K'-S*inv(R)*(S');
   Q=(Q+Q')/2;
   F=A-S*inv(R)*C_i;
   try
       % Valore esatto della traccia della varianza d'errore di predizione
       [P,eigenvalues,L] = dare(F',C_i',Q,R);
       VettTracce(i)=trace(P);
       MatriceVarPost=P-P*C_i'*inv(C_i*P*C_i'+R)*C_i*P;
       VettTraccePost(i)=trace(MatriceVarPost);
       VarUscite=C*MatriceVarPost*C';
       VettTracceMis(i)=trace(VarUscite);
   catch
       fprintf('bug!! uffi!!\n');
   end   
end

RigheSelezionatePrior=1*ones(N,1);
RigheSelezionatePost=1*ones(N,1);
RigheSelezionateMis=1*ones(N,1);
for i=1:(N-K)
    OttimoLocalePrior=1e100;
    OttimoLocalePost=1e100;
    OttimoLocaleMis=1e100;
    RigaDaToglierePrior=-1;
    RigaDaToglierePost=-1;
    RigaDaTogliereMis=-1;
    for j=1:N
        if (ismember(j,find(RigheSelezionatePrior)))
            SensProva=[setdiff(find(RigheSelezionatePrior),j)];
            C_i=C(SensProva,:);
            R=VarianzaErr(PermutazioneScelta(SensProva),PermutazioneScelta(SensProva));
            S=K*VarianzaErr(:,PermutazioneScelta(SensProva));
            Q=K*VarianzaErr*K'-S*inv(R)*(S');
            Q=(Q+Q')/2;
            F=A-S*inv(R)*C_i;            
            [P,eigenvalues,L] = dare(F',C_i',Q,R);
            if (trace(P)<OttimoLocalePrior)
                OttimoLocalePrior=trace(P);
                RigaDaToglierePrior=j;
            end            
        end
        if (ismember(j,find(RigheSelezionatePost)))
            SensProva=[setdiff(find(RigheSelezionatePost),j)];
            C_i=C(SensProva,:);
            R=VarianzaErr(PermutazioneScelta(SensProva),PermutazioneScelta(SensProva));
            S=K*VarianzaErr(:,PermutazioneScelta(SensProva));
            Q=K*VarianzaErr*K'-S*inv(R)*(S');
            Q=(Q+Q')/2;
            F=A-S*inv(R)*C_i;            
            [P,eigenvalues,L] = dare(F',C_i',Q,R);
            MatriceVarPost=P-P*C_i'*inv(C_i*P*C_i'+R)*C_i*P;
            if (trace(MatriceVarPost)<OttimoLocalePost)
                OttimoLocalePost=trace(MatriceVarPost);
                RigaDaToglierePost=j;
            end            
        end
        if (ismember(j,find(RigheSelezionateMis)))
            SensProva=[setdiff(find(RigheSelezionateMis),j)];
            C_i=C(SensProva,:);
            R=VarianzaErr(PermutazioneScelta(SensProva),PermutazioneScelta(SensProva));
            S=K*VarianzaErr(:,PermutazioneScelta(SensProva));
            Q=K*VarianzaErr*K'-S*inv(R)*(S');
            Q=(Q+Q')/2;
            F=A-S*inv(R)*C_i;            
            [P,eigenvalues,L] = dare(F',C_i',Q,R);
            MatriceVarPost=P-P*C_i'*inv(C_i*P*C_i'+R)*C_i*P;
            VarMis=C*MatriceVarPost*C';
            if (trace(VarMis)<OttimoLocaleMis)
                OttimoLocaleMis=trace(VarMis);
                RigaDaTogliereMis=j;
            end            
        end
    end
    RigheSelezionatePrior(RigaDaToglierePrior)=0;
    RigheSelezionatePost(RigaDaToglierePost)=0;
    RigheSelezionateMis(RigaDaTogliereMis)=0;
end


[VettTracce Perm]=sort(VettTracce);
[VettTraccePost Perm]=sort(VettTraccePost);
[VettTracceMis Perm]=sort(VettTracceMis);


plot(VettTracce,'b');
hold on;
plot(VettTraccePost,'r');
plot(VettTracceMis,'g');
plot(OttimoLocalePrior*ones(NComb,1),'b');
plot(OttimoLocalePost*ones(NComb,1),'r');
plot(OttimoLocaleMis*ones(NComb,1),'g');
legend('Varianza a priori usando K sensori','Varianza a posteriori usando K sensori','Varianza di misure a posteriori usando K sensori','Varianza a priori usando K sensori (GREEDY)','Varianza a posteriori usando K sensori (GREEDY)','Varianza di misure a posteriori usando K sensori (GREEDY)','location','NorthWest');
