% close all; clear all; clc;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% N A C T E N I    V S T U P N I C H    D A T    
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
load data_prakt2

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% U P R A V A    V S T U P N I C H    D A T
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Ts = Data(2,1);
if exist('testim')
    window = int16(testim/Ts+1);
end
Td = round(td/Ts);                %dopravni zpozdeni
[m,n] = size(Data);         % m...radky, n...sloupce
% fi...matice vstupnich a vystupnich dat
switch unum
    case 2
        uk1 = Data(window+2:m-1-Td,3)/regu;   % fi(:,3) u(k-1)
        uk2 = Data(window+1:m-2-Td,3)/regu;
        yk1 = Data(window+2+Td:m-1,2)/regy;  % fi(:,2) y(k-1) 
        yk2 = Data(window+1+Td:m-2,2)/regy;
        Y = Data(window+3+Td:m,2)/regy;  
    case 3   
        uk1 = Data(window+3:m-1-Td,3)/regu;   % fi(:,3) u(k-1)
        uk2 = Data(window+2:m-2-Td,3)/regu;
        uk3 = Data(window+1:m-3-Td,3)/regu;
        yk1 = Data(window+3+Td:m-1,2)/regy;  % fi(:,2) y(k-1) 
        yk2 = Data(window+2+Td:m-2,2)/regy;
        yk3 = Data(window+1+Td:m-3,2)/regy;
        Y = Data(window+4+Td:m,2)/regy;  
    case 4
        uk1 = Data(window+4:m-1-Td,3)/regu;   % fi(:,3) u(k-1)
        uk2 = Data(window+3:m-2-Td,3)/regu;
        uk3 = Data(window+2:m-3-Td,3)/regu;
        uk4 = Data(window+1:m-4-Td,3)/regu;
        yk1 = Data(window+4+Td:m-1,2)/regy;  % fi(:,2) y(k-1) 
        yk2 = Data(window+3+Td:m-2,2)/regy;
        yk3 = Data(window+2+Td:m-3,2)/regy;
        yk4 = Data(window+1+Td:m-4,2)/regy;
        Y = Data(window+5+Td:m,2)/regy;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% E S T I M A C E     P A R A M E T R U
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
N=length(Y);
tic
I = eye(wnum);
Itr_old = 1e20;
w = rand(wnum,1)*wrndm-wrndm/2;
%Vypis theta pri inicializaci
% disp(['theta_init: ' num2str(w(1))])
% for i=2:wnum
%     disp(['            ' num2str(w(i))])
% end
w_old = rand(wnum,1)*2-1;
Itr=zeros(1,iterm);
iter = 1;

switch gm
%=========================Steepest Descent=================================    
    case 'sd'       
        while (sum(round(w*10000)) ~= sum(round(w_old*10000))) && (iter < iterm)
            eval(['E = Y - (' se ');']);   %chyba
            Itr(iter) = E'*E;
            eval(['J = ' sJ]);
            g = J'*E;                               %gradient
            switch unum
                case 2
                    eta = fminsearchbnd(@(eta) fblank2r(etaY,eta,uk1,uk2,yk1,yk2,Y,w,g),0,0,40,optimset('MaxIter',200));
                case 3
                    eta = fminsearchbnd(@(eta) fblank3r(etaY,eta,uk1,uk2,uk3,yk1,yk2,yk3,Y,w,g),0,0,40,optimset('MaxIter',200));
                case 4
                    eta = fminsearchbnd(@(eta) fblank4r(etaY,eta,uk1,uk2,uk3,uk4,yk1,yk2,yk3,yk4,Y,w,g),0,0,40,optimset('MaxIter',200));
            end
            w_old = w;
            w = w - eta*g;
            if(Itr(iter)==Itr_old) 
                break; 
            end
            Itr_old = Itr(iter);
            iter = iter+1;
        end
%===========================Kvazi-Newtonova================================        
    case 'kn'       
        S = eye(wnum);
        w_old = rand(wnum,1)*2-1;
        eval(['E = Y - (' se ');']);   %chyba
        Itr(1) = E'*E;
        eval(['J = ' sJ]);
        g = J'*E;                                   %gradient
        iter = 2;
        while (sum(round(w*10000)) ~= sum(round(w_old*10000))) && (iter < iterm)
            g_old = g;
            w_old = w;
            p = S*g;
            switch unum
                case 2
                    eta = fminsearchbnd(@(eta) fblank2r(etaY,eta,uk1,uk2,yk1,yk2,Y,w,p),0,0,40,optimset('MaxIter',200));
                case 3
                    eta = fminsearchbnd(@(eta) fblank3r(etaY,eta,uk1,uk2,uk3,yk1,yk2,yk3,Y,w,p),0,0,40,optimset('MaxIter',200));
                case 4
                    eta = fminsearchbnd(@(eta) fblank4r(etaY,eta,uk1,uk2,uk3,uk4,yk1,yk2,yk3,yk4,Y,w,p),0,0,40,optimset('MaxIter',200));
            end
            w = w - eta*p;
            if(Itr(iter-1)==Itr_old) 
                break; 
            end
            Itr_old = Itr(iter-1);
            eval(['E = Y - (' se ');']);   %chyba
            Itr(iter) = E'*E;
            eval(['J = ' sJ]);
            g = J'*E;                               %gradient
            dw = w - w_old;
            dg = g - g_old;
            S = (I - dw*dg'/(dw'*dg))*S*(I - dw*dg'/(dw'*dg))' + dw*dw'/(dw'*dg);
            iter = iter+1;
        end
%========================Levenberg-Marquardt===============================        
    case 'lm'       
        while (sum(round(w*10000)) ~= sum(round(w_old*10000))) && (iter < iterm)
            eval(['E = Y - (' se ');']);   %chyba
            Itr(iter) = E'*E;
            eval(['J = ' sJ]);
            g = J'*E;                               %gradient
%            S = inv(J'*J + alfa*I);
%            p = S*g;
            H = (J'*J + alfa*I);
            p=H\g;
            eta = 1;
            switch unum
                case 2
                    eta = fminsearchbnd(@(eta) fblank2r(etaY,eta,uk1,uk2,yk1,yk2,Y,w,p),1,0,40,optimset('MaxIter',200));
                case 3
                    eta = fminsearchbnd(@(eta) fblank3r(etaY,eta,uk1,uk2,uk3,yk1,yk2,yk3,Y,w,p),1,0,40,optimset('MaxIter',200));
                case 4
                    eta = fminsearchbnd(@(eta) fblank4r(etaY,eta,uk1,uk2,uk3,uk4,yk1,yk2,yk3,yk4,Y,w,p),1,0,40,optimset('MaxIter',200));
            end
            w_old = w;
            w = w - eta*p;
            if(Itr(iter)==Itr_old) 
                break; 
            end
            Itr_old = Itr(iter);
            iter = iter+1;
        end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% V Y P I S     P O     E S T I M A C I
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if iter<iterm
    Itr(iter:end)=[];
end   
elapsedTime = toc;
disp(['Iteraci: ' num2str(iter)])
disp(['Itrain: ' num2str(regy^2*Itr(length(Itr)))])
disp(['elapsedTime: ' num2str(elapsedTime)])
disp(['theta: ' num2str(w(1))])
for i=2:wnum
    disp(['       ' num2str(w(i))])
end

eval(['ytr = ' se ';']);
figure(1)
plot(Data(end-N+1:end,1),Y)
hold on
plot(Data(end-N+1:end,1),ytr,'r')
title('Porovnani identifikovane soustavy a modelu (verifikace)')
xlabel('t (s)')
ylabel('n (ot/min)')
legend('Soustava',model)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% V A L I D A C E    D A T
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
u = Data(1:window,3)/regu;
ym=zeros(1,window-Td);
switch unum
    case 2
        syppom = syp;
        syppom = strrep(syppom,'u(i-2)','0');
        syppom = strrep(syppom,'ym(i-2)','0');
        i=2;
        eval(['ym(2) = ' syppom ';'])
        for i=3:window-Td
            eval(['ym(i) = ' syp ';'])
        end
    case 3
        syppom = syp;
        syppom = strrep(syppom,'u(i-2)','0');
        syppom = strrep(syppom,'ym(i-2)','0');
        syppom = strrep(syppom,'u(i-3)','0');
        syppom = strrep(syppom,'ym(i-3)','0');
        i=2;
        eval(['ym(2) = ' syppom ';'])
        syppom = syp;
        syppom = strrep(syppom,'u(i-3)','0');
        syppom = strrep(syppom,'ym(i-3)','0');
        i=3;
        eval(['ym(3) = ' syppom ';'])
        for i=4:window-Td
            eval(['ym(i) = ' syp ';'])
        end
    case 4
        syppom = syp;
        syppom = strrep(syppom,'u(i-2)','0');
        syppom = strrep(syppom,'ym(i-2)','0');
        syppom = strrep(syppom,'u(i-3)','0');
        syppom = strrep(syppom,'ym(i-3)','0');
        syppom = strrep(syppom,'u(i-4)','0');
        syppom = strrep(syppom,'ym(i-4)','0');
        i=2;
        eval(['ym(2) = ' syppom ';'])
        syppom = syp;
        syppom = strrep(syppom,'u(i-3)','0');
        syppom = strrep(syppom,'ym(i-3)','0');
        syppom = strrep(syppom,'u(i-4)','0');
        syppom = strrep(syppom,'ym(i-4)','0');
        i=3;
        eval(['ym(3) = ' syppom ';'])
        syppom = syp;
        syppom = strrep(syppom,'u(i-4)','0');
        syppom = strrep(syppom,'ym(i-4)','0');
        i=4;
        eval(['ym(4) = ' syppom ';'])
        for i=5:window-Td
            eval(['ym(i) = ' syp ';'])
        end
end

ym = [zeros(1,Td) ym*regy];

Ikrit = ((Data(1:window,2)'-ym)*(Data(1:window,2)'-ym)')/double(i);
sikrit=sprintf('Ikrit = %1.5e',Ikrit);
disp(sikrit)
% disp(['Ikrit = ' num2str(Ikrit)])
% 
% save('arx2r.mat', 'Ikrit', 'ym')
% 
figure(2)
plot(Data(1:window,1),Data(1:window,2))
hold on
plot(Data(1:window,1),ym,'r')
title('Porovnani identifikovane soustavy a modelu (validace)')
xlabel('t (s)')
ylabel('n (ot/min)')
% legend('Soustava',model)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% L I N E A R I Z A C E     M O D E L U
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
yl=zeros(1,window-Td);
switch unum
    case 2
        linpom = Lf(1:end-1);
        linpom = strrep(linpom,'u(i-2)','0');
        linpom = strrep(linpom,'yl(i-2)','0');
        i=2;
        eval(['yl(2) = ' linpom ';'])
        for i=3:window-Td
            eval(['yl(i) = ' Lf(1:end-1) ';'])
            eval(['b1l(i) = ' b1 ';']);
            eval(['b2l(i) = ' b2 ';']);
            eval(['a1l(i) = -' a1 ';']);
            eval(['a2l(i) = -' a2 ';']);
        end
        s1=sprintf('         b1       b2       a1       a2');
        s2=sprintf('min: %8.3g %8.3g %8.3g %8.3g',min(b1l(3:end)),min(b2l(3:end)),min(a1l(3:end)),min(a2l(3:end)));
        s3=sprintf('str: %8.3g %8.3g %8.3g %8.3g',sum(b1l(3:end))/double((i-2)),sum(b2l(3:end))/double((i-2)),sum(a1l(3:end))/double((i-2)),sum(a2l(3:end))/double((i-2)));
        s4=sprintf('max: %8.3g %8.3g %8.3g %8.3g',max(b1l(3:end)),max(b2l(3:end)),max(a1l(3:end)),max(a2l(3:end)));
        figure(3)
        plot(Data(1:window,1),[zeros(1,Td) b1l])
        hold on
        plot(Data(1:window,1),[zeros(1,Td) b2l],'g')
        plot(Data(1:window,1),[zeros(1,Td) a1l],'r')
        plot(Data(1:window,1),[zeros(1,Td) a2l],'c')
        title('Zmena parametru linearizovaneho modelu v case')
        xlabel('t (s)')
        ylabel('parametr')
        legend('b1','b2','a1','a2')
    case 3
        linpom = Lf(1:end-1);
        linpom = strrep(linpom,'u(i-2)','0');
        linpom = strrep(linpom,'yl(i-2)','0');
        linpom = strrep(linpom,'u(i-3)','0');
        linpom = strrep(linpom,'yl(i-3)','0');
        i=2;
        eval(['yl(2) = ' linpom ';'])
        linpom = Lf(1:end-1);
        linpom = strrep(linpom,'u(i-3)','0');
        linpom = strrep(linpom,'yl(i-3)','0');
        i=3;
        eval(['yl(3) = ' linpom ';'])
        for i=4:window-Td
            eval(['yl(i) = ' Lf(1:end-1) ';'])
            eval(['b1l(i) = ' b1 ';']);
            eval(['b2l(i) = ' b2 ';']);
            eval(['b3l(i) = ' b3 ';']);
            eval(['a1l(i) = ' a1 ';']);
            eval(['a2l(i) = ' a2 ';']);
            eval(['a3l(i) = ' a3 ';']);
        end
        s1=sprintf('         b1       b2       b3       a1       a2       a3');
        s2=sprintf('min: %8.3g %8.3g %8.3g %8.3g %8.3g %8.3g',min(b1l(4:end)),min(b2l(4:end)),min(b3l(4:end)),min(a1l(4:end)),min(a2l(4:end)),min(a3l(4:end)));
        s3=sprintf('str: %8.3g %8.3g %8.3g %8.3g %8.3g %8.3g',sum(b1l(4:end))/double((i-3)),sum(b2l(4:end))/double((i-3)),sum(b3l(4:end))/double((i-3)),sum(a1l(4:end))/double((i-3)),sum(a2l(4:end))/double((i-3)),sum(a3l(4:end))/double((i-3)));
        s4=sprintf('max: %8.3g %8.3g %8.3g %8.3g %8.3g %8.3g',max(b1l(4:end)),max(b2l(4:end)),max(b3l(4:end)),max(a1l(4:end)),max(a2l(4:end)),max(a3l(4:end)));
        figure(3)
        plot(Data(1:window,1),[zeros(1,Td) b1l])
        hold on
        plot(Data(1:window,1),[zeros(1,Td) b2l],'g')
        plot(Data(1:window,1),[zeros(1,Td) b3l],'r')
        plot(Data(1:window,1),[zeros(1,Td) a1l],'c')
        plot(Data(1:window,1),[zeros(1,Td) a2l],'m')
        plot(Data(1:window,1),[zeros(1,Td) a3l],'y')
        title('Zmena parametru linearizovaneho modelu v case')
        xlabel('t (s)')
        ylabel('parametr')
        legend('b1','b2','b3','a1','a2','a3')
    case 4
        linpom = Lf(1:end-1);
        linpom = strrep(linpom,'u(i-2)','0');
        linpom = strrep(linpom,'yl(i-2)','0');
        linpom = strrep(linpom,'u(i-3)','0');
        linpom = strrep(linpom,'yl(i-3)','0');
        linpom = strrep(linpom,'u(i-4)','0');
        linpom = strrep(linpom,'yl(i-4)','0');
        i=2;
        eval(['yl(2) = ' linpom ';'])
        linpom = Lf(1:end-1);
        linpom = strrep(linpom,'u(i-3)','0');
        linpom = strrep(linpom,'yl(i-3)','0');
        linpom = strrep(linpom,'u(i-4)','0');
        linpom = strrep(linpom,'yl(i-4)','0');
        i=3;
        eval(['yl(3) = ' linpom ';'])
        linpom = Lf(1:end-1);
        linpom = strrep(linpom,'u(i-4)','0');
        linpom = strrep(linpom,'yl(i-4)','0');
        i=4;
        eval(['yl(4) = ' linpom ';'])
        for i=5:window-Td
            eval(['yl(i) = ' Lf(1:end-1) ';'])
            eval(['b1l(i) = ' b1 ';']);
            eval(['b2l(i) = ' b2 ';']);
            eval(['b3l(i) = ' b3 ';']);
            eval(['b4l(i) = ' b4 ';']);
            eval(['a1l(i) = ' a1 ';']);
            eval(['a2l(i) = ' a2 ';']);
            eval(['a3l(i) = ' a3 ';']);
            eval(['a4l(i) = ' a4 ';']);
        end
        s1=sprintf('         b1       b2       b3       b4       a1       a2       a3       a4');
        s2=sprintf('min: %8.3g %8.3g %8.3g %8.3g %8.3g %8.3g %8.3g %8.3g',b1l(5),b2l(5),b3l(5),b4l(5),a1l(5),a2l(5),a3l(5),a4l(5));
        s3=sprintf('str: %8.3g %8.3g %8.3g %8.3g %8.3g %8.3g %8.3g %8.3g',b1l(5),b2l(5),b3l(5),b4l(5),a1l(5),a2l(5),a3l(5),a4l(5));
        s4=sprintf('max: %8.3g %8.3g %8.3g %8.3g %8.3g %8.3g %8.3g %8.3g',b1l(5),b2l(5),b3l(5),b4l(5),a1l(5),a2l(5),a3l(5),a4l(5));
        figure(3)
        plot(Data(1:window,1),[zeros(1,Td) b1l])
        hold on
        plot(Data(1:window,1),[zeros(1,Td) b2l],'g')
        plot(Data(1:window,1),[zeros(1,Td) b3l],'r')
        plot(Data(1:window,1),[zeros(1,Td) b4l],'c')
        plot(Data(1:window,1),[zeros(1,Td) a1l],'m')
        plot(Data(1:window,1),[zeros(1,Td) a2l],'y')
        plot(Data(1:window,1),[zeros(1,Td) a3l],'k')
        plot(Data(1:window,1),[zeros(1,Td) a4l],'b--')
        title('Zmena parametru linearizovaneho modelu v case')
        xlabel('t (s)')
        ylabel('parametr')
        legend('b1','b2','b3','b4','a1','a2','a3','a4')
end

yl = [zeros(1,Td) yl*regy];

Ikritl = ((Data(1:window,2)'-yl)*(Data(1:window,2)'-yl)')/double(i);
sikritl=sprintf('Ikritl = %1.5e',Ikritl);
disp(sikritl)
% figure(4)
% plot(Data(1:window,1),Data(1:window,2))
figure(2)
hold on
% plot(Data(1:window,1),yl,'r')
plot(Data(1:window,1),yl,'g')
% title('Porovnani identifikovane soustavy a linearizovaneho modelu')
title('Porovnani identifikovane soustavy, modelu a linearizovaneho modelu (validace)')
xlabel('t (s)')
% ylabel('n (ot/min)')               %simulace
ylabel('Uout (V)') 
legend('Soustava',model,['lin. ' model])
% axis([75 85 -200 1400])           %simulace
axis([0 50 -1 8])
hFig = figure(2);
set(hFig, 'Position', [10 300 700 400])

disp(s1)
disp(s2)
disp(s3)
disp(s4)