clc;
close all;
clearvars;

N = 10;
system = "K/p^2 sinus BT"
IODelay = NaN;

switch system
    case "K"
        NP = 1;
        NZ = 0;
        TS = 0.1;
    case "K/p"
        NP = 0;
        NZ = 0;
        TS = 0.1;
    case "K/p^2 PRBS T"
        NP = 2;
        NZ = 1;
        TS = 0.01;
    case "K/p+1"
        NP = 1;
        NZ = 1;
        TS = 0.1;
    case "K/p sinus"
        NP = 0;
        NZ = 0;
        TS = 0.1;
    case "K/p^2 PRBS BT"
        NP = 2;
        NZ = 1;
        TS = 0.01;
    case "K/p^2 sinus T"
        NP = 1;
        NZ = 1;
        TS = 0.01;
    case "K/p^2 sinus BT"
        NP = 1;
        NZ = 1;
        TS = 0.01;
end


for i = 1:N
    
    switch system
        case "K/p"
            load(['Merane_data\NaZpracovanie\K--p\K5--p__prbs3_', num2str(i),'.mat']);
            load("Merane_data\NaZpracovanie/K--p\DelayVect_N1-10.mat");
        case "K"
            load(['Merane_data\NaZpracovanie\K\K10__prbs2_', num2str(i),'.mat']);
            load("Merane_data\NaZpracovanie/K\DelayVect_N1-10.mat");
        case "K/p^2 PRBS T"
            load(['Merane_data\BP/Tuhost/PRBS/PRBS_T_1-p^2_N', num2str(i),'.mat']);
            load("Merane_data\BP\Tuhost\PRBS\DelayVect_N1-10.mat");
            %load(['Merane_data\NaZpracovanie\K--p^2\K1--p^2__prbs10_', num2str(i),'.mat']);
        case "K/p+1"
            load(['Merane_data\NaZpracovanie\K--p+1\K10--p+1__prbs3_', num2str(i),'.mat']);
            load("Merane_data\NaZpracovanie/K--p+1\DelayVect_N1-10.mat");
        case "K/p sinus"
            load(['Merane_data\NaZpracovanie\sinus\K5--p__amp0.2_freq1_2_3rad__N', num2str(i),'.mat']);
            load("Merane_data\NaZpracovanie/sinus\DelayVect_N1-10.mat");
        case "K/p^2 PRBS BT"
            load(['Merane_data\BP\BezTuhosti\PRBS\PRBS_BT_1-p^2_N',num2str(i),'.mat']);
            load("Merane_data\BP\BezTuhosti\PRBS\DelayVect_N1-10.mat");
        case "K/p^2 sinus T"
            load(['Merane_data\BP\Tuhost\Sinus\SIN_T_1-p^2_N',num2str(i),'.mat']);
            load("Merane_data\BP\Tuhost\Sinus\DelayVect_N1-10.mat");
        case "K/p^2 sinus BT"
            load(['Merane_data\BP\BezTuhosti\Sinus\SIN_BT_1-p^2_N',num2str(i),'.mat']);
            load("Merane_data\BP\BezTuhosti\Sinus\DelayVect_N1-10.mat");
    end
    
    
    data = iddata(out.action.data,out.error.data, TS);
%     switch system
%         case {"K/p^2 PRBS T", "K/p^2 PRBS BT"}
%             sys = tfest(data, NP, NZ, DelayVect(i));
%         otherwise
%             sys = tfest(data, NP, NZ, IODelay);
%     end
    sys = tfest(data, NP, NZ, DelayVect(i));
    % sys = tfest(data, NP, NZ, IODelay);
    
    OpTf{i}{1} = sys.Numerator;
    OpTf{i}{2} = sys.Denominator;
    OpTf{i}{3} = sys.IODelay;
    OpTf{i}{4} = sys.Report.Fit.FitPercent;
    OpTf{i}{5} = sys;
    
    %if system == "K" || system == "K/p+1"
        w = logspace(-3, 2, 500);
        [mag{i} phase{i}] = bode(sys, w);
    %end
        
end

AvgNum = 0;
AvgDen = 0;
AvgIOD = 0;
AvgFit = 0;

for i = 1:N
   
   AvgNum = OpTf{i}{1} + AvgNum;
   AvgDen = OpTf{i}{2} + AvgDen;
   AvgIOD = OpTf{i}{3} + AvgIOD;
   AvgFit = OpTf{i}{4} + AvgFit; 
   
   switch system
       case "K/p"
           K(i) = OpTf{i}{1};
           tau(i) = OpTf{i}{3};
       case "K/p sinus"
           K(i) = OpTf{i}{1};
           tau(i) = OpTf{i}{3};
       case "K"
           K(i) = OpTf{i}{1} / OpTf{i}{2}(2);
           T(i) = 1 / OpTf{i}{2}(2);
           tau(i) = OpTf{i}{3};
       case {"K/p^2 PRBS T", "K/p^2 PRBS BT"}     % Fr =  (Ap + B) / ( p^2 + Cp + D )
%            A(i) = OpTf{i}{1}(1);
%            B(i) = OpTf{i}{1}(2);
%            C(i) = OpTf{i}{2}(2);
%            D(i) = OpTf{i}{2}(3);
%            tau(i) = OpTf{i}{3};
             [W, x] = damp(OpTf{i}{5});
             %T(i) = 2*pi/W(1);
             T(i) = sqrt(1/OpTf{i}{2}(3));
             xi(i) = x(1);
             tau(i) = OpTf{i}{3};
             T1(i) = OpTf{i}{1}(1) / OpTf{i}{1}(2);
             K(i) = OpTf{i}{1}(2)/OpTf{i}{2}(3);
       case "K/p+1"
           T1(i) = OpTf{i}{1}(1) / OpTf{i}{1}(2);
           T2(i) = OpTf{i}{2}(1) / OpTf{i}{2}(2);
           K(i) = OpTf{i}{1}(2) / OpTf{i}{2}(2);
           tau(i) = OpTf{i}{3};
       case {"K/p^2 sinus BT" , "K/p^2 sinus T"}
           T1(i) = OpTf{i}{1}(1) / OpTf{i}{1}(2);
           T2(i) = OpTf{i}{2}(1) / OpTf{i}{2}(2);
           K(i) = OpTf{i}{1}(2) / OpTf{i}{2}(2);
           tau(i) = OpTf{i}{3};
   end
   
end

AvgNum = AvgNum/N;
AvgDen = AvgDen/N;
AvgIOD = AvgIOD/N;
AvgFit = AvgFit/N;

switch system
    case "K/p"
%         Kavg = AvgNum/AvgDen(2);
%         Tavg = 1/AvgDen(2);
%         disp('Operator model:');
%         disp('Fr = ');
%         disp(['              ' num2str(Kavg)]);
%         disp(['exp(', num2str(AvgIOD) 'p) ------------']);
%         disp(['           ', num2str(Tavg), 'p + 1']);
%         disp(['Average model fit: ', num2str(AvgFit), '%']);
        
        figure(1);
        subplot(1,2,1);
        bar(K);
        xlabel('Číslo merania');
        ylabel('K_R');
        title('Hodnoty K_R');
        axis([0 N+1 0 0.2]);
        subplot(1,2,2);
        bar(tau);
        xlabel('Číslo merania');
        ylabel('\tau');
        title('Dopravné oneskorenie');
        axis([0 N+1 0 0.9]);
        
    case "K"
%         Kavg = AvgNum/AvgDen(2);
%         Tavg = 1/AvgDen(2);
%         disp('Operator model:');
%         disp('Fr = ');
%         disp(['              ' num2str(Kavg)]);
%         disp(['exp(', num2str(AvgIOD) 'p) ------------']);
%         disp(['           ', num2str(Tavg), 'p + 1']);
%         disp(['Average model fit: ', num2str(AvgFit), '%']);
        
        figure(1);
        subplot(1,2,1);
        bar(K./T);
        xlabel('Číslo merania');
        ylabel('K_R/T');
        title('Pomer K_R/T');
        axis([0 N+1 0 0.2]);
        subplot(1,2,2);
        bar(tau);
        xlabel('Číslo merania');
        ylabel('\tau');
        title('Dopravné oneskorenie');
        axis([0 N+1 0 0.5]);
        
%         figure(2)
%         for i = 1:N
%           semilogx(w, 20*log10(mag{i}(:)));
%           hold on;
%         end
%         grid on;
%         xlabel('\omega [rad/s]');
%         ylabel('|F_R(j\omega)| [dB]');
%         title('Frekvenčné charakteristiky jednotlivých prenosov');
        
    case "K/p^2 PRBS T"    % Fr = K(T1p+1) / ( T^2p^2 + Txip + 1 )
        
        figure(3)
        subplot(1,4,1);
        bar(K.*T1);
        xlabel('Číslo merania');
        ylabel('K_R \cdot T_1');
        title('Hodnoty K_R \cdot T_1');
        axis([0 N+1 0 0.6]);
        %xtickangle(45);

        subplot(1,4,2);
        bar(T);
        xlabel('Číslo merania');
        ylabel('T');
        title('Hodnoty T');
        axis([0 N+1 0 1]);
        %xtickangle(45);
        
        subplot(1,4,3);
        bar(xi);
        xlabel('Číslo merania');
        ylabel('\xi');
        title('Hodnoty \xi');
        axis([0 N+1 0 0.35]);
        
        subplot(1,4,4);
        bar(tau);
        xlabel('Číslo merania');
        ylabel('\tau');
        title('Dopravné oneskorenie');
        axis([0 N+1 0 0.9]);
        
        figure(1)
        subplot(1,2,1);
        bar(T1);
        xlabel('Číslo merania');
        ylabel('T1');
        title('Hodnoty T1');
        axis([0 N+1 0 13]);
        xtickangle(45);
        
        subplot(1,2,2);
        bar(K);
        xlabel('Číslo merania');
        ylabel('K_R');
        title('Hodnoty K_R');
        axis([0 N+1 0 0.1]);
        xtickangle(45);

    case "K/p^2 PRBS BT"

        figure(3)
        subplot(1,4,1);
        bar(K.*T1);
        xlabel('Číslo merania');
        ylabel('K_R \cdot T_1');
        title('Hodnoty K_R \cdot T_1');
        axis([0 N+1 0 0.6]);
        %xtickangle(45);

        subplot(1,4,2);
        bar(T);
        xlabel('Číslo merania');
        ylabel('T');
        title('Hodnoty T');
        axis([0 N+1 0 1]);
        %xtickangle(45);
        
        subplot(1,4,3);
        bar(xi);
        xlabel('Číslo merania');
        ylabel('\xi');
        title('Hodnoty \xi');
        axis([0 N+1 0 0.35]);
        
        subplot(1,4,4);
        bar(tau);
        xlabel('Číslo merania');
        ylabel('\tau');
        title('Dopravné oneskorenie');
        axis([0 N+1 0 0.9]);
        
        figure(1)
        subplot(1,2,1);
        bar(T1);
        xlabel('Číslo merania');
        ylabel('T1');
        title('Hodnoty T1');
        axis([0 N+1 0 13]);
        xtickangle(45);
        
        subplot(1,2,2);
        bar(K);
        xlabel('Číslo merania');
        ylabel('K_R');
        title('Hodnoty K_R');
        axis([0 N+1 0 0.1]);
        xtickangle(45);
        
    case "K/p+1"
        figure(3);
        subplot(1,3,1);
        bar(K./T2);
        xlabel('Číslo merania');
        ylabel('K_R/T_2');
        title('Hodnoty K_R/T_2');
        axis([0 N+1 0 0.1]);
        subplot(1,3,2);
        bar(T1);
        xlabel('Číslo merania');
        ylabel('T_1');
        title('Hodnoty T_1');
        axis([0 N+1 0 1.5]);
        subplot(1,3,3)
        bar(tau);
        xlabel('Číslo merania');
        ylabel('\tau');
        title('Dopravné oneskorenie');
        axis([0 N+1 0 0.7]);
        
    case "K/p sinus"
        
        figure(1);
        subplot(1,2,1);
        bar(K);
        xlabel('Číslo merania');
        ylabel('K_R');
        title('Hodnoty K_R');
        axis([0 N+1 0 0.6]);
        subplot(1,2,2);
        bar(tau);
        xlabel('Číslo merania');
        ylabel('\tau');
        title('Dopravné oneskorenie');
        axis([0 N+1 0 0.25]);

    case {"K/p^2 sinus BT" , "K/p^2 sinus T"}
        figure(3);
        subplot(1,4,1);
        bar(K);
        xlabel('Číslo merania');
        ylabel('K_R');
        title('Hodnoty K_R');
        subplot(1,4,2);
        bar(T1);
        xlabel('Číslo merania');
        ylabel('T_1');
        title('Hodnoty T_1');
        %axis([0 N+1 0 0.15]);
        subplot(1,4,3);
        bar(T2);
        xlabel('Číslo merania');
        ylabel('T_2');
        title('Hodnoty T_2');
        %axis([0 N+1 0 1]);
        subplot(1,4,4)
        bar(tau);
        xlabel('Číslo merania');
        ylabel('\tau');
        title('Dopravné oneskorenie');
        axis([0 N+1 0 0.7]);
end

figure(2)
        for i = 1:N
          semilogx(w, 20*log10(mag{i}(:)), 'linewidth', 1);
          hold on;
        end
        grid on;
        xlabel('\omega [rad/s]');
        ylabel('|F_R(j\omega)| [dB]');
        title('Frekvenčné charakteristiky jednotlivých prenosov');

set(figure(1), 'position', [450 400 500 300]);
set(figure(2), 'position', [450 400 500 350]);
set(figure(3), 'position', [450 400 780 300]);



%set(figure(4), 'position', [450 400 500 350]);