% This program tests Klein's solution algorithm (its complete form). 
% The algorithm is based on the generalized eigenvalue decomposition of matrices A and B.
%Optimal Taylor Rule (current y and p)
%ORDERING OF THE VARIABLES IS AS IN SALEMI: yt pt rt yt-1 pt-1 rt-1
function [loss3, RF3, RF_exp3, roots, M3, L3, N3]=klein3(theta,a1,a2,alfa1,alfa2,b,beta,lambda,omega_str,W_tilda,delta)

% Define the A and B matrices.

A=[1 0 0 0 0 0 
   0 1 0 0 0 0
   0 0 1 0 0 0
   0 0 0 1 0 0
   0 0 -b 0 lambda b
   0 0 0 0 0 alfa1];
B=[0 0 0 0 1 0
   0 0 0 0 0 1
   0 0 0 0 theta(1) theta(2)
   1 0 0 0 0 0 
   -a1 0 0 -a2 1 0
   0 -alfa2 0 0 -beta 1];
D=[0 0 0
   0 0 0
   0 0 1
   0 0 0
   -1 0 0
   0 -1 0];
%Compute the generalized eigenvalue decomposition of A and B.

[S,T,Q,Z]=qz(A,B);

% Reorder U.T. matrices S, T, orthonormal matrices Q,Z so that abs(T(i,i)/S(i,i)) are in descending order
% while preserving U.T. and orthonormal properties and Q'AZ' and Q'BZ'.
[S,T,Q,Z]=reorder(S,T,Q,Z);

%Form the appropriate submatrices and combine them in Klein's formula.

Z11=Z(1:4,1:4); % dimmension of the number of predetermined variables (nk=4) and of the number of stable roots (ns=4) 
Z12=Z(1:4,5:6);
Z21=Z(5:6,1:4);
Z22=Z(5:6,5:6);

S11=S(1:4,1:4);
S12=S(1:4,5:6);
S21=S(5:6,1:4);
S22=S(5:6,5:6);

T11=T(1:4,1:4);
T12=T(1:4,5:6);
T21=T(5:6,1:4);
T22=T(5:6,5:6);

Q1=Q(1:4,:);
Q2=Q(5:6,:);

%Compute MM
vec_MM=inv(kron(omega_str(1:3,1:3)',S22)-kron(eye(3),T22))*reshape(Q2*D,6,1);
MM=reshape(vec_MM,2,3);

%Compute L
L3=-Z11*inv(S11)*T11*inv(Z11)*Z12*MM+Z11*inv(S11)*(T12*MM-S12*MM*...
    omega_str(1:3,1:3)+Q1*D)+Z12*MM*omega_str(1:3,1:3);

%Compute N
N3=(Z22-Z21*inv(Z11)*Z12)*MM;

RF=Z11*inv(S11)*T11*inv(Z11); % matrix for mapping in reduced form Klein's solution (for predetermined)
RF_exp3=Z21*inv(Z11); % matrix for mapping in reduced form Klein's solution (for not predetermined)

%Compute omega (reduced form cov. matrix) using structural cov. matrix
omega=zeros(6,6);
omega(1:4,1:4)=L3*omega_str(1:3,1:3)*L3';

%Compute the eigenvalues of A and B

alpha=diag(S);
beta=diag(T);
roots=beta./alpha;

%Compute Loss Function

%Reformate RF
RF3=[RF zeros(4,2)
    zeros(2,1) eye(2) zeros(2,3)];

%Compute number of unstable roots
nomb=0;
for j=1:size(roots,1)
    if abs(roots(j))>=1
        nomb=nomb+1;
    end;
end;


%Innitial parameters for iterration on M
i=0;
dif=eye(6);
M3=zeros(6,6);


%Iterrations on M
 while  (max(max(dif))>=eps);
    M3_new=M3+(delta^i)*(RF3^i)*omega*ctranspose(RF3^i)/(1-delta);
    dif=M3_new-M3;
    M3=M3_new; 
    i=i+1;
 end;


loss_form=1; % 0 - constant penalty; 1 - variable penalty (Salemi)

switch loss_form
    case 0
        %Penalizing for having "wrong" number of unstable roots by adding a fixed penalty
        penalty=0;
        nomb;
        if (nomb~=2);
            penalty=1000000;
        end;
        weight=1;
    case 1
        %Penalizing for having "wrong" number of unstable roots by adding a smooth penalty (Salemi)
        penalty=0;
        nomb;
        if (nomb~=2);
            indicator=zeros(size(roots,1),1);
            for j=1:4
                if abs(roots(j))>=1
                   indicator(j)=1;
                end;
            penalty=penalty+indicator(j)*(abs(roots(j))-1)^2;
            end;
            for j=5:6
                if abs(roots(j))<1
                   indicator(j)=1;
                end;
            penalty=penalty+indicator(j)*(abs(roots(j))-1)^2;
            end;
        end;
    weight=1000;% relative weight makes penalty comparable with the loss function
    end;

loss3=abs(trace(W_tilda*M3)+weight*penalty);