% L Q O C  _ U S .  M
% *******************

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                                %
% Linearni kvadraticky optimalne rizeny maly model ekonomiky USA %
%            x*_t - x*_t-1  = A x*_t-1 + B u*_t + C z_t          %
%             x*_0   = xo                                        %
%               J*  --> min                                      %
%                                                                %
% Zpracoval: Osvald Vasicek, KAMI ESF MU Brno                    %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

 clear all
 close all

 load state_us

% Historicka data stavu
  x= [CC I R Y R_1 Y_1 R_2 R_3];
% Historicke data rizeni
  u= [G d_M];
% Historicka data exogennich prom.
  zz= ones(size(G));

% Rozsah dat
  Ndat=length(x);
  n=8;
  r=2;
  z=1;

% Horni a dolni mez optimalizace 
% ******************************
  Nf = Ndat-1; %<---<<<           % VOLBA HORNI MEZE OPTIMALIZACE   ! ! !
  Ns = 132;    %<---<<< Ndat-148  % VOLBA DOLNI MEZE OPTIMALIZACE   ! ! !
  
  disp('   ');
  disp('   ');
  disp('   Zacatek a konec intervalu optimalniho rizeni');
  disp('   ********************************************');
  disp(['   t_s  =  ',num2str(t(Ns))]);
  disp(['   t_f  =  ',num2str(t(Nf))]);

% Historicke hodnoty
  x_hist=x(Ns:Nf,:)';
  u_hist=u(Ns:Nf,:)';
  

  N=max(size(x_hist)); 
  tt = t;
  t_r  = t(Ns:Nf);
  t_r1 = t(Ns-1:Nf);
  t_r3 = t(Ns-3:Nf);
  
% Regresni interpolace
% ********************
  t=(1:length(t_r))';
  t1=(1:length(t_r1))';
  t3=(1:length(t_r3))';

  A0=[ones(size(t)),t];
  A1=[ones(size(t1)),t1];
  A3=[ones(size(t3)),t3];

  [par1,i_par1,e1,i_e1,Stat1]=regress(log(CC(Ns:Nf)),A0,0.05);
  CC_r=exp(log(CC(Ns:Nf))-e1)';
  [par2,i_par2,e2,i_e2,Stat2]=regress(log(I(Ns:Nf)),A0,0.05);
  I_r=exp(log(I(Ns:Nf))-e2)';
  [par3,i_par3,e3,i_e3,Stat3]=regress(log(R(Ns-3:Nf)),A3,0.05);
  R_r=exp(log(R(Ns-3:Nf))-e3)';
  N_Rr=length(R_r);
  [par4,i_par4,e4,i_e4,Stat4]=regress(log(Y(Ns-1:Nf)),A1,0.05);
  Y_r=exp(log(Y(Ns-1:Nf))-e4)';
  N_Yr=length(Y_r);
  [par5,i_par5,e5,i_e5,Stat5]=regress(log(G(Ns:Nf)),A0,0.05);
  G_r=exp(log(G(Ns:Nf))-e5)';
  [par6,i_par6,e6,i_e6,Stat6]=regress(d_M(Ns:Nf),A0,0.05);
  d_M_r=(d_M(Ns:Nf)-e6)';
  
% Zadavane nominalni trajektorie stavu "x_nom"
% ********************************************
% Zadani:                     Historicke        Planovane(vyrovnane)
  x_nom(1,:) = CC_r;          %...x(Ns:Nf,1)';  %...CC_r;          %... 1.  C_t
  x_nom(2,:) = I_r;           %...x(Ns:Nf,2)';  %...I_r;           %... 2.  I_t
  x_nom(3,:) = R_r(4:N_Rr);   %...x(Ns:Nf,3)';  %...R_r(4:N_Rr);   %... 3.  R_t
  x_nom(4,:) = Y_r(2:N_Yr);   %...x(Ns:Nf,4)';  %...Y_r(2:N_Yr);   %... 4.  Y_t 
  x_nom(5,:) = R_r(3:N_Rr-1); %...x(Ns:Nf,5)';  %...R_r(3:N_Rr-1); %... 7.  R_t-1
  x_nom(6,:) = Y_r(1:N_Yr-1); %...x(Ns:Nf,6)';  %...Y_r(1:N_Yr-1); %... 6.  Y_t-1
  x_nom(7,:) = R_r(2:N_Rr-2); %...x(Ns:Nf,7)';  %...R_r(2:N_Rr-2); %... 7.  R_t-2
  x_nom(8,:) = R_r(1:N_Rr-3); %...x(Ns:Nf,8)';  %...R_r(1:N_Rr-3); %... 8.  R_t-3
  
% Zadavane nominalni trajektorie rizeni "u_nom"
% *********************************************
  u_nom(1,:) = u_hist(1,:);   %...u_hist(1,:);  %...G_r;           %... 1.  G_t;
  u_nom(2,:) = u_hist(2,:);   %...u_hist(2,:);  %...d_M_r;         %... 1.  diff_M_t;
  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
% P O Z O R !!!                                                            %
% #############                                                            %
% Zvolime-li historicky / vyrovnany vyvoj za "nominalni" vyvoj daneho stavu%
% musime stejny opozdeny vyvoj zvolit pro   v s e c h n y   stavy zabezpe- %
% cujici zpzozdeni tohoto stavu                                            %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  z=ones(size(u_nom(1,:)));

% Volba vah pro stavy
%       1      2     3     4     5     6     7     8  
%       CC     I     R     Y     R_1   Y_1   R_2   R_3  
%       ______________________________________________
  w_Q=[ 0      0     0     1e-6  0     0     0    0 ]; %... VOLBA VAH STAVU !!!<---<<<
% w_Q=[ 0      0     1e+8  1e-6  0     0     0    0 ];
% w_Q=[ 0      0     1e+8  1e+6  0     0     0    0 ];

% Matice Q   
  Q=diag(w_Q);

% Volba vah pro rizeni
%       G     d_M    
%       __________
  w_R=[ 1e+6  1e+6];                                    %... VOLBA VAH RIZENI !!!<---<<<
      
% Matice R
  R=diag(w_R);

% Okrajove podninky
  
  x_opt_0=x_nom(:,1);
  x_sim_0=x_nom(:,1);
  x_opt_N=x_nom(:,N);
  K_N = Q;
  g_N = -Q*x_nom(:,N);


% (i)   R e s e n i   R i c c a t i h o   r o v n i c e   p r o   K_i
%       *************************************************************
        Io =eye(n);
        K=zeros(n,N*n);

      % Koncove podminky
        K(1:n,(N-1)*n+1:N*n)= K_N;
        g(1:n,N)            = g_N;

      % Vypocet K_i
    for j=N-1:-1:1,
          disp('   K_i, g_i');
          disp('    i =');
          disp(j)
        K1 = K(1:n,(j*n+1):(j+1)*n);
        K(1:n,((j-1)*n+1):j*n)=Q+(Io+A)'*(K1-K1*B*inv(R+B'*K1*B)*B'*K1)*(Io+A);

% (ii)  V y p o c e t   u r c u j i c i   r o v n i c e   p r o   g_i     
%       *************************************************************
        g1 = g(1:n,j+1); 
        g(1:n,j)= -(Io+A)'*(K1-K1*B*inv(R+B'*K1*B)*B'*K1)*B*inv(R)*B'*g1...
                  +(Io+A)'*g1...
                  +(Io+A)'*(K1-K1*B*inv(R+B'*K1*B)*B'*K1)*(B*u_nom(1:2,j)+C*z(1,j))...
                  -Q*x_nom(1:n,j);

  end

% (iii) V y p o c e t   o p t i m a l n i h o  r i z e n i   u_opt_i   
%                     a o p t i m a l n i c h   s t a v u    x_opt_i  
%       ************************************************************
      % Pocatecni podminka   
        x_opt(1:n,1)=x_opt_0;
        x_sim(1:n,1)=x_sim_0;
    for j = 1:N-1,
            disp('   u_opt_i, x_opt_i');
            disp('    i =');
            disp(j)
        K1 = K(1:n,(j*n+1):(j+1)*n);
        g1 = g(1:n,j+1);  
        u_opt(1:r,j)=-inv(R+B'*K1*B)*B'*K1*(Io+A)*x_opt(:,j)...
                     +inv(R+B'*K1*B)*B'*K1*B*inv(R)*B'*g1...
                     -inv(R)*B'*g1-inv(R+B'*K1*B)*B'*K1...
                     *(B*u_nom(:,j)+C*z(:,j))+u_nom(:,j);
                   
        d_x_opt(:,j+1) = A*x_opt(:,j) + B*u_opt(:,j) + C*z(:,j);
          x_opt(:,j+1) =  x_opt(:,j)+ d_x_opt(:,j+1);
        d_x_sim(:,j+1) = A*x_sim(:,j) + B*u_hist(:,j) + C*z(:,j);
          x_sim(:,j+1) = x_sim(:,j)+d_x_sim(:,j+1);
    end 
    
u_op=u_opt';
x_op=x_opt';

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% G R A F Y
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
set(0,'DefaultAxesFontSize',8)
set(0,'DefaultLineLineWidth',0.35)
figure(1)
clf
set(gcf,'Name','Actual values / nominal path / optimal path of controls u and states x');
subplot(321)
plot(t_r(1:N-1),u_hist(1,1:N-1)','g.-');
hold on
plot(t_r(1:N-1),u_nom(1,1:N-1)','k');
hold on
plot(t_r(1:N-1),u_op(1:N-1,1),'rx-');
hold off
legend('actual','planed','optimalized');
axis([ min(t_r(1:N-1))-1 max(t_r(1:N-1))+1,... 
min([min(u_hist(1,1:N-1)'),min(u_nom(1,1:N-1)'),min(u_op(1:N-1,1))])-15,...
max([max(u_hist(1,1:N-1)'),max(u_nom(1,1:N-1)'),max(u_op(1:N-1,1))])+15]); 
title('G   Real Government Spending');
ylabel('billions of 1982 dollars');
%...xlabel('quarters');
grid

subplot(322)
plot(t_r(1:N-1),u_hist(2,1:N-1)','g.-');
hold on
plot(t_r(1:N-1),u_nom(2,1:N-1)','k');
hold on
plot(t_r(1:N-1),u_op(1:N-1,2),'rx-');
hold off
axis([ min(t_r(1:N-1))-1 max(t_r(1:N-1))+1,... 
min([min(u_hist(2,1:N-1)'),min(u_nom(2,1:N-1)'),min(u_op(1:N-1,2))])-3,...
max([max(u_hist(2,1:N-1)'),max(u_nom(2,1:N-1)'),max(u_op(1:N-1,2))])+3]); 
legend('actual','planed','optimalized');
title('diff M  First Differences of Real Money Stock  ( M1 )');
ylabel('billions of 1982 dollars');
%...xlabel('quarters')
grid

 
subplot(323)
plot(t_r(1:N),x_hist(1,1:N)','g.-');
hold on
plot(t_r(1:N),x_nom(1,1:N),'k');
hold on
plot(t_r(1:N),x_op(1:N,1),'rx-');
hold on
plot(t_r(1:N),x_sim(1,1:N),'m');
hold off
axis([ min(t_r(1:N-1))-1 max(t_r(1:N-1))+1,... 
min([min(x_sim(1,1:N)),min(x_nom(1,1:N)),min(x_op(1:N,1)),min(x_hist(1,1:N))])-100,...
max([max(x_sim(1,1:N)),max(x_nom(1,1:N)),max(x_op(1:N,1)),max(x_hist(1,1:N))])+100]); 
legend('actual','planed','otimalized','simulated');
title('C   Real Aggregate Personal Consumption');
ylabel('billions of 1982 dollars');
%...xlabel('quarters');
grid

subplot(324)
plot(t_r(1:N),x_hist(2,1:N)','g.-');
hold on
plot(t_r(1:N),x_nom(2,1:N),'k');
hold on
plot(t_r(1:N),x_op(1:N,2),'rx-');
hold on
plot(t_r(1:N),x_sim(2,1:N),'m');
hold off
axis([ min(t_r(1:N-1))-1 max(t_r(1:N-1))+1,... 
min([min(x_sim(2,1:N)),min(x_nom(2,1:N)),min(x_op(1:N,2)),min(x_hist(2,1:N))])-25,...
max([max(x_sim(2,1:N)),max(x_nom(2,1:N)),max(x_op(1:N,2)),max(x_hist(2,1:N))])+25]); 
legend('actual','planed','otimalized','simulated');
title('I   Real Gross Domestic Investment')
ylabel('billions of 1982 dollars');
%...xlabel('quarters');
grid

subplot(325)
plot(t_r(1:N),x_hist(3,1:N)','g.-');
hold on
plot(t_r(1:N),x_nom(3,1:N),'k');
hold on
plot(t_r(1:N),x_op(1:N,3),'rx-');
hold on
plot(t_r(1:N),x_sim(3,1:N),'m');
hold off
axis([ min(t_r(1:N-1))-1 max(t_r(1:N-1))+1,... 
min([min(x_sim(3,1:N)),min(x_nom(3,1:N)),min(x_op(1:N,3)),min(x_hist(3,1:N))])-0.5,...
max([max(x_sim(3,1:N)),max(x_nom(3,1:N)),max(x_op(1:N,3)),max(x_hist(3,1:N))])+0.5]); 
legend('actual','planed','otimalized','simulated');
title('R   Interest Rate on 3-month Treasury Bills')
ylabel('percent per year');
xlabel('quarters');
grid

subplot(326)
plot(t_r(1:N),x_hist(4,1:N)','g.-');
hold on
plot(t_r(1:N),x_nom(4,1:N),'k');
hold on
plot(t_r(1:N),x_op(1:N,4),'rx-');
hold on
plot(t_r(1:N),x_sim(4,1:N),'m');
hold off
axis([ min(t_r(1:N-1))-1 max(t_r(1:N-1))+1,... 
min([min(x_sim(4,1:N)),min(x_nom(4,1:N)),min(x_op(1:N,4)),min(x_hist(4,1:N))])-100,...
max([max(x_sim(4,1:N)),max(x_nom(4,1:N)),max(x_op(1:N,4)),max(x_hist(4,1:N))])+100]); 
legend('actual','planed','otimalized','simulated');
title('Y  Real   G N P (net of exports and inports)')
ylabel('billions of 1982 dollars');
xlabel('quarters');
grid

  disp('   ');
  disp('   ');
  disp('   Zacatek a konec intervalu optimalniho rizeni');
  disp('   ********************************************');
  disp(['   t_s  =  ',num2str(tt(Ns))]);
  disp(['   t_f  =  ',num2str(tt(Nf))]);
  disp('   ');
  disp('   Vahy   s t a v u');
  disp(w_Q(1:4))
  disp('   Vahy   v y s t u p u');
  disp(w_R(1:2))