function [pr,sd]=gp3_iden_m2(p,M,yy,MT,N,ord,ff)

% GP3_IDEN_M2       Slow algorithm, written in M code to perform prediction
% using the State-space-Time-series combined Covariance matrix.
%
%   [pr,sd] = GP3_IDEN_M2(p,M,yy,NT,N,ord,ff)
%
% Inputs:
%       p   = Hyperparameters, [a1 a2 gts g1...gk v]
%       M   = Input state-space explanatory variables
%       yy  = Outcome for every explanatory variables
%       MT  = Input time-series explanatory variables
%       N   = Predicting input state-space explanatory variables
%       ord = Order of reduced state-space size
%       ff  = Fudge Factor, e
%
% Outputs:
%   pr  = Prediction, at corresponding input explanatory variables
%   sd  = Standard deviation, at corresponding input explanatory variables
%
%   Note:
%           
%           Q = a1* [ Qf+v(I+eI)      vI    ]
%                   [    vI      Qg+v(I+eI) ]
%
%       such that, Qf = exp(-0.5*Zf^2*d)
%                  Qg = a2*exp(-0.5*Zg^2*dg)
%                   e = machine precision
%
%
% (C) Gaussian Process SSTS Toolbox 1.0
% (C) Copyright 2006-2007, Keith Neo <keith.neo@nuim.ie>
%                          http://www.hamilton.ie

[N1,L0]=size(M);
a1=p(1);
a2=p(1)*p(2);
g1=p(3);
g2=p(4:L0+3);
v =p(L0+4);
av=a1*v;
vxp=av*(1+ff);
NN=get_ord(ord,N1);
TM=getord(ord,M);
Ty=getord(ord,yy);

expA1=a1*constr_cov(MT,MT,g1);
expA2=a2*constr_cov(TM,TM,g2);

Q=diag_add([expA1 av*NN';av*NN expA2],vxp);
invQy=Q\[yy;Ty];
Y=invQy(N1+1:end);

Cxt=expacov(TM,N,[a2 g2]);
pr=Cxt'*Y;
invQf=inv(diagadd(expA1,vxp));
MatGF=inv(diagadd(expA2,vxp)-av*av*(NN*invQf*NN'));
sd=sqrt(a2-diag(Cxt'*MatGF*Cxt));


function expA=constr_cov(M,N,g)
N0=size(M,1);
Nt=size(N,1);
expT=repmat(permute(M,[1,3,2]),[1,Nt,1])-repmat(permute(N',[3,2,1]),[N0,1,1]);
expC=(expT.^2).*repmat(permute(g',[3 2 1]),[N0,Nt,1]);
expA=exp(-0.5*sum(expC,3));


function Q=diag_add(Q,v)
N0=size(Q,1);
for i=1:N0
    Q(i,i)=Q(i,i)+v;
end

function z=getord(x,y)
if x==0
    z=y;
else
    D0=ceil(size(y,1)/(x+1));
    z=zeros(D0,size(y,2));
    for k=1:D0
        pp=(x+1)*(k-1)+1;
        z(k,:)=y(pp,:);
    end
end

function z=invord(x,y,N1)
Y0=size(y);
if x==0 && Y0(1)==N1
    z=y;
else
    z=zeros(N1,Y0(2));
    for k=1:Y0(2)
        tmpZ=[y(:,k)';zeros(x,Y0(1))];
        tmpZ=reshape(tmpZ,Y0(1)*(x+1),1);
        z(:,k)=tmpZ(1:N1);
    end
end