clear all;
close all;
clc;
load data
lum_ = ['051';'007';'137';'033';'132'];
tempE_ = ['131';'147';'157';'010'];

% % %--->>> idea: uso solo il sensore a nord di temperatura (147) meno soggetto
% % %             a sbarellamenti di temperatura
tempE = ['147'];
lum = ['051'];

% idea 2 aggiungo un sensore di temperatura interno come ingresso
% indipendente
tempE = [tempE; '141'];

pos_uT = name2pos(tempE,data)
pos_uL = name2pos(lum,data)
pos_y = setdiff([1:length(data.name)],[name2pos(tempE,data) name2pos(lum,data) name2pos(lum_,data) name2pos(tempE_,data)])

% tolgo il 102, qcosa nn va secondo me
pos_y = setdiff(pos_y,name2pos(['102';'154'],data))

camp = 20; % 5 minuti
d = iddata(double(data.T(pos_y,1:camp:end)'),...
    double([ data.T(pos_uT,1:camp:end)' data.aR(pos_uL,1:camp:end)']),30*camp)

d1 = d(1:round(2*size(d)/3));
d2 = d(round(2*size(d)/3+1):end);

%% n4sid su metà dati e num di stati pari a uscite
tic
m= n4sid(d1,53,'focus','simulation','nk',ones(1,d.NU))
%m = n4sid(d1,10,'focus','simulation')
toc

%% trova forma equivalente con C = I
% Ti = m.C;
% T = inv(Ti);
% C_ = m.C*T;
% B_ = Ti*m.B;
% figure(100)
% imagesc(B_)
% A_ = Ti*m.A*T;
% figure(101)
% imagesc(A_)

%% simula in catena aperta a partire dalle cond iniziali calcolate
sys = ss(m.A,m.B,m.C,m.D,-1);
y_sim = lsim(sys,d.u,1:length(d.u),m.x0);

%%
for canale = 1:length(pos_y)
    figure(1)
    plot([ y_sim(:,canale) d.OutputData(:,canale)])
    axis tight
    legend('sim','vero')
    grid on
    figure(2)
    err = y_sim(:,canale)-d.OutputData(:,canale);
    plot(err)
    axis tight
    ax = axis();
    axis([ax(1) ax(2) -4 4]);
    grid on
    figure(3)
    hold off
    for k = 1:length(data.name)
        if k == pos_y(canale)
            plot3(data.p(k,1),data.p(k,2),data.p(k,3),'rx','MarkerSize',10 )
        else
            plot3(data.p(k,1),data.p(k,2),data.p(k,3),'+','MarkerSize',3 )
        end
        text(data.p(k,1)+0.1,data.p(k,2)+0.1,data.p(k,3),data.name(k,:))
        hold on 
    end
    xlabel('x')
    ylabel('y')
    zlabel('z')
    figure(4)
    plot(xcorr(err,'coeff'))
    title(sprintf('%s, fit %2.3f',data.name(pos_y(canale),:),100*(1 - norm(err)/norm(d.OutputData(:,canale)-mean(d.OutputData(:,canale))))))
    pause
end

%%

% 
% % %% KALMAN tutti le uscite
% % sys = ss(m.A,[m.B m.K],m.C,0,1);
% % [kest,L,P] = kalman(sys,m.NoiseVariance,m.NoiseVariance,m.NoiseVariance)
% % y_e = lsim(kest,[d.u d.y],1:length(d.u)) 
% % for canale = 1:length(pos_y)
% %     figure(1)
% %     plot([ d.y(:,canale) y_e(:,canale)])
% %     pause
% % end
% 
% %% KALMAN alcune uscite
% 
% save('Prova.mat');

%%


n=size(m.A,1);

N=size(m.C,1);
K=[10];
K=sort(K);
NScelte=size(K,2);
SensoriOttimi=-1*ones(NScelte,N);
RigheSelezionate=1*ones(1,N);
hProgBar=progressbar( [],0,'Selezione sensori ottimali' );
for i=1:(N-K(1))
    OttimoLocale=1e100;
    RigaDaTogliere=-1;
    for j=1:N
        if (ismember(j,find(RigheSelezionate)))
            SensAtt=setdiff(find(RigheSelezionate),j);
            R=m.NoiseVariance(SensAtt,SensAtt);
            S=m.K*m.NoiseVariance(:,SensAtt);
            Q=m.K*m.NoiseVariance*((m.K)')-S*inv(R)*(S');
            Q=(Q+Q')/2;
            F=m.A-S*inv(R)*m.C(SensAtt,:);
            try
                [P,eigenvalues,L] = dare(F',m.C(SensAtt,:)',Q,R);
                MatriceVarPost=P-P*m.C(SensAtt,:)'*inv(m.C(SensAtt,:)*P*m.C(SensAtt,:)'+R)*m.C(SensAtt,:)*P;
                VarUscite=m.C*MatriceVarPost*m.C';
                if (trace(VarUscite)<OttimoLocale)
                    OttimoLocale=trace(VarUscite);
                    RigaDaTogliere=j;
                end  
            catch
                fprintf('bug!! uffi!!\n');
            end
        end
    end
    hProgBar = progressbar( hProgBar,1/(N-K(1)));
    RigheSelezionate(RigaDaTogliere)=0;
    [Esiste Loc]=ismember(N-i,K);
    if (Esiste)
        SensoriOttimi(Loc,:)=RigheSelezionate;
    end
end
progressbar(hProgBar,-1 );

%quali=find(RigheSelezionate);

% quali=[3 6 10 20 24 30 34 43 49 51];

%quali=[];

% R=m.NoiseVariance;
% S=m.K*m.NoiseVariance;
% Q=m.K*m.NoiseVariance*m.K'-S*inv(R)*S';
% Q=(Q+Q')/2;
% F=m.A-S*inv(R)*m.C;
% C=m.C;
% [P,eigenvalues,L]=dare(F',C',Q,R);
% TracciaOttima=trace(P);

SceltaPersonalizzata=zeros(1,N);
SceltaPersonalizzata([1 6 9 15 17 31 42 43 46 50])=1;

SensoriOttimi=[SensoriOttimi; SceltaPersonalizzata];
NScelte=size(SensoriOttimi,1);

NCamp=size(d.y,1);
x_post=zeros(n,NCamp,NScelte);
x_prior=zeros(n,NCamp+1,NScelte);
x_prior(:,1,:)= repmat(zeros(size(m.x0)),[1 1 NScelte]);
y_tot_post=zeros(size(m.C,1),NCamp,NScelte);

for SceltaAttuale=1:NScelte
    quali=find(SensoriOttimi(SceltaAttuale,:));
    R=m.NoiseVariance(quali,quali); 
    S=m.K*m.NoiseVariance(:,quali);
    Q=m.K*m.NoiseVariance*m.K'-S*inv(R)*S';
    Q=(Q+Q')/2;
    F=m.A-S*inv(R)*m.C(quali,:);
    C=m.C(quali,:);
    B=m.B;
    [P,eigenvalues,L]=dare(F',C',Q,R);
    L=P*C'*inv(C*P*C'+R);
    %fprintf(strcat('Fattore di qualit� dei sensori scelti: ',num2str(trace(P)/TracciaOttima-1)));

    App=S*inv(R);
    for i=1:NCamp
        x_post(:,i,SceltaAttuale)=x_prior(:,i,SceltaAttuale)+L*(d.y(i,quali)'-C*x_prior(:,i,SceltaAttuale));
        x_prior(:,i+1,SceltaAttuale)=F*x_post(:,i,SceltaAttuale)+App*d.y(i,quali)'+B*d.u(i,:)';
    end

    y_tot_post(:,:,SceltaAttuale)=m.C*x_post(:,:,SceltaAttuale);
end

Errore=repmat(d.y',[1 1 NScelte])-y_tot_post;
Errore=Errore.^2;
Errore=sum(Errore,1);
Errore=reshape(Errore,[NCamp NScelte]);
Errore=Errore./size(d.y,2);
VettColori=['r' 'm' 'k' 'c' 'y'];

figure(73)
hold;
Legenda=zeros(0,30);
for i=1:NScelte
    if (i>length(K))
        Str='Scelta personalizzata';
    else
        Str='Num di sensori =';
        Str=strcat(Str,num2str(K(i)));
    end
    Str=strcat(Str,' '*ones(1,30-size(Str,2)));
    plot(Errore(:,i),VettColori(i));
    Legenda=[Legenda; Str];
end 
legend(Legenda);
title('Errori di filtraggio in base alla scelta di sensori fatta');
hold;

for canale = 1:length(pos_y)
    figure(67);
    plot([ d.y(:,canale) y_sim(:,canale)]);
    hold;
    Legenda=['Grandezza Misurata                     ';
             'Simulazione open loop                  '];
    for i=1:NScelte
        plot([y_tot_post(canale,:,i)'],VettColori(i));
        if (i>length(K))
            Str='Scelta personalizzata';
        else
            Str='Num di sensori =';
            Str=strcat(Str,num2str(K(i)));
        end
        if (ismember(canale,find(SensoriOttimi(i,:))))
            Str=strcat(Str,' Sensore misurato');
        else
            Str=strcat(Str,' Sensore simulato');
        end
        Str=strcat(Str,' '*ones(1,size(Legenda(1,:),2)-size(Str,2)));
        Legenda=[Legenda; Str];
    end

    legend(Legenda);
    title(strcat('Sensore N° ',data.name(pos_y(canale),:)));
    pause
    hold;
end


%%
figure(1000)
for k = 1:length(data.name)
    if any(k == pos_y(find(SensoriOttimi(2,:))))
        plot3(data.p(k,1),data.p(k,2),data.p(k,3),'rx','MarkerSize',10 )
    else
        plot3(data.p(k,1),data.p(k,2),data.p(k,3),'+','MarkerSize',3 )
    end
    text(data.p(k,1)+0.1,data.p(k,2)+0.1,data.p(k,3),data.name(k,:))
    hold on
end

%%

[aa1,aa2] = ismember(name2pos(['160';'009';'140';'145';'163';'005';'130';'107'],data),pos_y)


