function ildp4cor2
    tic
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %pocatecni konstanty
    Iterace = 2; %iterace
    K = 20; %casy
    N = 250; %vzorky
    
    sigma = 1;
    Sigmas = [[sigma^2 0 0]; [0 0 0]; [0 0 0]];
    h = 0;
    hdx = [0; 0; 0];
    hdxdx = [[0 0 0]; [0 0 0]; [0 0 0]];
    Rk = ones(1,K);
    x0 = [0; 0; 1];

    Wv = zeros(19,K); 
%     Wpi = zeros(19,K);
    Kpi = ones(4,K);
        Kpi(4,:) = zeros(1,K);
%     Wpit = zeros(19,K);
    Kpit = ones(4,K);
%     Wpin = zeros(19,K);
    Kpin = ones(4,K);

    Epsilon = zeros(3,N);
        Epsilon(1,:) = 1*randn(1,N);
        Epsilon(2,:) = 1*randn(1,N);
        Epsilon(3,:) = abs(1*randn(1,N));
    
    mFI = [ones(1,N);...
           Epsilon(1,:);... 
           Epsilon(2,:);...
           Epsilon(3,:);...
           Epsilon(1,:).^2;... 
           Epsilon(1,:).*Epsilon(2,:);...
           Epsilon(1,:).*Epsilon(3,:);...
           Epsilon(2,:).^2;... 
           Epsilon(2,:).*Epsilon(3,:);...
           Epsilon(3,:).^2;...

           Epsilon(1,:).^3;... 
           Epsilon(1,:).^2.*Epsilon(2,:);...
           Epsilon(1,:).^2.*Epsilon(3,:);...
           Epsilon(2,:).^2.*Epsilon(1,:);...
           Epsilon(2,:).^3;...
           Epsilon(2,:).^2.*Epsilon(3,:);...
           Epsilon(3,:).^2.*Epsilon(1,:);...
           Epsilon(3,:).^2.*Epsilon(2,:);...
           Epsilon(3,:).^3];
    FiFiTInvFi = (mFI*mFI')\mFI;
%     max(max(abs(FiFiTInvFi)))

    Xall = zeros(3,K,N);

    %globalni
    kappa = 0;
    nu = 0;
    
    for i = 1:Iterace,
       
        for n = 1:N,
           Xall(:,1,n) = x0 + [sigma*randn(); 0; 0];
           for k = 1:K-1,
              Upi = nPi(k,Xall(:,:,n));
              Xall(1,k+1,n) = Xall(1,k,n) + Xall(2,k,n)*Upi + sigma*randn();
              Xall(2,k+1,n) = Xall(2,k,n) + (Xall(1,k+1,n) - Xall(1,k,n) - Xall(2,k,n)*Upi)*Upi*Xall(3,k,n)/(Upi^2*Xall(3,k,n) + sigma^2);
              Xall(3,k+1,n) = Xall(3,k,n) - (Upi^2*Xall(3,k,n)^2)/(Upi^2*Xall(3,k,n) + sigma^2);
           end
        end
        
%         Xstripe = zeros(3,K);
        Xstripe = mean(Xall, 3);

        if(i == 1)
           Xstripe(1,:) = zeros(1,K); 
        end       
        
%         subplot(2,1,1)
%         plot(1:K,Xall(1,:,1),1:K,Xall(1,:,2),1:K,Xall(1,:,3),1:K,Xall(1,:,4),1:K,Xall(1,:,5),1:K,Xall(1,:,6),...
%             1:K,Xall(1,:,7),1:K,Xall(1,:,8),1:K,Xall(1,:,9),1:K,Xall(1,:,10),1:K,Xall(1,:,11),1:K,Xall(1,:,12),...
%             1:K,Xall(1,:,13),1:K,Xall(1,:,14),1:K,Xall(1,:,15),1:K,Xall(1,:,16),1:K,Xall(1,:,17),1:K,Xall(1,:,18),...
%             1:K,Xall(1,:,19),1:K,Xall(1,:,20),1:K,Xall(1,:,21),1:K,Xall(1,:,22),1:K,Xall(1,:,23),1:K,Xall(1,:,24),...
%             1:K,Xall(1,:,25))
%         subplot(2,1,2)
%         plot(1:K,Xstripe(1,:))

        for n = 1:N,
            Xall(:,K,n) = Xstripe(:,K) + Epsilon(:,n);
        end
        
        for k = K-1:-1:1,
            [i k]
%             1]
            for n = 1:N,
                Xall(:,k,n) = Xstripe(:,k) + Epsilon(:,n);
            end
%             2]
            for n = 1:N,
                kappa = k;
                nu = n;
                [Uop(n), Hmin(n)] = fminunc(@Hamilt, nPi(k,Xall(:,:,n)), optimset('GradObj','on'));
            end
%             3]
            for n = 1:N,
                Vn(n) = Hmin(n) + Vtilde(k+1,Xall(:,k,n)-Xstripe(:,k+1));
            end
%             4]
            Wv(:,k) = FiFiTInvFi*Vn';
%             Wpit(:,k) = FiFiTInvFi*Uop';
            yt = zeros(1,N);
            bt = zeros(1,N);
            pt = zeros(1,N);
            for n = 1:N,
%                 ytm1(n) = 0;
%                 if(k > 1)
%                     ytm1(n) = Xall(1,k-1,n);
%                 end
                yt(n) = Xall(1,k,n);
                bt(n) = Xall(2,k,n);
                pt(n) = Xall(3,k,n);
            end          

            mPsi = [yt',...
                    bt'.*Uop',...
                    pt'.*Uop',...
                    Uop'];
            PsiPsiTInvPsi = (mPsi'*mPsi)\mPsi'; 
            
            Kpi(:,k) = PsiPsiTInvPsi * (Rk(k)*ones(N,1));
            Kpi(:,k)
        end
        
%         linesearch
            X = zeros(3, K); 
            Ual = zeros(1,K);
            X(:,1) = x0 + [sigma*randn(); 0; 0];  
            for k = 1:K-1,                    
                    Upi = nPiex(k, X, Kpi);
                    Ual(k) = Upi;
                    Ktmp = Upi*X(3,k)/(Upi^2*X(3,k) + sigma^2);
                    X(1,k+1) = X(1,k)+X(2,k)*Upi + sigma*randn(1);
                    X(2,k+1) = X(2,k) + Ktmp*(X(1,k+1) - X(1,k) - X(2,k)*Upi);
                    X(3,k+1) = X(3,k)*(1-Ktmp*Upi);
            end 
            oldloss = sum( (X(1,:) - Rk).^2 + X(3,:).*Ual.^2 );

            alfa = 0;
            loss = Inf;
        while((loss > oldloss)||(alfa<1))
%             Wpin = (1-alfa)*Wpit + alfa*Wpi;
             Kpin = (1-alfa)*Kpit + alfa*Kpi;              
                        
            X = zeros(3, K); 
            Ual = zeros(1,K);
            X(:,1) = x0 + [sigma*randn(); 0; 0];  
            for k = 1:K-1,                    
                    Upi = nPiex(k, X, Kpin);                    
                    Ual(k) = Upi;
                    Ktmp = Upi*X(3,k)/(Upi^2*X(3,k) + sigma^2);
                    X(1,k+1) = X(1,k)+X(2,k)*Upi + sigma*randn(1);
                    X(2,k+1) = X(2,k) + Ktmp*(X(1,k+1) - X(1,k) - X(2,k)*Upi);
                    X(3,k+1) = X(3,k)*(1-Ktmp*Upi);
            end 
            loss = sum( (X(1,:) - Rk).^2 + X(3,:).*Ual.^2 );
            
            alfa = alfa + 0.1;
            if(alfa > 1)
               disp('zadne zlepseni')
%                loss = -1;
            end
        end
%         if(loss > -1)
%            Wpi = Wpin;
           Kpi = Kpin;
%         end
        
    end
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    Kpi
    %graficky vystup
            X = zeros(3, K); 
            UU = zeros(1,K);
            
                X(:,1) = x0 + [sigma*randn(1); 0; 0];  
                for k = 1:K-1,                    
                    Upi = nPi(k, X);
                    UU(k) = Upi;
                    Ktmp = Upi*X(3,k)/(Upi^2*X(3,k) + sigma^2);
                    X(1,k+1) = X(1,k)+X(2,k)*Upi + sigma*randn(1);
                    X(2,k+1) = X(2,k) + Ktmp*(X(1,k+1) - X(1,k) - X(2,k)*Upi);
                    X(3,k+1) = X(3,k)*(1-Ktmp*Upi);
                end            
            
                subplot(5,1,1);
                plot(1:K,X(1,:))
                subplot(5,1,2);
                plot(1:K,X(2,:))
                subplot(5,1,3);
                plot(1:K,X(3,:))
                subplot(5,1,4);
                plot(1:K,UU)
     
    %napoctena chyba
    yy = zeros(1,K);
    losses = zeros(1,100);
    for n=1:100,
        bb = randn();
        yy(1) = x0(1) + sigma*randn();
        for k = 1:K-1,                    
                    Upi = nPi(k,X);                
                    yy(k+1) = yy(k)+bb*Upi + sigma*randn(1);                   
        end 
        losses(n) = sum( (yy - Rk).^2 );
    end
    
    [min(losses) median(losses) max(losses)]
    subplot(5,1,5);
    hist(log(losses))
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
    function [valpi] = nPi(kf,Xknf)%nPi(kf,Epsil)%nPi(kf,Xknf)
        if(kf > 1)
            yti1 = Xknf(1,kf-1);
        else
            yti1 = 0;
        end
        valpi = (Rk(kf)-Kpi(1,kf)*Xknf(1,kf))/(Kpi(2,kf)*Xknf(2,kf) + Kpi(3,kf)*Xknf(3,kf) + Kpi(4,kf));
%         valpi = [1;...
%                        Epsil(1);... 
%                        Epsil(2);...
%                        Epsil(3);...
%                        Epsil(1)^2;... 
%                        Epsil(1)*Epsil(2);...
%                        Epsil(1)*Epsil(3);...
%                        Epsil(2)^2;... 
%                        Epsil(2)*Epsil(3);...
%                        Epsil(3)^2;...
%                        Epsil(1)^3;... 
%                        Epsil(1)^2*Epsil(2);...
%                        Epsil(1)^2*Epsil(3);...
%                        Epsil(2)^2*Epsil(1);...
%                        Epsil(2)^3;...
%                        Epsil(2)^2*Epsil(3);...
%                        Epsil(3)^2*Epsil(1);...
%                        Epsil(3)^2*Epsil(2);...
%                        Epsil(3)^3]'*Wpi(:,kf);
    end

    function [valpi] = nPiex(kf,Xknf,Kpix)%nPiex(kf,Epsil,Ww)%nPi(kf,Xknf,Kpix)
        if(kf > 1)
            yti1 = Xknf(1,kf-1);
        else
            yti1 = 0;
        end
        valpi = (Rk(kf)-Kpi(1,kf)*Xknf(1,kf))/(Kpix(2,kf)*Xknf(2,kf) + Kpix(3,kf)*Xknf(3,kf) + Kpix(4,kf));
%         valpi = [1;...
%                        Epsil(1);... 
%                        Epsil(2);...
%                        Epsil(3);...
%                        Epsil(1)^2;... 
%                        Epsil(1)*Epsil(2);...
%                        Epsil(1)*Epsil(3);...
%                        Epsil(2)^2;... 
%                        Epsil(2)*Epsil(3);...
%                        Epsil(3)^2;...
%                        Epsil(1)^3;... 
%                        Epsil(1)^2*Epsil(2);...
%                        Epsil(1)^2*Epsil(3);...
%                        Epsil(2)^2*Epsil(1);...
%                        Epsil(2)^3;...
%                        Epsil(2)^2*Epsil(3);...
%                        Epsil(3)^2*Epsil(1);...
%                        Epsil(3)^2*Epsil(2);...
%                        Epsil(3)^3]'*Ww(:,kf);
    end

    function [valHam, valGrad] = Hamilt(Uin)
        valHam = (Xall(1,kappa+1,nu)-Rk(kappa))^2 + Xall(3,kappa,nu)*Uin^2 ...
               + [Xall(2,kappa,nu)*Uin;...
                  (Xall(1,kappa+1,nu) - Xall(1,kappa,nu) - Xall(2,kappa,nu)*Uin)*Uin*Xall(3,kappa,nu)/(Uin^2*Xall(3,kappa,nu) + sigma^2);...
                  -(Uin^2*Xall(3,kappa,nu)^2)/(Uin^2*Xall(3,kappa,nu) + sigma^2)]'*Vtildedx(kappa+1,Epsilon(:,nu)+Xstripe(:,kappa)-Xstripe(:,kappa+1)) + 1/2*trace(Sigmas*Vtildedxdx(kappa+1,Epsilon(:,nu)+Xstripe(:,kappa)-Xstripe(:,kappa+1)));
        valGrad = 2*Xall(3,kappa,nu)*Uin ...
                + [Xall(2,kappa,nu);...
                   (2*Uin^2*Xall(3,kappa,nu)^2*(Xall(1,kappa,nu) - Xall(1,kappa+1,nu) + Uin*Xall(2,kappa,nu)))/(sigma^2 + Uin^2*Xall(3,kappa,nu))^2 - (Uin*Xall(2,kappa,nu)*Xall(3,kappa,nu))/(sigma^2 + Uin^2*Xall(3,kappa,nu)) - (Xall(3,kappa,nu)*(Xall(1,kappa,nu) - Xall(1,kappa+1,nu) + Uin*Xall(2,kappa,nu)))/(sigma^2 + Uin^2*Xall(3,kappa,nu));...
                   (2*Uin^3*Xall(3,kappa,nu)^2)/(sigma^2 + Uin^2*Xall(3,kappa,nu))^2 - (2*Uin*Xall(3,kappa,nu))/(sigma^2 + Uin^2*Xall(3,kappa,nu))]'*Vtildedx(kappa+1,Epsilon(:,nu)+Xstripe(:,kappa)-Xstripe(:,kappa+1));
                
    end

    function [valVtl] = Vtilde(kin,Epsil)
        if(kin == K)
            valVtl = h;
        else            
             valVtl = [1;...
                       Epsil(1);... 
                       Epsil(2);...
                       Epsil(3);...
                       Epsil(1)^2;... 
                       Epsil(1)*Epsil(2);...
                       Epsil(1)*Epsil(3);...
                       Epsil(2)^2;... 
                       Epsil(2)*Epsil(3);...
                       Epsil(3)^2;...
                       Epsil(1)^3;... 
                       Epsil(1)^2*Epsil(2);...
                       Epsil(1)^2*Epsil(3);...
                       Epsil(2)^2*Epsil(1);...
                       Epsil(2)^3;...
                       Epsil(2)^2*Epsil(3);...
                       Epsil(3)^2*Epsil(1);...
                       Epsil(3)^2*Epsil(2);...
                       Epsil(3)^3]'*Wv(:,kin);
                      
        end
    end

    function [valVtlx] = Vtildedx(kin,Epsil)
        if(kin == K)
            valVtlx = hdx;
        else
             valVtlx = [0                         0                       0;...
                       1                          0                       0;... 
                       0                          1                       0;...
                       0                          0                       1;...
                       2*Epsil(1)                 0                       0;... 
                       Epsil(2)                   Epsil(1)                0;...
                       Epsil(3)                   0                       Epsil(1);...
                       0                          2*Epsil(2)              0;... 
                       0                          Epsil(3)                Epsil(2);...
                       0                          0                       2*Epsil(3);...
                       3*Epsil(1)^2               0                       0;... 
                       2*Epsil(1)*Epsil(2)        Epsil(1)^2              0;...
                       2*Epsil(1)*Epsil(3)        0                       Epsil(1)^2;...
                       Epsil(2)^2                 2*Epsil(2)*Epsil(1)     0;...
                       0                          3*Epsil(2)^2            0;...
                       0                          2*Epsil(2)*Epsil(3)     Epsil(2)^2;...
                       Epsil(3)^2                 0                       2*Epsil(3)*Epsil(1);...
                       0                          Epsil(3)^2              2*Epsil(3)*Epsil(2);...
                       0                          0                       3*Epsil(3)^2]'...
                       *Wv(:,kin);
                      
        end
    end

    function [valVtlxx] = Vtildedxdx(kin,Epsil)
        if(kin == K)
            valVtlxx = hdxdx;
        else
            valVtlxx = zeros(3,3);
            valVtlxx(1,1) = 2*Wv(5,kin) + 6*Epsil(1)*Wv(11,kin) + 2*Epsil(2)*Wv(12,kin) + 2*Epsil(3)*Wv(13,kin);
            valVtlxx(2,2) = 2*Wv(8,kin) + 2*Epsil(1)*Wv(14,kin) + 6*Epsil(2)*Wv(15,kin) + 2*Epsil(3)*Wv(16,kin);
            valVtlxx(3,3) = 2*Wv(10,kin) + 2*Epsil(1)*Wv(17,kin) + 2*Epsil(2)*Wv(18,kin) + 6*Epsil(3)*Wv(19,kin);

            valVtlxx(1,2) = Wv(6,kin) + 2*Epsil(1)*Wv(12,kin) + 2*Epsil(2)*Wv(14,kin);
            valVtlxx(1,3) = Wv(7,kin) + 2*Epsil(1)*Wv(13,kin) + 2*Epsil(3)*Wv(17,kin);
            valVtlxx(2,3) = Wv(4,kin) + 2*Epsil(2)*Wv(16,kin) + 2*Epsil(3)*Wv(18,kin);

            valVtlxx(2,1) = valVtlxx(1,2);
            valVtlxx(3,1) = valVtlxx(1,3);
            valVtlxx(3,2) = valVtlxx(2,3);
        end
    end
   
keyboard
end