function [yDp,yDsig2,rho,nbD]=MFSLF(xs,ys,yLs,xp,trD,noise)
% Fit Bayesian multi-fidelity surrogate with LF function
% INPITS: xs: High-fidelity sample locations
%         ys: High-fidelity samples
%         yLs: Low-fidelity samples
%         xp: Prediction points
%         trD: Order of discrepancy function (0, 1, 2)
%         noise: 0~no noise samples, 1~noisy samples
% OUTPUTS: yDp: Discrepancy predictions
%          yDsig2: Discrepancy prediction variance
%          rho: LF scale factor
%          nbD: # of coefficients in discrepancy function
%
 [ny,ndim]=size(xs);nvar=ndim+1;np=size(xp,1);            % Scaling samples
 meanx=mean(xs);stdx=std(xs);            % Mean and STD of sample locations
 xs=(xs-meanx)./stdx;                            % Scaling sample locations
 meany=mean(ys);stdy=std(ys);                     % Mean and STD of samples
 ys=(ys-meany)/stdy;yLs=(yLs-meany)/stdy;                 % Scaling samples
 xp=(xp-meanx)./stdx;                        % Scaling of prediction points
 %
 nij=ny*(ny-1)/2; l=0;             % # of upper-triangular components of RD
 dH=zeros(nij,ndim);                          % Distance between HF samples
 ijH=zeros(nij,2);                            % Index of correlation matrix
 for i=1:ny-1
   l=l(end)+(1:ny-i);
   ijH(l,:)=[repmat(i,ny-i,1),(i+1:ny)'];
   dH(l,:)=repmat(xs(i,:),ny-i,1)-xs(i+1:ny,:);
 end
 %
 % Estimating discrepancy hyper-parameters
 parD=struct('xs',xs,'ys',ys,'yLs',yLs,'d',dH,'ij',ijH,'trPRS',min(trD,2));
 hyp0 = [ny^(-1/4)*ones(ndim,1); min(0.1,noise)];
 lb=[0.01*ones(ndim,1);0];ub=[6*ones(ndim,1);noise];% Lower- & upper-bounds
 options=optimset('Display','off','TolFun',1e-6,'DiffMinChange',0.001);
 %hypOpt=fmincon(@(hyp) fitDIS(hyp,parD),hyp0,[],[],[],[],lb,ub,[],options);
 hypOpt = ga(@(hyp) fitDIS(hyp,parD),nvar,[],[],[],[],lb,ub,[],options)';
 %hypOpt = simulannealbnd(@(hyp) fitDIS(hyp,parD),hyp0,lb,ub,options);
 hypD=hypOpt(1:size(xs,2)); tau=max(0,hypOpt(end));
 fprintf('Discrepancy hyperparam:%8.3g\n',hypD.*stdx');
 if noise ~= 0, fprintf('Discrepancy noise fraction:%8.3g\n',tau); end
 %
 [~,fitD]=fitDIS(hypOpt,parD);                  % Fitting Kriging surrogate
 rho=fitD.rho; fprintf('LF scale factor:%8.3g\n',rho);
 %
 % Evaluating discrepancy surrogate at prediction points
 xi = trend(xp,trD); nbD=size(xi,2);      % Trend basis vector at np points
 xrep=repmat(xp',ny,1); xrep=transpose(reshape(xrep,ndim,np*ny));
 d=xrep-repmat(xs,np,1);               % Distance b/w predictions & samples
 r = (1-tau)*exp(sum(-(d./hypD').^2,2));  % Correlation b/w preds & samples
 r = reshape(r,ny,np);
 yDp=xi*fitD.beta + (fitD.alpha*r)';                  % Kriging predictions
 yDp=yDp*stdy+(1-rho)*meany;                        % Unscaling predictions
 %
 % Prediction variance of discreepancy surrogate
 rt=fitD.C\r;
 q=fitD.G\(xi'-fitD.Ft'*rt);
 sigma2=fitD.sigma2.*stdy.^2;                            % Process variance
 yDsig2=sigma2.*(1-sum(rt.^2,1)+sum(q.^2,1))';        % Prediction variance
 fprintf('Disc. process std :%8.3g\n',sqrt(sigma2)*stdy);
 if noise~=0, fprintf('Disc. Noise std :%8.3g\n',sqrt(tau*sigma2)*stdy);end
end
%
%
function [negLogLK,fit]=fitDIS(xvar,par)
% Negative log-likelihood estimate for given hyper-parameter
% INPUTS  xvar          : Hyper-parameter (hyp) & noise (tau)
%         par           : Model parameters structure
% OUTPUTS negLogLK      : Negative log-likelihood function
%         fit           : Necessary parameters for function evaluation
%
 hyp=xvar(1:size(par.xs,2)); tau=xvar(end);
 H = trend(par.xs,par.trPRS);                               % Design matrix
 ny=size(par.ys,1); nb=size(H,2);       % ny: # of samples, nb: # of coeff.
 r = (1-tau)*exp(sum(-(par.d./hyp(:)').^2, 2));   % Correlation b/w samples
 idx=find(r>0);              % Index of nonzero components in sparse matrix
 ii=(1:ny)';                                               % Diagonal index
 mu=ones(ny,1)+(10+ny)*eps;         % Small diagonal to prevent singularity
 R=sparse([par.ij(idx,1);ii],[par.ij(idx,2);ii],[r(idx);mu]); % Correlation
 [C,~]=chol(R);                           % Cholesky decomposition R = C'*C
 C=C';                                                % Transpose: R = C*C'
 Ft=C\H;                                                      % Ft=[C^1][X]
 [Q,G]=qr(Ft,0);                               % qr-factorization, Ft = Q*G
 %
 Ylt=C\par.yLs;            % Maximum likelihood estimate of LF scale factor
 Yht=C\par.ys;
 taul=(eye(ny)-Q*Q')*Ylt;
 tauh=(eye(ny)-Q*Q')*Yht;
 rhol=max(0.1,min(3,taul'*tauh/(taul'*taul)));            % LF scale factor
 %
 dis=par.ys-rhol*par.yLs;                             % Discrepancy samples
 Yt=C\dis;
 b=G\(Q'*Yt);                              % Coefficients of trend function
 rho=(Yt-Ft*b);                                         % rho = Cinv*(y-Xb)
 alpha=rho'/C;                                       % alpha = (y-Xb)'*Rinv
 detR = sum(log(full(diag(C)).^2));     % Determinant of correlation matrix
 sigma2=sum(rho.^2)/(ny-nb);                            % Process variance
 negLogLK=detR + (ny-nb)*log(sigma2);             % Negative log-likelihood
 fit = struct('sigma2',sigma2,'beta',b,'alpha',alpha,'C',C,'Ft',Ft,...
              'G',G','rho',rhol);
end
%
function H=trend(x,order)
% Basis vector of trend function
%
 [m,n]=size(x);
 if order==0,     H=ones(m,1);              % Constant trend function basis
 elseif order==1, H=[ones(m,1), x];           % Linear trend fucntion basis
 elseif order==2                           % Quadratic trend function basis
  nn=(n+1)*(n+2)/2;
  H=[ones(m,1) x zeros(m,nn-n-1)];
  j=n+1; q=n;
  for k=1:n
   H(:,j+(1:q))=repmat(x(:,k),1,q).*x(:,k:n);
   j=j+q; q=q-1;
  end
 end
end