% ==============clear Console======================
clc

clear all
close all
%    load 'LOGERDATA.mat'
%    load 'TPDATA.mat'
%    load 'FLUXDATA.mat'

epsi1=1.e-15;
epsiSV=1.e-15;
DeTe=1.0/(5*60);
q_out_offset = -8 ;
Smooth=1;
if Smooth ==1
    load LOESS.mat
    Tu = LOESS_TP_DATA (:,2);  % Unutrasnja temperatura zida
    Ts = LOESS_TP_DATA (:,1);  % Spoljasnja temperatura zida
    qs = LOESS_FLUX_DATA (:,1);  % Fluks na spoljasnjoj strani zida
    qu = LOESS_FLUX_DATA (:,2);  % Fluks na unutrasnjoj strani zida
    Nq = length(qs);
    qs = qs + q_out_offset*ones(Nq,1) ;
elseif Smooth==0
    load TPDATA.mat
    load FLUXDATA.mat 
    NStart = 2;
    NEndT = 3601;
    NEndQ = 18001;
    Tu = TP_IN_T_WALLC (NStart:NEndT);  % Inside surface wall temperature
    Ts = TP_OUT_T_WALL (NStart:NEndT);  % Outside surface wall temperature
    qs = OUT_FLUX_CALC (NStart:NEndQ);  % Heat flux rate on outside wall surface 
    qu = IN_FLUX_CALC (NStart:NEndQ);  % Heat flux rate on inside wall surface
    Nq = length(qs);
    qs = qs + q_out_offset*ones(Nq,1) ;
end


%Resampling q
%qu = resample(qu,1,5);
%qs = resample(qs,1,5);

%Ts = Ts(1:1800);
%Tu = Tu(1:1800);
%qs = qs(1:1800);
%qu = qu(1:1800);

% Resampling TP
%{
qu = resample(qu,1,2);
qs = resample(qs,1,2);

Tu = resample(Tu,5,2);
Ts = resample(Ts,5,2);
%}
qu = resample(qu,1,5);
qs = resample(qs,1,5);

FlInOut = 3 ;
%FlInOut = 2 ;
%FlInOut = 3 ;
%FlInOut = 4 ;
%FlInOut = 5 ;
%FlInOut = 6 ;
if FlInOut ==1
    Input1= Tu;
    Input2= qu;
    Output1= Ts;
    Output2= qs;
elseif FlInOut ==2
    Input1= Ts;
    Input2= qs;
    Output1= Tu;
    Output2= qu;    
elseif FlInOut ==3
    Input1= Tu;
    Input2= Ts;
    Output1= qu;
    Output2= qs;
    
elseif FlInOut ==4
    Input1= qu;
    Input2= qs;
    Output1= Tu;
    Output2= Ts;
    
elseif FlInOut ==5
    Input1= Tu;
    Input2= qs;
    Output1= Ts;
    Output2= qu;
    
elseif FlInOut ==6    
    Input1= Ts;
    Input2= qu;
    Output1= Tu;
    Output2= qs;

end    
% Total number of measurements 
 
NInput1 = length(Input1);
NInput2 = length(Input2);
NOutput1 = length(Output1);
NOutput2 = length(Output2);
NN = NOutput1 ;
%Tu = resample(Tu,5,2);
%Ts = resample(Ts,5,2);

 %Tu = Tu - mean(Tu); 
 %Ts = Ts - mean(Ts); 
 %qs = qs - mean(qs); 
 %qu = qu - mean(qu);
 
 %Tu = 10*Tu;
 %qu = 10*qu;
 
% Tu = detrend(Tu); %input
% Ts = detrend(Ts); %input
% qs = detrend(qs); %output
% qu = detrend(qu); %output

% Input & Output data 
%Tu = Tu(1:3600);
%Ts = Ts(1:3600);
%qu = qu(1:3600);
%qs = qs(1:3600);
%========================================================================

%{
fUD1 = fopen('UD1.dat');
UD1 = fscanf(fUD1, '%f');
fclose(fUD1);

fUD2 = fopen('UD2.dat');
UD2 = fscanf(fUD2, '%f');
fclose(fUD2);

WD11 = importdata('WD11.dat');
WD12 = importdata('WD12.dat');

%fHD11 = fopen('HD11.txt');
%HD11 = fscanf(fHD11, '%f');
%fclose(fHD11);
%fHD12 = fopen('HD12.txt');
%HD12 = fscanf(fHD12, '%f');
%fclose(fHD12);

fY1 = fopen('Y1.dat');
YY1 = fscanf(fY1, '%f');
fclose(fY1);
%}
%========================================================================
%{

%figure (1);
%subplot(2,1,1);
%plot(UD1,'r');
%hold on;
%subplot(2,1,1);
%plot(UD2,'b');
%legend('UD1','UD2');
%title(' Input signals');
%grid on;
%hold off;

%figure (2);
%plot(YY1, 'r');
%legend('Y1');
%title('Output signal');
%grid on;
%hold off;

%Time delay
%TD = finddelay(Ts,Tu);
%qD = finddelay(qs,qu);

%figure(3);
%subplot (2,1,1);
%plot(WD11(1:2000));
%title ('Green Function W11');
%grid on;
%subplot (2,1,2);
%plot(WD12(1:2000));
%title ('Green Function W12');
%grid on;
%hold off;
%}

figure(33);
subplot (2,1,1);
plot(Tu , 'r');
hold on
plot(Ts , 'b');
hold on
grid on;
legend('Tu','Ts');
title ('Temperatures');
subplot (2,1,2);
plot(qu , 'r');
hold on
plot(qs , 'b');
grid on;
legend('qu','qs');
title ('Flyxes');
hold off;

N=round(NN/2);

%for m = 500 % m - number of previous data points the impulse response depends on
    %              INPUTS  
NNm = 150 ;    
m11=NNm;
m12=NNm;
m21=NNm;
m22=NNm;
m=max([m11 m12 m21 m22]);
%A1u = 1;
%A2u = 5 ;
%TPE1 = 1440; 
%TPE2 = 1440; 
%DETE = 5;
%Om1 = 2*pi/TPE1 ;
%Om2 = 2*pi/TPE2 ;
 %   for i=1:NN
 %    Tu(i) = Tu(i) + A1u*sin(Om1*(i-1)*DETE);
 %    qu(i) = qu(i) + A2u*sin(Om2*(i-1)*DETE);
     
%     Ts(i) =  A1*sin(Om1*(i-1)*DETE);
%     qs(i) =  A2*sin(Om2*(i-1)*DETE);
     
 %   end       
    
x1 = Input1(1:N) ;
x2 = Input2(1:N) ;

%{    
%figure (3)
%subplot(2,1,1)
%plot(x1, 'r')
%hold on
%plot(Tu,'b')
%legend('Tu','Ts')
%title('Measured data (temperature detrended)')
%hold on
%grid on
%subplot(2,1,2)
%plot(x2, 'r') 
%hold on
%plot(qu,'b') 
%legend('qu','qs')
%title('Measured data (flux detrended)')
%hold off;
%grid on
%}
y1=Output1(m:N);
y2=Output2(m:N);
U1=zeros(N-m+1,m);
U2=zeros(N-m+1,m);
for i  = 1:N-m+1
% Vector of the system output
%y1 = qs, y2 = qu

%              OUTPUTS
%     y1(i) = Ts(m+i-1);
%     y2(i) = qs (m+i-1);
          
% Convolution matrix of measured inputs
%U1 = Ts, U2 = Tu
    for j = 1:m
         U1(i,j) = x1(m+i-j);
         U2(i,j) = x2(m+i-j);
    end
end
%============================================================================        
        U11 = (U1(1:N-m+1,1:m11))'*U1(1:N-m+1,1:m11);       
        U12 = (U1(1:N-m+1,1:m11))'*U1(1:N-m+1,1:m12);
        U21 = (U2(1:N-m+1,1:m12))'*U1(1:N-m+1,1:m11);
        U22 = (U2(1:N-m+1,1:m12))'*U2(1:N-m+1,1:m12);
        UU1 = [U11 U12; U21 U22];
        Ut1 = [(U1(1:N-m+1,1:m11))'; (U2(1:N-m+1,1:m12))'];
        UX1 = [(U1(1:N-m+1,1:m11)) (U2(1:N-m+1,1:m12))];
%============================================================================         
        U11 = (U1(1:N-m+1,1:m21))'*U1(1:N-m+1,1:m21);
        U12 = (U1(1:N-m+1,1:m21))'*U2(1:N-m+1,1:m22);
        U21 = (U2(1:N-m+1,1:m22))'*U1(1:N-m+1,1:m21);
        U22 = (U2(1:N-m+1,1:m22))'*U2(1:N-m+1,1:m22);
        UU2 = [U11 U12; U21 U22];
        Ut2 = [(U1(1:N-m+1,1:m21)'); (U2(1:N-m+1,1:m22))'];
        UX2 = [(U1(1:N-m+1,1:m21)) (U2(1:N-m+1,1:m22))];
        %P1=U1'*y1';
        %P2=U2'*y1';
        %AA1=U21-U22*inv(U12)*U11;
        %BB1=P2-U22*inv(U12)*P1;
        %g1=inv(AA1)*BB1;
        
       % ConAA1 = cond(AA1);
       % DetAA1 =  det(AA1);
        %InU = inv(U);
        %Check1 = InU*U;
        %Check2 = U*InU;
        %disp(['ConNumber (AA1) =', ConAA1 ]);
        %disp(['Det(AA1) =', DetAA1 ]);
        %fprintf('%s %d %d %d ', 'Check1 = ', Check1(1,1), Check1(1,2) , Check1(1,3));
        %fprintf('%s %d %d %d ', 'Check1 = ', Check2(1,1), Check2(1,2) , Check2(1,3));
      %,epsiSV
      %,epsiSV
        Vec1=Ut1*y1;
        Vec2=Ut2*y2;
%        g1 = pinv(UU1)*Vec1;
%        g2 = pinv(UU2)*Vec2;
        g1 = UU1\Vec1;
        g2 = UU2\Vec2;
        
        gg11=g1(1:m11);
        gg12=g1(m11+1:m11+m12);
        gg21=g2(1:m21);
        gg22=g2(m21+1:m21+m22);
        
        [~,SS,~] = svd(UU1);
        N1=size(SS,1);
        Sing_Val1=zeros(N1,1);
        for i = 1: N1
            Sing_Val1(i) = SS(i,i)+epsi1;
        end
         [~,SS,~] = svd(UU2);
        N1=size(SS,1);
        Sing_Val2=zeros(N1,1);
        for i = 1: N1
            Sing_Val2(i) = SS(i,i)+epsi1;
        end
        CN1= cond(UU1);
        CN2= cond(UU2);
        %======================================
        %g1 = inv(U)*Ut*y1';
        %g2 = inv(U)*Ut*y2';
        %[U,S,V] = svd(A)
%        g2 = pinv(U)*Ut*y2';
%============================Estimation=========================================================  

        Y1EstIn  = UX1*g1;
        Y2EstPIn = UX2*g2;
        
        %WEST_IN11 = g1(1:m);
        %WEST_IN12 = g1(m+1:2*m);
        %WEST_PS_IN11 = g2(1:m);
        %WEST_PS_IN12 = g2(m+1:2*m);
      
%============================Green Function obtained by inversion and pseudo inversion =========================================================           
    figure(4)    
    subplot (2,1,1);
    plot(g1, 'r');
    legend('g1 ');
    title (' g1 VS g2 ');
    grid on;
    hold on;  
    subplot (2,1,2);
    plot(g2, 'b');
    legend('g2 ');
    grid on; 
    hold off;   
%================================= Singular Values==============================================================================    
    %+epsi
    figure(5) 
    subplot (2,1,1);
    plot(log10(Sing_Val1), 'r');
    legend('SV U1 ');
    title (' SV for U1 and U2 ');    
    hold on;
    grid on;    
    subplot (2,1,2);
    plot(log10(Sing_Val2), 'b');       
    legend('SV U2');
    grid on; 
    hold off; 
    disp(['Condition number(UU1) = '  sprintf('%g',CN1)]);
    disp(['Condition number(UU2) = '  sprintf('%g',CN2)]);
    %disp(CN1);
    %disp(CN2);

%=================================================================================================================
%     finish
%=================================================================================================================
          
    
%============================Compare Exact and Calculated Green Function =========================================================     
%{
    figure(5)
    subplot (2,1,1);
    plot(WD11(1:m), 'r');
    hold on;  
    plot(WEST_IN11, 'b')
    legend('W11-Exact','W11 Estimate by Inversion');
    title (' Exact Green VS Estimate by Inversion ');
    grid on;
    hold on;   
    
    subplot (2,1,2);
    plot(WD12(1:m), 'r');
    hold on;  
    plot(WEST_IN12, 'b')
    legend('W12-Exact','W12 Estimate by Inversion');    
    grid on;
    hold off; 
    
    figure(6)
    subplot (2,1,1);
    plot(WD11(1:m), 'r');    
    hold on;  
    plot(WEST_PS_IN11, 'b');
    legend('W11-Exact','W11 Estimate by Pseudo Inversion');
    title (' Exact Green VS Estimate by Pseudo Inversion '); 
    grid on;
    hold on;   
    subplot (2,1,2);
    plot(WD12(1:m), 'r');    
    hold on;  
    plot(WEST_PS_IN12, 'b')
    legend('W12-Exact','W12 Estimate by Pseudo Inversion');
       
    grid on;
    hold off;  
        
%============================Compare Output Signal and Estimation========================================================= 
%}
    figure(7)
    subplot (2,1,1);
    plot(y1, 'r');
    hold on;  
    plot(Y1EstIn, 'b');
    legend('Signal y1','Estimation ');
    title ('Signal VS Estimation ');    
    grid on;
    hold on;   
    subplot (2,1,2);
    plot(y2, 'r');
    hold on;  
    plot(Y2EstPIn, 'b');
    legend('Signal y1','Estimation');
    grid on;
    hold off;
        

%=================================================================================================================
    finish
%=================================================================================================================
    
    
%==================== Quality of Prediction=========================
       
        NY_START = N + 1 ;
        NX_START = NY_START-m+1 ;
        N_END = NN ;
        
        x1 = Input1(NX_START:N_END) ;
        x2 = Input2(NX_START:N_END) ;
        y1 = Output1 (NY_START:N_END);
        y2 = Output2 (NY_START:N_END);
                
for i  = 1:N_END-NY_START+1
%--------------------------------------------------------------------------
% Vector of the system output
%y1 = qs, y2 = qu
          
% Convolution matrix of measured inputs
%U1 = Ts, U2 = Tu
    for j = 1:m
         U1(i,j) = x1(m+i-j);
         U2(i,j) = x2(m+i-j);
    end
end
    UX1 = [(U1(1:N_END-NY_START+1,1:m11)) (U2(1:N_END-NY_START+1,1:m12))];
    UX2 = [(U1(1:N_END-NY_START+1,1:m21)) (U2(1:N_END-NY_START+1,1:m22))];
    Y1_Prediction  = UX1*g1;
    Y2_Prediction = UX2*g2;
                    
    figure(8)
    subplot (2,1,1);
    plot( y1, 'r');
    hold on;  
    plot( Y1_Prediction, 'b');
    legend('Signal y1','Prediction1 ');
    title ('Prediction ');    
    grid on;
    hold on;   
    subplot (2,1,2);
    plot(y2, 'r');
    hold on;  
    plot( Y2_Prediction, 'b');
    legend('Signal y2','Prediction2 ');
    grid on;
    hold off;
        
%=================================================================================================================
%     thermal losses
%==========================================================================
Q1_Pred  =zeros(N,1);
Q2_Pred  =zeros(N,1);
DeQ_Pred =zeros(N,1);
%==========================================================================
Q1  =zeros(N,1);
Q2  =zeros(N,1);
DeQ =zeros(N,1);

for i = 1:N 
    for j = 1:i
        Q1_Pred(i) = Q1_Pred(i) + Y1_Prediction(j) ;
        Q2_Pred(i) = Q2_Pred(i) + Y2_Prediction(j) ;        
        Q1(i) = Q1(i) + y1(j) ;
        Q2(i) = Q2(i) + y2(j) ;                
    end
        Q1_Pred(i) = DeTe*(Q1_Pred(i)-0.5*(Y1_Prediction(1)+Y1_Prediction(i)));
        Q2_Pred(i) = DeTe*(Q2_Pred(i)-0.5*(Y2_Prediction(1)+Y2_Prediction(i)));        
        Q1(i) = DeTe*(Q1(i) - 0.5*( y1(1) + y1(i)));
        Q2(i) = DeTe*(Q2(i) - 0.5*( y2(1) + y2(i)));                
        DeQ_Pred(i) = Q2_Pred(i)-Q1_Pred(i);
        DeQ(i)      = Q2(i)-Q1(i);
end

%=================================================================================================================
%     finish
%=================================================================================================================

figure(9)
    subplot (3,1,1);
    plot( Q1, 'r');
    hold on;  
    plot( Q1_Pred, 'b');
    legend('Q1','Q1_Prediction ');
    title ('Prediction for Q1,Q2 & Q2-Q1 ');    
    grid on;
    hold on;       
    subplot (3,1,2);
    plot(Q2, 'r');
    hold on;  
    plot( Q2_Pred, 'b');
    legend('Q2','Q2_Prediction ');
    grid on;
    hold on;      
    subplot (3,1,3);
    plot(DeQ, 'r');
    hold on;  
    plot( DeQ_Pred, 'b');
    legend('DeQ','DeQ_Pred ');
    grid on;
    hold off;

Check11=0;
Check12=0;
Check21=0;
Check22=0;
for i = 1:m11
         Check11 = Check11 + g1(i);         
end
for i = 1:m12
         Check12 = Check12 + g1(m11+i);         
end
for i = 1:m21
         Check21 = Check21 + g2(i);         
end
for i = 1:m22
         Check22 = Check22 + g2(m21+i);         
end

if Check12 ~=0
    U = -1/Check12;
end
disp(['Condition number(UU1) = '  sprintf('%g',CN1)]);
disp(['Condition number(UU2) = '  sprintf('%g',CN2)]);

disp(['Check11 = '  sprintf('%g',Check11)]);
disp(['Check12 = '  sprintf('%g',Check12)]);
disp(['Check21 = '  sprintf('%g',Check21)]);
disp(['Check22 = '  sprintf('%g',Check22)]);
disp(['U = '  sprintf('%g',U)]);


% =================================
 G11 = fft(gg11);
 G12 = fft(gg12);
 G21 = fft(gg21);
 G22 = fft(gg22);
% =================================

 figure (10)
 plot(abs(G11),'b','linewidth', 2);hold on
 title('absG11')
 hold off
 grid on
% =================================
 figure (11)
 plot(abs(G12),'r','linewidth', 2);hold on
 title('absG12')
 hold off
 grid on
% =================================
 figure (12)
 plot(abs(G21),'g','linewidth', 2);hold on
 title('absG21')
 hold off
 grid on
% =================================
 figure (13)
 plot(abs(G22),'b','linewidth', 2);hold on
 title('absG22')
 hold off
 grid on

HH11 = zeros(m11,1);
HH12 = zeros(m12,1);
HH21 = zeros(m21,1);
HH22 = zeros(m22,1);
for i=1:m11
    HH11(i) =- G11(i)/G12(i);
    HH12(i) = 1/G12(i);
    HH21(i) = G21(i)-G22(i)*G11(i)/G12(i);    
    HH22(i) =G22(i)/G12(i);
end

hh11=ifft(HH11);
hh12=ifft(HH12);
hh21=ifft(HH21);
hh22=ifft(HH22);

hh11=real(hh11);
hh12=real(hh12);
hh21=real(hh21);
hh22=real(hh22);

% S11=0;
% S12=0;
% S21=0;
% S22=0;

S11=sum(hh11);
S12=sum(hh12);
S21=sum(hh21);
S22=sum(hh22);

disp(['S11 = '  sprintf('%g',S11)]);
disp(['S12 = '  sprintf('%g',S12)]);
disp(['S21 = '  sprintf('%g',S21)]);
disp(['S22 = '  sprintf('%g',S22)]);

figure(71)
plot(hh11,'r','linewidth', 2);hold on
title('h11')
hold off
grid on

figure(72)
plot(hh12,'g','linewidth', 2);hold on
title('h12')
hold off
grid on

figure(73)
plot(hh21,'b','linewidth', 2);hold on
title('h21')
hold off
grid on


figure(74)
plot(hh22,'r','linewidth', 2);hold on
title('h22')
hold off
grid on

disp(' ==================E N D==================================')   
%======================= OLD CODE by Zorana Petojevic=================================
%{
        figure(m)
        subplot(3,1,1)
        plot(Ts(m+1:3600),'b');hold on,plot(TsEst(1:3600-m),'r');hold on
        title('qs vs. qsEst')
        xlabel( 'Length of signal (3600-m)')
        ylabel ('W/(m2*K)')
        grid on
        subplot(3,1,2)
        plot(Tu(m+1:3600),'b');hold on,plot(TuEst(1:3600-m),'r');hold on
        title('qu vs. quEst')
        xlabel( 'Length of signal (3600-m)')
        ylabel ('W/(m2*K)')
        grid on
        subplot(3,1,3)
        plot(Ts(m+1:3600)-TsEst(1:3600-m),'g');hold on
        plot(Tu(m+1:3600)-TuEst(1:3600-m),'m');hold on
        title('Errors')
        xlabel( 'Length of signal (3600-m)')
        ylabel ('W/(m2*K)')
        legend('qs','qu')
        grid on

%Goodness of fit between estimate and reference data in MSE sence
       cost_func = 'NMSE';
       fit_qs = goodnessOfFit(TsEst(1:3600-m),Ts(m+1:3600),cost_func);
       fit_qu = goodnessOfFit(TuEst(1:3600-m),Tu(m+1:3600),cost_func);
% Correlation coefficient between estimate and reference data      
        cc = corrcoef(Ts(m+1:3600),TsEst(1:3600-m));
        coef = cc(1,2);
        x = m;
   figure (3) %Plot correlation coefficients between qs and qsEst for m
        plot(x,coef, '--rs','LineWidth',2,'MarkerEdgeColor','k','MarkerFaceColor','g','MarkerSize',5); hold on
        title ('Correlation coefficients between qs and qsEst')
        xlabel('m - number of previous data points the impulse response depends on')
        grid on
        cc = corrcoef(Tu(m+1:3600),TuEst(1:3600-m));
        coef = cc(1,2);
   figure (4) %Plot correlation coefficients between qu and quEst for m
        plot(x,coef, '--rs','LineWidth',2,'MarkerEdgeColor','k','MarkerFaceColor','g','MarkerSize',5); hold on
        title ('Correlation coefficients between qu and quEst')
        xlabel('m - number of previous data points the impulse response depends on')
        grid on
        
   % eval(['qsEst_' int2str(m) '= qsEst']);
   % eval(['quEst_' int2str(m) '= quEst']);
   % eval(['fit_qs_' int2str(m) '= fit_qs']);
   % eval(['fit_qu_' int2str(m) '= fit_qu']);
    figure(5)
        x = m;
        y = fit_qs;
        plot(x,y,'--rs','LineWidth',2,'MarkerEdgeColor','k','MarkerFaceColor','g','MarkerSize',5); hold on
        title('MSE between estimate qs and reference qs data') 
        xlabel('m - number of previous data points the impulse response depends on')
        ylabel('MSE')
        grid on
    figure(6)
        z = fit_qu;
        plot(x,z,'--rs','LineWidth',2,'MarkerEdgeColor','k','MarkerFaceColor','g','MarkerSize',5); hold on
        title('MSE between estimate qu and reference qu data')
        xlabel('m - number of previous data points the impulse response depends on')
        ylabel('MSE')
        grid on
%end
figure(8)
subplot(2,1,1)
plot(g1); hold on
subplot (2,1,2)
plot(g2); hold off
grid on

%}