%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This function maximizes the balanced panel model numerically in minit
% tries out of maxit.
%
% Input:
% yt...    NTx1 vector, dependent variable
% Xt...    NTxK matrix, K explanatory variables including the constant
% WN...    NxN sparse spatial weighting matrix
% N...     number of cross-section units
% T...     number of time periods
% z0...    3x1 vector of starting values for phi, rho1 and rho2
% minit... number of ml-maximizations at random starting values
% maxit... maximial number of ml-maximizations at random starting values
% tol....  precision, stopping creterion for maximization
%
% Output:
% estp...  K+4 vector of estimated paramters with maximal Liklihood out of
%          minit maximizations
% Logl...  Log Liklihood
% timer... minit x 1 vector with epalsed maximization time
% iterv... maxit x 1 vector with number of iterations
% V1tb ... KxK VC matrix of estimated parameters beta
% V2tb ... 4x4 VC matrix of estimated parameterts  (s2m,s2n,r4ho1, rho2)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Maximum Likelihood maximization function
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [estp,Logl,timer,iterv, V1tb, V2tb] =GSPRE_MLE_bal(yt,Xt,WN,N,T,z0, minit,maxit,tol)

% Initialization
C=sparse(zeros(N,N));
BB=sparse(zeros(N,N));
z00=z0;

% Paramterspace for rho1 and rho2
opt.tol = 1e-3; opt.disp = 0;
lambda = eigs(WN,1,'SR',opt);
rmin = 1/lambda+0.0001;
rmax = 1-0.0001;
clear lambda;

% Likelihood

    function [F] = SPRE_MLE_BAL(z)

        C=sparse(zeros(N,N));
        BB=sparse(zeros(N,N));

        %% Calculate B'B,det(B)
        BB=sparse((IN-z(3)*WpWN+(z(3)^2)*WWN));
        ldetBB=4*sum(log(diag(chol(IN-z(3)*WN))));

        %% Calculate C=inv(A'A/Tphi+B'B) and  inv(inv(A'A/Tphi)+inv(B'B))
        C=(sparse(chol((IN-z(2)*WpWN+(z(2)^2)*WWN)/(T*z(1))+BB)\IN));
        C=sparse(-BB*C*C'*BB);

        %% det(inv(A'A/Tphi)+inv(B'B))
        ldetAB=- 2*sum(log(diag(chol(BB+C))));

        %% s2n
        SSu=0;
        ABIui=zeros(N,1);
        for i=1:T
            SS1j=0;
            ABIui=C*u(N*(i-1)+1:N*i,1)/T ;
            for j=1:T
                SS1j=SS1j+u(N*(j-1)+1:N*j,1)'*ABIui;
            end
            SSu=SSu+ (SS1j+u(N*(i-1)+1:N*i,1)'*BB*u(N*(i-1)+1:N*i,1))/(N*T);
        end

        % log likelihood
        F=-(-(N*T/2)*log(pi)-(N*T)*log(SSu)/2 -ldetAB/2+(T-1)*ldetBB/2);

    end

% Constraints for parameter space
    function [c2, ceq2] = SPRE_MLE_C(z)
        % Nonlinear inequality constraints
        c2 = [-z(1)+0.01;  -z(2)+rmin; z(2)-rmax; -z(3)+rmin; z(3)-rmax];
        % Nonlinear equality constraints
        ceq2 = [];
    end

% Stopping ML-iterations
    function stop = outfun(z, optimValues,state)
        stop = false;
        % Check if fval is less than .000001
        if abs(optimValues.fval) > 1000000000
            stop = true;
        end
    end


% P, Q, JT, JQT, ET,
JQT=ones(T,T)/T;
IN=speye(N);
IT=speye(T);

% W'+W, W'W
WpWN=sparse(WN+WN');
WWN=sparse(WN'*WN);

% Initial residuals
u=yt-Xt*((Xt'*Xt)^-1)*Xt'*yt;

% Loop for ML-estimation starting
l=1;
m=1;
resnorm=ones(minit,1)*10000;
estpar=ones(minit,3)*10000;
timer=ones(minit,1);
iterv=ones(minit,1);
runv=ones(minit,1);

while l<=minit
    iter=0;
    m=m+1;
    rtol=10000;
    rtol1=10000;
    k=1;

    % Starting guess for the remaining parameters
    rand('state',sum(100*clock));
    z0=z00+0.2*(rand(3,1)-0.5*ones(3,1))';   %starting values

    tic
    while rtol > tol
        if 10^(-2*(k-1)-1)>10^(-6)
            otol=10^(-2*(k-1)-1);
        else
            otol=tol;
        end

        % Options and tolerance
        options=optimset('Display','off',  'Diagnostics','off', 'MaxFunEvals',10000, ...
            'LargeScale','off', 'MaxIter',200,'OutputFcn', @outfun, ...
            'TolFun',otol, 'TolX',otol,'Diagnostics','off');

        % Call ML-maximizer

        [theta,resnorm,exitflag,output] = fmincon(@SPRE_MLE_BAL,z0,[],[],[],[],[],[],@SPRE_MLE_C,options);

        % Estimated phi, rho1 and rho2
        phitb=theta(1,1);
        rho1tb=theta(1,2);
        rho2tb=theta(1,3);
        z0=[phitb,rho1tb,rho2tb];

        % Calculate residuals and beta_hat
        XXT=zeros(2,2);
        XYT=zeros(2,1);
        CXt=zeros(N,1);
        Cyt=zeros(N,1);
        for i=1:T
            XXT1=zeros(2,2);
            XYT1=zeros(2,1);
            CXt=C*Xt(N*(i-1)+1:N*i,:)/T;
            Cyt=C*yt(N*(i-1)+1:N*i,1)/T;
            for j=1:T
                XXT1=XXT1+Xt(N*(j-1)+1:N*j,:)'*CXt;
                XYT1=XYT1+Xt(N*(j-1)+1:N*j,:)'*Cyt;
            end
            XXT=XXT + (XXT1+Xt(N*(i-1)+1:N*i,:)'*BB*Xt(N*(i-1)+1:N*i,:));
            XYT=XYT + (XYT1+Xt(N*(i-1)+1:N*i,:)'*BB*yt(N*(i-1)+1:N*i,1));
        end

        % Estimated resuduals yt-XT*beta

        u=yt-Xt*inv(XXT)*XYT;

        % Loop control
        rtol2=resnorm;
        rtol=abs((rtol1-rtol2)/rtol1);
        rtol1=rtol2;
        iter=iter+output.iterations;
        step=([k,resnorm,rtol,otol]);
        k=k+1;
    end
    if (exitflag >0)
        maxvalue(l,1)=resnorm;
        estpar(l,:)=theta;
        iterv(l,1)=iter;
        timer(l,1)=toc;
        runv(l,1)=l;
        l=l+1;

    end
    if m>maxit, break, end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

format short
runs=[runv,timer,maxvalue,estpar];
[Logl,zmin]=min(resnorm);

fprintf('***************************************************************\n');
disp('      runs      sec     Logl       phid     rho1d     rho2d ')
disp(runs)


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Collect estimated paramters and estimate slope paramters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Final estimates of phi rho1 and rho2
phitb=estpar(zmin,1);
rho1tb=estpar(zmin,2);
rho2tb=estpar(zmin,3);

% Final estimates of beta
bmtb=inv(XXT)*XYT;
b1tb=bmtb(1,1);
b2tb=bmtb(2,1);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Estimates s2n and s2m
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%estimate of s2n and s2m
SSu=0;
Cu=zeros(N,1);
for i=1:T
    SS1j=0;
    Cu=C*u(N*(i-1)+1:N*i,1)/T;
    for j=1:T
        SS1j=SS1j+u(N*(j-1)+1:N*j,1)'*Cu;
    end
    SSu=SSu+ (SS1j+u(N*(i-1)+1:N*i,1)'*BB*u(N*(i-1)+1:N*i,1))/(N*T);
end
s2ntb=SSu;
s2mtb=estpar(zmin,1)* s2ntb;

estp=[b1tb b2tb s2mtb s2ntb rho1tb rho2tb Logl N T]';
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% VC-matrix of estimated  b
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
V1tb=s2ntb*inv(XXT);
clear yt Xt u XYT XXT1 XYT1 Cu Cyt CXt ;
clear S1 S2 S3 S4 WpWN WWN XXT;
pack
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% VC-matrix of estimated s2m s2n rho1 rho2
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
AAI=((IN-rho1tb*WN)'*(IN-rho1tb*WN))\IN;
C=C+BB;
V1=(T/s2ntb)*sparse(C*AAI);
V3=(T/s2ntb)*sparse(C*(s2ntb*phitb*AAI*(WN'+WN-2*rho1tb*WN'*WN)*AAI));
clear AAI

BB=BB\IN;
V2=(1/s2ntb)*C*BB;
V41=sparse(C*(BB*(WN'+WN-2*rho2tb*WN'*WN)*BB)) ;
V42=sparse((WN'+WN-2*rho2tb*WN'*WN)*BB);
clear C WN BB IT JQT;

V2tb=zeros(4,4);
V2tb(1,1)=0.5*reshape((V1)',1,(N)^2)*reshape((V1),1,(N)^2)';
V2tb(1,2)=0.5*reshape((V1)',1,(N)^2)*reshape(V2,1,(N)^2)';

V2tb(1,3)=0.5*reshape((V1)',1,(N)^2)*reshape((V3),1,(N)^2)';
V2tb(1,4)=0.5*reshape((V1)',1,(N)^2)*reshape((V41),1,(N)^2)';

V2tb(2,1)=V2tb(1,2);

V2tb(2,2)=0.5*reshape((V2)',1,(N)^2)*reshape((V2),1,(N)^2)'+...
    (T-1)*(1/s2ntb^2)*N/2;

V2tb(2,3)=0.5*reshape((V2)',1,(N)^2)*reshape((V3),1,(N)^2)';
V2tb(2,4)=0.5*reshape((V2)',1,(N)^2)*reshape((V41),1,(N)^2)'+...
    0.5*((T-1)/s2ntb)*trace(V42);

V2tb(3,1)=V2tb(1,3);
V2tb(3,2)=V2tb(2,3);
V2tb(3,3)=0.5*reshape((V3)',1,(N)^2)*reshape((V3),1,(N)^2)';
V2tb(3,4)=0.5*reshape((V3)',1,(N)^2)*reshape((V41),1,(N)^2)';

V2tb(4,1)=V2tb(1,4);
V2tb(4,2)=V2tb(2,4);
V2tb(4,3)=V2tb(3,4);
V2tb(4,4)=0.5*reshape((V41)',1,(N)^2)*reshape((V41),1,(N)^2)'...
    +(T-1)*0.5*reshape((V42)',1,(N)^2)*reshape((V42),1,(N)^2)';

V2tb=inv(V2tb);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end

