% Willi Mutschler (willi@mutschler.eu)
% This script illustrates the perturbation approach at first, second, and third order
% Simply run it after a stoch_simul(order=3) command with any mod file...
% The third-order is not correct, any help is much appreciated!

% some common objects
tol = 1e-7; %judgement for numerical zero
[I,~] = find(M_.lead_lag_incidence');
params = M_.params;
yy0    = oo_.dr.ys(I);
ex0    = oo_.exo_steady_state';
ys     = oo_.dr.ys;
order_var = oo_.dr.order_var;
k1 = nonzeros(M_.lead_lag_incidence(:,order_var)');
kk1 = [k1; length(k1)+(1:M_.exo_nbr)'];
nk = size(kk1,1);
ic = [ M_.nstatic+(1:M_.nspred) M_.endo_nbr+(1:size(oo_.dr.ghx,2)-M_.nspred) ]'; %state variables
Ic = eye(length(ic));
klag  = M_.lead_lag_incidence(1,order_var);
kcurr = M_.lead_lag_incidence(2,order_var);
klead = M_.lead_lag_incidence(3,order_var);
kk2 = transpose(reshape(1:nk^2,nk,nk)); %kk2(i,j,k) provides the index for the second dynamic derivative, i.e. df/(dx_i*dx_j)
kk3 = permute(reshape(transpose(reshape(1:nk^3,nk,nk^2)),nk,nk,nk),[2 1 3]); %kk3(i,j,k) provides the index for the third dynamic derivative, i.e. df/(dx_i*dx_j*dx_k)

if strcmp(dynare_version,'4.6-unstable')
    [~, g1, g2, g3] = feval([M_.fname,'.dynamic'], yy0, ex0, params, ys, 1);
    g3 = unfold_g3(g3, length(yy0)+length(ex0));
else
    [~, g1, g2, g3] = feval([M_.fname,'_dynamic'], yy0, ex0, params, ys, 1);
end

%% First-order approximation
fy  = g1(:,kk1(1:length(yy0))); % reordering first order derivatives into DR order (without exogenous)
%solution matrices
gy_ = oo_.dr.ghx(ic,:);        % g_y^{**} for predetermined variables
gy0 = oo_.dr.ghx(kcurr~=0,:);  % g_y      for current variables
gyp = oo_.dr.ghx(klead~=0,:);  % g_y^{*}  for forward looking variables
%auxiliary first-derivative of dynamic variables
zx = [Ic;
      gy0;
      gyp*gy_];

% RESULT1: We have the following expression, which is (numerically) zero:
nnz(abs(fy*zx) > tol)


%% Second-order Approximation
fyy   = g2(:,kk2(kk1(1:length(yy0)),kk1(1:length(yy0)))); % reordering second order derivatives into DR order (without exogenous)
%solution matrices
gyy_  = oo_.dr.ghxx(ic,:);       % g_yy^{**} for predetermined variables
gyy0  = oo_.dr.ghxx(kcurr~=0,:); % g_yy      for current variables
gyyp  = oo_.dr.ghxx(klead~=0,:); % g_yy^{*}  for forward variables
%auxiliary second-derivative of dynamic variables
zxx = [zeros(length(ic),length(ic)^2);
      gyy0;
      gyyp*kron(gy_,gy_) + gyp*gyy_];

% RESULT 2: We have the following expression, which is (numerically) zero:
nnz(abs(fyy*kron(zx,zx) + fy*zxx) > tol)

%% Third-order Approximation
fyyy = g3(:,kk3(kk1(1:length(yy0)),kk1(1:length(yy0)),kk1(1:length(yy0)))); % reordering third order derivatives into DR order (without exogenous)
%solution matrices
gyyy_  = oo_.dr.ghxxx(ic,:);       % g_yyy^{**} for predetermined variables
gyyy0  = oo_.dr.ghxxx(kcurr~=0,:); % g_yyy      for current variables
gyyyp  = oo_.dr.ghxxx(klead~=0,:); % g_yyy^{*}  for forward variables
%auxiliary third derivative of dynamic variables
M=reshape(1:length(ic)^3,1,length(ic),length(ic),length(ic));
Ix=speye(length(ic)^3);
OMEGA1=Ix(:,ipermute(M,[1,3,4,2]))+Ix(:,ipermute(M,[1,2,4,3]))+Ix(:,ipermute(M,[1,2,3,4])); %matrix that sums permuted second-order tensors (reshaped to matrix)
zxxx = [zeros(length(ic),length(ic)^3);
       gyyy0;
       gyyyp*kron(gy_,kron(gy_,gy_)) + gyyp*kron(gy_,gyy_)*OMEGA1 + gyp*gyyy_ ];

% RESULT 3: We have the following expression, which is (numerically) zero:
nnz(abs(fyyy*kron(zx,kron(zx,zx)) + fyy*kron(zx,zxx)*OMEGA1 + fy*zxxx) > tol)



