function ildp

tic
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %pocatecni konstanty
    
%     [b=1, yr=1] - to jsme zkouseli spolu  sigma = 0.1; rho = 0.5;
%     [b=-1, yr=1] sigma = 0.1; rho = 0.5; !aprior
%     [b=10, yr=5] sigma = 0.5; rho = 2.0;
%     [b=0.6, yr=10] sigma = 0.1; rho = 1.0;
    
    Iterace = 10; %iterace
    K = 5; %casy
    N = 100; %vzorky
    
    sigma = 0.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 = 1*ones(1, K);
    x0 = [0; 1; 1];
    
    %velikost okoli pro lokalni metodu
    rho = 0.5;
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %globalni promenne

    Kpi = ones(4, K);
        Kpi(4, :) = zeros(1, K);
%         Kpi(3, :) = zeros(1, K);
        
    Wv = zeros(9, K);
    
    Xkn = zeros(3, K, N);
    Xstripe = zeros(3, K);   
  
    gka = 0;
    gnu = 0;
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %iteracni smycka

    for i = 1:Iterace,
       %generovani stavu
       for n = 1:N,
            Xkn(:, 1, n) = x0 + [sigma*randn(); 0; 0];
            for k = 1:K-1,
               Uk = uPi(k, Xkn(:, k, n));
               Kk = Uk*Xkn(3, k, n)/(Uk^2*Xkn(3, k, n) + sigma^2);
               Xkn(1, k+1, n) = Xkn(1, k, n) + Xkn(2, k, n)*Uk + sigma*randn();
               Xkn(2, k+1, n) = Xkn(2, k, n) + Kk*(Xkn(1, k+1, n) - Xkn(1, k, n) - Xkn(2, k, n)*Uk);
               Xkn(3, k+1, n) = (1 - Kk*Uk)*Xkn(3, k, n);
            end            
       end
       Xstripe = mean(Xkn, 3);
                 
       for k = K-1:-1:1,
            gka = k;
%             1]
            for n = 1:N,
                %krive okoli
               Xkn(1, k, n) = Xstripe(1, k) + rho*randn();
               Xkn(2, k, n) = Xstripe(2, k) + rho*randn();  
               Xkn(3, k, n) = Xstripe(3, k)*exp(rho*randn()); 
            end            
%             2]
            for n = 1:N,
               gnu = n; 
                [Uopt(n), Hmin(n)] = fminunc(@Hamilt, uPi(k, Xkn(:, k, n)), optimset('GradObj','on','Display','notify'));
            end
%             3]
            for n = 1:N,                
               Vn(n) = Hmin(n) + Vtilde(k+1, Xkn(:, k, n)); 
            end            
%             4] 
            Epsilon = zeros(3, N);
            for n = 1:N,
                Epsilon(1, n) = Xkn(1, k, n) - Xstripe(1, k);
                Epsilon(2, n) = Xkn(2, k, n) - Xstripe(2, k);
                Epsilon(3, n) = Xkn(3, k, n)/Xstripe(3, k);
            end
            mFi = matrixFi(Epsilon);
            FiFiTInvFi = (mFi*mFi')\mFi;
            Wv(:,k) = FiFiTInvFi * Vn';
            for n = 1:N,
               yt(n) = Xkn(1, k, n);
               bt(n) = Xkn(2, k, n);
               pt(n) = Xkn(3, k, n);
            end
            mPsi = [yt',...
                    bt'.*Uopt',...
                    pt'.*Uopt',...
                    Uopt'];
            PsiPsiTInvPsi = (mPsi'*mPsi)\mPsi';             
            Kpi(:,k) = PsiPsiTInvPsi * (Rk(k)*ones(N,1));            
       end
    end
    %%%%%%%%%%%
    toc
    
    Kpi
    
                for n = 1:N,
                    Xkn(:, 1, n) = x0 + [sigma*randn(); 0; 0];
                    for k = 1:K-1,
                       Uk = uPi(k, Xkn(:, k, n));
                            UU(k,n) = Uk;
                       Kk = Uk*Xkn(3, k, n)/(Uk^2*Xkn(3, k, n) + sigma^2);
                       Xkn(1, k+1, n) = Xkn(1, k, n) + Xkn(2, k, n)*Uk + sigma*randn();
                       Xkn(2, k+1, n) = Xkn(2, k, n) + Kk*(Xkn(1, k+1, n) - Xkn(1, k, n) - Xkn(2, k, n)*Uk);
                       Xkn(3, k+1, n) = (1 - Kk*Uk)*Xkn(3, k, n);       
                    end
                        hold all
                        subplot(4,1,1);
                        plot(1:K,Xkn(1,:,n))
                        hold all
                        subplot(4,1,2);
                        plot(1:K,Xkn(2,:,n))
                        hold all
                        subplot(4,1,3);
                        plot(1:K,Xkn(3,:,n))
                        hold all
                        subplot(4,1,4);
                        plot(1:K-1,UU(:,n))

                end
                title('iLDP')
                figure
               for n = 1:N,
                    Xkn(:, 1, n) = x0 + [sigma*randn(); 0; 0];
                    for k = 1:K-1,
                       Uk = (Rk(k) - Xkn(1, k, n))/(Xkn(2, k, n) + Xkn(3, k, n));
                            UU(k,n) = Uk;
                       Kk = Uk*Xkn(3, k, n)/(Uk^2*Xkn(3, k, n) + sigma^2);
                       Xkn(1, k+1, n) = Xkn(1, k, n) + Xkn(2, k, n)*Uk + sigma*randn();
                       Xkn(2, k+1, n) = Xkn(2, k, n) + Kk*(Xkn(1, k+1, n) - Xkn(1, k, n) - Xkn(2, k, n)*Uk);
                       Xkn(3, k+1, n) = (1 - Kk*Uk)*Xkn(3, k, n);      
                    end
                        hold all
                        subplot(4,1,1);
                        plot(1:K,Xkn(1,:,n))
                        hold all
                        subplot(4,1,2);
                        plot(1:K,Xkn(2,:,n))
                        hold all
                        subplot(4,1,3);
                        plot(1:K,Xkn(3,:,n))
                        hold all
                        subplot(4,1,4);
                        plot(1:K-1,UU(:,n))

               end
               title('CE')
               
               for vzorek = 1:100,
                   loss(vzorek) = 0;                   
                   bb = randn() + x0(2);
                   yy(1) = x0(1);
                   for k=1:K-1,
                       yy(k+1)=yy(k)+bb*uPi(k,[yy(k); bb; 0]);
                       loss(vzorek) = loss(vzorek) + (yy(k+1) - Rk(k+1))^2;
                   end                 
               end
               figure
               hist(log(loss))
               
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %pomocne funkce
    
    function [val_uPi] = uPi(k_uPi, x_uPi)
        val_uPi = (Rk(k_uPi) - Kpi(1, k_uPi)*x_uPi(1))/(Kpi(2, k_uPi)*x_uPi(2) + Kpi(3, k_uPi)*x_uPi(3) + Kpi(4, k_uPi)); 

    end

    function [val_ham, val_grad] = Hamilt(u_ham) 
%         Vtddx = Vtilde_dx(gka+1, Xkn(:, gka, gnu));
       val_ham = (Xkn(1, gka, gnu) + Xkn(2, gka, gnu)*u_ham - Rk(gka+1))^2 + Xkn(3, gka, gnu)*u_ham^2 ... + sigma^2 ...  %ztrata l
               + [Xkn(2, gka, gnu)*u_ham; ...                                   
                  Xkn(3, gka, gnu)*u_ham*(Xkn(1, gka+1, gnu) - Xkn(1, gka, gnu) - Xkn(2, gka, gnu)*u_ham)/(Xkn(3, gka, gnu)*u_ham^2 + sigma^2); ...
                  -Xkn(3, gka, gnu)^2*u_ham^2/(Xkn(3, gka, gnu)*u_ham^2 + sigma^2)]' ... %fce f               
                *Vtilde_dx(gka+1, Xkn(:, gka, gnu)) ...
                + Wv(5, gka+1)*sigma;%+ Wv(4, gka+1)*sigma;
           
       val_grad = 2*(Xkn(1, gka, gnu) + Xkn(2, gka, gnu)*u_ham - Rk(gka+1))*Xkn(2, gka, gnu) + 2*Xkn(3, gka, gnu)*u_ham ...  %ztrata du
                + [Xkn(2, gka, gnu);...                                                                                                                                                                                                                                                     
                   (2*u_ham^2*Xkn(3, gka, gnu)^2*(Xkn(1, gka, gnu) - Xkn(1, gka+1, gnu) + u_ham*Xkn(2, gka, gnu)))/(sigma^2 + u_ham^2*Xkn(3, gka, gnu))^2 - (u_ham*Xkn(2, gka, gnu)*Xkn(3, gka, gnu))/(sigma^2 + u_ham^2*Xkn(3, gka, gnu)) - (Xkn(3, gka, gnu)*(Xkn(1, gka, gnu) - Xkn(1, gka+1, gnu) + u_ham*Xkn(2, gka, gnu)))/(sigma^2 + u_ham^2*Xkn(3, gka, gnu));...
                   (2*u_ham^3*Xkn(3, gka, gnu)^3)/(sigma^2 + u_ham^2*Xkn(3, gka, gnu))^2 - (2*u_ham*Xkn(3, gka, gnu)^2)/(sigma^2 + u_ham^2*Xkn(3, gka, gnu))]' ... %fce f du
                * Vtilde_dx(gka+1, Xkn(:, gka, gnu)); %derivace Bellman fce                
    end

    function [val_Vt] = Vtilde(k_Vt, x_Vt)
        if(k_Vt == K)
            val_Vt = h;
        else
            Epsl = zeros(3, 1);            
                Epsl(1) = x_Vt(1) - Xstripe(1, k_Vt);
                Epsl(2) = x_Vt(2) - Xstripe(2, k_Vt);
                Epsl(3) = x_Vt(3)/Xstripe(3, k_Vt);
            
            val_Vt = vectFi(Epsl)' * Wv(:,k_Vt);
        end
    end

    function [val_Vt] = Vtilde_dx(k_Vt, x_Vt)
        if(k_Vt == K)
            val_Vt = hdx;
        else
             Epsl = zeros(3, 1);            
                Epsl(1) = x_Vt(1) - Xstripe(1, k_Vt);
                Epsl(2) = x_Vt(2) - Xstripe(2, k_Vt);
                Epsl(3) = x_Vt(3)/Xstripe(3, k_Vt);
                
            val_Vt = difFi(Epsl)' * Wv(:,k_Vt);
        end
    end

    function [val_Fi] = vectFi(x_Fi)
        val_Fi = [ ...
                   1; ...
                   x_Fi(1); ...
                   x_Fi(2); ...
                   log(x_Fi(3)); ...
                   x_Fi(1)^2; ...
                   x_Fi(1)*x_Fi(2); ...
                   x_Fi(1)*log(x_Fi(3)); ...
                   x_Fi(2)^2; ...
                   x_Fi(2)*log(x_Fi(3)); ...
%                    2*ln(x_Fi(3)); ...
                 ];
    end

    function [val_Fi] = matrixFi(x_Fi)
        val_Fi = [ ...
                   ones(1, N); ...
                   x_Fi(1, :); ...
                   x_Fi(2, :); ...
                   log(x_Fi(3, :)); ...
                   x_Fi(1, :).^2; ...
                   x_Fi(1, :).*x_Fi(2, :); ...
                   x_Fi(1, :).*log(x_Fi(3, :)); ...
                   x_Fi(2, :).^2; ...
                   x_Fi(2, :).*log(x_Fi(3, :)); ...
                 ];

    end

    function [val_Fi] = difFi(x_Fi)
        val_Fi = [ ...
                   0            0           0; ...
                   1            0           0; ...
                   0            1           0; ...
                   0            0           1/(x_Fi(3)); ...
                   2*x_Fi(1)    0           0; ...
                   x_Fi(2)      x_Fi(1)     0; ...
                   log(x_Fi(3))    0           x_Fi(1)/(x_Fi(3)); ...
                   0            2*x_Fi(2)   0; ...
                   0            log(x_Fi(3))   x_Fi(2)/(x_Fi(3)); ...
                 ];
    end
end