function [thetahat,fval,exitflag,output] = filtervecfnc_DE(datmat,N,T,xvec,xvec_RE)

%Read in data
Y=datmat;

%Minimize log likelihood 
rng(2501);
shocks.e=randn(1,1000);
shocks.v=randn(1,1000);
shocks.w=randn(1,1000);
objfnc=@(theta) MLEfnc(Y,theta,shocks,xvec,xvec_RE);

lb=0.0;%[varphi];
ub=99.0;

%Initial parameter vector
theta0=0.5*(lb+ub);
fminconoptions=optimoptions('fmincon','ConstraintTolerance',1e-6,'StepTolerance',1e-6,'OptimalityTolerance',1e-6);
[thetahat,fval,exitflag,output]=fmincon(objfnc,theta0,[],[],[],[],lb,ub,[],fminconoptions);

end

function NLL = MLEfnc(Y,param,shocks,xvec,xvec_RE)
N=size(Y,1);    %number of forecasters
Nper=size(Y,2); %number of periods

%within forcaster-date settings:
Nz=3; %number of observations
Ns=10; %number of state variables

%reshape the data
fe=reshape(Y(:,:,1),[N*Nper,1]);
fr=reshape(Y(:,:,2),[N*Nper,1]);
lfe=reshape(Y(:,:,3),[N*Nper,1]);
yit=reshape([fe fr lfe]',[Nz*N,Nper]); 

%parameters
rho1=xvec(1);
rho2=xvec(2);
sigw=xvec(3);

sigv=xvec_RE(1);
varphi=param(1);

%% First, run through filter to get k1 and k2
T=length(shocks.w);
s=zeros(1,T);
for t=3:T
    s(1,t) = rho1*s(1,t-1) + rho2*s(1,t-2) + sigw*shocks.w(1,t);
end
y=s + sigv*shocks.v;

AA = [rho1 rho2; 1 0]; 
BB = [sigw;0];
CC = [1 0];
DD = sigv;
s0 = zeros(2,1); 
P0 = eye(2);

% Run Kalman filter to get k1 k2
t=1;
    %predict
    sbm1 = AA*s0;
    Pbm1 = AA*P0*AA' + BB*BB';
    %update
    z = y(1,t);
    K = Pbm1*CC'/(CC*Pbm1*CC'+DD*DD');
    sb = sbm1 + K*(z-CC*sbm1);
    Pb = (eye(2)-K*CC)*Pbm1;
for t=2:T
    %predict
     sbm1 = AA*sb;
     Pbm1 = AA*Pb*AA' + BB*BB';
       
    %update
    z = y(1,t);
    K = Pbm1*CC'/(CC*Pbm1*CC'+DD*DD');
    sb = sbm1 + K*(z-CC*sbm1);
    Pb = (eye(2)-K*CC)*Pbm1;
end %t
k1=K(1,1); 
k2=K(2,1);

%% Next run through another filter to construct LL
a = [rho1 rho2 0 0 0 0 0 0 0 0;
    1 0 0 0 0 0 0 0 0 0;
    0 0 0 0 0 0 0 0 0 0;
    k1*rho1 k1*rho2 0 (1-k1)*rho1 (1-k1)*rho2 0 0 0 0 0;
    k2*rho1 k2*rho2 0 1-k2*rho1 -k2*rho2 0 0 0 0 0;
    0 0 0 rho1 rho2 0 0 0 0 0;
    0 0 0 1 0 0 0 0 0 0;
    0 0 0 0 0 1 0 0 0 0;
    0 0 0 0 1 0 0 0 0 0;
    0 0 0 0 0 0 1 0 0 0];

b = [0 1 0;
    0 0 0;
    1 0 0;
    0 k1 k1;
    0 k2 k2;
    0 0 0;
    0 0 0;
    0 0 0;
    0 0 0;
    0 0 0];

c = [rho1 rho2 1 -(1+varphi)*rho1 -(1+varphi)*rho2 varphi*rho1 varphi*rho2 0 0 0;
    0 0 0 (1+varphi)*rho1 (1+varphi)*rho2 -(1+2*varphi)*rho1 -(1+2*varphi)*rho2 0 varphi*(rho1+rho2) varphi*rho2;
    0 1 0 0 0 0 -(1+varphi) 0 varphi 0];

%define relevant matrices (kron products are taken such that they are
%consistent with how yit is shaped)
T = kron(speye(N),a); 
B = kron(speye(N),b); 
Q=kron(speye(N),[sigw^2 0 0; 0 sigw^2 0; 0 0 sigv^2]);
W = (kron(speye(N),c));


%Setting initial state and variance
S0=kron(speye(N),zeros(Ns,1)); P0=speye(N*Ns); 

F={}; Fstar={}; 
K={}; Pttm1={}; 
dttm1y={}; 
d0y =sparse(zeros(Ns*N,1));  
Syy=0; term2=0;

term1=0; %counting actual observations

for t=1:Nper
	yt=yit(:,t);
	Mt=speye(Nz*N);
    Mt(isnan(yt),:)=[]; 
    yt(isnan(yt))=[];
    term1 = size(Mt,1) + term1;
    Wt=Mt*W;
    %Kalman Filter
    if t==1
        F{t} = Wt*(T*P0*T'+B*Q*B')*Wt' ;
        K{t} = (T*P0*T'+B*Q*B')*Wt'/F{t};
        ystar{t} = yt - Wt*T*d0y;
        dttm1y{t} = T*d0y + K{t}*ystar{t};
        Pttm1{t} = (T*P0*T'+B*Q*B') - K{t}*Wt*(T*P0*T'+B*Q*B');
    else
        F{t} = Wt*(T*Pttm1{t-1}*T'+B*Q*B')*Wt' ;
        K{t} = (T*Pttm1{t-1}*T'+B*Q*B')*Wt'/F{t};
        ystar{t} = yt - Wt*T*dttm1y{t-1};
        dttm1y{t} = T*dttm1y{t-1} + K{t}*ystar{t};
        Pttm1{t} = (T*Pttm1{t-1}*T'+B*Q*B') - K{t}*Wt*(T*Pttm1{t-1}*T'+B*Q*B');
    end
    Fstar{t} = F{t};
    Syy = ystar{t}'/Fstar{t}*ystar{t} + Syy;
    try
        term2 = 2*sum(log(diag(chol(Fstar{t})))) + term2;%determinant (Inf issues using det with large N)
    catch
        term2 = Inf;
    end
end
%Compute Log Likelihood
LL = -0.5 * ( term1*log(2*pi) + term2 + Syy);
NLL= -LL;

end
