%2 ad hoc priklady na kvadraticky linearni problem (deterministicke rizeni)

clc
close all
clear all


%chceme ridit v casovem horizontu
T=10;
%deterministicky lin. system x_t+1=A*x_t+B*u_t+c_t
A=eye(3);
B=[1;1;2];
c=zeros(3,T);
%vzhledem ke ztratove funkci urcene maticemi
n_x=size(A,1);
n_u=size(B,2);
W=diag([100,100,100]);
F=zeros(n_x,n_u);
Lambda=0;
%pozadovanym hodnotam
x_target=100*ones(n_x,1);
u_target=zeros(n_u,1);
%a pocatecnimu stavu
x_0=[10;10;10];


[u,x,K,p,v,G,g]=det_opt_control(T,W,F,Lambda,x_target,u_target,A,B,c,x_0);
if T~=0
    figure
    subplot(2,1,1)
    plot(0:T,x)
    legend(['x_1';'x_2'],'Location','NE')
    subplot(2,1,2)
    plot(0:T-1,u)
    legend(['u_1'],'Location','Best')
end

%Cost-To-Go
J=0.5*x(:,1)'*K(:,:,1)*x(:,1)+p(:,1)'*x(:,1)+v(1)

%kontrola, zda nam ztrata vyjde stejne jako v pripade, ze ji spocteme primo
% ze ztratove funkce a realizaci x_t a u_t
J_kontrola=0;
for j=0:T
    if j~=T
        J_kontrola=J_kontrola+0.5*(x(:,j+1)-x_target)'*W*(x(:,j+1)-x_target)+(x(:,j+1)-x_target)'*F*(u(:,j+1)-u_target)+0.5*(u(:,j+1)-u_target)'*Lambda*(u(:,j+1)-u_target);
    else
        J_kontrola=J_kontrola+0.5*(x(:,j+1)-x_target)'*W*(x(:,j+1)-x_target);
    end
end
J_kontrola


%************************************************************************

%chceme ridit v casovem horizontu
T=50;

%maly gapovy NK makro model (viz makro_model.ps)
alpha=0.72;
beta=-0.11;
delta=0.42;
gamma=0.39;

M1=[   1      beta     0
   -delta   gamma-1   0
      0       0       1];
M2=[alpha      0      0
      0       -1    gamma
      0        1      0];
M3=[beta;0;0];

A=inv(M1)*M2;
B=inv(M1)*M3;
c= zeros(3,T); %exogenni faktor

eigenvalues=eig(A);
rho_A=max(abs(eigenvalues));% spektralni polomer  matice A (system je stabilni, podud je <1)

%vzhledem ke ztratove funkci urcene maticemi
W=[ 1  0  0
    0  0  0
    0  0 10];
    
F=[0
    0
    0];
Lambda=[0];

%pozadovanym hodnotam
x_target=zeros(3,1);
u_target=0;

%a pocatecnimu stavu
x_0=[5
     -2
     0];

%reseni 
[u,x,K,p,v,G,g]=det_opt_control(T,W,F,Lambda,x_target,u_target,A,B,c,x_0);
if T~=0
    %vykresleni vysledku
    figure
    subplot(2,1,1)
    plot((0:T),x)
    legend(strvcat('y_t','\pi_{t+1}','\pi_t'),'Location','SE')
    subplot(2,1,2)
    plot((0:T-1),u)
    legend(['i_t'],'Location','Best')
end



%Cost-To-Go
J=0.5*x(:,1)'*K(:,:,1)*x(:,1)+p(:,1)'*x(:,1)+v(1)

%kontrola, zda nam ztrata vyjde stejne jako v pripade, ze ji spocteme primo
% ze ztratove funkce a realizaci x_t a u_t
J_kontrola=0;
for j=0:T
    if j~=T
        J_kontrola=J_kontrola+0.5*(x(:,j+1)-x_target)'*W*(x(:,j+1)-x_target)+(x(:,j+1)-x_target)'*F*(u(:,j+1)-u_target)+0.5*(u(:,j+1)-u_target)'*Lambda*(u(:,j+1)-u_target);
    else
        J_kontrola=J_kontrola+0.5*(x(:,j+1)-x_target)'*W*(x(:,j+1)-x_target);
    end
end
J_kontrola
