% HPfilter from DYNARE
% using Uhlig notation
%%%%%%%%%%%%%%%%% construct var representation
    lambda =1600;
     ngrid = 512;%options_.hp_ngrid;DYNARE OPTION
dr_=oo_.dr;
lgy_=M_.endo_names;
nstat=dr_.nstatic;
npred=dr_.nspred;

listofvariables=str2cell(lgy_);
shocks=M_.exo_names;

% [i,j]=sort(dr_.order_var);
lgy2_=lgy_(dr_.order_var,:); % variables as appear in the canonical VAR rapresentation
[a,b,c]=intersect(deblank(listofvariables),str2cell(lgy2_));
c=c(:);
listofvariables=a;
%position in forecast vector and VMA representation
for jj=1:length(c);
    eval([deblank(listofvariables{jj}),'_pos=',num2str(length(nstat+[1:npred])+jj),';']);
end

%c is the order in the output; So lgy2_(c,:) should return listofvariables

% Rewrite the law of motion as a state space representation of a VAR
% The VAR matrix includes all the states plus list of variables,
% 
% 
% 
% reordering (from Dynare manual)
% % When reordered, the variables are stored in the following order: static variables, purely predetermined
% % variables (variables that appear only at the current and lagged periods in the model), variables that are
% % both predetermined and forward-looking (variables that appear at the current, future and lagged
% % periods in the model), purely forward-looking variables (variables that appear only at the current and
% % future periods in the model). In each category, the variables are arranged alphabetically.

% The dynamics is fully described by the predetermined variables only.
% so 
% A_var = [dr_.ghx([nstat+[1:npred],c'],:),zeros(size(dr_.ghx([nstat+[1:npred],c'],:),1),length(c))];%[[dr_.ghx(16:37,:);dr_.ghx(4,:);dr_.ghx(11,:);dr_.ghx(38,:)],zeros(size(dr_.ghx,2)+3,3)];
P =[zeros(size(lgy_,1),dr_.nstatic),dr_.ghx,zeros(size(lgy_,1),size(lgy_,1)-dr_.npred-dr_.nstatic)];
% Sigma matrix for VAR representation. 
% Sig_var = dr_.ghu([nstat+[1:npred],c'],:);%[dr_.ghu(16:37,:);dr_.ghu(4,:);dr_.ghu(11,:);dr_.ghu(38,:)] ;
Q = dr_.ghu;%[dr_.ghu(16:37,:);dr_.ghu(4,:);dr_.ghu(11,:);dr_.ghu(38,:)] ;
%%%%%%%%%%%%%%%%%%%%

    R = eye(size(P,1));%ghx(iky,:);
    S = zeros(size(P,1),size(Q,2));%ghu(iky,:);
nvar=size(R,1);

   
    freqs = 0 : ((2*pi)/ngrid) : (2*pi*(1 - .5/ngrid)); 
    tpos  = exp( sqrt(-1)*freqs);
    tneg  =  exp(-sqrt(-1)*freqs);
    hp1 = 4*lambda*(1 - cos(freqs)).^2 ./ (1 + 4*lambda*(1 - cos(freqs)).^2);
    mathp_col = [];
    IA = eye(size(P,1));
    IE = eye(size(Q,2));
    for ig = 1:ngrid
        f_omega  =(1/(2*pi))*( [inv(IA-P*tneg(ig))*Q;IE]...
                               *M_.Sigma_e*[Q'*inv(IA-P'*tpos(ig)) ...
                            IE]); % state variables
        g_omega = [R*tneg(ig) S]*f_omega*[R'*tpos(ig); S']; % selected variables
        f_hp = hp1(ig)^2*g_omega; % spectral density of selected filtered series
        mathp_col = [mathp_col ; (f_hp(:))'];    % store as matrix row
                                                 % for ifft
    end;
    % Covariance of filtered series
    imathp_col = real(ifft(mathp_col))*(2*pi);
    
    covHP=reshape(imathp_col(1,:),nvar,nvar);