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(3*size(d)/4));
% d2 = d(round(3*size(d)/4+1):end);
% 
% %% n4sid su metÃ  dati e num di stati pari a uscite
% tic
% m = n4sid(d,length(pos_y),'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
% y_sim = sim(m,d)
% 
% %%
% for canale = 1:length(pos_y)
%     figure(1)
%     plot([ y_sim.OutputData(:,canale) d.OutputData(:,canale)])
%     axis tight
%     legend('sim','vero')
%     grid on
%     figure(2)
%     err = y_sim.OutputData(:,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');
load('Prova.mat');

n=size(m.A,1);

% N=size(m.C,1);
% K=1;
% RigheSelezionate=1*ones(N,1);
% hProgBar=progressbar( [],0,'Selezione sensori ottimali' );
% for i=1:(N-K)
%     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);
%             %catch
%                 
%             %end
%             if (trace(P)<OttimoLocale)
%                 OttimoLocale=trace(P);
%                 RigaDaTogliere=j;
%             end            
%         end
%     end
%     hProgBar = progressbar( hProgBar,1/(N-K));
%     RigheSelezionate(RigaDaTogliere)=0;
% 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);

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)));

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

y_tot_post=m.C*x_post;

for canale = 1:length(pos_y)
    figure(1)
    plot([ d.y(:,canale) y_tot_post(canale,:)'])
    if (ismember(canale,quali))
        title('Sensore misurato');
    else
        title('Sensore eliminato');
    end
    pause
end




