% function [gamma,x,w,sigu,like]=champagne(y,f,sigu,nem,nd,vcs,nupd,gamma0,retx);
% Output:
% gamma(nd,nd,nv) = voxel covariance matrices
% x(nv*nd,nt) = voxel current density
% w(nv*nd,nk) = reconstruction filter
% sigu(nk,nk) = noise covariance matrix
% like(nem,1) = likelihood
%
% Input:
% #s:
% nk = # of sensors
% nv = # of voxels
% nt = # of data time points

% y(nk,nt) = sensor data % preferable to be scaled to pT units
% f(nk,nv*nd) = lead field matrix % preferable to be normalized.
% sigu(nk,nk) = noise covariance matrix,  [] for automatic initialization
% nem = maximum # of iterations
% nd = # of orientations
% vcs = voxel covariance structure: 0 = scalar, 1 = diagonal, 2 = general
% nupd = 2: learn diagonal noise covariance, = 1: learn scalar, = 0: use input noise cov
% gamma0(nd,nd,nv) = initial voxel covariances, [] for automatic initialization
% retx = 1: return voxel current density, = 0: don't

% Two parameters in code that can be adjusted are:
% eps1 = Default is 1e-8 For numerical stability while inverting model covariance matrix.
% eps1z = Default is 1e-8 For numerical stability while calculating
%         voxel variance and auxilliary variable z

function [gamma,x,w,sigu,like]=champagne(y,f,sigu,nem,nd,vcs,nupd,gamma0,retx, fig);

if vcs==2 && nd>1
    [gamma x w sigu like]=champ_mat(y,f,sigu,nem,nd,nupd,gamma0,retx, fig);
else
    [gamma x w sigu like]=champ_vec(y,f,sigu,nem,nd,vcs,nupd,gamma0,retx, fig);
end

return

function [gamma,x,w,sigu,like]=champ_vec(y,f,sigu,nem,nd,vcs,nupd,gamma0,retx, fig);

eps1=1e-8;
eps1z=1e-8;
[nk nvd]=size(f);
nv=nvd/nd;
nt=size(y,2);
nb=1000;

cyy=y*y'/nt;

% Initialize voxel variances

if isempty(gamma0)
    f2=sum(f.^2,1);
    invf2=zeros(1,nvd);
    ff=find(f2>0);
    invf2(ff)=1./f2(ff);
    w=spdiags(invf2',0,nvd,nvd)*f';

    if nt>nb
        inu0=zeros(nvd,1);
        for it=1:nb:nt-nb+1
            inu0=inu0+sum((w*y(:,it:it+nb-1)).^2,2);
        end
        inu0=(inu0+sum((w*y(:,it+nb:nt)).^2,2))/nt;
    else
        inu0=mean((w*y).^2,2);
    end
%    disp(max(inu0-mean((w*y).^2,2)));
    inu0v=mean(reshape(inu0,nd,nv),1);
    inu1=reshape(ones(nd,1)*inu0v,nvd,1);
    vvec=inu1;
else
    vvec=zeros(nvd,1);
    for iv=1:nv
        jv=(iv-1)*nd+1:iv*nd;
        if vcs==0
            vvec(jv)=mean(diag(gamma0(:,:,iv)));
        else
            vvec(jv)=diag(gamma0(:,:,iv));
        end
    end
end

% initialize noise covariance

if nupd>0
    if isempty(sigu)
        ndr=.1;
        sigu=ndr*mean(mean(y.^2))*eye(nk);
    else
        sigu=mean(diag(sigu))*eye(nk);
    end
end

% Learn voxel variances

v=zeros(nv,1);
figure(fig);

like=zeros(nem,1);
for iem=1:nem
    vmat=spdiags(vvec,0,nvd,nvd);
    c=f*vmat*f'+sigu;
    [p d]=svd(c);
    d=max(real(diag(d)),0);
    invd=zeros(nk,1);
    ff=find(d>=eps1);
    invd(ff)=1./d(ff);
%    invd=1./d;
    invc=p*spdiags(invd,0,nk,nk)*p';

%    like(iem)=-.5*(sum(log(d))+nk*log(2*pi))-.5*sum(sum(y.*(invc*y)))/nt;
    like(iem)=-.5*(sum(log(max(d,eps1)))+nk*log(2*pi))-.5*sum(sum(invc.*cyy));
    subplot(2,2,1);plot((1:iem),like(1:iem));
    title(['Likelihood: ' int2str(iem) ' / ' int2str(nem)]);
    xlabel('iteration');
    set(gca(),'XLim',[0 iem]);

    fc=f'*invc;
    w=vmat*fc;
    x2=sum((w*cyy).*w,2);
    z=sum(fc.*f',2);

    if vcs==0
        x20=sum(reshape(x2,nd,nv),1);
        z0=sum(reshape(z,nd,nv),1);
        v0=(sqrt(z0)./max(z0,eps1z)).*sqrt(x20);
        vvec=reshape(ones(nd,1)*v0,nvd,1);
    else
        vvec=(sqrt(z)./max(z,eps1z)).*sqrt(x2);
    end

    if nupd>0
        fw=eye(nk)-f*w;
        sigy1=sum((fw*cyy).*fw,2);
        fgf=sum((fw*f*vmat).*f,2);
        ilam=sigy1+fgf;
        if nupd==1
            sigu=mean(ilam)*eye(nk);
        else
            sigu=diag(ilam);
        end
    end

    v=sum(reshape(vvec,nd,nv),1);
    subplot(2,2,2);plot((1:nv),v);
    title(['Voxel power: ' num2str(nv) ' / ' num2str(nv)]);
    xlabel('voxel index');
    set(gca(),'XLim',[1 nv]);
    drawnow
end

if retx==1
    x=w*y;
else
    x=[];
end

if nd==1
    gamma=reshape(vvec,1,1,nv);
else
    gamma=zeros(nd,nd,nv);
    for iv=1:nv
        gamma(:,:,iv)=diag(vvec((iv-1)*nd+1:iv*nd));
    end
end

return

function [gamma,x,w,sigu,like]=champ_mat(y,f,sigu,nem,nd,nupd,gamma0,retx, fig);

eps1=1e-8;
eps1z=1e-8;
[nk nvd]=size(f);
nv=nvd/nd;
nt=size(y,2);
nb=1000;

cyy=y*y'/nt;

% Initialize voxel covariances

if isempty(gamma0)
    f2=sum(f.^2,1);
    invf2=zeros(1,nvd);
    ff=find(f2>0);
    invf2(ff)=1./f2(ff);
    w=spdiags(invf2',0,nvd,nvd)*f';

    if nt>nb
        inu0=zeros(nvd,1);
        for it=1:nb:nt-nb+1
            inu0=inu0+sum((w*y(:,it:it+nb-1)).^2,2);
        end
        inu0=(inu0+sum((w*y(:,it+nb:nt)).^2,2))/nt;
    else
        inu0=mean((w*y).^2,2);
    end
%    disp(max(inu0-mean((w*y).^2,2)));
    inu0v=mean(reshape(inu0,nd,nv),1);
    inu1=reshape(ones(nd,1)*inu0v,nvd,1);
    vmat=spdiags(inu1,0,nvd,nvd);
else
    vmat=sparse(nvd,nvd);
    for iv=1:nv
        jv=(iv-1)*nd+1:iv*nd;
        vmat(jv,jv)=gamma0(:,:,iv);
    end
end

% initialize noise covariance

if nupd>0
    if isempty(sigu)
        ndr=.1;
        sigu=ndr*mean(mean(y.^2))*eye(nk);
    else
        sigu=mean(diag(sigu))*eye(nk);
    end
end

% Learn voxel covariances

v=zeros(nv,1);
figure;

like=zeros(nem,1);
for iem=1:nem
    c=f*vmat*f'+sigu;
    [p d]=svd(c);
    d=max(real(diag(d)),0);
    invd=zeros(nk,1);
    ff=find(d>=eps1);
    invd(ff)=1./d(ff);
%    invd=1./d;
    invc=p*spdiags(invd,0,nk,nk)*p';

%    like(iem)=-.5*(sum(log(d))+nk*log(2*pi))-.5*sum(sum(y.*(invc*y)))/nt;
    like(iem)=-.5*(sum(log(max(d,eps1)))+nk*log(2*pi))-.5*sum(sum(invc.*cyy));
     figure(fig)
    subplot(2,2,1);plot((1:iem),like(1:iem));
    title(['Likelihood: ' int2str(iem) ' / ' int2str(nem)]);
    xlabel('iteration');
    set(gca(),'XLim',[0 iem]);

    fc=f'*invc;
    w=vmat*fc;
%    x=w*y;
%    x2=mean(x.^2,2);
%    z=sum(fc.*f',2);

    for iv=1:nv
        jv=((iv-1)*nd+1:iv*nd);
        x2=w(jv,:)*cyy*w(jv,:)';
        z=fc(jv,:)*f(:,jv);

        [pz dz]=eig(z);
        dzd=max(real(diag(dz)),0);
        dz5=sqrt(dzd);
        z5=pz*diag(dz5)*pz';
        invdz5=1./sqrt(max(dzd,eps1z));
        invz5=pz*diag(invdz5)*pz';

        [px dx]=eig(z5*x2*z5);
        dx5=sqrt(max(real(diag(dx)),0));
        cx5=px*diag(dx5)*px';
        vmat(jv,jv)=invz5*cx5*invz5;
        v(iv)=sum(diag(vmat(jv,jv)));
    end

    if nupd>0
        fw=eye(nk)-f*w;
        sigy1=sum((fw*cyy).*fw,2);
        fgf=sum((fw*f*vmat).*f,2);
        ilam=sigy1+fgf;
        if nupd==1
            sigu=mean(ilam)*eye(nk);
        else
            sigu=diag(ilam);
        end
    end

    subplot(2,2,2);plot((1:nv),v);
    title(['Voxel power: ' num2str(nv) ' / ' num2str(nv)]);
    xlabel('voxel index');
    set(gca(),'XLim',[1 nv]);
    drawnow
end

if retx==1
    x=w*y;
else
    x=[];
end

gamma=zeros(nd,nd,nv);
for iv=1:nv
    jv=(iv-1)*nd+1:iv*nd;
    gamma(:,:,iv)=vmat(jv,jv);
end

return


function [g1, g2] = lfmaXlfmaKernGradient(lfmKern1, lfmKern2, t1, t2, covGrad, meanVector)

% LFMAXLFMAKERNGRADIENT Compute a cross gradient between a LFMA and a LFMA.
% FORMAT
% DESC computes cross gradient of parameters of a cross kernel
% between two lfm kernels, both lfm kernels correspond to the accelerations.
% with the multiple output kernel.
% ARG lfmKern1 : the kernel structure associated with the first LFM
% kernel (acceleration).
% ARG lfmKern2 : the kernel structure associated with the second LFM
% kernel (acceleration).
% ARG t1 : inputs for which kernel is to be computed.
% ARG covGrad : gradient of the objective function with respect to
% the elements of the cross kernel matrix.
% RETURN g1 : gradient of the parameters of the first kernel, for
% ordering see lfmKernExtractParam.
% RETURN g2 : gradient of the parameters of the second kernel, for
% ordering see lfmKernExtractParam.
%
% FORMAT
% DESC computes cross gradient of parameters of a cross kernel
% between two lfm kernels, both lfm kernels correspond to the accelerations.
% with the multiple output kernel.
% ARG lfmKern1 : the kernel structure associated with the first LFM
% kernel (acceleration).
% ARG lfmKern2 : the kernel structure associated with the second LFM
% kernel (acceleration).
% ARG t1 : row inputs for which kernel is to be computed.
% ARG t2 : column inputs for which kernel is to be computed.
% ARG covGrad : gradient of the objective function with respect to
% the elements of the cross kernel matrix.
% RETURN g1 : gradient of the parameters of the first kernel, for
% ordering see lfmKernExtractParam.
% RETURN g2 : gradient of the parameters of the second kernel, for
% ordering see lfmKernExtractParam.
%
% FORMAT
% DESC computes cross gradient of parameters of a cross kernel
% between two lfm kernels. Both kernels correspond to accelerations.
% It is supposed to be used together with the multiple output kernel.
% ARG lfmKern1 : the kernel structure associated with the first LFM
% kernel (acceleration).
% ARG lfmKern2 : the kernel structure associated with the second LFM
% kernel (acceleration).
% ARG t1 : row inputs for which kernel is to be computed.
% ARG t2 : column inputs for which kernel is to be computed.
% ARG covGrad : gradient of the objective function with respect to
% the elements of the cross kernel matrix.
% ARG meanVec : precomputed factor that is used for the switching dynamical
% latent force model.
% RETURN g1 : gradient of the parameters of the first kernel, for
% ordering see lfmKernExtractParam.
% RETURN g2 : gradient of the parameters of the second kernel, for
% ordering see lfmKernExtractParam.
%
% SEEALSO : multiKernParamInit, multiKernCompute, lfmvKernParamInit
%
% COPYRIGHT : Mauricio Alvarez, 2010

% KERN

subComponent = false; % This is just a flag that indicates if this kernel is part of a bigger kernel (SDLFM)

if nargin == 4
    covGrad = t2;
    t2 = t1;
elseif nargin == 6
    subComponent = true;
    if numel(meanVector)>1
        if size(meanVector,1) == 1,
            if size(meanVector, 2)~=size(covGrad, 2)
                error('The dimensions of meanVector don''t correspond to the dimensions of covGrad')
            end
        else
            if size((meanVector'), 2)~=size(covGrad,2)
                error('The dimensions of meanVector don''t correspond to the dimensions of covGrad')
            end
        end
    else
        if numel(t1)==1 && numel(t2)>1
            % matGrad will be row vector and so should be covGrad
            dimcovGrad = length(covGrad);
            covGrad = reshape(covGrad, [1 dimcovGrad]);
        elseif numel(t1)>1 && numel(t2)==1
            % matGrad will be column vector and sp should be covGrad
            dimcovGrad = length(covGrad);
            covGrad = reshape(covGrad, [dimcovGrad 1]);
        end
    end
end

if size(t1, 2) > 1 || size(t2, 2) > 1
    error('Input can only have one column');
end
if lfmKern1.inverseWidth ~= lfmKern2.inverseWidth
    error('Kernels cannot be cross combined if they have different inverse widths.')
end

% Parameters of the simulation (in the order providen by kernExtractParam)

m = [lfmKern1.mass lfmKern2.mass];                  % Par. 1
D = [lfmKern1.spring lfmKern2.spring];              % Par. 2
C = [lfmKern1.damper lfmKern2.damper];              % Par. 3
sigma2 = 2/lfmKern1.inverseWidth;                   % Par. 4
sigma = sqrt(sigma2);
S = [lfmKern1.sensitivity lfmKern2.sensitivity];    % Par. 5

alpha = C./(2*m);
omega = sqrt(D./m-alpha.^2);

% Initialization of vectors and matrices

g1 = zeros(1,5);
g2 = zeros(1,5);

% Precomputations
computeH  = cell(4,1);
computeUpsilonMatrix = cell(2,1);
computeUpsilonVector = cell(2,1);
gradientUpsilonMatrix = cell(4,1);
gradientUpsilonVector = cell(4,1);
gamma1_p = alpha(1) + j*omega(1);
gamma1_m = alpha(1) - j*omega(1);
gamma2_p = alpha(2) + j*omega(2);
gamma2_m = alpha(2) - j*omega(2);
preExp1 = zeros(length(t1),2);
preExp2 = zeros(length(t2),2);
preExpg1 = zeros(length(t1),2);
preExpgg1 = zeros(length(t1),2);
preExpg2 = zeros(length(t2),2);
preExpgg2 = zeros(length(t2),2);
preExpt1 = zeros(length(t1),2);
preExpt2 = zeros(length(t2),2);
preGamma(1) = gamma1_p + gamma2_p;
preGamma(2) = gamma1_p + gamma2_m;
preGamma(3) = gamma1_m + gamma2_p;
preGamma(4) = gamma1_m + gamma2_m;
preGamma2 = preGamma.^2;
preConst = 1./preGamma;
preConst2 = 1./(preGamma2);
preFactors(1) = preConst(2) - preConst(1);
preFactors(2) = preConst(3) - preConst(4);
preFactors(3) = preConst(3) - preConst(1);
preFactors(4) = preConst(2) - preConst(4);
preFactors2(1) = -preConst2(2) + preConst2(1);
preFactors2(2) = -preConst2(3) + preConst2(4);
preFactors2(3) = -preConst2(3) + preConst2(1);
preFactors2(4) = -preConst2(2) + preConst2(4);
preExp1(:,1) = exp(-gamma1_p*t1);
preExp1(:,2) = exp(-gamma1_m*t1);
preExp2(:,1) = exp(-gamma2_p*t2);
preExp2(:,2) = exp(-gamma2_m*t2);
preExpg1(:,1) = (2*gamma1_p)*preExp1(:,1);
preExpg1(:,2) = (2*gamma1_m)*preExp1(:,2);
preExpg2(:,1) = (2*gamma2_p)*preExp2(:,1);
preExpg2(:,2) = (2*gamma2_m)*preExp2(:,2);
preExpgg1(:,1) = (gamma1_p^2)*preExp1(:,1);
preExpgg1(:,2) = (gamma1_m^2)*preExp1(:,2);
preExpgg2(:,1) = (gamma2_p^2)*preExp2(:,1);
preExpgg2(:,2) = (gamma2_m^2)*preExp2(:,2);
preExpt1(:,1) = t1.*preExpgg1(:,1);
preExpt1(:,2) = t1.*preExpgg1(:,2);
preExpt2(:,1) = t2.*preExpgg2(:,1);
preExpt2(:,2) = t2.*preExpgg2(:,2);
[computeH{1}, computeUpsilonMatrix{1}, computeUpsilonMatrixLocal{1}] =  lfmComputeH3AA(gamma1_p, gamma1_m, sigma2, t1,t2,preFactors([1 2]), 0);
[computeH{2}, computeUpsilonMatrix{2}, computeUpsilonMatrixLocal{2}] =  lfmComputeH3AA(gamma2_p, gamma2_m, sigma2, t2,t1,preFactors([3 4]), 1);
[computeH{3}, computeUpsilonVector{1}, computeUpsilonVectorLocal{1}] =  lfmComputeH4AA(gamma1_p, gamma1_m, sigma2, t1, preGamma([1 2 4 3]), preExpgg2, 0 );
[computeH{4}, computeUpsilonVector{2}, computeUpsilonVectorLocal{2}] =  lfmComputeH4AA(gamma2_p, gamma2_m, sigma2, t2, preGamma([1 3 4 2]), preExpgg1, 1 );
preKernel = ( computeH{1} + computeH{2}.' + computeH{3} + computeH{4}.');

% Precompute derivatives
gradientUpsilonMatrix{1} = lfmaaGradientUpsilonMatrix(gamma1_p,sigma2,t1, t2, 0, computeUpsilonMatrixLocal{1}{1});
gradientUpsilonMatrix{2} = lfmaaGradientUpsilonMatrix(gamma1_m,sigma2,t1, t2, 0, computeUpsilonMatrixLocal{1}{2});
gradientUpsilonMatrix{3} = lfmaaGradientUpsilonMatrix(gamma2_p,sigma2,t2, t1, 1, computeUpsilonMatrixLocal{2}{1});
gradientUpsilonMatrix{4} = lfmaaGradientUpsilonMatrix(gamma2_m,sigma2,t2, t1, 1, computeUpsilonMatrixLocal{2}{2});
gradientUpsilonVector{1} = lfmapGradientUpsilonVector(gamma1_p,sigma2,t1, computeUpsilonVectorLocal{1}{1});
gradientUpsilonVector{2} = lfmapGradientUpsilonVector(gamma1_m,sigma2,t1, computeUpsilonVectorLocal{1}{2});
gradientUpsilonVector{3} = lfmapGradientUpsilonVector(gamma2_p,sigma2,t2, computeUpsilonVectorLocal{2}{1});
gradientUpsilonVector{4} = lfmapGradientUpsilonVector(gamma2_m,sigma2,t2, computeUpsilonVectorLocal{2}{2});

if lfmKern1.isNormalised
    K0 =  lfmKern1.sensitivity*lfmKern2.sensitivity/(8*sqrt(2)*lfmKern1.mass*lfmKern2.mass*prod(omega));
    K02 = 1/(8*sqrt(2)*prod(m)*prod(omega));
else
    K0 =  sigma*sqrt(pi)*lfmKern1.sensitivity*lfmKern2.sensitivity/(8*lfmKern1.mass*lfmKern2.mass*prod(omega));
    K02 = sigma*sqrt(pi)/(8*prod(m)*prod(omega));
end

% Gradient with respect to m, D and C
for ind_theta = 1:3 % Parameter (m, D or C)
    for ind_par = 0:1 % System (1 or 2)
        % Choosing the right gradients for m, omega, gamma1 and gamma2
        switch ind_theta
            case 1  % Gradient wrt m
                gradThetaM = [1-ind_par ind_par];
                gradThetaAlpha = -C./(2*(m.^2));
                gradThetaOmega = (C.^2-2*m.*D)./(2*(m.^2).*sqrt(4*m.*D-C.^2));
            case 2  % Gradient wrt D
                gradThetaM = zeros(1,2);
                gradThetaAlpha = zeros(1,2);
                gradThetaOmega = 1./sqrt(4*m.*D-C.^2);
            case 3  % Gradient wrt C
                gradThetaM = zeros(1,2);
                gradThetaAlpha = 1./(2*m);
                gradThetaOmega = -C./(2*m.*sqrt(4*m.*D-C.^2));
        end
        gradThetaGamma1 = gradThetaAlpha + j*gradThetaOmega;
        gradThetaGamma2 = gradThetaAlpha - j*gradThetaOmega;
        % Gradient evaluation
        gradThetaGamma11 = [gradThetaGamma1(1) gradThetaGamma2(1)];
        gradThetaGamma2 = [gradThetaGamma1(2) gradThetaGamma2(2)];
        gradThetaGamma1 = gradThetaGamma11;

        if ~ind_par % ind_par = k or d
            matGrad = K0 ...
                * ( lfmGradientH31( preFactors([1 2]), preFactors2([1 2]), gradThetaGamma1, ...
                gradientUpsilonMatrix{1}, gradientUpsilonMatrix{2}, computeUpsilonMatrix{1}{1}, ...
                computeUpsilonMatrix{1}{2}, 1) + ...
                lfmGradientH32( preGamma2,  gradThetaGamma1, computeUpsilonMatrix{2}{1}, ...
                computeUpsilonMatrix{2}{2}, 1).' + ...
                lfmGradientH41( preGamma, preGamma2, gradThetaGamma1, preExpgg2, ...
                gradientUpsilonVector{1}, gradientUpsilonVector{2}, computeUpsilonVector{1}{1},...
                computeUpsilonVector{1}{2}, 1) + ...
                lfmGradientH42AP(preGamma, preGamma2, gradThetaGamma1, preExpg1, preExpgg1, preExpt1, ...
                computeUpsilonVector{2}{1}, computeUpsilonVector{2}{2}).'...
                - (gradThetaM(1+ind_par)/m(1+ind_par) ...
                + gradThetaOmega(1+ind_par)/omega(1+ind_par)) ...
                *preKernel);
        else % ind_par = r or d'
            matGrad = K0 ...
                * ( lfmGradientH31( preFactors([3 4]), preFactors2([3 4]), gradThetaGamma2, ...
                gradientUpsilonMatrix{3}, gradientUpsilonMatrix{4}, computeUpsilonMatrix{2}{1}, ...
                computeUpsilonMatrix{2}{2}, 1).' + ...
                lfmGradientH32( preGamma2([1 3 2 4]),  gradThetaGamma2, computeUpsilonMatrix{1}{1}, ...
                computeUpsilonMatrix{1}{2}, 1) + ...
                lfmGradientH41( preGamma([1 3 2 4]), preGamma2([1 3 2 4]), gradThetaGamma2, preExpgg1, ...
                    gradientUpsilonVector{3}, gradientUpsilonVector{4}, computeUpsilonVector{2}{1},...
                    computeUpsilonVector{2}{2}, 1).' + ...
                lfmGradientH42AP(preGamma([1 3 2 4]), preGamma2([1 3 2 4]), gradThetaGamma2, preExpg2, preExpgg2, preExpt2, ...
                computeUpsilonVector{1}{1}, computeUpsilonVector{1}{2})...
                - (gradThetaM(1+ind_par)/m(1+ind_par) ...
                + gradThetaOmega(1+ind_par)/omega(1+ind_par)) ...
                *preKernel);
        end

        if subComponent
            if size(meanVector,1) ==1,
                matGrad = matGrad*meanVector;
            else
                matGrad = (meanVector*matGrad).';
            end
        end

        % Check the parameter to assign the derivative
        if ind_par == 0
            g1(ind_theta) = sum(sum(matGrad.*covGrad));
        else
            g2(ind_theta) = sum(sum(matGrad.*covGrad));
        end
    end
end

% Gradients with respect to sigma
if lfmKern1.isNormalised
    matGrad = K0*(lfmGradientSigmaH3AA(gamma1_p, gamma1_m, sigma2, t1, t2, preFactors([1 2]), 0)...
        +  lfmGradientSigmaH3AA(gamma2_p, gamma2_m, sigma2, t2, t1, preFactors([3 4]), 1).'...
        +  lfmGradientSigmaH4AA(gamma1_p, gamma1_m, sigma2, t1, preGamma([1 2 4 3]), preExpgg2)...
        +  lfmGradientSigmaH4AA(gamma2_p, gamma2_m, sigma2, t2, preGamma([1 3 4 2]), preExpgg1).');
else
    matGrad = (prod(S)*sqrt(pi)/(8*prod(m)*prod(omega))) ...
        * (preKernel ...
        + sigma*(lfmGradientSigmaH3AA(gamma1_p, gamma1_m, sigma2, t1, t2, preFactors([1 2]), 0)...
        +  lfmGradientSigmaH3AA(gamma2_p, gamma2_m, sigma2, t2, t1, preFactors([3 4]), 1).'...
        +  lfmGradientSigmaH4AA(gamma1_p, gamma1_m, sigma2, t1, preGamma([1 2 4 3]), preExpgg2)...
        +  lfmGradientSigmaH4AA(gamma2_p, gamma2_m, sigma2, t2, preGamma([1 3 4 2]), preExpgg1).' ));
end

if subComponent
    if size(meanVector,1) ==1,
        matGrad = matGrad*meanVector;
    else
        matGrad = (meanVector*matGrad).';
    end
end

g1(4) = sum(sum(matGrad.*covGrad))*(-(sigma^3)/4);
g2(4) = g1(4);

% Gradients with respect to S

matGrad = K02*preKernel;

if subComponent
    if size(meanVector,1) ==1,
        matGrad = matGrad*meanVector;
    else
        matGrad = (meanVector*matGrad).';
    end
end

g1(5) = sum(sum(S(2)*matGrad.*covGrad));
g2(5) = sum(sum(S(1)*matGrad.*covGrad));

g2(4) = 0; % Otherwise is counted twice, temporarly changed by Mauricio Alvarez

g1 = real(g1);
g2 = real(g2);


function [positions, time] = tracker_affine(video_path, img_files, pos, target_sz, ...
	padding, kernel, lambda, output_sigma_factor, interp_factor, cell_size, ...
	features, show_visualization)
%TRACKER Kernelized/Dual Correlation Filter (KCF/DCF) tracking.
%   This function implements the pipeline for tracking with the KCF (by
%   choosing a non-linear kernel) and DCF (by choosing a linear kernel).
%
%   It is meant to be called by the interface function RUN_TRACKER, which
%   sets up the parameters and loads the video information.
%
%   Parameters:
%     VIDEO_PATH is the location of the image files (must end with a slash
%      '/' or '\').
%     IMG_FILES is a cell array of image file names.
%     POS and TARGET_SZ are the initial position and size of the target
%      (both in format [rows, columns]).
%     PADDING is the additional tracked region, for context, relative to
%      the target size.
%     KERNEL is a struct describing the kernel. The field TYPE must be one
%      of 'gaussian', 'polynomial' or 'linear'. The optional fields SIGMA,
%      POLY_A and POLY_B are the parameters for the Gaussian and Polynomial
%      kernels.
%     OUTPUT_SIGMA_FACTOR is the spatial bandwidth of the regression
%      target, relative to the target size.
%     INTERP_FACTOR is the adaptation rate of the tracker.
%     CELL_SIZE is the number of pixels per cell (must be 1 if using raw
%      pixels).
%     FEATURES is a struct describing the used features (see GET_FEATURES).
%     SHOW_VISUALIZATION will show an interactive video if set to true.
%
%   Outputs:
%    POSITIONS is an Nx2 matrix of target positions over time (in the
%     format [rows, columns]).
%    TIME is the tracker execution time, without video loading/rendering.
%
%   Joao F. Henriques, 2014

	%if the target is large, lower the resolution, we don't need that much
	%detail
	resize_image = (sqrt(prod(target_sz)) >= 100);  %diagonal size >= threshold
	if resize_image,
		pos = floor(pos / 2);
		target_sz = floor(target_sz / 2);
	end

	%window size, taking padding into account
	window_sz = floor(target_sz * (1 + padding));

% 	%we could choose a size that is a power of two, for better FFT
% 	%performance. in practice it is slower, due to the larger window size.
% 	window_sz = 2 .^ nextpow2(window_sz);

	%create regression labels, gaussian shaped, with a bandwidth
	%proportional to target size
	output_sigma = sqrt(prod(target_sz)) * output_sigma_factor / cell_size;
	yf = fft2(gaussian_shaped_labels(output_sigma, floor(window_sz / cell_size)));

	%store pre-computed cosine window
	cos_window = hann(size(yf,1)) * hann(size(yf,2))';

	if show_visualization,  %create video interface
		update_visualization = show_video(img_files, video_path, resize_image);
	end

	%note: variables ending with 'f' are in the Fourier domain.

	time = 0;  %to calculate FPS
	positions = zeros(numel(img_files), 2);  %to calculate precision

	for frame = 1:numel(img_files),
		%load image
		im = imread([video_path img_files{frame}]);
		if size(im,3) > 1,
			im = rgb2gray(im);
		end
		if resize_image,
			im = imresize(im, 0.5);
		end

		tic()

		if frame == 1
			camera_feat.detector = cv.FeatureDetector('SURF');
			camera_feat.extractor = cv.DescriptorExtractor('SURF');
			camera_feat.matcher = cv.DescriptorMatcher('FlannBased');
			camera_feat.last_keypoints = camera_feat.detector.detect(im);
			camera_feat.last_descriptors = camera_feat.extractor.compute(im, camera_feat.last_keypoints);
		end

		if frame > 1,
			[camera_feat, camera_H] = find_affine(camera_feat, im, pos, target_sz);
			pos = project_t(pos([2, 1]), camera_H);
            pos = pos([2, 1]);
            pos = min([size(im,1),size(im,2)], pos);
            pos = max([0,0], pos);
			%obtain a subwindow for detection at the position from last
			%frame, and convert to Fourier domain (its size is unchanged)
			patch = get_subwindow(im, pos, window_sz);
			zf = fft2(get_features(patch, features, cell_size, cos_window));

			%calculate response of the classifier at all shifts
			switch kernel.type
			case 'gaussian',
				kzf = gaussian_correlation(zf, model_xf, kernel.sigma);
			case 'polynomial',
				kzf = polynomial_correlation(zf, model_xf, kernel.poly_a, kernel.poly_b);
			case 'linear',
				kzf = linear_correlation(zf, model_xf);
			end
			response = real(ifft2(model_alphaf .* kzf));  %equation for fast detection

			%target location is at the maximum response. we must take into
			%account the fact that, if the target doesn't move, the peak
			%will appear at the top-left corner, not at the center (this is
			%discussed in the paper). the responses wrap around cyclically.
			[vert_delta, horiz_delta] = find(response == max(response(:)), 1);
			if vert_delta > size(zf,1) / 2,  %wrap around to negative half-space of vertical axis
				vert_delta = vert_delta - size(zf,1);
			end
			if horiz_delta > size(zf,2) / 2,  %same for horizontal axis
				horiz_delta = horiz_delta - size(zf,2);
			end
			pos = pos + cell_size * [vert_delta - 1, horiz_delta - 1];
		end

		%obtain a subwindow for training at newly estimated target position
		patch = get_subwindow(im, pos, window_sz);
		xf = fft2(get_features(patch, features, cell_size, cos_window));

		%Kernel Ridge Regression, calculate alphas (in Fourier domain)
		switch kernel.type
		case 'gaussian',
			kf = gaussian_correlation(xf, xf, kernel.sigma);
		case 'polynomial',
			kf = polynomial_correlation(xf, xf, kernel.poly_a, kernel.poly_b);
		case 'linear',
			kf = linear_correlation(xf, xf);
		end
		alphaf = yf ./ (kf + lambda);   %equation for fast training

		if frame == 1,  %first frame, train with a single image
			model_alphaf = alphaf;
			model_xf = xf;
		else
			%subsequent frames, interpolate model
			model_alphaf = (1 - interp_factor) * model_alphaf + interp_factor * alphaf;
			model_xf = (1 - interp_factor) * model_xf + interp_factor * xf;
		end

		%save position and timing
		positions(frame,:) = pos;
		time = time + toc();

		%visualization
		if show_visualization,
			box = [pos([2,1]) - target_sz([2,1])/2, target_sz([2,1])];
			stop = update_visualization(frame, box);
			if stop, break, end  %user pressed Esc, stop early

			drawnow
% 			pause(0.05)  %uncomment to run slower
		end

	end

	if resize_image,
		positions = positions * 2;
	end
end


function y = cot(x)
%COT          Implements  cot(x)  for intervals
%
%   y = cot(x)
%
%interval standard function implementation
%

% written  12/30/98     S.M. Rump
% modified 09/13/99     S.M. Rump  complex allowed, sparse input, NaN input,
%                                  major revision, improved accuracy
% modified 09/02/00     S.M. Rump  rounding unchanged after use
% modified 04/04/04     S.M. Rump  set round to nearest for safety
%                                    accelaration for sparse input
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 12/04/05     S.M. Rump  extreme values for approximate part
% modified 09/06/07     S.M. Rump  approximate std fcts removed
% modified 09/28/08     S.M. Rump  check for rounding to nearest improved
% modified 10/20/08     S.M. Rump  check for zero omitted
% modified 10/18/08     S.M. Rump  StdFctsException ignore/NaN
% modified 08/26/12     S.M. Rump  global variables removed
% modified 10/13/12     S.M. Rump  INTLAB_INTVAL_STDFCTS
%

  e = 1e-30;
  if 1+e==1-e                           % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  if x.complex
    if issparse(x.mid)
      x.mid = full(x.mid);
      x.rad = full(x.rad);
    end
    y = cos(x)./sin(x);
    if rndold
      setround(rndold)
    end
    return
  end

  INTLAB_STDFCTS_PI = getappdata(0,'INTLAB_STDFCTS_PI');
  if issparse(x.inf)
    x.inf = full(x.inf);
    x.sup = full(x.sup);
  end

  y = x;

  % transform x.inf and x.sup mod pi/2
  [ xinfinf , xinfsup , Sinf ] = modpi2(x.inf);
  [ xsupinf , xsupsup , Ssup ] = modpi2(x.sup);

  % indices with result +/- inf
  setround(1)
  delta = x.sup-x.inf;
  Tinf = Sinf + 2;
  Tinf(Tinf>7) = Tinf(Tinf>7) - 8;
  Tsup = Ssup + 2;
  Tsup(Tsup>7) = Tsup(Tsup>7) - 8;
  indexinf = ( delta >= INTLAB_STDFCTS_PI.PIINF ) | ...
    ( floor(Tinf/4) ~= floor(Tsup/4) ) | ...
    ( ( x.inf<=0 ) & ( x.sup>=0 ) );
  Sinf(Sinf>3) = Sinf(Sinf>3) - 4;
  Ssup(Ssup>3) = Ssup(Ssup>3) - 4;

  % transformation of input arguments by modpi2:
  %   [ xinf,xsup,s ] = modpi2(y)  ==>  0 <= s <= 7, 0 <= x <= pi/4 and
  %   x = -pi/2 + y + s*pi/4 + 2k*pi        for s even
  %   x =       - y + (s-1)*pi/4 + 2k*pi    for s odd

  y.inf(:) = -inf;
  y.sup(:) = inf;

  % treat non-infinity intervals
  Sinf(indexinf) = -1;
  Ssup(indexinf) = -1;

  % save warning status
  wng = warning;
  warning off

  % treat infimum
  index = ( Ssup==0 );
  if any(index(:))
    y.inf(index) = - tan_pos(xsupsup(index),1);
  end
  index = ( Ssup==1 );
  if any(index(:))
    y.inf(index) = 1 ./ ( - tan_pos(xsupinf(index),-1) );
  end
  index = ( Ssup==2 );
  if any(index(:))
    res = tan_pos(xsupsup(index),1);
    setround(-1)
    y.inf(index) = 1 ./ res;
  end
  index = ( Ssup==3 );
  if any(index(:))
    y.inf(index) = tan_pos(xsupinf(index),-1);
  end

  % treat supremum
  index = ( Sinf==0 );
  if any(index(:))
    y.sup(index) = - tan_pos(xinfinf(index),-1);
  end
  index = ( Sinf==1 );
  if any(index(:))
    y.sup(index) = 1 ./ ( - tan_pos(xinfsup(index),1) );
  end
  index = ( Sinf==2 );
  if any(index(:))
    res = tan_pos(xinfinf(index),-1);
    setround(1)
    y.sup(index) = 1 ./ res;
  end
  index = ( Sinf==3 );
  if any(index(:))
    y.sup(index) = tan_pos(xinfsup(index),1);
  end

  % restore warning status
  warning(wng);
  setround(0)                        % set rounding to nearest

  index = isnan(x.inf);
  if any(index(:))
    y.inf(index) = NaN;
    y.sup(index) = NaN;
  end
  INTLAB_STDFCTS_EXCPTN = getappdata(0,'INTLAB_STDFCTS_EXCPTN');
  if INTLAB_STDFCTS_EXCPTN==3      % ignore input out of range (ignore-mode)
    index = ( x.inf==0 ) & ( x.sup==0 );
    if ~isempty(find(index))              % completely exceptional arguments to NaN
      setappdata(0,'INTLAB_STDFCTS_EXCPTN_',1);
      y.inf(index) = NaN;
      y.sup(index) = NaN;
    end
  elseif INTLAB_STDFCTS_EXCPTN==2  % any input out of range to NaN (NaN-mode)
    index = ( x.inf<=0 ) & ( 0<=x.sup );
    if ~isempty(find(index))              % exceptional arguments to NaN
      setappdata(0,'INTLAB_STDFCTS_EXCPTN_',1);
      y.inf(index) = NaN;
      y.sup(index) = NaN;
    end
  end

  setround(rndold)


function Offspring = OperatorGA(Problem,Parent,Parameter)
%OperatorGA - Crossover and mutation operators of genetic algorithm.
%
%   Off = OperatorGA(Pro,P) uses genetic operators to generate offsprings
%   for problem Pro based on parents P. If P is an array of SOLUTION
%   objects, then Off is also an array of SOLUTION objects. While if P is a
%   matrix of decision variables, then Off is also a matrix of decision
%   variables, i.e., the offsprings are not evaluated. P is split into two
%   subsets P1 and P2 with the same size, where each object or row of P1
%   and P2 is used to generate two offsprings. Different operators are used
%   for different encoding schemes.
%
%   Off = OperatorGA(Pro,P,{proC,disC,proM,disM}) specifies the parameters
%   of operators, where proC is the probability of crossover, disC is the
%   distribution index of simulated binary crossover, proM is the
%   expectation of the number of mutated variables, and disM is the
%   distribution index of polynomial mutation.
%
%   Example:
%       Offspring = OperatorGA(Problem,Parent)
%       Offspring = OperatorGA(Problem,Parent.decs,{1,20,1,20})
%
%   See also OperatorGAhalf

%------------------------------- Reference --------------------------------
% [1] K. Deb, K. Sindhya, and T. Okabe, Self-adaptive simulated binary
% crossover for real-parameter optimization, Proceedings of the Annual
% Conference on Genetic and Evolutionary Computation, 2007, 1187-1194.
% [2] K. Deb and M. Goyal, A combined genetic adaptive search (GeneAS) for
% engineering design, Computer Science and informatics, 1996, 26: 30-45.
% [3] L. Davis, Applying adaptive algorithms to epistatic domains,
% Proceedings of the International Joint Conference on Artificial
% Intelligence, 1985, 162-164.
% [4] D. B. Fogel, An evolutionary approach to the traveling salesman
% problem, Biological Cybernetics, 1988, 60(2): 139-144.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    if nargin > 2
        [proC,disC,proM,disM] = deal(Parameter{:});
    else
        [proC,disC,proM,disM] = deal(1,20,1,20);
    end
    if isa(Parent(1),'SOLUTION')
        evaluated = true;
        Parent    = Parent.decs;
    else
        evaluated = false;
    end
    Parent1   = Parent(1:floor(end/2),:);
    Parent2   = Parent(floor(end/2)+1:floor(end/2)*2,:);
    Offspring = zeros(2*size(Parent1,1),size(Parent1,2));
    Type      = arrayfun(@(i)find(Problem.encoding==i),1:5,'UniformOutput',false);
    if ~isempty([Type{1:2}])    % Real and integer variables
        Offspring(:,[Type{1:2}]) = GAreal(Parent1(:,[Type{1:2}]),Parent2(:,[Type{1:2}]),Problem.lower([Type{1:2}]),Problem.upper([Type{1:2}]),proC,disC,proM*length([Type{1:2}])/size(Parent1,2),disM);
    end
    if ~isempty(Type{3})        % Label variables
        Offspring(:,Type{3}) = GAlabel(Parent1(:,Type{3}),Parent2(:,Type{3}),Problem.lower(Type{3}),Problem.upper(Type{3}),proC,proM*length(Type{3})/size(Parent1,2));
    end
    if ~isempty(Type{4})        % Binary variables
        Offspring(:,Type{4}) = GAbinary(Parent1(:,Type{4}),Parent2(:,Type{4}),proC,proM*length(Type{4})/size(Parent1,2));
    end
    if ~isempty(Type{5})        % Permutation variables
        Offspring(:,Type{5}) = GApermutation(Parent1(:,Type{5}),Parent2(:,Type{5}),proC);
    end
    if evaluated
        Offspring = Problem.Evaluation(Offspring);
    end
end

function Offspring = GAreal(Parent1,Parent2,lower,upper,proC,disC,proM,disM)
% Genetic operators for real and integer variables

    %% Simulated binary crossover
    [N,D] = size(Parent1);
    beta  = zeros(N,D);
    mu    = rand(N,D);
    beta(mu<=0.5) = (2*mu(mu<=0.5)).^(1/(disC+1));
    beta(mu>0.5)  = (2-2*mu(mu>0.5)).^(-1/(disC+1));
    beta = beta.*(-1).^randi([0,1],N,D);
    beta(rand(N,D)<0.5) = 1;
    beta(repmat(rand(N,1)>proC,1,D)) = 1;
    Offspring = [(Parent1+Parent2)/2+beta.*(Parent1-Parent2)/2
                 (Parent1+Parent2)/2-beta.*(Parent1-Parent2)/2];

    %% Polynomial mutation
    Lower = repmat(lower,2*N,1);
    Upper = repmat(upper,2*N,1);
    Site  = rand(2*N,D) < proM/D;
    mu    = rand(2*N,D);
    temp  = Site & mu<=0.5;
    Offspring       = min(max(Offspring,Lower),Upper);
    Offspring(temp) = Offspring(temp)+(Upper(temp)-Lower(temp)).*((2.*mu(temp)+(1-2.*mu(temp)).*...
                      (1-(Offspring(temp)-Lower(temp))./(Upper(temp)-Lower(temp))).^(disM+1)).^(1/(disM+1))-1);
    temp = Site & mu>0.5;
    Offspring(temp) = Offspring(temp)+(Upper(temp)-Lower(temp)).*(1-(2.*(1-mu(temp))+2.*(mu(temp)-0.5).*...
                      (1-(Upper(temp)-Offspring(temp))./(Upper(temp)-Lower(temp))).^(disM+1)).^(1/(disM+1)));
end

function Offspring = GAlabel(Parent1,Parent2,lower,upper,proC,proM)
% Genetic operators for label variables

    %% Uniform crossover
    [N,D] = size(Parent1);
    k     = rand(N,D) < 0.5;
    k(repmat(rand(N,1)>proC,1,D)) = false;
    Offspring1    = Parent1;
    Offspring2    = Parent2;
    Offspring1(k) = Parent2(k);
    Offspring2(k) = Parent1(k);
    Offspring     = [Offspring1;Offspring2];

    %% Bitwise mutation
    Site = rand(2*N,D) < proM/D;
    Rand = round(unifrnd(repmat(lower,2*N,1),repmat(upper,2*N,1)));
    Offspring(Site) = Rand(Site);
end

function Offspring = GAbinary(Parent1,Parent2,proC,proM)
% Genetic operators for binary variables

    %% Uniform crossover
    [N,D] = size(Parent1);
    k     = rand(N,D) < 0.5;
    k(repmat(rand(N,1)>proC,1,D)) = false;
    Offspring1    = Parent1;
    Offspring2    = Parent2;
    Offspring1(k) = Parent2(k);
    Offspring2(k) = Parent1(k);
    Offspring     = [Offspring1;Offspring2];

    %% Bit-flip mutation
    Site = rand(2*N,D) < proM/D;
    Offspring(Site) = ~Offspring(Site);
end

function Offspring = GApermutation(Parent1,Parent2,proC)
% Genetic operators for permutation variables

    %% Order crossover
    [N,D]     = size(Parent1);
    Offspring = [Parent1;Parent2];
    k = randi(D,1,2*N);
    for i = 1 : N
        if rand < proC
            Offspring(i,k(i)+1:end)   = setdiff(Parent2(i,:),Parent1(i,1:k(i)),'stable');
            Offspring(i+N,k(i)+1:end) = setdiff(Parent1(i,:),Parent2(i,1:k(i)),'stable');
        end
    end

    %% Slight mutation
    k = randi(D,1,2*N);
    s = randi(D,1,2*N);
    for i = 1 : 2*N
        if s(i) < k(i)
            Offspring(i,:) = Offspring(i,[1:s(i)-1,k(i),s(i):k(i)-1,k(i)+1:end]);
        elseif s(i) > k(i)
            Offspring(i,:) = Offspring(i,[1:k(i)-1,k(i)+1:s(i)-1,k(i),s(i):end]);
        end
    end
end


function mpc = order_radial(mpc)
%ORDER_RADIAL  Performs oriented ordering to buses and branches.
%
%   mpc = order_radial(mpc)
%
%   orders the branches by the the principle of oriented ordering:
%   indicies of sending nodes are smaller than the indicies of the
%   receiving nodes. The branch index is equal to the index of their
%   receiving node. The ordering is taken from:
%   D. Rajicic, R. Ackovski and R. Taleski, "Voltage correction power flow,"
%   IEEE Transactions on Power Delivery, vol. 9, no. 2, pp. 1056-1062, Apr 1994.
%
%   See also RADIAL_PF.

%% define named indices into bus, gen, branch matrices
define_constants;
%% Initialize
slack = mpc.bus(mpc.bus(:,BUS_TYPE) == REF, 1);
[f, t] = deal(mpc.branch(:,F_BUS),mpc.branch(:,T_BUS));
nl = size(mpc.branch,1);
branch_number = (1:nl)';
mpc.branch_order = [];
mpc.loop = [];
mpc.bus_order = slack;
%% Bus and branch ordering
iter = 1;
while ~isempty(f) && iter <= nl
    % For each of the "from" buses that are in bus_order,
    % add the corresponding "to" buses to it. If both "from" and "to" are
    % in bus_order, the branch is forming loop. Add corresponding branch
    % numbers to branch_order or to loop.
    mf = ismember(f,mpc.bus_order);
    mt = ismember(t,mpc.bus_order);
    is_loop = mf & mt;
    % Add branch to loop and delete it from the list
    if any(is_loop)
        mpc.loop = [mpc.loop; branch_number(is_loop)];
        mf(is_loop) = [];
%         mt(is_loop) = [];
        f(is_loop) = [];
        t(is_loop) = [];
        branch_number(is_loop) = [];
    end
    % Add unique buses to bus_order
    if any(mf)
        u = unique(t(mf)); % unique buses
        [junk,i] = intersect(t.*mf,u); % first occurence of unique buses in t
        mpc.bus_order = [mpc.bus_order; t(i)];
        % Add branch to branch_order and delete it from the list
        mpc.branch_order = [mpc.branch_order; branch_number(i)];
        mf(i) = [];
%         mt(i) = [];
        f(i) = [];
        t(i) = [];
        branch_number(i) = [];
    end
    % Add any remaining branch to loop and delete it from the list
    if any(mf)
        mpc.loop = [mpc.loop; branch_number(mf)];
        f(mf) = [];
        t(mf) = [];
        branch_number(mf) = [];
    end
    % For each of the "to" buses that are in bus_order,
    % add the corresponding "from" buses to it. If both "from" and "to" are
    % in bus_order, the branch is forming loop. Add corresponding branch
    % numbers to branch_order or to loop.
    mf = ismember(f,mpc.bus_order);
    mt = ismember(t,mpc.bus_order);
    is_loop = mf & mt;
    % Add branch to loop and delete it from the list
    if any(is_loop)
        mpc.loop = [mpc.loop; branch_number(is_loop)];
        mt(is_loop) = [];
%         mf(is_loop) = [];
        f(is_loop) = [];
        t(is_loop) = [];
        branch_number(is_loop) = [];
    end
    % Add unique buses to bus_order
    if any(mt)
        u = unique(f(mt)); % unique buses
        [junk,i] = intersect(f.*mt,u); % first occurence of unique buses in f
        mpc.bus_order = [mpc.bus_order; f(i)];
        % Add branch to branch_order and delete it from the list
        mpc.branch_order = [mpc.branch_order; branch_number(i)];
%         mf(i) = [];
        mt(i) = [];
        f(i) = [];
        t(i) = [];
        branch_number(i) = [];
    end
    % Add any remaining branch to loop and delete it from the list
    if any(mt)
        mpc.loop = [mpc.loop; branch_number(mt)];
        f(mt) = [];
        t(mt) = [];
        branch_number(mt) = [];
    end
    iter = iter + 1;
end
if ~isempty(f)
    mpc.not_connected = branch_number;
else
    mpc.not_connected = [];
end
%% Reorder bus, branch and gen.
if isempty(mpc.loop)
    % Permute rows in branch
    mpc.branch = mpc.branch(mpc.branch_order,:);
    % Make an "inverse" vector out of bus_order
    mpc.bus_order_inv = sparse(mpc.bus_order,ones(nl+1,1),1:nl+1);
    % Swap indicies in "from" and "to" buses using bus_order_inv
    [f, t] = deal(mpc.branch(:,F_BUS),mpc.branch(:,T_BUS));
    f = mpc.bus_order_inv(f);
    t = mpc.bus_order_inv(t);
    % Reverse branch orientation of "from" is biger than "to"
    mpc.br_reverse = f > t;
    [f(mpc.br_reverse), t(mpc.br_reverse)] = deal(t(mpc.br_reverse), f(mpc.br_reverse));
    % Put new "from" and "to" indicies in branch
    mpc.branch(:,[F_BUS T_BUS]) = [f t];
    % Make an "inverse" vector out of branch_order
    mpc.branch_order_inv = sparse(mpc.branch_order,ones(nl,1),1:nl);
    % Permute rows in bus and replace bus indicies
    mpc.bus = mpc.bus(mpc.bus_order,:);
    mpc.bus(:,BUS_I) = mpc.bus_order_inv(mpc.bus(:,BUS_I));
    % Replace bus indicies in gen
    mpc.gen(:,GEN_BUS) = mpc.bus_order_inv(mpc.gen(:,GEN_BUS));
end


function gf=gradloglikGaP(x, varargin)
% f=loglikGaP(x, varargin)
% complete log likellihood of the GaP model
% Vxt = varargin{1};      %data
% sigpsf = varargin{2};  %std deviation of the PSF gaussian approx
% alpha = varargin{3}; %parameters of the Gamma prior on the blinking
% beta = varargin{4}; %parameters of the Gamma prior on the blinking
% peval = varargin{5}; %parameters
% x(1:end-2*peval.ncomp) is Hkt

Vxt = varargin{1};      %data
sigpsf = varargin{2};  %std deviation of the PSF gaussian approx
alpha = varargin{3}; %parameters of the Gamma prior on the blinking
beta = varargin{4}; %parameters of the Gamma prior on the blinking
peval = varargin{5}; %parameters

% % % Hkt_linear=x(1:end-peval.ncomp*2); % intensities
cx=x(end-peval.ncomp*2+1:end-peval.ncomp); %x-coordinates of the centers
cy=x(end-peval.ncomp+1:end); % y-coordinates of the centers
% % %
% % % Hkt_linear=x; % intensities
Hkt_linear=varargin{6};
% % % cy=varargin{7};
% % %

sigpsf_vec=repmat(sigpsf,peval.ncomp,1); %all psfs same sigma
cxy_vec=[cx'+1, cy'+1];
% cxy_vec=[cx', cy'];
a_vec=1./(sigpsf_vec.^2*2*pi); % all normalised to 1

Hkt=reshape(Hkt_linear, peval.ncomp, peval.nt);
% generate PSFs from given parameters:
Wxkpix=gauss2dmultislice([peval.nx, peval.ny, peval.ncomp], cxy_vec, sigpsf_vec, a_vec);
Wxkpix=normalizePSF(Wxkpix); %normalize PSFs to 1
Wxk=reshape(Wxkpix,peval.nx*peval.ny, peval.ncomp);
[Wxkbg,Hktbg]=addbg(Wxk, Hkt, peval.bg);
P=Wxkbg*Hktbg; %current approximation

%linear grasdient shifted by cx
xxvc = lineargrad([peval.nx, peval.ny, peval.ncomp], cx, 'xx');
yyvc = lineargrad([peval.nx, peval.ny, peval.ncomp], cy, 'yy');

% dW/dcx:
Wxtcx=1/sigpsf^2*xxvc.*Wxk;
% dW/dcy:
Wxtcy=1/sigpsf^2*yyvc.*Wxk;

% d(log(L))/dHkt:
% % % gfHkt=(alpha-1)*1./Hkt - 1/beta +
% Wxk'*(Vxt./P-eye(peval.nx*peval.ny,peval.nt));
gfHkt= Wxk'*(Vxt./P)-1; %without background (->not Wxkgb) and d(log(L)/dcx)
% d(log(L))/dcx:
gfcx=diag(Wxtcx'*(Vxt./P-ones(peval.nx*peval.ny, peval.nt))*Hkt');

% d(log(L))/dcy:
gfcy=diag(Wxtcy'*(Vxt./P-ones(peval.nx*peval.ny, peval.nt))*Hkt');

% % % gf = [reshape(gfHkt',1,peval.nt*peval.ncomp), gfcx', gfcy'];
%gf = [reshape(gfHkt,1,peval.nt*peval.ncomp)];
gf = [gfcx', gfcy'];
end
function Wnorm=normalizePSF(W)
sw=size(W);
Wr=reshape(W, sw(1)*sw(2),sw(3));
q=squeeze(sum(Wr,1));
Wrnorm=Wr./repmat(q,sw(1)*sw(2),1);
Wnorm=reshape(Wrnorm,sw(1), sw(2), sw(3));
end
function xxvc = lineargrad(sizevec, cx, dir)
switch dir
    case 'xx'
%         xxp=double(xx(sizevec, 'true')); %linear function - pixels
        xxp=double(xx(sizevec, 'corner')); %linear function - pixels
    case 'yy'
%         xxp=double(yy(sizevec, 'true')); %linear function - pixels
        xxp=double(yy(sizevec, 'corner')); %linear function - pixels
    otherwise
        error('Wrong dir')
end

xxv=reshape(xxp,sizevec(1)*sizevec(2),sizevec(3)); %linear function - vector
xxvc=xxv-repmat(cx,sizevec(1)*sizevec(2),1);
% xxp=double(xx([peval.nx, peval.ny, peval.ncomp], 'true')); %linear function - pixels
%yyp=double(yy([peval.nx, peval.ny, peval.ncomp], 'true'));
% xxv=reshape(xxp,peval.nx*peval.ny,peval.ncomp); %linear function - vector
%yyv=reshape(yyp,peval.nx*peval.ny,peval.ncomp);
% xxvc=xxv-repmat(cx,peval.ncomp,1);
%yyvc=yyv-repmat(cy,peval.ncomp,1);
end


function [ alphas, betas, scaling, finalLikelihood] = CCRF_training_gradient_descent( nIterations, nExamples, learningRate, threshold, x, y, yUnnormed, masks, alphas, betas, lambda_a, lambda_b, similarityFNs, useIndicators, verbose)
%GRADIENTDESCENTCCRF Performs CCRF gradient descen given the initial state
%and gradient descent parameters
%   Detailed explanation goes here

    if(verbose)
        logLikelihood = zeros(round(nIterations/10)+1, 1);
        alphaTrack = zeros(nIterations, numel(alphas));
        betaTrack = zeros(nIterations, numel(betas));
    end

    logAlphas = log(alphas);
    logBetas = log(betas);

    K = numel(similarityFNs);

    %calculate similarity measures for each of the sequences
    Similarities = cell(nExamples, 1);
    PrecalcQ2s = cell(nExamples,1);
    PrecalcQ2sFlat = cell(nExamples,1);

    PrecalcYqDs = zeros(nExamples, K);

    for q = 1 : nExamples

        yq = y{q};
        xq = x{q};
        mask = masks{q};

        n = size(yq, 1);
        Similarities{q} = zeros([n, n, K]);
%         PrecalcQ2s{q} = zeros([n, n, K]);
        PrecalcQ2s{q} = cell(K,1);
%         PrecalcQ2sFlat{q} = cell(K,1);
        PrecalcQ2sFlat{q} = zeros((n*(n+1))/2,K);
        % go over all of the similarity metrics and construct the
        % similarity matrices
        for k=1:K
            Similarities{q}(:,:,k) = similarityFNs{k}(xq, mask);
            S = Similarities{q}(:,:,k);
            D =  diag(sum(S));
            B = D - S;
%             PrecalcQ2s{q}(:,:,k) = B;
            PrecalcQ2s{q}{k} = B;
%             PrecalcQ2sFlat{q}{k} = PrecalcQ2s{q}{k}(logical(tril(ones(size(S)))));
            PrecalcQ2sFlat{q}(:,k) = B(logical(tril(ones(size(S)))));
            PrecalcYqDs(q,k) = -yq'*B*yq;
        end
    end

    %stochastic gradient descent
    for iter = 1 : nIterations
        prevAlphas = alphas;
        prevBetas = betas;

        for q = 1 : nExamples

            yq = y{q};
            xq = x{q};
            mask = masks{q};

            PrecalcQ2 = PrecalcQ2s{q};
            PrecalcQ2Flat = PrecalcQ2sFlat{q};
            [ logGradientsAlphas, logGradientsBetas] = gradientCCRF(alphas, betas, lambda_a, lambda_b, PrecalcQ2, xq, yq, mask, PrecalcYqDs(q, :), useIndicators, PrecalcQ2Flat);

%             [logGradientAlphasAnalytical, logGradientBetasAnalytical] = gradientAnalytical(PrecalcQ2, alphas, betas, lambda, xq, yq, mask);
%
%             diffInGradientsAlpha = mean(abs(logGradientsAlphas - logGradientAlphasAnalytical));
%             diffInGradientsBeta = mean(abs(logGradientsBetas - logGradientBetasAnalytical));

            %update log alpha
            logAlphas = logAlphas + learningRate * logGradientsAlphas;
            alphas = exp(logAlphas);

            %update log beta
            logBetas = logBetas + learningRate * logGradientsBetas;
            betas = exp(logBetas);

            if(verbose)
                %record alpha and beta values for each iteration for debug purposes
                alphaTrack(iter,:) = alphas(:);
                betaTrack(iter,:) = betas;
            end
        end

        %check for convergence
        if (norm([prevAlphas;prevBetas] - [alphas;betas])/norm([prevAlphas;prevBetas]) < threshold || norm([logGradientsAlphas;logGradientsBetas]) < threshold)
            break;
        end

        if(verbose)
            if(mod(iter, 10)==0)
                logLikelihood(iter/10 + 1) = LogLikelihoodCCRF(y, x, masks, alphas, betas, lambda_a, lambda_b, PrecalcQ2sFlat, useIndicators);
                fprintf('Iteration %d; logL %f\n', iter, logLikelihood(iter/10 + 1));
            end

        end
    end

    % establish the scaling
    scaling = getScaling(alphas, betas, x, yUnnormed, masks, PrecalcQ2s, useIndicators);

    if(verbose)
        figure
        subplot(1,3,1)
        plot(betaTrack(1:iter,:));
        title('beta');
        subplot(1,3,2)
        plot(alphaTrack(1:iter,:))
        title('alpha');
        subplot(1,3,3)
        plot(logLikelihood(1:round(iter/10),:))
        title('log likelihood');
        finalLikelihood = LogLikelihoodCCRF(y, x, masks, alphas, betas, lambda_a, lambda_b, PrecalcQ2sFlat, useIndicators);
        fprintf('Final log likelihood at iteration %d; logL %f, learning rate %f\n', iter, finalLikelihood, learningRate);
    else
        finalLikelihood = LogLikelihoodCCRF(y, x, masks, alphas, betas, lambda_a, lambda_b, PrecalcQ2sFlat, useIndicators);
        fprintf('Final log likelihood at iteration %d; logL %f, learning rate %f\n', iter, finalLikelihood, learningRate);
    end

end


function [ra, dec] = apstar (tjd, n, rai, deci, pmra, pmdec, parlax, radvel)

% apparent place of a star

%  tjd    = tt julian date for apparent place (in)

%  n      = body identification number for the earth (in) (no longer used)

%  rai    = icrs right ascension in hours (in)

%  deci   = icrs declination in degrees (in)

%  pmra   = icrs proper motion in ra in milliarcseconds/year (in)

%  pmdec  = icrs proper motion in dec in milliarcseconds/year (in)

%  parlax = parallax in milliarcseconds (in)

%  radvel = radial velocity in kilometers/second (in)

%  ra     = apparent right ascension in hours (out)

%  dec    = apparent declination in degrees (out)

% note: coordinate system for output ra and dec is equator and equinox of date

% ported from NOVAS 3.1

%%%%%%%%%%%%%%%%%%%%%%%

locatn = 0;

icoord = 1;

ttjd = tjd;

object = 'star';

star(1) = rai;

star(2) = deci;

star(3) = pmra;

star(4) = pmdec;

star(5) = parlax;

star(6) = radvel;

observ = zeros(6, 1);

skypos = place (ttjd, object, locatn, icoord, star, observ);

ra = skypos(4);

dec = skypos(5);


function [ abd, ipvt, info ] = sgbfa ( abd, lda, n, ml, mu )

%*****************************************************************************80
%
%% SGBFA factors a real band matrix by elimination.
%
%  Discussion:
%
%    SGBFA is usually called by SGBCO, but it can be called
%    directly with a saving in time if RCOND is not needed.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 November 2006
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Dongarra, Moler, Bunch and Stewart,
%    LINPACK User's Guide,
%    SIAM, (Society for Industrial and Applied Mathematics),
%    3600 University City Science Center,
%    Philadelphia, PA, 19104-2688.
%    ISBN 0-89871-172-X
%
%  Parameters:
%
%    Input, real ABD(LDA,N), the matrix in band
%    storage.  The columns of the matrix are stored in the columns of ABD
%    and the diagonals of the matrix are stored in rows ML+1 through
%    2*ML+MU+1 of ABD.
%
%    Input, integer LDA, the leading dimension of the array ABD.
%    2*ML + MU + 1 <= LDA is required.
%
%    Input, integer N, the order of the matrix.
%
%    Input, integer ML, MU, the number of diagonals below and above the
%    main diagonal.  0 <= ML < N, 0 <= MU < N.
%
%    Output, real ABD(LDA,N), an upper triangular matrix in band storage
%    and the multipliers which were used to obtain it.  The factorization
%    can be written A = L*U where L is a product of permutation and unit lower
%    triangular matrices and U is upper triangular.
%
%    Output, integer IPVT(N), the pivot indices.
%
%    Output, integer INFO, error flag.
%    0, normal value.
%    K, if U(K,K) == 0.0D+00.  This is not an error condition for this
%      subroutine, but it does indicate that SGBSL will divide by zero if
%      called.  Use RCOND in SGBCO for a reliable indication of singularity.
%
  m = ml + mu + 1;
  info = 0;
%
%  Zero initial fill-in columns.
%
  j0 = mu + 2;
  j1 = min ( n, m ) - 1;

  for jz = j0 : j1
    i0 = m + 1 - jz;
    abd(i0:ml,jz) = 0.0;
  end

  jz = j1;
  ju = 0;
%
%  Gaussian elimination with partial pivoting.
%
  for k = 1 : n-1
%
%  Zero out the next fill-in column.
%
    jz = jz + 1;
    if ( jz <= n )
      abd(1:ml,jz) = 0.0;
    end
%
%  Find L = pivot index.
%
    lm = min ( ml, n-k );
    l = isamax ( lm+1, abd(m:m+lm,k), 1 ) + m - 1;
    ipvt(k) = l + k - m;
%
%  Zero pivot implies this column already triangularized.
%
    if ( abd(l,k) == 0.0 )

      info = k;
%
%  Interchange if necessary.
%
    else

      if ( l ~= m )
        t = abd(l,k);
        abd(l,k) = abd(m,k);
        abd(m,k) = t;
      end
%
%  Compute multipliers.
%
      abd(m+1:m+lm,k) = - abd(m+1:m+lm,k) / abd(m,k);
%
%  Row elimination with column indexing.
%
      ju = min ( max ( ju, mu+ipvt(k) ), n );
      mm = m;

      for j = k+1 : ju
        l = l - 1;
        mm = mm - 1;
        t = abd(l,j);
        if ( l ~= mm )
          abd(l,j) = abd(mm,j);
          abd(mm,j) = t;
        end
        abd(mm+1:mm+lm,j) = ...
          saxpy ( lm, t, abd(m+1:m+lm,k), 1, abd(mm+1:mm+lm,j), 1 );
      end

    end

  end

  ipvt(n) = n;

  if ( abd(m,n) == 0.0 )
    info = n;
  end

  return
end


function [post] = spm_mci_post (mcmc,M,U,Y,true_P)
% Estimate posterior density
% FORMAT [post] = spm_mci_post (mcmc,M,U,Y,true_P)
%
% mcmc          .inference = 'amc','ais','vl' or 'langevin'
%               .verbose = 0 or 1 to plot progress (default 0)
%               .maxits = max number of iterations for sampling
%               .init = init parameter values (default is prior mean)
% M             model structure
% U             inputs (shouldn't be empty)
% Y             data
% true_P        true parameters (if known)
%
% post          structure containing posterior (mean, samples etc)
%__________________________________________________________________________
% Copyright (C) 2014 Wellcome Trust Centre for Neuroimaging

% Will Penny
% $Id: spm_mci_post.m 6697 2016-01-27 14:57:28Z spm $

try, verbose=mcmc.verbose; catch, verbose=0; end
if nargin < 5 || isempty(true_P)
    tp=0;
else
    tp=1;
end

tic;
switch mcmc.inference,

    case 'ais',
        disp('Annealed Importance Sampling');
        disp(' ');

        mcmc.nsamp=mcmc.maxits;

        tic;
        post = spm_mci_ais (mcmc,M,U,Y);
        toc

        Nsamp=size(post.P,2);
        post.ind=[1:Nsamp];
        post.Ep=mean(post.P(:,post.ind)')';
        post.mcmc=mcmc;

        % Eigenrep needed for spm_mci_joint later
        M = spm_mci_minit (M);

    case 'multi-amc',
        disp('Multiple chains of Adaptive Monte Carlo');
        disp(' ');
        Nsamp=ceil(0.5*mcmc.J);
        Nscale=0;
        Ntune=Nsamp;
        mc = spm_mci_popdef (Nscale,Ntune,Nsamp);
        mc.verbose=verbose;

        MM{1}=M;
        UU{1}=U;

        for it=1:mcmc.maxits,
            % Loop over chains
            mcmc.init{1}=spm_normrnd(M.pE,M.pC,1);
            Psamp = spm_mci_pop (mc,MM,UU,Y);
            if it ==1
                P=Psamp{1}.theta;
            else
                P=[P,Psamp{1}.theta];
            end
        end
        post.ind=[1:size(P,2)];
        post.Ep=mean(P(:,post.ind)')';
        post.P=P;
        post.mcmc=mcmc;

        % Eigenrep needed for spm_mci_joint later
        M = spm_mci_minit (M);

    case 'amc',
        disp('Adaptive Monte Carlo');
        disp(' ');

        % MH defaults
        tune=1;
        if tune
            Nscale=ceil(0.25*mcmc.maxits);
            Ntune=Nscale;
            Nsamp=ceil(0.5*mcmc.maxits);
        else
            Nscale=ceil(0.5*mcmc.maxits);
            Ntune=0;
            Nsamp=Nscale;
        end
        mc = spm_mci_popdef (Nscale,Ntune,Nsamp);
        mc.verbose=verbose;

        % Draw initialisation point from prior ?
        %mcmc.init{1}=spm_normrnd(M.pE,M.pC,1);
        try, mc.init{1}=mcmc.init; catch, mc.init{1}=spm_vec(M.pE); end

        MM{1}=M;
        UU{1}=U;
        tic;
        [Psamp,logev,D,MM] = spm_mci_pop (mc,MM,UU,Y);
        toc
        M=MM{1};

        if tp
            [post.Ep,post.SDp]=spm_mci_report (Psamp,mc,true_P);
        else
            disp('Initial params:');
            disp(mc.init{1});
            [post.Ep,post.SDp]=spm_mci_report (Psamp,mc);
        end

        post.Ep=post.Ep';
        post.P=Psamp{1}.theta;
        post.E=-Psamp{1}.logq;
        post.dL=Psamp{1}.dL;
        post.bayes_fb=zeros(1,length(post.E));
        post.acc=Psamp{1}.acc;
        post.logev=logev;
        post.D=D;
        post.mcmc=mcmc;
        post.ind=mc.ind_samp;

    case 'vl',
        disp('Variational Laplace');
        disp(' ');

        D.y=Y;
        if ~verbose
            M.nograph=1;
        end

        if isstruct(U)
            UI=U;
        else
            UI.u=U';
            UI.dt=M.T/M.N;
        end

        % Change VL defaults
        if isfield(mcmc,'maxits'), M.Nmax=mcmc.maxits; end
        if isfield(mcmc,'init'), M.P=mcmc.init; end
        if isfield(mcmc,'hE'), M.hE=mcmc.hE; end
        if isfield(mcmc,'hC'), M.hC=mcmc.hC; end

        [Ep,Cp,Eh,F,tmp1,tmp2,tmp3,k] = spm_nlsi_GN (M,UI,D);

        post.Ep=spm_vec(Ep);
        post.Cp=Cp;
        post.Eh=Eh;
        post.Ce=diag(1./exp(Eh));
        post.logev=F;
        post.its=k;
        if isfield(M,'P')
            post.init=M.P;
        end

        % Eigenrep needed for spm_mci_joint later
        M = spm_mci_minit (M);

    case 'langevin',
        disp('Langevin Monte Carlo');
        disp(' ');

        [M,stats]=spm_mci_lgv(mcmc,M,U,Y);
        Psamp=stats.P';
        Nsamp=size(Psamp,1);
        burn_in=round(0.3*size(Psamp,1));
        post=stats;
        post.ind=[burn_in+1:Nsamp];
        post.targ=Psamp(post.ind,:);
        post.Ep=mean(post.targ)';

        % 2.5%, 50% and 97.5% quantiles
        q = [.025 .5 .975];
        for j=1:M.Np,
            sx = post.P(j,post.ind);
            post.quantiles(j,:) = quantile(sx,q);
        end

    otherwise
        disp('Unknown inference method');
end

if strcmp(mcmc.inference,'VL')
    Up=UI;
else
    Up=U;
end

% Generate data fit from posterior mean
if isfield(M,'IS')
    if strcmp(M.IS,'spm_gen_erp')
        Ps=spm_unvec(post.Ep,M.pE);
        post.Yhat=feval('spm_gen_erp',Ps,M,Up);
    else
        post.Yhat = feval(M.IS,post.Ep,M,Up);
    end
else
    post.Yhat = spm_mci_fwd (post.Ep,M,Up);
end

post.els=toc;
disp(sprintf('Optimisation time = %1.2f seconds',post.els));

lw=2;
if ~isfield(M,'IS')
    % Plot time series
    figure
    rm=ceil(sqrt(M.l));
    for i=1:M.l,
        if M.l>3
            subplot(rm,rm,i);
        else
            subplot(M.l,1,i);
        end
        plot(M.t,Y(:,i),'LineWidth',lw);
        hold on
        plot(M.t,post.Yhat(:,i),'r','LineWidth',lw);
        grid on
        set(gca,'FontSize',16);
        legend('Data','Fit');
        xlabel('Time');
        ylabel(sprintf('y(%d)',i));
    end
end

% get parameters in reduced space
Pr=M.V'*(post.Ep-M.vpE);
post.L_est = spm_mci_joint (Pr,M,U,Y);
disp(sprintf('Estimated Log Joint=%1.2f',post.L_est));

if tp
    % get parameters in reduced space
    Pr=M.V'*(spm_vec(true_P)-M.vpE);
    post.L_true = spm_mci_joint (Pr,M,U,Y);
    disp(sprintf('True Log Joint=%1.2f',post.L_true));
    disp(' ');
end

pt=4;
if M.Np > pt
    hp=figure;
    set(hp,'Name','Parameters');
    plot(post.Ep,'r','LineWidth',lw);
    xlabel('Parameter');
    set(gca,'FontSize',16);
    grid on
else
    disp('Estimated (latent) params:');
    disp(post.Ep);
end

if tp
    if M.Np > pt
        hold on
        plot(spm_vec(true_P),'LineWidth',lw);
        legend('Estimated','True');
    else
        disp('True (latent) params:');
        disp(spm_vec(true_P));
    end
end

switch mcmc.inference,
    case {'amc','langevin'},
        post.type='sample';
    otherwise
        post.type='gaussian';
end


function z = plus( x, y )
    %PLUS Addition of two TT/MPS operators.
    %   Z = PLUS(X,Y) adds to TT/MPS operators. The resulting TT/MPS operator
    %   has rank equal to the sum of the individual ranks.
    %
    %   TTeMPS Toolbox.
    %   Michael Steinlechner, 2013-2016
    %   Questions and contact: michael.steinlechner@epfl.ch
    %   BSD 2-clause license, see LICENSE.txt

    % add sanity check...
    rx = x.rank;
    ry = y.rank;

    z = TTeMPS_op( cell(1, x.order) );

    % first core:
    tmp = zeros( 1, x.size_col(1), x.size_row(1), rx(2)+ry(2) );
    tmp( 1, :, :, 1:rx(2) ) = x.U{1};
    tmp( 1, :, :, rx(2)+1:end ) = y.U{1};
    z.U{1} = tmp;

    %z.U{1} = reshape( [unfold( x.U{1}, 'left'), unfold( y.U{1}, 'left')], [1, x.size(1), x.rank(2) + y.rank(2)]);

    % central cores:
    for i = 2:x.order-1
        tmp = zeros( rx(i)+ry(i), x.size_col(i), x.size_row(i), rx(i+1)+ry(i+1) );
        tmp( 1:rx(i), :, :, 1:rx(i+1) ) = x.U{i};
        tmp( rx(i)+1:end, :, :, rx(i+1)+1:end ) = y.U{i};
        z.U{i} = tmp;
    end

    % last core:
    tmp = zeros( rx(end-1)+ry(end-1), x.size_col(end), x.size_row(end), 1 );
    tmp( 1:rx(end-1), :, :, 1 ) = x.U{end};
    tmp( rx(end-1)+1:end, :, :, 1 ) = y.U{end};
    z.U{end} = tmp;

    z = update_properties( z );
end


function x = stretch_rs ( r, s, i )

%*****************************************************************************80
%
%% STRETCH_RS returns a data component given (R,S).
%
%  Discussion:
%
%    This routine shifts by (1,2) and stretches by (3,4).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 October 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real R, S, the coordinates of a point that lies on the
%    boundary of the square.
%
%    Input, integer I, the component of the data which is to be returned.
%
%    Output, real X, the I-th component of the data vector X, evaluated
%    at the point (R,S), which is on a corner, or edge, of the unit square.
%
  if ( i == 1 )
    x = 3.0 * r + 1.0;
  elseif ( i == 2 )
    x = 4.0 * s + 2.0;
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, 'STRETCH_RS - Fatal error!\n' );
    fprintf ( 1, '  Illegal component index I = %d\n', i );
    error ( 'STRETCH_RS - Fatal error!' );
  end

  return
end


% Focussed 2D Array with Directional Elements
%
% This example demonstrates the use of k-Wave to compute the outputs from a
% curved detector array which consists of several elements, each of which
% consists of a number of grid points.
%
% author: Ben Cox
% date: 29th October 2010
% last update: 22nd February 2012
%
% This function is part of the k-Wave Toolbox (http://www.k-wave.org)
% Copyright (C) 2009-2014 Bradley Treeby and Ben Cox

% This file is part of k-Wave. k-Wave is free software: you can
% redistribute it and/or modify it under the terms of the GNU Lesser
% General Public License as published by the Free Software Foundation,
% either version 3 of the License, or (at your option) any later version.
%
% k-Wave is distributed in the hope that it will be useful, but WITHOUT ANY
% WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
% FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public License for
% more details.
%
% You should have received a copy of the GNU Lesser General Public License
% along with k-Wave. If not, see <http://www.gnu.org/licenses/>.

clear all;

% =========================================================================
% SETUP THE GRID
% =========================================================================

% create the computational grid
Nx = 180;           % number of grid points in the x (row) direction
Ny = 180;           % number of grid points in the y (column) direction
dx = 0.1e-3;        % grid point spacing in the x direction [m]
dy = 0.1e-3;        % grid point spacing in the y direction [m]
kgrid = makeGrid(Nx, dx, Ny, dy);

% define the properties of the propagation medium
medium.sound_speed = 1500;    % [m/s]

% define the array of time points [s]
[kgrid.t_array, dt] = makeTime(kgrid, medium.sound_speed);
Nt = round(0.75*length(kgrid.t_array));
kgrid.t_array = (0:Nt-1)*dt;

% =========================================================================
% DEFINE A FOCUSSED ARRAY OF DIRECTIONAL ELEMENTS
% =========================================================================

% define a semicircular sensor centered on the grid
semicircle_radius = 65; % [grid points]
arc = makeCircle(Nx, Ny, Nx/2, Ny/2, semicircle_radius, pi);

% find total number and indices of the grid points constituting the
% semicircle
arc_indices = find(arc == 1);
Nv = length(arc_indices);

% calculate angles between grid points in the arc and the centre of the
% grid
arc_angles = atan((kgrid.y(arc_indices))./kgrid.x(arc_indices));

% sort the angles into ascending order, and adjust the indices accordingly
[sorted_arc_angles,sorted_index] = sort(arc_angles);
sorted_arc_indices = arc_indices(sorted_index);

% divide the semicircle into Ne separate sensor elements
Ne = 13;
sensor.mask = zeros(Nx,Ny);
for loop = 1:Ne

    % the indices of the grid points belonging to one element.
    % (There is a two grid point gap between the elements.)
    voxel_indices = sorted_arc_indices(floor((loop-1)*Nv/Ne)+2:floor(loop*Nv/Ne)-1);

    % add the element to the sensor.mask
    sensor.mask(voxel_indices) = 1;

end

% =========================================================================
% SIMULATION
% =========================================================================

% Define an infinitely wide plane wave source. (This is achieved by turning
% off the PML.)
source.p_mask = zeros(Nx,Ny);
source.p_mask(140,:) = 1;

% set the display mask for the simulation
display_mask = source.p_mask + sensor.mask;

% define a time varying sinusoidal source
source_freq = 1e6;
source_mag = 0.5;
source.p = source_mag*sin(2*pi*source_freq*kgrid.t_array);
source.p = filterTimeSeries(kgrid, medium, source.p);

% run the simulation with the PML switched off on the sides, so that the
% source continues up to the edge of the domain (and from there infinitely,
% because of the periodic assumption implicit in pseudospectral methods).
input_args = {'PMLAlpha', [2 0], 'DisplayMask', display_mask, 'PlotScale', [-0.75 0.75]};
sensor_data = kspaceFirstOrder2D(kgrid, medium, source, sensor, input_args{:});

% split up the data, recorded on all the grid points, between the elements
element_data = zeros(Ne,Nt);
for loop = 1:Ne

    % the indices of the sensor grid points in the sensor mask
    sensor_indices = find(sensor.mask==1);

    % the indices of the grid points belonging to one element.
    voxel_indices = sorted_arc_indices(floor((loop-1)*Nv/Ne)+2:floor(loop*Nv/Ne)-1);

    % indices of sensor_data that refer to the data for this element
    data_indices = zeros(length(voxel_indices),1);
    for loop2 = 1:length(voxel_indices)
        data_indices(loop2) = find(sensor_indices == voxel_indices(loop2));
    end

    % for one element per loop, average the time series from each of the
    % element's grid points to give one time series for the whole element
    element_data(loop,:) = mean(sensor_data(data_indices,:),1);

end

% =========================================================================
% VISUALISATION
% =========================================================================

% plot the elements of the array, and the source mask
figure;
imagesc(display_mask);
axis image;
colormap(flipud(gray));

% plot the time series recorded at each array element
figure;
stackedPlot(kgrid.t_array*1e6, element_data);
xlabel('time [\mus]');
ylabel('time series, one for each element');


function [len, labels] = imLength(img, varargin)
% Compute total length of a binary 1D structure.
%
%   LEN = imLength(IMG);
%   When IMG is a binary image, returns the total length of the structure,
%   equal to the number of pixels.
%   This function is intended to be used for debugging purpose.
%
%   LEN = imLength(IMG, RESOL);
%   Compute the length in user unit, by multiplying length in pixel by the
%   resolution.
%
%   [LEN, LABELS] = imLength(LBL, ...);
%   When LBL is a label image, returns the total length of each label
%   different from 0. Returns also the set of unique values in LBL.
%

% ------
% Author: David Legland
% e-mail: david.legland@inrae.fr
% Created: 2010-10-18,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2010 INRA - Cepia Software Platform.

%   HISTORY

% check image dimension
if ndims(img)>2 || min(size(img))>1 %#ok<ISMAT>
    error('first argument should be a 1D image');
end

% in case of a label image, return a vector with a set of results
labels = 1;
if ~islogical(img)
    labels = unique(img);
    labels(labels==0) = [];

    len = zeros(length(labels), 1);
    for i=1:length(labels)
        len(i) = imLength(img==labels(i), varargin{:});
    end
    return;
end

% extract resolution if present
resol = 1;
if ~isempty(varargin)
    resol = varargin{1};
end

% Total length of stucture is equal to the number of vertices multiplied by
% the resolution
len = sum(img(:)) * resol;


function soln = optimTraj(problem)
% soln = optimTraj(problem)
%
% Solves a trajectory optimization problem.
%
% INPUT: "problem" -- struct with fields:
%
%   func -- struct for user-defined functions, passed as function handles
%
%       Input Notes:  square braces show size:  [a,b] = size()
%               t = [1, nTime] = time vector (grid points)
%               x = [nState, nTime] = state vector at each grid point
%               u = [nControl, nTime] = control vector at each grid point
%               t0 = scalar = initial time
%               tF = scalar = final time
%               x0 = [nState, 1] = initial state
%               xF = [nState, 1] = final state
%
%       dx = dynamics(t,x,u)
%               dx = [nState, nTime] = dx/dt = derivative of state wrt time
%
%       dObj = pathObj(t,x,u)
%               dObj = [1, nTime] = integrand from the cost function
%
%       obj = bndObj(t0,x0,tF,xF)
%               obj = scalar = objective function for boundry points
%
%       [c, ceq] = pathCst(t,x,u)
%               c = column vector of inequality constraints  ( c <= 0 )
%               ceq = column vector of equality constraints ( c == 0 )
%
%       [c, ceq] = bndCst(t0,x0,tF,xF)
%               c = column vector of inequality constraints  ( c <= 0 )
%               ceq = column vector of equality constraints ( c == 0 )
%
%       How to pass parameters to your functions:
%           - suppose that your dynamics function is pendulum.m and it
%           accepts a struct of parameters p. When you are setting up the
%           problem, define the struc p in your workspace and then use the
%           following command to pass the function:
%               problem.func.dynamics = @(t,x,u)( pendulum(t,x,u,p) );
%
%       Analytic Gradients:
%           Both the "trapezoid" and "hermiteSimpson" methods in OptimTraj
%       support analytic gradients. Type help "trapezoid" or help
%       "hermiteSimpson" for details on how to pass gradients to the
%       transcription method.
%
%
%   bounds - struct with bounds for the problem:
%
%       .initialTime.low = [scalar]
%       .initialTime.upp = [scalar]
%
%       .finalTime.low = [scalar]
%       .finalTime.upp = [scalar]
%
%       .state.low = [nState,1] = lower bound on the state
%       .state.upp = [nState,1] = lower bound on the state
%
%       .initialState.low = [nState,1]
%       .initialState.upp = [nState,1]
%
%       .finalState.low = [nState,1]
%       .finalState.upp = [nState,1]
%
%       .control.low = [nControl, 1]
%       .control.upp = [nControl, 1]
%
%
%
%   guess - struct with an initial guess at the trajectory
%
%       .time = [1, nGridGuess]
%       .state = [nState, nGridGuess]
%       .control = [nControl, nGridGuess]
%
%   options = options for the transcription algorithm (this function)
%
%       .nlpOpt = options to pass through to fmincon
%
%       .method = string to pick which method is used for transcription
%           'trapezoid'
%           'hermiteSimpson'
%           'chebyshev'
%           'rungeKutta'
%           'gpops'     ( Must have GPOPS2 installed )
%
%       .[method] = a struct to pass method-specific parameters. For
%       example, to pass the number of grid-points to the trapezoid method,
%       create a field .trapezoid.nGrid = [number of grid-points].
%
%       .verbose = integer
%           0 = no display
%           1 = default
%           2 = display warnings, overrides fmincon display setting
%           3 = debug
%
%       .defaultAccuracy = {'low','medium','high'}
%           Sets the default options for each transcription method
%
%       * if options is a struct array, the optimTraj will run the optimization
%       by running options(1) and then using the result to initialize a new
%       solve with options(2) and so on, until it runs options (end). This
%       allows for successive grid and tolerance opdates.
%
%
%
%
%
% OUTPUT: "soln"  --  struct with fields:
%
%   .grid = trajectory at the grid-points used by the transcription method
%       .time = [1, nTime]
%       .state = [nState, nTime]
%       .control = [nControl, nTime];
%
%   .interp = functions for interpolating state and control for arbitrary
%       times long the trajectory. The interpolation method will match the
%       underlying transcription method. This is particularily important
%       for high-order methods, where linear interpolation between the
%       transcription grid-points will lead to large errors. If the
%       requested time is not on the trajectory, the interpolation will
%       return NaN.
%
%       .state = @(t) = given time, return state
%           In: t = [1,n] vector of time
%           Out: x = [nState,n] state vector at each point in time
%
%       .control = @(t) = given time, return control
%           In: t = [1,n] vector of time
%           Out: u = [nControl,n] state vector at each point in time
%
%   .info = information about the optimization run
%       .nlpTime = time (seconds) spent in fmincon
%       .exitFlag = fmincon exit flag
%       .objVal = value of the objective function
%       .[all fields in the fmincon "output" struct]
%
%   .problem = the problem as it was passed to the low-level transcription,
%       including the all default values that were used
%
%
%   * If problem.options was a struct array, then soln will also be a
%   struct array, with soln(1) being the solution on the first iteration,
%   and soln(end) being the final solution.
%
%   * Both the trapezoid and hermiteSimpson method support additional
%   features. The first is that they can use analytic gradients, if
%   provided by the user. ">> help trapezoid" or ">> help hermiteSimpson"
%   for more information. These methods additionally provide error
%   estimates in two forms. The continuous collocation constraint error is
%   provided as an additional function handle (soln.interp.collCst), and
%   the absolute local error estimate for each segment is provided in
%   soln.info.error.
%

problem = inputValidation(problem);   %Check inputs
problem = getDefaultOptions(problem); % Complete options struct

% Loop over the options struct to solve the problem
nIter = length(problem.options);
soln(nIter) = struct('grid',[],'interp',[],'info',[],'problem',[]);
P = problem;  %Temp variable for passing on each iteration
for iter=1:nIter
    P.options = problem.options(iter);

    if P.options.verbose > 0    %then print out iteration count:
        disp('~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~');
        disp(['Running OptimTraj, iteration ' num2str(iter)]);
    end

    if iter > 1  %Use previous soln as new guess
        P.guess = soln(iter-1).grid;
    end

    %%%% This is the key part: call the underlying transcription method:
    switch P.options.method
        case 'trapezoid'
            soln(iter) = trapezoid(P);
        case 'hermiteSimpson'
            soln(iter) = hermiteSimpson(P);
        case 'chebyshev'
            soln(iter) = chebyshev(P);
        case 'multiCheb'
            soln(iter) = multiCheb(P);
        case 'rungeKutta'
            soln(iter) = rungeKutta(P);
        case 'gpops'
            soln(iter) = gpopsWrapper(P);
        otherwise
            error('Invalid method. Type: ''help optimTraj'' for a valid list.');
    end

end
end


function [Forecasteval]=TVEmafeval(data_endo_a,data_endo_c,data_endo_c_lags,data_exo_c,data_exo_c_lags,stringdates3,Fstartdate,Fcenddate,Fcperiods,Fcomp,const,n,m,p,k1,k3,It,Bu,beta_gibbs,sigma_gibbs,forecast_record,forecast_estimates,names,endo,pref,theta_gibbs,TVEHfuture,ss_record,indH)

Forecasteval.RMSE=[];
Forecasteval.MAE=[];
Forecasteval.MAPE=[];
Forecasteval.Ustat=[];
Forecasteval.CRPS_estimates=[];
Forecasteval.S1_estimates=[];
Forecasteval.S2_estimates=[];

% first, note that forecast evaluation can only be conducted if there is some observable data after the beginning of the forecast
if Fcomp==1

   % preliminary task: obtain a matrix of forecasts over the common periods
   for ii=1:n
   forecast_c(:,ii)=forecast_estimates{ii,1}(2,1:Fcperiods)';
   end
   % then compute the matrix of forecast errors
   ferrors=data_endo_c-forecast_c;

   % compute first the sequential RMSE, defined in (a.8.11)

   % square the forecast error matrix entrywise
   sferrors=ferrors.^2;
   % sum entries sequentially
   sumsferrors=sferrors(1,:);
   for ii=2:Fcperiods
   sumsferrors(ii,:)=sumsferrors(ii-1,:)+sferrors(ii,:);
   end
   % divide by the number of forecast periods and take square roots to obtain RMSE
   for ii=1:Fcperiods
   Forecasteval.RMSE(ii,:)=((1/ii)*sumsferrors(ii,:)).^0.5;
   end

   % compute then the sequential MAE, defined in (a.8.12)

   % take the absolute value of the forecast error matrix
   absferrors=abs(ferrors);
   % sum entries sequentially
   sumabsferrors=absferrors(1,:);
   for ii=2:Fcperiods
   sumabsferrors(ii,:)=sumabsferrors(ii-1,:)+absferrors(ii,:);
   end
   % divide by the number of forecast periods to obtain MAE
   for ii=1:Fcperiods
   Forecasteval.MAE(ii,:)=(1/ii)*sumabsferrors(ii,:);
   end

   % compute the sequential MAPE, defined in (a.8.13)

   % divide entrywise by actual values and take absolute values
   absratioferrors=abs(ferrors./data_endo_c);
   % sum entries sequentially
   sumabsratioferrors=absratioferrors(1,:);
   for ii=2:Fcperiods
   sumabsratioferrors(ii,:)=sumabsratioferrors(ii-1,:)+absratioferrors(ii,:);
   end
   % divide by 100*(number of forecast periods) to obtain MAPE
   for ii=1:Fcperiods
   Forecasteval.MAPE(ii,:)=(100/ii)*sumabsratioferrors(ii,:);
   end

   % compute the Theil's inequality coefficient, defined in (a.8.14)

   % first compute the left term of the denominator
   % square entrywise the matrix of actual data
   sendo=data_endo_c.^2;
   % sum entries sequentially
   sumsendo=sendo(1,:);
   for ii=2:Fcperiods
   sumsendo(ii,:)=sumsendo(ii-1,:)+sendo(ii,:);
   end
   % divide by the number of forecast periods and take square roots
   for ii=1:Fcperiods
   leftterm(ii,:)=((1/ii)*sumsendo(ii,:)).^0.5;
   end
   % then compute the right term of the denominator
   % square entrywise the matrix of forecast values
   sforecasts=forecast_c.^2;
   % sum entries sequentially
   sumsforecasts=sforecasts(1,:);
   for ii=2:Fcperiods
   sumsforecasts(ii,:)=sumsforecasts(ii-1,:)+sforecasts(ii,:);
   end
   % divide by the number of forecast periods and take square roots
   for ii=1:Fcperiods
   rightterm(ii,:)=((1/ii)*sumsforecasts(ii,:)).^0.5;
   end
   % finally, compute the U stats
   Forecasteval.Ustat=Forecasteval.RMSE./(leftterm+rightterm);

   % now compute the continuous ranked probability score

   % create the cell storing the results
   CRPS=cell(n,1);
   % loop over endogenous variables
   for ii=1:n
      % loop over forecast periods on which actual data is known
      for jj=1:Fcperiods
      % compute the continuous ranked probability score
      score=bear.crps(forecast_record{ii,1}(:,jj),forecast_estimates{ii,1}(2,jj));
      CRPS{ii,1}(1,jj)=score;
      end
   end
   Forecasteval.CRPS_estimates=(cell2mat(CRPS))';

   % finally, consider the big piece: the computation of the log predictive score
   % this part implements algorithm a.8.1, described p 115 of technical guide
   % details of the procedure can be found p 111-115 of the technical guide

   % preliminary task: if there is a constant in the model, concatenate a column of ones to data_exo_c
   if const==1
   data_exo_c=[ones(Fcperiods,1) data_exo_c];
   data_exo_c_lags=[ones(p,1) data_exo_c_lags];
   end

   % create the cells storing the results
   S1=cell(n,1);
   S2=cell(n,1);

   for ll=1:It-Bu
      % step 2: draw beta, Delta and sigma
      beta=beta_gibbs(:,ll);
      sigma=reshape(sigma_gibbs(:,ll),n,n);
      % reshape beta to obtain B
      B=reshape(beta,k1,n);
      theta=theta_gibbs(:,ii);
      tempeq = [];
      for nnn = 1:n
        tempeq = [tempeq ss_record{nnn,1}(ii,end-p+1:end)'];
      end

      % step 3: obtain mu, the mean vector
      temp=bear.TVEmaforecastsim(data_endo_a,B,p,n,Fcperiods,theta,TVEHfuture,tempeq, indH);
      mu=reshape(temp',n*Fcperiods,1);

      % step 4: obtain upsilon, the covariance matrix
      % recover the A1,A2,... matrices by reshaping beta
      temp=B';
      Amatrices=cell(1,p);
      for ii=1:p
      Amatrices{1,ii}=temp(:,n*(ii-1)+1:n*ii);
      end
      % initiate upsilon with upsilon_1,1
      upsilon=cell(Fcperiods,Fcperiods);
      upsilon{1,1}=sigma;
      % complete the first column  using (a.8.23)
      for ii=2:Fcperiods
      upsilon_ij=Amatrices{1,1}*upsilon{ii-1,1};
      summ=min(ii-1,p);
         for jj=2:summ
         upsilon_ij=upsilon_ij+Amatrices{1,jj}*upsilon{ii-jj,1};
         end
      upsilon{ii,1}=upsilon_ij;
      end
      % complete the first row, using symmetry
      for ii=2:Fcperiods
      upsilon{1,ii}=upsilon{ii,1}';
      end
      % now take care of the other columns
      for jj=2:Fcperiods
      % first complete the variance matrix (the diagonal one), using (a.8.24)
      upsilon_jj=sigma;
      summ=min(jj-1,p);
         for kk=1:summ
         upsilon_jj=upsilon_jj+Amatrices{1,kk}*upsilon{jj-kk,jj};
         end
      upsilon{jj,jj}=upsilon_jj;
      % then complete the rest of the column, using (a.8.23)
         for ii=jj+1:Fcperiods
         % initiate upsilon_ij
         upsilon_ij=Amatrices{1,1}*upsilon{ii-1,jj};
         % continue the summation
         summ=min(ii-1,p);
            for kk=2:summ
            upsilon_ij=upsilon_ij+Amatrices{1,kk}*upsilon{ii-kk,jj};
            end
         upsilon{ii,jj}=upsilon_ij;
         end
         % complete the corresponding row, using symmetry
         for kk=jj+1:Fcperiods
         upsilon{jj,kk}=upsilon{kk,jj}';
         end
      end
      upsilon=cell2mat(upsilon);

      % step 5: obtain the predictive density

      % first consider scenario 1 (forecast at period T+i)
      % loop over variables
      for ii=1:n
         % loop over periods
         for jj=1:Fcperiods
         % define R
         R=zeros(1,n*Fcperiods);
         R(1,n*(jj-1)+ii)=1;
         % define the mean vector
         mean=R*mu;
         % define the covariance matrix
         covar=R*upsilon*(R');
         % define the vector of actual values
         actual=data_endo_c(jj,ii);
         % determine the density
         [~,density]=bear.mndensity(actual,mean,covar,1);
         % record the result
         S1{ii,1}(ll,jj)=density;
         end
      end
      % then consider scenario 2 (forecast from beginning until period T+i)
      % loop over variables
      for ii=1:n
      R=[];
         % loop over periods
         for jj=1:Fcperiods
         % define R
         R=[R;zeros(1,n*Fcperiods)];
         R(jj,n*(jj-1)+ii)=1;
         % define the mean vector
         mean=R*mu;
         % define the covariance matrix
         covar=R*upsilon*(R');
         % define the vector of actual values
         actual=data_endo_c(1:jj,ii);
         % determine the density
         [~,density]=bear.mndensity(actual,mean,covar,jj);
         % record the result
         S2{ii,1}(ll,jj)=density;
         end
      end
   end

   % step 6: compute the log predictive score from (XXX.11)
   Forecasteval.S1_estimates=cell(n,1);
   Forecasteval.S2_estimates=cell(n,1);
   for ii=1:n
      for jj=1:Fcperiods
      Forecasteval.S1_estimates{ii,1}(1,jj)=log((1/(It-Bu))*sum(S1{ii,1}(:,jj)));
      Forecasteval.S2_estimates{ii,1}(1,jj)=log((1/(It-Bu))*sum(S2{ii,1}(:,jj)));
      end
   end
   Forecasteval.S1_estimates=(cell2mat(Forecasteval.S1_estimates))';
   Forecasteval.S2_estimates=(cell2mat(Forecasteval.S2_estimates))';

% if forecast evaluation is not possible, do not do anything
elseif Fcomp==0
end

% now, print the results and display them

filelocation=fullfile(pref.results_path, [pref.results_sub '.txt']);
fid=fopen(filelocation,'at');

fprintf('%s\n','');
fprintf(fid,'%s\n','');
fprintf('%s\n','');
fprintf(fid,'%s\n','');
fprintf('%s\n','');
fprintf(fid,'%s\n','');
fprintf('%s\n','');
fprintf(fid,'%s\n','');

Fevalinfo='Forecast evaluation:';
fprintf('%s\n',Fevalinfo);
fprintf(fid,'%s\n',Fevalinfo);

fprintf('%s\n','');
fprintf(fid,'%s\n','');

% if forecast evaluation is not possible, return a message to signal it

if Fcomp==0

finfo1=['Forecast evaluation cannot be conducted.'];
fprintf('%s\n',finfo1);
fprintf(fid,'%s\n',finfo1);
finfo2=['Forecasts start in ' Fstartdate ', while observable data is available only until ' names{end,1} '.'];
fprintf('%s\n',finfo2);
fprintf(fid,'%s\n',finfo2);
finfo3=['To obtain forecast evaluation, the forecast start date must be anterior to the end of the data set.'];
fprintf('%s\n',finfo3);
fprintf(fid,'%s\n',finfo3);

% if forecast evaluation is possible, display the results
elseif Fcomp==1

finfo1=['Evaluation conducted over ' num2str(Fcperiods) ' periods (from ' Fstartdate ' to ' Fcenddate ').'];
fprintf('%s\n',finfo1);
fprintf(fid,'%s\n',finfo1);

   % loop over endogenous variables
   for ii=1:n

   fprintf('%s\n','');
   fprintf(fid,'%s\n','');

   endoinfo=['Endogenous: ' endo{ii,1}];
   fprintf('%s\n',endoinfo);
   fprintf(fid,'%s\n',endoinfo);

   temp='fprintf(''%12s';
      for jj=1:Fcperiods-1
      temp=[temp ' %10s'];
      end
   temp=[temp ' %10s\n'','''''];
      for jj=1:Fcperiods
      temp=[temp ',''' stringdates3{jj,1} ''''];
      end
   temp=[temp ');'];
   eval(temp);
   temp='fprintf(fid,''%12s';
      for jj=1:Fcperiods-1
      temp=[temp ' %10s'];
      end
   temp=[temp ' %10s\n'','''''];
      for jj=1:Fcperiods
      temp=[temp ',''' stringdates3{jj,1} ''''];
      end
   temp=[temp ');'];
   eval(temp);

   label='RMSE:       ';
   values=Forecasteval.RMSE(1:Fcperiods,ii)';
   temp='fprintf(''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);
   temp='fprintf(fid,''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);

   label='MAE:        ';
   values=Forecasteval.MAE(1:Fcperiods,ii)';
   temp='fprintf(''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);
   temp='fprintf(fid,''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);

   label='MAPE:       ';
   values=Forecasteval.MAPE(1:Fcperiods,ii)';
   temp='fprintf(''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);
   temp='fprintf(fid,''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);

   label='Theil''s U:  ';
   values=Forecasteval.Ustat(1:Fcperiods,ii)';
   temp='fprintf(''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);
   temp='fprintf(fid,''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);

   label='CRPS:       ';
   values=Forecasteval.CRPS_estimates(1:Fcperiods,ii)';
   temp='fprintf(''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);
   temp='fprintf(fid,''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);

   label='Log score 1:';
   values=Forecasteval.S1_estimates(1:Fcperiods,ii)';
   temp='fprintf(''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);
   temp='fprintf(fid,''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);

   label='Log score 2:';
   values=Forecasteval.S2_estimates(1:Fcperiods,ii)';
   temp='fprintf(''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);
   temp='fprintf(fid,''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);

    end

end

fclose(fid);