function [u,x,K,p,v,G,g]=det_opt_control(T,W_T,W,F,Lambda,x_target,u_target,A,B,c,x_0)
%deterministicke optimalni rizeni lin. systemu
%       x_t+1=A*x_t + B*u_t + c_t
%vzhledem ke kvadraticke ztratove funkci
%      1/2*[x_T-x_target(T)]'*W_T*[x_T-x_target(T)] + sum_j=0^T-1(1/2*[x_j-x_target(j)]'*W*[x_j-x_target(j)] + 
%       [x_j-x_target(j)]'*F*[u_j-u_target(j)] +
%      1/2*[u_j-u_target(j)]'*Lambda*[u_j-u_target(j)])
%
%output: u,x.....hodnoty vektoru rizeni a stavoveho vektoru
%        K,p,v...viz prednaska




nx=length(x_0); %pocet slozek stavoveho vektoru
nu=size(B,2); %pocet slozek ridiciho vektoru

if T<0
    error('no periods ahead for Certainty-equivalence optimal control')
%vetev pro pripad, kdy pocet obdobi do konce rizeni je roven 0
elseif T==0
    K=nan(nx,nx,1);
    p=nan(nx,1);
    v=nan;

    K(:,:,1)=W_T;
    p(:,1)=-W_T*x_target(:,T+1);
    v=0.5*x_target(:,T+1)'*W_T*x_target(:,T+1);
    
    G=nan(nu,nx,1);
    g=nan(nu,1);
    
    u(:,1)=nan(nu,1);
    x(:,1)=x_0;
%vetev pro pripad, kdy pocet obdobi do konce rizeni je vetsi nez 0    
else

    %inicializace matic
    G=nan(nu,nx,T);
    g=nan(nu,T);
    K=nan(nx,nx,T);
    p=nan(nx,T);
    v=nan(1,T);

    
    K(:,:,T)=W_T;
    p(:,T)=-W_T'*x_target(:,T+1);
    v(T)=0.5*x_target(:,T+1)'*W_T*x_target(:,T+1);
    
    Phi=A'*K(:,:,T)*A+W;
    Theta=B'*K(:,:,T)*B+Lambda;
    Psi=A'*K(:,:,T)*B+F;
    phi=A'*(K(:,:,T)'*c(:,T)+p(:,T))-(W'*x_target(:,T)+F*u_target(:,T));
    theta=B'*(K(:,:,T)'*c(:,T)+p(:,T))-(Lambda'*u_target(:,T)+F'*x_target(:,T));
    eta=0.5*c(:,T)'*K(:,:,T)*c(:,T)+p(:,T)'*c(:,T)+v(T)+0.5*x_target(:,T)'*W*x_target(:,T)+x_target(:,T)'*F*u_target(:,T)+0.5*u_target(:,T)'*Lambda*u_target(:,T);
    
    G(:,:,T)=-inv(Theta')*Psi';
    g(:,T)=-inv(Theta')*theta;
    
    %vetev pro pripad, kdy pocet obdobi do konce rizeni je vetsi nez 1
    if T>=2
        for j=T-1:-1:1
            
            K(:,:,j)=Phi+G(:,:,j+1)'*Theta*G(:,:,j+1)+2*Psi*G(:,:,j+1);
            p(:,j)=(Psi+G(:,:,j+1)'*Theta)*g(:,j+1)+G(:,:,j+1)'*theta+phi;
            v(j)=-0.5*theta'*inv(Theta')*theta+eta;
            
            Phi=A'*K(:,:,j)*A+W;
            Theta=B'*K(:,:,j)*B+Lambda;
            Psi=A'*K(:,:,j)*B+F;
            phi=A'*(K(:,:,j)'*c(:,j)+p(:,j))-(W'*x_target(:,j)+F*u_target(:,j));
            theta=B'*(K(:,:,j)'*c(:,j)+p(:,j))-(Lambda'*u_target(:,j)+F'*x_target(:,j));
            eta=0.5*c(:,j)'*K(:,:,j)*c(:,j)+p(:,j)'*c(:,j)+v(j)+0.5*x_target(:,j)'*W*x_target(:,j)+x_target(:,j)'*F*u_target(:,j)+0.5*u_target(:,j)'*Lambda*u_target(:,j);
            
            G(:,:,j)=-inv(Theta')*Psi';
            g(:,j)=-inv(Theta')*theta;
        end
    end
    
    %Spocteme je K_0, p_0, v_0 pro cas t=0 a pridame do matic K,p,v 
    K=cat(3,Phi+G(:,:,1)'*Theta*G(:,:,1)+2*Psi*G(:,:,1),K);
    p=[(Psi+G(:,:,1)'*Theta)*g(:,1)+G(:,:,1)'*theta+phi, p];
    v=[-0.5*theta'*inv(Theta')*theta+eta, v];

    %Spocteme trajektorii stavoveho vektoru a vektoru rizeni
    x(:,1)=x_0;
    for j=1:T
        u(:,j)=G(:,:,j)*x(:,j) + g(:,j);
        x(:,j+1)=A*x(:,j) + B*u(:,j) + c(:,j);
    end

end

