function [W,CovW,X,CovX] = vbgppcamv_full(Y,D,inW,inX,covfuncW,logthetaW, ...
                                          covfuncX, logthetaX,varargin)
% [W,CovW,X,CovX] = vbgppcamv(Y,D,inW,inX,covfuncW,logthetaW,covfuncX, ...
% logthetaX,varargin)
%
% Variational Bayesian (VB) Gaussian process (GP) principal component
% analysis (PCA) with handling of missing values (MV).
%
% This version does NOT factorize with respect to the components.
%
% At the moment, pseudo inputs are not yet supported.
%

[M,N] = size(Y);

opts = struct( ...
    'init',          [],...
    'rotate', true, ...
    'autosavetime', 0,...
    'autosavefile', 'vbgppcamv_autosave',...
    'testset', [], ...
    'pseudodensityx', 1, ...
    'pseudodensityw', 1, ...
    'updatepseudox', true, ...
    'updatepseudow', true, ...
    'initpseudow', [], ...
    'initpseudox', [], ...
    'reconstruct', true, ...
    'loglikelihood', true, ...
    'updatehyper', 1, ...
    'maxsearchx', 10, ...
    'maxsearchw', 10, ...
    'checkgradx', false, ...
    'checkgradw', false, ...
    'maxiter', 100);

[ opts, errmsg, wrnmsg ] = argschk( opts, varargin{:} );
if ~isempty(errmsg), error( errmsg ), end
if ~isempty(wrnmsg), warning( wrnmsg ), end

Obs = ~isnan(Y);
NMobs = sum(Obs(:));

inWorig = inW;
inXorig = inX;

% Initialize posterior values
[W,varW,X,varX,tau] = initialize(opts.init, D, inW,inX,covfuncW, ...
                                 logthetaW,covfuncX, logthetaX);

% Remove empty rows/columns
rowrm = (colsum(Obs) == 0);
colrm = (rowsum(Obs) == 0);
if any(rowrm)
  disp('Removing empty rows');
  inW(:,rowrm) = [];
  Y(rowrm,:) = [];
  Obs(rowrm,:) = [];
  Morig = M;
  M = rows(Y);
  W(rowrm,:) = [];
  varW(rowrm,:) = [];
end
if any(colrm)
  disp('Removing empty columns');
  inX(:,colrm) = [];
  Y(:,colrm) = [];
  Obs(:,colrm) = [];
  Norig = N;
  N = cols(Y);
  X(:,colrm) = [];
  varX(:,colrm) = [];
end

log2pi = log(2*pi);

%%
%% Initialize posterior parameters

% $$$ % Number of pseudo inputs for each component
% $$$ opts.pseudodensityx = opts.pseudodensityx(:) .* ones(D,1);
% $$$ opts.pseudodensityw = opts.pseudodensityw(:) .* ones(D,1);
% $$$ Np = ceil(opts.pseudodensityx.*cols(inX)); % no. of pseudos for X components
% $$$ Mp = ceil(opts.pseudodensityw.*cols(inW)); % no. of pseudos for W components
% $$$
% $$$ dimX = rows(inX);   % dimensionality of the input space of X
% $$$ dimW = rows(inW);   % dimensionality of the input space of W
% $$$
% $$$ % Initialize pseudo inputs
% $$$ pseudoX = cell(D,1);
% $$$ pseudoW = cell(D,1);
% $$$ usepseudoX = 1==zeros(D,1);
% $$$ usepseudoW = 1==zeros(D,1);
% $$$ for d=1:D
% $$$   if Np(d)==N
% $$$     pseudoX{d} = inX; % full, don't use pseudo inputs
% $$$     usepseudoX(d) = false;
% $$$   else
% $$$     permN = randperm(N);
% $$$     pseudoX{d} = inX(:,permN(1:Np(d)));
% $$$     usepseudoX(d) = true;
% $$$   end
% $$$   if Mp(d)==M
% $$$     pseudoW{d} = inW; % full, don't use pseudo inputs
% $$$     usepseudoW(d) = false;
% $$$   else
% $$$     permM = randperm(M);
% $$$     pseudoW{d} = inW(:,permM(1:Mp(d)));
% $$$     usepseudoW(d) = true;
% $$$   end
% $$$ end
% $$$
% $$$ if ~isempty(opts.initpseudow)
% $$$   pseudoW = opts.initpseudow;
% $$$   usepseudoW(:) = true;
% $$$ end
% $$$ if ~isempty(opts.initpseudox)
% $$$   pseudoX = opts.initpseudox;
% $$$   usepseudoX(:) = true;
% $$$ end
% $$$
% $$$ cholKWpp = cell(D,1);
% $$$ cholKXpp = cell(D,1);

mu = zeros(M,1);
v_mu = zeros(M,1);

logP = -inf;
costcpu = 0;

% $$$ % Wp
% $$$ Wp = cell(D,1);
% $$$ CovWp = cell(D,1);
% $$$
% $$$ % Xp
% $$$ Xp = cell(D,1);
% $$$ CovXp = cell(D,1);

%% tau precision (inverse noise)
atau = 1e-4;
btau = 1e-4;
a_tau = nan; % posterior pdf parameter
b_tau = nan; % posterior pdf parameter
logtau = nan;

lastsave = cputime;

%%
%% VB learning
% $$$ K = myfeval(covfuncW{1}{:}, logthetaW{1}, inW, inW);
% $$$ figure
% $$$ imagesc(K)
% $$$ return

% Index order (component-wise, i.e., block-diagonal K)
indsW = reshape(1:(D*M), [M D]);
indsX = reshape(1:(D*N), [N D])';

CovW = eye(D*M);
CovX = eye(D*N);

% $$$ %% TEMP
% $$$ W(:) = 1;
% $$$ CovW(:) = 0;

% Initialize help variables
WW = zeros(D,D,M);
for m=1:M
  WW(:,:,m) = W(m,:)'*W(m,:) + CovW(indsW(m,:),indsW(m,:));
end
XX = zeros(D,D,N);
for n=1:N
  XX(:,:,n) = X(:,n)*X(:,n)' + CovX(indsX(:,n),indsX(:,n));
end

% Evaluate prior covariance matrices
KW = zeros(D*M);
KX = zeros(D*N);
for d=1:D
  first = (d-1)*M + 1;
  last = first + M - 1;
  inds = first:last;
  covfunc = covfuncW{d};
  if ~iscell(covfunc)
    covfunc = {covfunc};
  end
  KW(inds,inds) = feval(covfunc{:}, logthetaW{d}, inW, inW);

  first = (d-1)*N + 1;
  last = first + N - 1;
  inds = first:last;
  covfunc = covfuncX{d};
  if ~iscell(covfunc)
    covfunc = {covfunc};
  end
  KX(inds,inds) = feval(covfunc{:}, logthetaX{d}, inX, inX);
end
% $$$ KW = regularize(KW, 1e1);
% $$$ LKW = chol(KW, 'lower');
% $$$ KX = regularize(KX, 1e2);
% $$$ LKX = chol(KX, 'lower');

% $$$ figure
% $$$ imagesc(KW)
% $$$ figure
% $$$ imagesc(KX)

reg = 1e-6;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% VB EM

for ind=1:opts.maxiter

  oldW = W;

  startitercpu = cputime;

  %% UPDATE VARIABLES

  update = false;
  if length(opts.updatehyper) > 1
    if any(ind==opts.updatehyper)
      update = true;
    end
  else
    if mod(ind, opts.updatehyper) == 0 && opts.updatehyper ~=0
      update = true;
    end
  end
  if update
    maxsearchx = opts.maxsearchx;
    maxsearchw = opts.maxsearchw;
  else
    maxsearchx = 0;
    maxsearchw = 0;
  end

  if ind < 5 && false
    % Scale X to unit variance. I think it should improve the lower bound
    % but it doesn't seem to do so in practice (at least always).. ??
    disp('Using adhoc whitening of second moment')
    %xx = diag(sum(XX,3))/N - mean(X,2).^2;   % variance
    xx = diag(sum(XX,3))/N;   % second moment
    sqrtxx = sqrt(xx);
    X = bsxfun(@rdivide, X, sqrtxx);
    XX = bsxfun(@rdivide, XX, (sqrtxx(:)*sqrtxx(:)'));
    W = bsxfun(@times, W, sqrtxx');
    WW = bsxfun(@times, WW, (sqrtxx(:)*sqrtxx(:)'));
  end
  %XX_norm = mean(XX,3)

  %% Update W
  U = spalloc(D*M,D*M, M*D*D);
  z = zeros(D*M,1);
  for m=1:M
    obs = Obs(m,:);
    U(indsW(m,:),indsW(m,:)) = tau * sum(XX(:,:,obs),3);
    z(indsW(m,:)) = tau * X(:,obs) * Y(m,obs)';
  end
  % Optimise hyperparameters
  if update
    logthetaW = optimise_hyperparameter(logthetaW, covfuncW, inW, U, z, D, M);
  end
  % Update prior covariance matrix
  KW = zeros(D*M);
  for d=1:D
    first = (d-1)*M + 1;
    last = first + M - 1;
    inds = first:last;
    covfunc = covfuncW{d};
    if ~iscell(covfunc)
      covfunc = {covfunc};
    end
    KW(inds,inds) = feval(covfunc{:}, logthetaW{d}, inW, inW);

    KW = KW + reg*eye(size(KW));

  end
  %KW = regularize(KW, 1e1);
  %LKW = chol(KW, 'lower');
  % Evaluate posterior
  if false % choose method
    L = chol(KW + KW*U*KW, 'lower');
    sqrtCovW = solve_tril(L, KW);
    CovW = sqrtCovW' * sqrtCovW;
    W(:) = CovW(indsW(:),indsW(:)) * z(indsW(:));
  else
    % Prefer this!
%    L = safechol(KW + inv(U), 1e-8, 'lower');
    L = chol(KW + inv(U), 'lower');
    tmp = solve_tril(L, KW);
    CovW = KW - tmp' * tmp;
    tmp = linsolve_chol(L, U \ z, 'lower');
    W(:) = KW(indsW(:),indsW(:)) * tmp(indsW(:));
  end
  WW = zeros(D,D,M);
  for m=1:M
    WW(:,:,m) = W(m,:)'*W(m,:) + CovW(indsW(m,:),indsW(m,:));
  end
% $$$   figure
% $$$   imagesc(WW(:,:,1))
% $$$   error('jou')

% $$$   figure
% $$$   imagesc(CovW)
% $$$   error('halo')

  %% Update X
  U = spalloc(D*N,D*N, N*D*D);
  z = zeros(D*N,1);
  for n=1:N
    obs = Obs(:,n);
    U(indsX(:,n),indsX(:,n)) = tau * sum(WW(:,:,obs),3);
    z(indsX(:,n)) = tau * W(obs,:)' * Y(obs,n);
  end
  % Optimise hyperparameters
  if update
    logthetaX = optimise_hyperparameter(logthetaX, covfuncX, inX, U, z, D, N);
  end
  % Update prior covariance matrix
  KX = zeros(D*N);
  for d=1:D
    first = (d-1)*N + 1;
    last = first + N - 1;
    inds = first:last;
    covfunc = covfuncX{d};
    if ~iscell(covfunc)
      covfunc = {covfunc};
    end
    KX(inds,inds) = feval(covfunc{:}, logthetaX{d}, inX, inX);

    KX = KX + reg*eye(size(KX));

  end
  %KX = regularize(KX, 1e2);
  %LKX = chol(KX, 'lower');
  % Evaluate posterior
% $$$   figure
% $$$   imagesc(inv(U))
% $$$   figure
% $$$   imagesc(KX)
% $$$   tmp = chol(U, 'lower');
% $$$   figure
% $$$   imagesc(linsolve_chol(tmp, eye(size(tmp))) == 0);
  if false
    L = chol(KX + KX*U*KX, 'lower');
    sqrtCovX = solve_tril(L, KX);
    CovX = sqrtCovX' * sqrtCovX;
    X(:) = CovX(indsX(:),indsX(:)) * z(indsX(:));
  else
    % Prefer this!
    L = chol(KX + inv(U), 'lower');
%    L = safechol(KX + inv(U), 1e-8, 'lower');
    tmp = solve_tril(L, KX);
    CovX = KX - tmp' * tmp;
    tmp = linsolve_chol(L, U \ z, 'lower');
    X(:) = KX(indsX(:),indsX(:)) * tmp(indsX(:));
  end
  XX = zeros(D,D,N);
  for n=1:N
    XX(:,:,n) = X(:,n)*X(:,n)' + CovX(indsX(:,n),indsX(:,n));
  end

  %XX_norm = mean(XX,3)

%  CovX(1:10,1:10)
%  tmp = chol(KX, 'lower');

% $$$   figure
% $$$   imagesc(CovX)
% $$$   error('halo')

% $$$   rcondX = rcond(CovX)
% $$$   rcondestX = rcond(CovX + 1e-4*normest(CovX)*eye(size(CovX)))
% $$$   tmp = chol(CovX,'lower');

%  [X,CovX,W,CovW] = rotate(X,CovX,indsX,LKX,W,CovW);

% $$$   for d=1:D
% $$$     others = [1:(d-1), (d+1):D];
% $$$     V = spalloc(M,M,M);
% $$$     c = zeros(M,1);
% $$$     for m=1:M
% $$$       obs = Obs(m,:);
% $$$       V(m,m) = tau * (X(d,obs)*X(d,obs)' + sum(varX(d,obs)));
% $$$       c(m) = tau * X(d,obs) * (Y(m,obs) - W(m,others)*X(others,obs))';
% $$$     end
% $$$
% $$$     if usepseudoW(d)
% $$$       pseudos = pseudoW{d};
% $$$     else
% $$$       pseudos = [];
% $$$     end
% $$$     [Wp{d}, CovWp{d}, pseudos, logthetaW{d}, tmp, Kwp, cholKWpp{d}] = ...
% $$$         gplearn(logthetaW{d}, covfuncW{d}, inW, [], V, pseudos, 'vy', c, ...
% $$$                 'maxsearch', maxsearchw, 'checkgrad', opts.checkgradw, ...
% $$$                 'updatepseudo', opts.updatepseudow);
% $$$
% $$$     if ~usepseudoW(d)
% $$$       % full
% $$$       W(:,d) = Wp{d};
% $$$       varW(:,d) = diag(CovWp{d});
% $$$     else
% $$$       % using pseudo inputs
% $$$       pseudoW{d} = pseudos;
% $$$       [W(:,d), varW(:,d)] = gppred(pseudoW{d}, Wp{d}, CovWp{d}, inW, ...
% $$$                                    logthetaW{d}, covfuncW{d}, 'cholkx', ...
% $$$                                    cholKWpp{d}, 'khx', Kwp);
% $$$     end
% $$$     ltW_in_vbgppca = logthetaW{d};
% $$$   end

% $$$   %% Update X
% $$$   for d=1:D
% $$$     others = [1:(d-1), (d+1):D];
% $$$     V = spalloc(N,N,N);
% $$$     c = zeros(N,1);
% $$$     for n=1:N
% $$$       obs = Obs(:,n);
% $$$       V(n,n) = tau * (W(obs,d)'*W(obs,d) + sum(varW(obs,d)));
% $$$       c(n) = tau * W(obs,d)' * (Y(obs,n) - W(obs,others)*X(others,n));
% $$$     end
% $$$     if usepseudoX(d)
% $$$       pseudos = pseudoX{d};
% $$$     else
% $$$       pseudos = [];
% $$$     end
% $$$     [Xp{d}, CovXp{d}, pseudos, logthetaX{d}, tmp, Kxp, cholKXpp{d}] = ...
% $$$         gplearn(logthetaX{d}, covfuncX{d}, inX, [], V, pseudos, 'vy', c, ...
% $$$                 'maxsearch', maxsearchx, 'checkgrad', opts.checkgradx, ...
% $$$                 'updatepseudo', opts.updatepseudox);
% $$$
% $$$     if ~usepseudoX(d)
% $$$       % full
% $$$       X(d,:) = Xp{d};
% $$$       varX(d,:) = diag(CovXp{d});
% $$$     else
% $$$       % pseudo inputs
% $$$       pseudoX{d} = pseudos;
% $$$       [X(d,:), varX(d,:)] = gppred(pseudoX{d}, Xp{d}, CovXp{d}, inX, ...
% $$$                                    logthetaX{d}, covfuncX{d}, 'cholkx', ...
% $$$                                    cholKXpp{d}, 'khx', Kxp);
% $$$     end
% $$$
% $$$     ltX_in_vbgppca = logthetaX{d};
% $$$   end

  disp('Updating tau')

  %% Update tau
  % TODO: You could loop over the longer dimension N or M? Then
  % vectorization would help the most. :)
  err2 = 0;
  for n=1:N
    obs = Obs(:,n);
    ymu = Y(obs,n) - mu(obs);
    err2 = err2 ...
           + ymu'*ymu ...
           + traceprod(sum(WW(:,:,obs),3), XX(:,:,n)) ...
           + sum(v_mu) ...
           - 2*(W(obs,:)*X(:,n))' * ymu;
  end
  a_tau = atau + 0.5*NMobs;
  b_tau = btau + 0.5*err2;
  tau = a_tau / b_tau;
  logtau = psi(a_tau) - log(b_tau);

  % CPU time used for calculations
  itercpu = cputime - startitercpu;

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  %% CALCULATE LOWER BOUND OF THE LOG-LIKELIHOOD

  disp('Evaluating lower bound.');

  if opts.loglikelihood

    startcostcpu = cputime;
    oldlogP = logP;

    % Cost from Y
    cost_Y = -0.5*tau*err2 - 0.5*NMobs*(log2pi - logtau);

    % WTF!?!?!?!!! If using safechol, the coefficient has a HUUUGE effect
    % on the loglikelihood lower bound!!!

    % Cost from W
    %rcond_KW = rcond(KW)
%    LKW = chol(KW(indsW(:),indsW(:)), 'lower');
    LKW = chol(KW(indsW(:),indsW(:))+reg*eye(size(KW)), 'lower');
%    LKW = safechol(KW(indsW(:),indsW(:)), 1e-15, 'lower');
    cost_W = mvnvbcost(W(:),CovW(indsW(:),indsW(:)), 0,0, LKW, 2*logdettri(LKW));

% $$$     cost_W = 0;
% $$$     for d=1:D
% $$$       cost_W = cost_W + mvnvbcost(Wp{d},CovWp{d}, 0,0, cholKWpp{d}, ...
% $$$                                   2*logdettri(cholKWpp{d}));
% $$$     end

    % Cost from X
%    rcond_KX = rcond(KX)
%    LKX = chol(KX(indsX(:),indsX(:)), 'lower');
    LKX = chol(KX(indsX(:),indsX(:))+reg*eye(size(KX)), 'lower');
%    LKX = safechol(KX(indsX(:),indsX(:)), 1e-15, 'lower');
    cost_X = mvnvbcost(X(:),CovX(indsX(:),indsX(:)), 0,0, LKX, 2*logdettri(LKX));
% $$$     cost_X = 0;
% $$$     for d=1:D
% $$$       cost_X = cost_X + mvnvbcost(Xp{d},CovXp{d}, 0,0, cholKXpp{d}, ...
% $$$                                   2*logdettri(cholKXpp{d}));
% $$$     end

    % Cost from tau
    cost_tau = -gamkl(tau,logtau,a_tau,b_tau,atau,btau);
% $$$
% $$$     oldlogP = logP;
% $$$
% $$$     % Lower bound of loglikelihood
    logP = cost_Y + cost_W + cost_X + cost_tau;% + cost_mu + cost_tau + cost_w;
    costcpu = cputime - startcostcpu;

    if logP < oldlogP
      warning('Cost increased!!');
    end

  end

  disp('Monitoring stuff.')

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  %% MONITORING STUFF

  % Change in subspace
  angle = subspace(oldW, W);

  %% Print progress
  fprintf('%d step: loglike=%e, angle=%.2e (itercpu=%.2fs, nllcpu=%.2fs)\n', ...
          ind, logP, angle, itercpu, costcpu);

  %% Save
  loglikelihood = logP;
  if opts.autosavetime > 0 && cputime - lastsave > opts.autosavetime
    fprintf('Saving to a file %s..', opts.autosavefile);
    iteration = ind;
    save(opts.autosavefile, 'Wp', 'CovWp', 'W', 'varW', 'pseudoW', ...
         'logthetaW', 'covfuncW', 'inW', 'Xp', 'CovXp', 'X', 'varX', ...
         'pseudoX', 'logthetaX', 'covfuncX', 'inX', 'tau', 'a_tau', 'b_tau', ...
         'loglikelihood', 'ind');
    fprintf(' done\n');
    lastsave = cputime;
  end

end

% $$$ figure
% $$$ imagesc(CovX == 0)

% Predict whole dataset, also rows and columns that were removed at the
% beginning because of they were empty
if opts.reconstruct
  if any(rowrm)
    % Change variables
    W_old = W;
    KW_old = KW;
    CovW_old = CovW;
    Morig = size(inWorig,2);
    W = zeros(Morig,D);
    KW = zeros(D*Morig);
    KWWold = zeros(D*Morig,D*M);
    CovW = zeros(D*Morig);
    indsWorig = reshape(1:(D*Morig), [Morig D]);
    % New prior covariance matrix
    for d=1:D
      first = (d-1)*Morig + 1;
      last = first + Morig - 1;
      inds = first:last;
      first = (d-1)*Morig + 1;
      last = first + Morig - 1;
      inds_old = first:last;
      covfunc = covfuncW{d};
      if ~iscell(covfunc)
        covfunc = {covfunc};
      end
      KWWold(inds,inds_old) = feval(covfunc{:}, logthetaW{d}, inWorig, inW);
      KW(inds,inds) = feval(covfunc{:}, logthetaW{d}, inWorig, inWorig);
      KW = KW + reg*eye(size(KW));
    end
    % New posterior quantities
    LKW_old = chol(KW_old(indsW(:),indsW(:)), 'lower');
    W(:) = KWWold(indsWorig(:),indsW(:)) * linsolve_chol(LKW_old, W_old(:), ...
                                                      'lower');
    A = linsolve_chol(LKW_old, KWWold(indsWorig(:),indsW(:))', 'lower');
    CovW(indsWorig(:),indsWorig(:)) = KW(indsWorig(:),indsWorig(:)) - ...
        A'*(KW_old(indsW(:),indsW(:))-CovW(indsW(:),indsW(:)))*A;

    M = Morig;
    indsW = indsWorig;
    inW = inWorig;
  end
  if any(colrm)
% $$$     figure
% $$$     imagesc(CovX)

    % Change variables
    X_old = X;
    KX_old = KX;
    CovX_old = CovX;
    Norig = size(inXorig,2);
    X = zeros(D,Norig);
    KX = zeros(D*Norig);
    KXXold = zeros(D*Norig,D*N);
    CovN = zeros(D*Norig);
    indsXorig = reshape(1:(D*Norig), [Norig D])';
    % New prior covariance matrix
    for d=1:D
% $$$       first = (d-1)*Morig + 1;
% $$$       last = first + Morig - 1;
% $$$       inds = first:last;
      first = (d-1)*Norig + 1;
      last = first + Norig - 1;
      inds = first:last;
      first = (d-1)*N + 1;
      last = first + N - 1;
      inds_old = first:last;
      covfunc = covfuncX{d};
      if ~iscell(covfunc)
        covfunc = {covfunc};
      end
      KXXold(inds,inds_old) = feval(covfunc{:}, logthetaX{d}, inXorig, inX);
      KX(inds,inds) = feval(covfunc{:}, logthetaX{d}, inXorig, inXorig);
      KX = KX + reg*eye(size(KX));
    end
    % New posterior quantities
    LKX_old = chol(KX_old(indsX(:),indsX(:)), 'lower');
    X(:) = KXXold(indsXorig(:),indsX(:)) * linsolve_chol(LKX_old, X_old(:), ...
                                                      'lower');
    A = linsolve_chol(LKX_old, KXXold(indsXorig(:),indsX(:))', 'lower');
    CovX(indsXorig(:),indsXorig(:)) = KX(indsXorig(:),indsXorig(:)) - ...
        A'*(KX_old(indsX(:),indsX(:))-CovX(indsX(:),indsX(:)))*A;

% $$$     figure
% $$$     imagesc(CovX)

    N = Norig;
    indsX = indsXorig;
    inX = inXorig;
  end
end
% $$$ if opts.reconstruct
% $$$   if any(rowrm)
% $$$     oldW = W;
% $$$     KW_old = KW;
% $$$     KW = feval(covfunc{:}, logthetaW{d}, inWorig, inWorig);
% $$$     W = zeros(Morig,D);
% $$$     varW = zeros(Morig,D);
% $$$     for d=1:D
% $$$       [W(:,d), varW(:,d)] = gppred(inW, oldW(:,d), CovW(indsW(:,d),indsW(:,d)), ...
% $$$                                    inWorig, logthetaW{d}, covfuncW{d});
% $$$     end
% $$$   end
% $$$   if any(colrm)
% $$$     oldX = X;
% $$$     X = zeros(D,Norig);
% $$$     varX = zeros(D,Norig);
% $$$     for d=1:D
% $$$       [X(d,:), varX(d,:)] = gppred(inX, oldX(d,:)', CovX(indsX(d,:),indsX(d,:)), ...
% $$$                                    inXorig, logthetaX{d}, covfuncX{d});
% $$$     end
% $$$   end
% $$$ end

varX = reshape(diag(CovX), [N D])';
varW = reshape(diag(CovW), [M D]);

if nargout == 1
  Q.W = W;
  Q.CovW = CovW;
% $$$   Q.W = W;
  Q.varW = varW;
% $$$   Q.pseudoW = pseudoW;
  Q.logthetaW = logthetaW;
  Q.covfuncW = covfuncW;
  Q.inW = inWorig;

  Q.X = X;
  Q.CovX = CovX;
% $$$   Q.X = X;
  Q.varX = varX;
% $$$   Q.pseudoX = pseudoX;
  Q.logthetaX = logthetaX;
  Q.covfuncX = covfuncX;
  Q.inX = inXorig;

  Q.tau = tau;
  Q.a_tau = a_tau;
  Q.b_tau = b_tau;

  Q.indsW = indsW;
  Q.indsX = indsX;

  Q.loglikelihood = loglikelihood;

  W = Q;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function logtheta = optimise_hyperparameter(logtheta, covfuncs, input, U, ...
                                            z, D, N)

% Because the cell logtheta must be presented as one long vector, mark
% the indeces for each component. That is, the hyperparameters of d-th
% component are vector( (indeces(d-1)+1):indeces(d) ) = logtheta{d}
vector = [];
indeces = zeros(D,1);
for d=1:D
  vector = [vector; logtheta{d}(:)];
  indeces(d) = length(logtheta{d});
end
indeces = cumsum(indeces);

invU = inv(U);

%hyperparameters_before = exp(vector(:))

% $$$ mycheckgrad(@hyperparameter_upperbound, vector, 1e-9, covfuncs, input, ...
% $$$             invU, z, D, N, indeces);

vector = minimize(vector, @hyperparameter_upperbound, 5, covfuncs, input, ...
                  invU, z, D, N, indeces);

%hyperparameters_after = exp(vector(:))

ind = 1;
for d=1:D
  logtheta{d} = vector(ind:indeces(d));
  ind = indeces(d) + 1;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [f, df] = hyperparameter_upperbound(logtheta_vector, covfuncs, ...
                                             input, invU, z, D, N, ...
                                             logtheta_indeces)

% Projected pseudo-observations
y = invU * z;

% Transform the hyperparameter vector to cell
ind = 1;
for d=1:D
  logtheta{d} = logtheta_vector(ind:logtheta_indeces(d));
  ind = logtheta_indeces(d) + 1;
end

dlogtheta = cell(D,1);

K = zeros(D*N);
for d=1:D
  % Indeces of a component block
  first = (d-1)*N + 1;
  last = first + N - 1;
  inds = first:last;
  % Covariance matrix for the block
  covfunc = covfuncs{d};
  if ~iscell(covfunc)
    covfunc = {covfunc};
  end
  [K(inds,inds), dlogtheta{d}] = feval(covfunc{:}, logtheta{d}, input, input);
end
%K = regularize(K, 1e2);
%LK = chol(K, 'lower');
%K =

%if rcond(K) < 1e-12
%  f = -

L = chol(K + invU, 'lower');

% Loglikelihood lower bound (mnorm_lpdf)
%f = mnorm_lpdf((invU*z)', 0, K + invU);
v = solve_tril(L, (y-0));
f = -0.5*length(L)*log(2*pi) - logdettri(L) - 0.5 * v'*v;

% Gradients
df = [];
% TODO: USE LINSOLVE TO OPTIMISE BACKSUBSTITUTIONS!!!!
%invLL = L' \ (L \ eye(size(L)));
%c = invLL * y;
c = linsolve_chol(L, y, 'lower');
for d=1:D
  % Indeces of a component block
  first = (d-1)*N + 1;
  last = first + N - 1;
  inds = first:last;
  Ld = L(inds,inds);
  invL = linsolve_chol(Ld, eye(size(Ld)), 'lower');
  % Evaluate gradients
  for i=1:length(logtheta{d})
    dK = dlogtheta{d}(:,:,i);
    %sz_c = size(c)
    %sz_dK = size(dK)
    df = [df; 0.5*c(inds)'*dK*c(inds) - 0.5*traceprod(invL,dK)];
  end
end

% Lower bound to upper bound (because using minimize)
f = -f;
df = -df;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [X,CovX,W,CovW] = rotate(X,CovX,indsX,LKX,W,CovW)

error('Ei taa oikein toimi, ei kannata kayttaa..')

disp('Finding rotation..')
[D,N] = size(X);
R = orth(randn(D));
R = R*R';

%mycheckgrad(@rotation_cost, R(:), 1e-6, X,CovX,indsX,LKX);

R = minimize(R(:), @rotation_cost, 3, X,CovX,indsX,LKX);

R = reshape(R,[D,D]);

% $$$ % Force positive definiteness
% $$$ [VR,DR] = eig(R);
% $$$ R = VR * exp(DR) * inv(VR);

X = R * X;
kronR = kron(R,eye(N));
CovX = kronR * CovX * kronR';

M = rows(W);
invR = inv(R);
W = W * invR;
kronR = kron(invR,eye(M));
CovW = kronR' * CovW * kronR;
disp('Rotation done.');

R
%error('done')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [f,df] = rotation_cost(R,X,CovX,indsX,LKX)
[D,N] = size(X);
R = reshape(R,[D D]);

% Force positive definiteness
% $$$ [VR,DR] = eig(R);
% $$$ R = VR * exp(DR) * inv(VR);

log_qX = -N * logdet(R);
if ~isreal(log_qX)
  f = inf;
  df = nan*ones(D*D,1);
  return
end

LinvKX = solve_tril(LKX, eye(D*N));
invKX = LinvKX' * LinvKX;
%invKX = solve_triu(LKX', solve_tril(LKX, eye(D*N)));
H = 0;
for n=1:N
  for m=1:N
    in = indsX(:,n);
    im = indsX(:,m);
    H = H + invKX(in,im) * R * (X(:,m)*X(:,n)' + CovX(im,in));
  end
end
error('Check that traceprod')
log_pX = -0.5 * traceprod(H,R);

f = -(log_pX - log_qX)
df = N*inv(R') - H;
df = -df(:);

% Test:
X = R * X;
kronR = kron(R,eye(N));
CovX = kronR * CovX * kronR';
cost_X = mvnvbcost(X(:),CovX(indsX(:),indsX(:)), 0,0, LKX, 2*logdettri(LKX))

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function ind = row2ind(M,N,m)
ind = (0:(N-1)) * M + m;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function ind = col2ind(M,N,n)
ind = (1:M) + (n-1)*M;

%%%%%%%%%%%%%%%%%%%%%
function Y = gpinv(X)
[Q,R] = qr(X);
Y = inv(R) * Q';

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [W,varW,X,varX,tau] = initialize(init,D,inW,inX,covfuncW, ...
                                          logthetaW,covfuncX, logthetaX)

M = cols(inW);
N = cols(inX);

% W
if isstruct(init) && isfield(init, 'W') && ~isempty(init.W)
  disp('Initialize W to given value');
  W = init.W;
else
%  W = randn(M,D);
  disp('Initialize W to zero');%by sampling');
  W = zeros(M,D);
% $$$   for d=1:D
% $$$     W(:,d) = gprnd(inW, logthetaW{d}, covfuncW{d});
% $$$   end
end

% varW
if isstruct(init) && isfield(init, 'varW') && ~isempty(init.varW)
  varW = init.varW;
else
  varW = 1*ones(M,D);
end

% X
if isstruct(init) && isfield(init, 'X') && ~isempty(init.X)
  disp('Initialize X to given value');
  X = init.X;
else
  %X = randn(D,N);
  disp('Initialize X by sampling');
  X = zeros(D,N);
  for d=1:D
    X(d,:) = gprnd(inX, logthetaX{d}, covfuncX{d});
  end
end

% varX
if isstruct(init) && isfield(init, 'varX') && ~isempty(init.varX)
  varX = init.varX;
else
  varX = 1 * ones(D,N);
end

%% tau precision (inverse noise)
if isstruct(init) && isfield(init, 'tau') && ~isempty(init.tau)
  tau = init.tau;
else
  tau = 1e3;
end


function z = conv(x,y)

%Disciplined convex programming information for CONV:
%   Convolutions are a combination of both multiplications and additions.
%   Therefore, to insure that the no-product rule is satisfied, CONV(X,Y)
%   requires that either X or Y be constant. Further conditions may also
%   apply depending on the exact content of X and Y. For example, if the
%   elements of Y are all convex, then the convolution kernel X must be
%   real, and all of the elements must have the same sign.
%
%Disciplined geometric programming information for TIMES:
%   CONV(X,Y) requires that either X or Y be constant, and that the
%   non-constant term be log-convex or log-affine. Strictly speaking,
%   CONV(X,Y) where X and Y are both log-convex satisfies the DGP rulest,
%   but this version does not support that scenario.

narginchk(2,2);
sx = size(x);
sy = size(y);
if sum(sx~=1)>1 || sum(sy~=1)>1,

    error( 'Arguments must be vectors.' );

elseif any(sx==0) && any(sy==0),

    error( 'At least one argument must be non-empty.' );

elseif cvx_isconstant(x) || cvx_isconstant(y),

    sz = sy;
    sx = prod(sx);
    sy = prod(sy);
    nz = sx + sy - 1;
    sz(sz>1) = nz;
    if cvx_isconstant(x),
        [xi,xj,xv] = find(cvx_constant(x));
        yb         = cvx_basis(y);
    else
        [xi,xj,xv] = find(cvx_constant(y));
        yb         = cvx_basis(x);
    end
    [yi,yj,yv] = find(yb);
    nx = length(xv);
    ny = length(yv);
    ox = ones(1,nx);
    yi = yi(:,ox);
    xi = reshape( xi + xj - 2, 1, nx );
    yj = yj(:,ox) + xi(ones(ny,1),:);
    yv = yv * reshape( xv, 1, nx );
    z  = sparse( yi, yj, yv, size(yb,1), nz );
    z  = cvx( sz, z );
    if nnz( cvx_classify( z ) == 13 ),
        error( 'Disciplined convex programming error:\n   Illegal affine combination of convex/concave terms in convolution.' );
    end

else

    error( 'At least one argument must be constant.' );

end

% Copyright 2005-2016 CVX Research, Inc.
% See the file LICENSE.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function elementalWeightMatrix = getElementalWeightMatrix(isotopeAbundance)
% Get the symbols for all chemical elements and their atomic weights
%
% USAGE:
%    elementalWeightMatrix = getElementalWeightMatrix(isotopeAbundance)
%
% OPTIONAL INPUTS:
%    isotopeAbundance:         * false to return standard atomic weights (default)
%                              * true to return the weights of naturally predominant isotopes for biological
%                                elements and standard weights for other elements.
%                              * `m` x 3 cell arrray with user defined isotope abundance:
%                                isotopeAbundance{i, 1} = 'Atomic_Symbol';
%                                isotopeAbundance{i, 2} = Mass_Number;
%                                isotopeAbundance{i, 3} = abundance;
%                                (where sum of abundances of all isotopes of an element must be one)
%    printWarning:             true to print warning if there are unknown elements in isotopeAbundance
%
% OUTPUT:
%    elementalWeightMatrix:    #elements x 2 cell array, the 1st coloumn being the symbols
%                              of chemial elements and the 2nd their standard atomic weights

if nargin == 0 || isempty(isotopeAbundance)
    isotopeAbundance = false;
end

atomicWeights = parse_Atomic_Weights_and_Isotopic_Compositions_for_All_Elements;
[symbols, ia] = unique([atomicWeights.data.AtomicSymbol]);
% remove XYZ (non-existing element) from the symbol list
elementRemove = strcmp(symbols, 'D') | strcmp(symbols, 'T') | strncmp(symbols, 'XYZ', 3);
[symbols, ia] = deal(columnVector(symbols(~elementRemove)), ia(~elementRemove));
% identify elements without standard weight
elementWoMW = arrayfun(@(x) isempty(x.StandardAtomicWeight), atomicWeights.data(ia));
% elements with standard weight come first
symbols = [symbols(~elementWoMW); symbols(elementWoMW)];
elementWeights = [[atomicWeights.data(ia(~elementWoMW)).StandardAtomicWeight], NaN(1, sum(elementWoMW))]';

if ~isscalar(isotopeAbundance) || isotopeAbundance

    % biological elements as defined in getMolecularMass.m
    %allBiologicalElements = {'C', 'O', 'P', 'N', 'S', 'H', 'Mg', 'Na', 'K', 'Cl', 'Ca', 'Zn', 'Fe', 'Cu', 'Mo', 'I'};
    if isscalar(isotopeAbundance)
        % use the defaulted most naturally predominant isotope of each biological element
        isotopeAbundance = {...
            'C', 12, 1; ...  % Carbon, all as C12
            'H', 1, 1; ...  % Hydrogen, all as H1
            'O', 16, 1; ...  % Oxygen, all as O16
            'N', 14, 1; ...  % Nitrogen, all as N14
            'S', 32, 1; ...  % Sulphur, all as S32 (94% naturally S32)
            'Mg', 24, 1; ...  % Magnesium, all as Mg24, (79 % naturally Mg24)
            'K', 39, 1; ...  % Potassium, all as K39, (93% naturally K39)
            'Cl', 35, 1; ...  % Chlorine, all as Cl35 (75.78% naturally Cl35)
            'Ca', 40, 1; ...  % Calcium, all as Ca40 (96% naturally Ca40)
            'Zn', 64, 1; ...  % Zinc, all as Zn64 (48% naturally Zn64)
            'Cu', 63, 1; ...  % Copper, all as Cu63 (69% naturally Cu63)
            'Mo', 98, 1; ...  % Molybdenum, all as Mo98 (24% naturally Mo98)
            'P', 31, 1; ...  % Phosphorus, all as P31
            'Na', 23, 1; ...  % Sodium, all as Na23
            'Fe', 56, 1; ...  % Iron, all as Fe56
            'I', 127, 1; ...  % Iodine, all as I127
            };
    end

    % get the list of all isotopes
    [isotopes, isoMassNums, isoWeights] = deal([atomicWeights.data.AtomicSymbol], ...
        [atomicWeights.data.MassNumber], [atomicWeights.data.RelativeAtomicMass]);
    % treat 'D' and 'T' as the same element as 'H'
    isotopes(strcmp(isotopes, 'D') | strcmp(isotopes, 'T')) = {'H'};
    isotopeAbundance(strcmp(isotopeAbundance(:, 1), 'D') | ...
        strcmp(isotopeAbundance(:, 1), 'T')) = {'H'};

    [eleWtIsoAb, ~, ib] = unique(isotopeAbundance(:, 1));
    [ynE, idE] = ismember(eleWtIsoAb, symbols);

    % sanity check
    if size(isotopeAbundance, 2) ~= 3
        error('Incorrect size. IsotopeAbundance must be a n-by-3 cell array.')
    elseif ~all(ynE)
        % unknown elements in isotope abundance profile
        error('%s in the supplied isotope abundance profile is/are not standard chemical elements.', strjoin(eleWtIsoAb(~ynE), ', '))
    end

    massNumbers = cell2mat(isotopeAbundance(:, 2));
    abundances = cell2mat(isotopeAbundance(:, 3));
    if any(massNumbers <= 0)  % positive masses
        error('Non-positive isotope mass in row(s) %s.', strjoin(cellstr(num2str(find(massNumbers <= 0))), ', '))
    elseif any(abundances < 0 | abundances > 1)  % 0 <= abundnace <= 1
        error('Invalid isotope abundance in row(s) %s. Must lie between 0 and 1.', ...
            strjoin(cellstr(num2str(find(abundances < 0 | abundances > 1))), ', '))
    end

    % get the order of the supplied isotopeAbundance in the atomic weight data
    eleWtIsoAbIDs = zeros(size(isotopeAbundance, 1), 1);
    for j = 1:size(isotopeAbundance, 1)
        f = strcmp(isotopeAbundance{j, 1}, isotopes(:)) & isoMassNums(:) == isotopeAbundance{j, 2};
        if sum(f) == 1
            eleWtIsoAbIDs(j) = find(f);
        end
    end
    % error if the isotopes cannot be matched
    if ~all(eleWtIsoAbIDs)
        notMatched = find(eleWtIsoAbIDs == 0);
        errMsg = '';
        for j = 1:numel(notMatched)
            errMsg = [errMsg, ['\n#' num2str(notMatched(j)) '\t' ...
                isotopeAbundance{notMatched(j), 1} '\t' num2str(isotopeAbundance{notMatched(j), 2})]];
        end
        error(['%s', errMsg], 'The following isotopes could not be identified:')
    end

    % get the corresponding relative atomic mass
    relativeMasses = columnVector(isoWeights(eleWtIsoAbIDs));

    % get the user-defined atomic masses
    sumIsoEq1 = false(numel(eleWtIsoAb), 1);
    averageWeight = zeros(numel(eleWtIsoAb), 1);
    for j = 1:numel(eleWtIsoAb)
        sumIsoEq1(j) = abs(sum(abundances(ib == j)) - 1) < 1e-7;
        averageWeight(j) = relativeMasses(ib == j)' * abundances(ib == j);
    end
    if any(~sumIsoEq1)  % sum(abundance) = 1
        error('Invalid isotope abundances for element(s) %s. The sum of isotope abundances for each element must be equal to 1.', ...
            strjoin(eleWtIsoAb(~sumIsoEq1), ', '))
    end

    % update the list of weights
    elementWeights(idE) = averageWeight;
end

elementalWeightMatrix = [symbols, num2cell(elementWeights)];


function [baseMVA, bus, gen, branch, areas, gencost] = c30
% CASE_IEEE30    Power flow data for IEEE 30 bus test case.
%   Please see 'help caseformat' for details on the case file format.
%   This data was converted from IEEE Common Data Format
%   (ieee30cdf.txt) on 20-Sep-2004 by cdf2matp, rev. 1.11
%   See end of file for warnings generated during conversion.
%
%   Converted from IEEE CDF file from:
%       http://www.ee.washington.edu/research/pstca/
%
%  08/20/93 UW ARCHIVE           100.0  1961 W IEEE 30 Bus Test Case

%   MATPOWER
%   $Id: case_ieee30.m,v 1.2 2004/09/21 01:45:05 ray Exp $

%%-----  Power Flow Data  -----%%
%% system MVA base
baseMVA = 100;

%% bus data
%	bus_i	type	Pd	Qd	Gs	Bs	area	Vm	Va	baseKV	zone	Vmax	Vmin
bus = [
	1	3	0	0	0	0	1	1.06	0	132	1	1.06	0.94;
	2	2	21.7	12.7	0	0	1	1.043	-5.48	132	1	1.06	0.94;
	3	1	2.4	1.2	0	0	1	1.021	-7.96	132	1	1.06	0.94;
	4	1	7.6	1.6	0	0	1	1.012	-9.62	132	1	1.06	0.94;
	5	2	94.2	19	0	0	1	1.01	-14.37	132	1	1.06	0.94;
	6	1	0	0	0	0	1	1.01	-11.34	132	1	1.06	0.94;
	7	1	22.8	10.9	0	0	1	1.002	-13.12	132	1	1.06	0.94;
	8	2	30	30	0	0	1	1.01	-12.1	132	1	1.06	0.94;
	9	1	0	0	0	0	1	1.051	-14.38	1	1	1.06	0.94;
	10	1	5.8	2	0	19	1	1.045	-15.97	33	1	1.06	0.94;
	11	2	0	0	0	0	1	1.082	-14.39	11	1	1.06	0.94;
	12	1	11.2	7.5	0	0	1	1.057	-15.24	33	1	1.06	0.94;
	13	2	0	0	0	0	1	1.071	-15.24	11	1	1.06	0.94;
	14	1	6.2	1.6	0	0	1	1.042	-16.13	33	1	1.06	0.94;
	15	1	8.2	2.5	0	0	1	1.038	-16.22	33	1	1.06	0.94;
	16	1	3.5	1.8	0	0	1	1.045	-15.83	33	1	1.06	0.94;
	17	1	9	5.8	0	0	1	1.04	-16.14	33	1	1.06	0.94;
	18	1	3.2	0.9	0	0	1	1.028	-16.82	33	1	1.06	0.94;
	19	1	9.5	3.4	0	0	1	1.026	-17	33	1	1.06	0.94;
	20	1	2.2	0.7	0	0	1	1.03	-16.8	33	1	1.06	0.94;
	21	1	17.5	11.2	0	0	1	1.033	-16.42	33	1	1.06	0.94;
	22	1	0	0	0	0	1	1.033	-16.41	33	1	1.06	0.94;
	23	1	3.2	1.6	0	0	1	1.027	-16.61	33	1	1.06	0.94;
	24	1	8.7	6.7	0	4.3	1	1.021	-16.78	33	1	1.06	0.94;
	25	1	0	0	0	0	1	1.017	-16.35	33	1	1.06	0.94;
	26	1	3.5	2.3	0	0	1	1	-16.77	33	1	1.06	0.94;
	27	1	0	0	0	0	1	1.023	-15.82	33	1	1.06	0.94;
	28	1	0	0	0	0	1	1.007	-11.97	132	1	1.06	0.94;
	29	1	2.4	0.9	0	0	1	1.003	-17.06	33	1	1.06	0.94;
	30	1	10.6	1.9	0	0	1	0.992	-17.94	33	1	1.06	0.94;
];

%% generator data
%	bus	Pg	Qg	Qmax	Qmin	Vg	mBase	status	Pmax	Pmin
gen = [
	1	260.2	-16.1	10	0	1.06	100	1	360.2	0;
	2	40	50	50	-40	1.045	100	1	140	0;
	5	0	37	40	-40	1.01	100	1	100	0;
	8	0	37.3	40	-10	1.01	100	1	100	0;
	11	0	16.2	24	-6	1.082	100	1	100	0;
	13	0	10.6	24	-6	1.071	100	1	100	0;
];

%% branch data
%	fbus	tbus	r	x	b	rateA	rateB	rateC	ratio	angle	status
branch = [
	1	2	0.0192	0.0575	0.0528	9900	0	0	0	0	1;
	1	3	0.0452	0.1652	0.0408	9900	0	0	0	0	1;
	2	4	0.057	0.1737	0.0368	9900	0	0	0	0	1;
	3	4	0.0132	0.0379	0.0084	9900	0	0	0	0	1;
	2	5	0.0472	0.1983	0.0418	9900	0	0	0	0	1;
	2	6	0.0581	0.1763	0.0374	9900	0	0	0	0	1;
	4	6	0.0119	0.0414	0.009	9900	0	0	0	0	1;
	5	7	0.046	0.116	0.0204	9900	0	0	0	0	1;
	6	7	0.0267	0.082	0.017	9900	0	0	0	0	1;
	6	8	0.012	0.042	0.009	9900	0	0	0	0	1;
	6	9	0	0.208	0	9900	0	0	0.978	0	1;
	6	10	0	0.556	0	9900	0	0	0.969	0	1;
	9	11	0	0.208	0	9900	0	0	0	0	1;
	9	10	0	0.11	0	9900	0	0	0	0	1;
	4	12	0	0.256	0	9900	0	0	0.932	0	1;
	12	13	0	0.14	0	9900	0	0	0	0	1;
	12	14	0.1231	0.2559	0	9900	0	0	0	0	1;
	12	15	0.0662	0.1304	0	9900	0	0	0	0	1;
	12	16	0.0945	0.1987	0	9900	0	0	0	0	1;
	14	15	0.221	0.1997	0	9900	0	0	0	0	1;
	16	17	0.0524	0.1923	0	9900	0	0	0	0	1;
	15	18	0.1073	0.2185	0	9900	0	0	0	0	1;
	18	19	0.0639	0.1292	0	9900	0	0	0	0	1;
	19	20	0.034	0.068	0	9900	0	0	0	0	1;
	10	20	0.0936	0.209	0	9900	0	0	0	0	1;
	10	17	0.0324	0.0845	0	9900	0	0	0	0	1;
	10	21	0.0348	0.0749	0	9900	0	0	0	0	1;
	10	22	0.0727	0.1499	0	9900	0	0	0	0	1;
	21	22	0.0116	0.0236	0	9900	0	0	0	0	1;
	15	23	0.1	0.202	0	9900	0	0	0	0	1;
	22	24	0.115	0.179	0	9900	0	0	0	0	1;
	23	24	0.132	0.27	0	9900	0	0	0	0	1;
	24	25	0.1885	0.3292	0	9900	0	0	0	0	1;
	25	26	0.2544	0.38	0	9900	0	0	0	0	1;
	25	27	0.1093	0.2087	0	9900	0	0	0	0	1;
	28	27	0	0.396	0	9900	0	0	0.968	0	1;
	27	29	0.2198	0.4153	0	9900	0	0	0	0	1;
	27	30	0.3202	0.6027	0	9900	0	0	0	0	1;
	29	30	0.2399	0.4533	0	9900	0	0	0	0	1;
	8	28	0.0636	0.2	0.0428	9900	0	0	0	0	1;
	6	28	0.0169	0.0599	0.013	9900	0	0	0	0	1;
];
mm=length(branch(:,1));
for i=1:mm
    if branch(i,9)==0;
        branch(i,9)=1;
    else
    end
end
%%-----  OPF Data  -----%%
%% area data
areas = [
	1	1;
];

%% generator cost data
%	1	startup	shutdown	n	x0	y0	...	xn	yn
%	2	startup	shutdown	n	c(n-1)	...	c0
gencost = [
	2	0	0	3	.5*0.02	2	0;
	2	0	0	3	.5*0.0175	1.75	0;
	2	0	0	3	.5*0.0625	1	0;
	2	0	0	3	.5*0.00834	3.25	0;
	2	0	0	3	.5*0.025	3	0;
	2	0	0	3	.5*0.025	3	0;
];

return;

% Warnings from cdf2matp conversion:
%
% ***** Qmax = Qmin at generator at bus    1 (Qmax set to Qmin + 10)
% ***** area data conversion not yet implemented (creating dummy area data)
% ***** MVA limit of branch 1 - 2 not given, set to 9900
% ***** MVA limit of branch 1 - 3 not given, set to 9900
% ***** MVA limit of branch 2 - 4 not given, set to 9900
% ***** MVA limit of branch 3 - 4 not given, set to 9900
% ***** MVA limit of branch 2 - 5 not given, set to 9900
% ***** MVA limit of branch 2 - 6 not given, set to 9900
% ***** MVA limit of branch 4 - 6 not given, set to 9900
% ***** MVA limit of branch 5 - 7 not given, set to 9900
% ***** MVA limit of branch 6 - 7 not given, set to 9900
% ***** MVA limit of branch 6 - 8 not given, set to 9900
% ***** MVA limit of branch 6 - 9 not given, set to 9900
% ***** MVA limit of branch 6 - 10 not given, set to 9900
% ***** MVA limit of branch 9 - 11 not given, set to 9900
% ***** MVA limit of branch 9 - 10 not given, set to 9900
% ***** MVA limit of branch 4 - 12 not given, set to 9900
% ***** MVA limit of branch 12 - 13 not given, set to 9900
% ***** MVA limit of branch 12 - 14 not given, set to 9900
% ***** MVA limit of branch 12 - 15 not given, set to 9900
% ***** MVA limit of branch 12 - 16 not given, set to 9900
% ***** MVA limit of branch 14 - 15 not given, set to 9900
% ***** MVA limit of branch 16 - 17 not given, set to 9900
% ***** MVA limit of branch 15 - 18 not given, set to 9900
% ***** MVA limit of branch 18 - 19 not given, set to 9900
% ***** MVA limit of branch 19 - 20 not given, set to 9900
% ***** MVA limit of branch 10 - 20 not given, set to 9900
% ***** MVA limit of branch 10 - 17 not given, set to 9900
% ***** MVA limit of branch 10 - 21 not given, set to 9900
% ***** MVA limit of branch 10 - 22 not given, set to 9900
% ***** MVA limit of branch 21 - 22 not given, set to 9900
% ***** MVA limit of branch 15 - 23 not given, set to 9900
% ***** MVA limit of branch 22 - 24 not given, set to 9900
% ***** MVA limit of branch 23 - 24 not given, set to 9900
% ***** MVA limit of branch 24 - 25 not given, set to 9900
% ***** MVA limit of branch 25 - 26 not given, set to 9900
% ***** MVA limit of branch 25 - 27 not given, set to 9900
% ***** MVA limit of branch 28 - 27 not given, set to 9900
% ***** MVA limit of branch 27 - 29 not given, set to 9900
% ***** MVA limit of branch 27 - 30 not given, set to 9900
% ***** MVA limit of branch 29 - 30 not given, set to 9900
% ***** MVA limit of branch 8 - 28 not given, set to 9900
% ***** MVA limit of branch 6 - 28 not given, set to 9900


%============================================================================
% Copyright (C) 2015, Heikki Hyyti
%
% Permission is hereby granted, free of charge, to any person obtaining a
% copy of this software and associated documentation files (the "Software"),
% to deal in the Software without restriction, including without limitation
% the rights to use, copy, modify, merge, publish, distribute, sublicense,
% and/or sell copies of the Software, and to permit persons to whom the
% Software is furnished to do so, subject to the following conditions:
%
% The above copyright notice and this permission notice shall be included in
% all copies or substantial portions of the Software.
%
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
% IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
% FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
% THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
% LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
% FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
% DEALINGS IN THE SOFTWARE.
%============================================================================

disp(' ');
disp('If you use the algorithm in any scientific context, please cite:');
disp('Heikki Hyyti and Arto Visala, "A DCM Based Attitude Estimation Algorithm');
disp('for Low-Cost MEMS IMUs," International Journal of Navigation and Observation,');
disp('vol. 2015, Article ID 503814, 18 pages, 2015. http://dx.doi.org/10.1155/2015/503814');
disp(' ');

clear all;
close all;
pause(0.1);

%% Configuration!
% select to test biases using Acceleration test (true) or Rotation test (false)
ACCtest = true;

% use c/compile.m before setting this to true (tested only with linux)
use_C_versions = false;

% fetch GPL licensed algortihms in matlab and C from here before use:
% http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
% Add the C code to c folder and matlab functions in this folder
% (please add also the quaternion library used by Madgwick).
% I apologize this mess. The Madgwick's GPS licenced code is separated from
% this MIT licenced product to avoid infecting this code with GPL licence.
use_comparisonAlgorithms = false;

if (ACCtest)
    disp('Calculating and plotting IMU bias test for Acceleration data');
else
    disp('Calculating and plotting IMU bias test for Rotation data');
end
data = load('allData.mat');

if (~exist('figures', 'dir')); mkdir('figures'); end;

ILtime = data.InertiaLink.tv_sec + (data.InertiaLink.tv_msec/1000);
SFtime = data.SparkFun6DOF.tv_sec + (data.SparkFun6DOF.tv_usec / 1000000);
Ktime = data.Kuka.tv_sec + (data.Kuka.tv_usec/1000000);

firstCommonTime = max([ILtime(1), SFtime(1), Ktime(1)]);
lastCommonTime = min([ILtime(end), SFtime(end), Ktime(end)]);
commonDuration = lastCommonTime - firstCommonTime;

ILtime = ILtime - firstCommonTime;
SFtime = SFtime - firstCommonTime;
Ktime = Ktime - firstCommonTime;

if (ACCtest)
    %ACC-test sequence: Linear motion with different accelerations
    StartIdx = find(ILtime > 468, 1, 'first');
    StopIdx = find(ILtime > 559, 1, 'first');
else
    %IMU-test sequence: free 6D motion
    StartIdx = find(ILtime > 564, 1, 'first');
    StopIdx = find(ILtime > 770, 1, 'first');
end

testName = 'Rotation test';
if (ACCtest)
    testName = 'Acceleration test';
end

markerSize = 8;

figPos = [105 105 645 660];

plotPos = [0.1 0.15 0.70 0.70];

plot21Pos = [0.1 0.53 0.86 0.40];
plot22Pos = plot21Pos + [0 -0.47 0 0];

plot1Pos = [0.1 0.69 0.86 0.25];
plot2Pos = plot1Pos + [0 -0.31 0 0];
plot3Pos = plot2Pos + [0 -0.31 0 0];

plot41Pos = [0.1 0.75 0.86 0.19];
plot42Pos = plot41Pos + [0 -0.23 0 0];
plot43Pos = plot42Pos + [0 -0.23 0 0];
plot44Pos = plot43Pos + [0 -0.23 0 0];

%% getting reference trajectory
R11 = data.Kuka.data_msrCartPos01;
R12 = data.Kuka.data_msrCartPos02;
R13 = data.Kuka.data_msrCartPos03;
x = data.Kuka.data_msrCartPos04;
R21 = data.Kuka.data_msrCartPos05;
R22 = data.Kuka.data_msrCartPos06;
R23 = data.Kuka.data_msrCartPos07;
y = data.Kuka.data_msrCartPos08;
R31 = data.Kuka.data_msrCartPos09;
R32 = data.Kuka.data_msrCartPos10;
R33 = data.Kuka.data_msrCartPos11;
z = data.Kuka.data_msrCartPos12;

[yaw, pitch, roll] = yawpitchroll(R11, R21, R31, R32, R33);
yaw = un_modulo(yaw, 2*pi); %remove 2pi jumps and make yaw continuous
yaw = yaw - yaw(1); %starting yaw from zero (same as IMU estimates)

% compute reference angular velocities using the KUKA robot
% (derivate between positions)
gyro = zeros(length(R11),3);
dt = median(Ktime(2:end)-Ktime(1:end-1)); % the most frequently used discretation time
for i = 2:length(R11)
    R_last = [R11(i-1), R12(i-1), R13(i-1); ...
              R21(i-1), R22(i-1), R23(i-1); ...
              R31(i-1), R32(i-1), R33(i-1)];
    R_cur = [R11(i), R12(i), R13(i); ...
             R21(i), R22(i), R23(i); ...
             R31(i), R32(i), R33(i)];

    Wh = (R_last'*R_cur - eye(3));
    W = 1/dt * Wh;
    gyro(i,1) = (W(3,2) - W(2,3))/2;
    gyro(i,2) = (W(1,3) - W(3,1))/2;
    gyro(i,3) = (W(2,1) - W(1,2))/2;
end

%% calibrate IMU data
imusWithKukaCalibration;

if (~use_comparisonAlgorithms)
    disp('To get reference algorithms, download their implementations from here:');
    disp('http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/');
    disp(' ');
end

%% start bias test computation (compute everything with test region using different biases)
% test and computation is done only for InertiaLink data
testBiases = ((0:0.5:7)*pi/180)';
%testBiases = ((0:0.5:1)*pi/180)';

yaw_DCM_RMSE = zeros(size(testBiases));
yaw_e1_RMSE = zeros(size(testBiases));
yaw_e2_RMSE = zeros(size(testBiases));

pitch_DCM_RMSE = zeros(size(testBiases));
pitch_e1_RMSE = zeros(size(testBiases));
pitch_e2_RMSE = zeros(size(testBiases));

roll_DCM_RMSE = zeros(size(testBiases));
roll_e1_RMSE = zeros(size(testBiases));
roll_e2_RMSE = zeros(size(testBiases));

acc2 = [acc2_x(StartIdx:StopIdx), acc2_y(StartIdx:StopIdx), acc2_z(StartIdx:StopIdx)];
gyro2_noBias = [w2_x(StartIdx:StopIdx), w2_y(StartIdx:StopIdx), w2_z(StartIdx:StopIdx)];
time = ILtime(StartIdx:StopIdx);
deltaT = [0; time(2:end) - time(1:end-1)];

%resample Kuka reference data to InertiaLink time
warning('off','interpolation:interpolation:noextrap');
yaw_in_IL = resample(timeseries(yaw, Ktime),time);
yaw_in_IL = yaw_in_IL.Data;
yaw_in_IL = yaw_in_IL - yaw_in_IL(1); %starting yaw from zero (same as IMU estimates)
pitch_in_IL = resample(timeseries(pitch, Ktime),time);
pitch_in_IL = pitch_in_IL.Data;
roll_in_IL = resample(timeseries(roll, Ktime),time);
roll_in_IL = roll_in_IL.Data;
warning('on','interpolation:interpolation:noextrap');

disp('Starting to compute RMSEs using different biases');

%check if matlabpool is opened
scheduler = findResource();
if (matlabpool('size') < scheduler.ClusterSize)
    if (matlabpool('size') > 0)
        matlabpool close;
    end

    matlabpool(scheduler.ClusterSize);
end

warning('off','MATLAB:mir_warning_maybe_uninitialized_temporary');
parfor i = 2:(length(testBiases))
    disp(['iteration ' int2str(i) ', bias ' num2str(testBiases(i)*180/pi) ' deg/s']);

    % add constant bias for testing purposes
    addedGyroBias = testBiases(i);
    gyro2 = gyro2_noBias + addedGyroBias*ones(size(gyro2_noBias));

    if (use_C_versions)
        [x_hist2, ypr_hist2, a_hist2, P_diag_hist2] = ...
            DCM_IMU_C(gyro2, acc2, deltaT);

        if (use_comparisonAlgorithms)
            quaternion1 = Madgwick_IMU_C(gyro2, acc2);
            quaternion2 = Mahony_IMU_C(gyro2, acc2);
        else
            quaternion1 = nan(size(gyro2,1),4);
            quaternion2 = nan(size(gyro2,1),4);
        end
    else
        DCM = DCM_IMU();

        if (use_comparisonAlgorithms)
            addpath('quaternion_library');      % include quaternion library

            IMU1t = MadgwickAHRS('SamplePeriod', 1/150, 'Beta', 0.1);
            IMU2t = MahonyAHRS('SamplePeriod', 1/150, 'Kp', 0.5);

            quaternion1 = zeros(length(time), 4);
            quaternion2 = zeros(length(time), 4);
        else
            quaternion1 = nan(length(time), 4);
            quaternion2 = nan(length(time), 4);
        end

        x_hist2 = zeros(length(time), 6);
        ypr_hist2 = zeros(length(time), 3);
        P_diag_hist2 = zeros(length(time), 6);
        for t = 1:length(time)
            DCM.UpdateIMU(gyro2(t,:), acc2(t,:), deltaT(t));	% gyroscope units must be radians
            x_hist2(t, :) = DCM.state';
            ypr_hist2(t, :) = [DCM.yaw, DCM.pitch, DCM.roll];
            P_diag_hist2(t, :) = diag(DCM.P)';

            if (use_comparisonAlgorithms)
                IMU1t.UpdateIMU(gyro2(t,:), acc2(t,:));	% gyroscope units must be radians
                quaternion1(t, :) = IMU1t.Quaternion;

                IMU2t.UpdateIMU(gyro2(t,:), acc2(t,:));	% gyroscope units must be radians
                quaternion2(t, :) = IMU2t.Quaternion;
            end
        end
    end

    % Plot algorithm output as Euler angles
    % The first and third Euler angles in the sequence (phi and psi) become
    % unreliable when the middle angles of the sequence (theta) approaches ~90
    % degrees. This problem commonly referred to as Gimbal Lock.
    % See: http://en.wikipedia.org/wiki/Gimbal_lock

    % use conjugate for sensor frame relative to Earth and convert to degrees.
    if (use_comparisonAlgorithms)
        euler1 = quatern2euler(quaternConj(quaternion1)) * (180/pi);
        euler2 = quatern2euler(quaternConj(quaternion2)) * (180/pi);
    else
        euler1 = nan(size(quaternion1,1),3);
        euler2 = nan(size(quaternion2,1),3);
    end

    % compute errors

    %yaw pitch roll errors (only InertiaLink data)
    ypr_hist2(:,1) = un_modulo(ypr_hist2(:,1), 2*pi); %remove 2pi jumps and make continuous
    ypr_hist2(:,1) = ypr_hist2(:,1) - ypr_hist2(1,1); %DCM

    euler1(:,3) = un_modulo(euler1(:,3), 360); %remove 2pi jumps and make continuous
    euler1(:,3) = euler1(:,3) - euler1(1,3); %Madgwick

    euler2(:,3) = un_modulo(euler2(:,3), 360); %remove 2pi jumps and make continuous
    euler2(:,3) = euler2(:,3) - euler2(1,3); %Mahony

    %yaw
    yaw_DCM_error = (ypr_hist2(:,1)-yaw_in_IL)*180/pi;
    yaw_DCM_RMSE(i) = sqrt(mean(yaw_DCM_error.^2));
    yaw_e1_error = euler1(:,3) - yaw_in_IL*180/pi;
    yaw_e1_RMSE(i) = sqrt(mean(yaw_e1_error.^2));
    yaw_e2_error = euler2(:,3) - yaw_in_IL*180/pi;
    yaw_e2_RMSE(i) = sqrt(mean(yaw_e2_error.^2));

    %pitch
    pitch_DCM_error = (ypr_hist2(:,2)-pitch_in_IL)*180/pi;
    pitch_DCM_RMSE(i) = sqrt(mean(pitch_DCM_error.^2));
    pitch_e1_error = euler1(:,2) - pitch_in_IL*180/pi;
    pitch_e1_RMSE(i) = sqrt(mean(pitch_e1_error.^2));
    pitch_e2_error = euler2(:,2) - pitch_in_IL*180/pi;
    pitch_e2_RMSE(i) = sqrt(mean(pitch_e2_error.^2));

    %roll
    roll_DCM_error = (ypr_hist2(:,3)-roll_in_IL)*180/pi;
    roll_DCM_RMSE(i) = sqrt(mean(roll_DCM_error.^2));
    roll_e1_error = euler1(:,1) - roll_in_IL*180/pi;
    roll_e1_RMSE(i) = sqrt(mean(roll_e1_error.^2));
    roll_e2_error = euler2(:,1) - roll_in_IL*180/pi;
    roll_e2_RMSE(i) = sqrt(mean(roll_e2_error.^2));
end
warning('on','MATLAB:mir_warning_maybe_uninitialized_temporary');
%% plotting

% compute same for smallest and some other index (for separate plotting)
figIdx = 1;
j_idx = [1 3];
for j = 1:2
    i = j_idx(j);

    disp(['iteration ' int2str(i) ', bias ' num2str(testBiases(i)*180/pi) ' deg/s']);

    % add constant bias for testing purposes
    addedGyroBias = testBiases(i);
    gyro2 = gyro2_noBias + addedGyroBias*ones(size(gyro2_noBias));

    if (use_C_versions)
        [x_hist2, ypr_hist2, a_hist2, P_diag_hist2] = ...
            DCM_IMU_C(gyro2, acc2, deltaT);

        if (use_comparisonAlgorithms)
            quaternion1 = Madgwick_IMU_C(gyro2, acc2);
            quaternion2 = Mahony_IMU_C(gyro2, acc2);
        else
            quaternion1 = nan(size(gyro2,1),4);
            quaternion2 = nan(size(gyro2,1),4);
        end
    else
        DCM = DCM_IMU();

        if (use_comparisonAlgorithms)
            addpath('quaternion_library');      % include quaternion library

            IMU1 = MadgwickAHRS('SamplePeriod', 1/150, 'Beta', 0.1);
            IMU2 = MahonyAHRS('SamplePeriod', 1/150, 'Kp', 0.5);

            quaternion1 = zeros(length(time), 4);
            quaternion2 = zeros(length(time), 4);
        else
            quaternion1 = nan(length(time), 4);
            quaternion2 = nan(length(time), 4);
        end

        x_hist2 = zeros(length(time), 6);
        ypr_hist2 = zeros(length(time), 3);
        P_diag_hist2 = zeros(length(time), 6);
        for t = 1:length(time)
            DCM.UpdateIMU(gyro2(t,:), acc2(t,:), deltaT(t));	% gyroscope units must be radians
            x_hist2(t, :) = DCM.state';
            ypr_hist2(t, :) = [DCM.yaw, DCM.pitch, DCM.roll];
            P_diag_hist2(t, :) = diag(DCM.P)';

            if (use_comparisonAlgorithms)
                IMU1.UpdateIMU(gyro2(t,:), acc2(t,:));	% gyroscope units must be radians
                quaternion1(t, :) = IMU1.Quaternion;

                IMU2.UpdateIMU(gyro2(t,:), acc2(t,:));	% gyroscope units must be radians
                quaternion2(t, :) = IMU2.Quaternion;
            end
        end
    end

    % Plot algorithm output as Euler angles
    % The first and third Euler angles in the sequence (phi and psi) become
    % unreliable when the middle angles of the sequence (theta) approaches ~90
    % degrees. This problem commonly referred to as Gimbal Lock.
    % See: http://en.wikipedia.org/wiki/Gimbal_lock

    % use conjugate for sensor frame relative to Earth and convert to degrees.
    if (use_comparisonAlgorithms)
        euler1 = quatern2euler(quaternConj(quaternion1)) * (180/pi);
        euler2 = quatern2euler(quaternConj(quaternion2)) * (180/pi);
    else
        euler1 = nan(size(quaternion1,1),3);
        euler2 = nan(size(quaternion2,1),3);
    end

    % compute errors

    % yaw pitch roll errors (only InertiaLink data)
    ypr_hist2(:,1) = un_modulo(ypr_hist2(:,1), 2*pi); %remove 2pi jumps and make continuous
    ypr_hist2(:,1) = ypr_hist2(:,1) - ypr_hist2(1,1); %DCM

    euler1(:,3) = un_modulo(euler1(:,3), 360); %remove 2pi jumps and make continuous
    euler1(:,3) = euler1(:,3) - euler1(1,3); %Madgwick

    euler2(:,3) = un_modulo(euler2(:,3), 360); %remove 2pi jumps and make continuous
    euler2(:,3) = euler2(:,3) - euler2(1,3); %Mahony

    %yaw
    yaw_DCM_error = (ypr_hist2(:,1)-yaw_in_IL)*180/pi;
    yaw_DCM_RMSE(i) = sqrt(mean(yaw_DCM_error.^2));
    yaw_e1_error = euler1(:,3) - yaw_in_IL*180/pi;
    yaw_e1_RMSE(i) = sqrt(mean(yaw_e1_error.^2));
    yaw_e2_error = euler2(:,3) - yaw_in_IL*180/pi;
    yaw_e2_RMSE(i) = sqrt(mean(yaw_e2_error.^2));

    %pitch
    pitch_DCM_error = (ypr_hist2(:,2)-pitch_in_IL)*180/pi;
    pitch_DCM_RMSE(i) = sqrt(mean(pitch_DCM_error.^2));
    pitch_e1_error = euler1(:,2) - pitch_in_IL*180/pi;
    pitch_e1_RMSE(i) = sqrt(mean(pitch_e1_error.^2));
    pitch_e2_error = euler2(:,2) - pitch_in_IL*180/pi;
    pitch_e2_RMSE(i) = sqrt(mean(pitch_e2_error.^2));

    %roll
    roll_DCM_error = (ypr_hist2(:,3)-roll_in_IL)*180/pi;
    roll_DCM_RMSE(i) = sqrt(mean(roll_DCM_error.^2));
    roll_e1_error = euler1(:,1) - roll_in_IL*180/pi;
    roll_e1_RMSE(i) = sqrt(mean(roll_e1_error.^2));
    roll_e2_error = euler2(:,1) - roll_in_IL*180/pi;
    roll_e2_RMSE(i) = sqrt(mean(roll_e2_error.^2));

    % plotting
    % Yaw, pitch and roll plot
    figure(figIdx); clf;
    subplot(3,1,1);
    yaw_dcm = ypr_hist2(:,1)*180/pi;
    pitch_dcm = ypr_hist2(:,2)*180/pi;
    roll_dcm = ypr_hist2(:,3)*180/pi;

    plot(time, yaw_dcm, 'b',  time, euler1(:,3), 'g--', time, euler2(:,3), 'r:', time, yaw_in_IL*180/pi, 'k--', 'LineWidth', 1);
    set(gca, 'Position', plot1Pos, 'FontSize', 12, 'FontName', 'Times');
    legend('DCM', 'Madgwick', 'Mahony', 'Reference', 'Location', legendLocation([yaw_dcm euler1(:,3) euler2(:,3)]));

    ylabel('yaw (deg)', 'FontSize', 12, 'FontName', 'Times');
    title(['Euler angles with a bias of ' num2str(addedGyroBias*180/pi, '%.0f') ' deg/s'], 'FontSize', 14, 'FontName', 'Times');
    set(gca, 'XLim', [time(1) time(end)]);

    subplot(3,1,2);
    plot(time, pitch_dcm, 'b',  time, euler1(:,2), 'g--', time, euler2(:,2), 'r:', time, pitch_in_IL*180/pi, 'k--', 'LineWidth', 1);
    set(gca, 'Position', plot2Pos, 'FontSize', 12, 'FontName', 'Times');
    ylabel('pitch (deg)', 'FontSize', 12, 'FontName', 'Times');
    set(gca, 'XLim', [time(1) time(end)]);

    subplot(3,1,3);
    plot(time, roll_dcm, 'b',  time, euler1(:,1), 'g--', time, euler2(:,1), 'r:', time, roll_in_IL*180/pi, 'k--', 'LineWidth', 1);
    set(gca, 'Position', plot3Pos, 'FontSize', 12, 'FontName', 'Times');
    xlabel('time (s)', 'FontSize', 12, 'FontName', 'Times');
    ylabel('roll (deg)', 'FontSize', 12, 'FontName', 'Times');
    set(gca, 'XLim', [time(1) time(end)]);

    set(gcf, 'Position', figPos + [(figIdx-1)*100 0 0 0]);
    set(gcf, 'PaperUnits', 'centimeters', 'PaperType', 'A4', 'PaperPosition', [0.63 0.63 19.72 28.41]);
    set(gcf,'PaperPositionMode','auto');
    saveas(gcf, ['figures/bias_fig_' testName(1:3) '_' int2str(figIdx) '.fig']);
    print('-depsc','-tiff','-r300',['figures/bias_fig_' testName(1:3) '_' int2str(figIdx) '.eps'])
    figIdx = figIdx + 1;

    % error plot
    figure(figIdx); clf;

    subplot(3,1,1);
    plot(time, yaw_DCM_error, 'b', time, yaw_e1_error, 'g--', time, yaw_e2_error, 'r:', 'LineWidth', 1);
    set(gca, 'Position', plot1Pos, 'FontSize', 12, 'FontName', 'Times');
    legend('DCM', 'Madgwick', 'Mahony', 'Location', legendLocation([yaw_DCM_error yaw_e1_error yaw_e2_error]));
    ylabel('yaw error (deg)', 'FontSize', 12, 'FontName', 'Times');
    title(['Errors to the reference measurement with a bias of ' num2str(addedGyroBias*180/pi, '%.0f') ' deg/s'], 'FontSize', 14, 'FontName', 'Times');
    set(gca, 'XLim', [time(1) time(end)]);

    subplot(3,1,2);
    plot(time, pitch_DCM_error, 'b', time, pitch_e1_error, 'g--', time, pitch_e2_error, 'r:', 'LineWidth', 1);
    set(gca, 'Position', plot2Pos, 'FontSize', 12, 'FontName', 'Times');
    ylabel('pitch error (deg)', 'FontSize', 12, 'FontName', 'Times');
    set(gca, 'XLim', [time(1) time(end)]);

    subplot(3,1,3);
    plot(time, roll_DCM_error, 'b',  time, roll_e1_error, 'g--', time, roll_e2_error, 'r:', 'LineWidth', 1);
    set(gca, 'Position', plot3Pos, 'FontSize', 12, 'FontName', 'Times');
    xlabel('time (s)', 'FontSize', 12, 'FontName', 'Times');
    ylabel('roll error (deg)', 'FontSize', 12, 'FontName', 'Times');
    set(gca, 'XLim', [time(1) time(end)]);

    set(gcf, 'Position', figPos + [(figIdx-1)*100 0 0 0]);
    set(gcf, 'PaperUnits', 'centimeters', 'PaperType', 'A4', 'PaperPosition', [0.63 0.63 19.72 28.41]);
    set(gcf,'PaperPositionMode','auto');
    saveas(gcf, ['figures/bias_fig_' testName(1:3) '_' int2str(figIdx) '.fig']);
    print('-depsc','-tiff','-r300',['figures/bias_fig_' testName(1:3) '_' int2str(figIdx) '.eps'])
    figIdx = figIdx + 1;

    % bias plot
    figure(figIdx); clf;

    x_sigma = sqrt(P_diag_hist2(:,4));
    y_sigma = sqrt(P_diag_hist2(:,5));
    z_sigma = sqrt(P_diag_hist2(:,6));

    bias_x = x_hist2(:,4)*180/pi;
    bias_y = x_hist2(:,5)*180/pi;
    bias_z = x_hist2(:,6)*180/pi;

    plotLims = 1.25*[mean(x_sigma(1:floor(end/3))), mean(y_sigma(1:floor(end/3))), mean(z_sigma(1:floor(end/3)))];

    subplot(3,1,1);
    plot([time(1) time(end)], [addedGyroBias addedGyroBias]*180/pi, 'k--', ...
        time, bias_x, 'b', time, (addedGyroBias-x_sigma)*180/pi, 'b-.', ...
        time, (addedGyroBias+x_sigma)*180/pi, 'b-.', 'LineWidth', 1, 'MarkerSize', markerSize);
    h = legend('Reference', 'Estimate', '1-sigma');
    set(gca, 'Position', plot1Pos, 'FontSize', 12, 'FontName', 'Times');

    ylabel('x_b_i_a_s (deg/s)', 'FontSize', 12, 'FontName', 'Times');
    title('Bias estimates and 1-sigma distances of standard deviations', 'FontSize', 14, 'FontName', 'Times');
    set(gca, 'XLim', [time(1) time(end)], 'YLim', [addedGyroBias-plotLims(1) addedGyroBias+plotLims(1)]*180/pi);

    subplot(3,1,2);
    plot([time(1) time(end)], [addedGyroBias addedGyroBias]*180/pi, 'k--', ...
        time, bias_y, 'b', time, (addedGyroBias-y_sigma)*180/pi, 'b-.', ...
        time, (addedGyroBias+y_sigma)*180/pi, 'b-.', 'LineWidth', 1, 'MarkerSize', markerSize);
    set(gca, 'Position', plot2Pos, 'FontSize', 12, 'FontName', 'Times');

    ylabel('y_b_i_a_s (deg/s)', 'FontSize', 12, 'FontName', 'Times');
    set(gca, 'XLim', [time(1) time(end)], 'YLim', [addedGyroBias-plotLims(2) addedGyroBias+plotLims(2)]*180/pi);

    subplot(3,1,3);
    plot([time(1) time(end)], [addedGyroBias addedGyroBias]*180/pi, 'k--', ...
        time, bias_z, 'b', time, (addedGyroBias-z_sigma)*180/pi, 'b-.', ...
        time, (addedGyroBias+z_sigma)*180/pi, 'b-.', 'LineWidth', 1, 'MarkerSize', markerSize);
    set(gca, 'Position', plot3Pos, 'FontSize', 12, 'FontName', 'Times');

    xlabel('time (s)', 'FontSize', 12, 'FontName', 'Times');
    ylabel('z_b_i_a_s (deg/s)', 'FontSize', 12, 'FontName', 'Times');
    set(gca, 'XLim', [time(1) time(end)], 'YLim', [addedGyroBias-plotLims(3) addedGyroBias+plotLims(3)]*180/pi);

    set(gcf, 'Position', figPos + [(figIdx-1)*100 0 0 0]);
    set(gcf, 'PaperUnits', 'centimeters', 'PaperType', 'A4', 'PaperPosition', [0.63 0.63 19.72 28.41]);
    set(gcf,'PaperPositionMode','auto');
    saveas(gcf, ['figures/bias_fig_' testName(1:3) '_' int2str(figIdx) '.fig']);
    print('-depsc','-tiff','-r300',['figures/bias_fig_' testName(1:3) '_' int2str(figIdx) '.eps'])
    figIdx = figIdx + 1;

    pause(0.2);
end

%% statistics plot
figure(figIdx); clf;

subplot(3,1,1);
semilogy(testBiases*180/pi, yaw_DCM_RMSE, 'b*-', testBiases*180/pi, yaw_e1_RMSE, 'g^-', testBiases*180/pi, yaw_e2_RMSE, 'ro-', 'LineWidth', 1, 'MarkerSize', markerSize);
set(gca, 'Position', plot1Pos, 'FontSize', 12, 'FontName', 'Times');
legend('DCM', 'Madgwick', 'Mahony', 'Location', 'SouthEast');
ylabel('yaw RMSE (deg)', 'FontSize', 12, 'FontName', 'Times');
title(['RMSEs as a function of gyroscope bias (' testName ')'], 'FontSize', 14, 'FontName', 'Times');

subplot(3,1,2);
semilogy(testBiases*180/pi, pitch_DCM_RMSE, 'b*-', testBiases*180/pi, pitch_e1_RMSE, 'g^-', testBiases*180/pi, pitch_e2_RMSE, 'ro-', 'LineWidth', 1, 'MarkerSize', markerSize);
set(gca, 'Position', plot2Pos, 'FontSize', 12, 'FontName', 'Times');
ylabel('pitch RMSE (deg)', 'FontSize', 12, 'FontName', 'Times');

subplot(3,1,3);
semilogy(testBiases*180/pi, roll_DCM_RMSE, 'b*-', testBiases*180/pi, roll_e1_RMSE, 'g^-', testBiases*180/pi, roll_e2_RMSE, 'ro-', 'LineWidth', 1, 'MarkerSize', markerSize);
set(gca, 'Position', plot3Pos, 'FontSize', 12, 'FontName', 'Times');
xlabel('Constant gyroscope bias (deg/s)', 'FontSize', 12, 'FontName', 'Times');
ylabel('roll RMSE (deg)', 'FontSize', 12, 'FontName', 'Times');

set(gcf, 'Position', figPos + [(figIdx-1)*100 0 0 0]);
set(gcf, 'PaperUnits', 'centimeters', 'PaperType', 'A4', 'PaperPosition', [0.63 0.63 19.72 28.41]);
set(gcf,'PaperPositionMode','auto');
saveas(gcf, ['figures/bias_fig_' testName(1:3) '_' int2str(figIdx) '.fig']);
print('-depsc','-tiff','-r300',['figures/bias_fig_' testName(1:3) '_' int2str(figIdx) '.eps'])


% RESON_dyProg_mat.m
% Function to carry out dynamic programming componenent of SE_VQ
%
% Octave compatible
%
% Description
% Function to carry out dynamic programming method described in Ney (1989)
% and used previously in the ESPS GCI detection algorithm. The method
% considers target costs and transition costs which are accumulated in
% order to select the `cheapest' path, considering previous context
%
% Inputs
%  rel_amp         : [samples] [MxNcand] Relative residual amplitudes
%  GCI_N           : [samples] [MxNcand] matrix containing M by Ncandidate GCI locations
%  F0mean          : [Hz]      [1x1] Mean fundamental frequency
%  x               : [samples] [Nx1] Speech signal
%  fs              : [Hz]      [1x1] sampling frequency
%  trans_wgt       : [integer] [1x1] Weight of transition cost
%  relAmp_wgt      : [integer] [1x1] Weight of target cost
%
% Outputs
%  GCI_opt         : [samples] [Mx1] Optimal GCI path
%
% Example
%  GCI_opt =
%  RESON_dyProg_mat(GCI_relAmp,GCI_N,F0_mean,x,fs,trans_wgt,relAmp_wgt)
%
% References
%  [1] Kane, J., Gobl, C., (2013) `Evaluation of glottal closure instant
%       detection in a range of voice qualities', Speech Communication
%       55(2), pp. 295-314.
%
% Copyright (c) 2013 Trinity College Dublin
%
% License
%  This code is a part of the Voice Analysis Toolkit with the following
%  licence:
%  The software product (and any modifications thereof) is distributed under
%  a dual licence: an open source license for individual, non-commercial
%  purposes, and a commercial license. The opensource licence under which
%  the product is distributed is GNU GPL v2. For individual users, this
%  licence suits their use as these are not distributing proprietary
%  modifications, additions to, or derivatives of the product and don't
%  require legal protection of a commercial licence. For commercial users,
%  where open source does not meet their requirements, we offer commercial
%  licensing of the product. A commercial license permits customers to
%  modify, add or produce derivative products without the obligation of
%  making the subsequent code open source. For more information regarding
%  our commercial licence, please contact john.whelan@tcd.ie
%
% This function is part of the Covarep project: http://covarep.github.io/covarep
%
% Author
%  John Kane kanejo@tcd.ie
%
% $Id <info set by the versioning system> $

function GCI_opt = RESON_dyProg_mat(GCI_relAmp,GCI_N,F0_mean,x,fs,trans_wgt,relAmp_wgt)

%% Initial settings
plots=0;
cost = (GCI_relAmp.*relAmp_wgt)';
ncands=size(GCI_N,1);
nframe=size(GCI_N,2);
GCI_N=GCI_N';
prev=zeros(nframe,ncands);      % traceback pointer
pulseLen = round(fs/F0_mean);
GCI_opt=zeros(1,nframe);

%% Do processing
for n=1:nframe

    if n>1
        costm=zeros(ncands,ncands);         % transition cost matrix: rows (previous), cols (current)

        for c=1:ncands
            % Transitions TO states in current frame
            start=GCI_N(n,c)-round(pulseLen/2);
            stop=GCI_N(n,c)+round(pulseLen/2);
            if stop > length(x)
                stop=length(x);
            end

            if start<1
                start=1;
            end
            pulse_cur = x(start:stop);

            for p=1:ncands
                % Transitions FROM states in previous frame
                start=GCI_N(n-1,p)-round(pulseLen/2);
                stop=GCI_N(n-1,p)+round(pulseLen/2);
                if start<1
                    start=1;
                end
                if stop > length(x)
                    stop=length(x);
                end
                if start<1
                    start=1;
                end

                pulse_prev = x(start:stop);

                if isempty(pulse_cur) ||isnan(pulse_cur(1)) || ...
                        isnan(pulse_prev(1))
                    costm(p,c)=0;
                else
                    if length(pulse_cur)~=length(pulse_prev)
                        cor_cur=0;
                    else
                        cor_cur=corrcoef(pulse_cur(:),pulse_prev(:));
			if length(cor_cur)==1
			   cor_cur=0;
			else cor_cur=cor_cur(2);
			end
                    end
                    costm(p,c) = (1-abs(cor_cur))*trans_wgt; % transition cost
                end
            end
        end

        costm=costm+repmat(cost(n-1,1:ncands)',1,ncands);  % add in cumulative costs
        [costi,previ]=min(costm,[],1);
        cost(n,1:ncands)=cost(n,1:ncands)+costi;
        prev(n,1:ncands)=previ;
    end

end

%% Do traceback
best=zeros(n,1);
[cbest,best(n)]=min(cost(n,1:ncands));
for i=n:-1:2
     best(i-1)=prev(i,best(i));
end

for n=1:nframe
    GCI_opt(n) = GCI_N(n,best(n));
end

%% Do plots
if plots
    GCI_norm=zeros(nframe,ncands);
    GCI_opt_norm=zeros(nframe,ncands);
    for n=1:nframe
        GCI_norm(n,:) = GCI_N(n,:)-GCI_N(n,1);
        GCI_opt_norm(n) = GCI_opt(n)-GCI_N(n,1);
    end
    subplot(211), plot(x), hold on, stem(GCI_N(:,1),ones(1,length(GCI_N(:,1)))*-.1,'r')
        stem(GCI_opt,ones(1,length(GCI_opt))*-.1,'k')
    subplot(212), plot(GCI_opt,GCI_norm,'rx'), ylim([-20 20]),
        hold on, plot(GCI_opt,GCI_opt_norm)
end


% sim_onepulse_shaped.m
% Jamie Near, McGill University 2014.
%
% USAGE:
% out = sim_onepulse_shaped(n,sw,Bfield,linewidth,sys,RF,tp,phCyc,dfdx,G)
%
% DESCRIPTION:
% %This function simulates the effect of a frequency selective or slice
% selective excitation, followed immediately by the acquisition window.
% This is mainly an exercise to see if I can get slice selective
% excitation working.
%
% Note that when simulating a frequency selective pulse, it is okay to
% specify only 8 arguments (no gradient needs to be specified).  If the
% 9th argument, G, is specified and is non-zero, then a slice selective
% pulse is assumed.
%
% INPUTS:
% n         = number of points in fid/spectrum
% sw        = desired spectral width in [Hz]
% Bfield    = main magnetic field strength in [T]
% linewidth = linewidth in [Hz]
% sys       = spin system definition structure
% RF        = RF pulse definition structure (obtain using 'io_loadRFwaveform.m')
% tp        = RF pulse duration in [ms]
% phCyc     = Phase of excitation rf pulse in [degrees].
% dfdx      = if simulating a frequency selective pulse, this argument
%             should be the frequency offset [Hz].  If simulating a slice
%             selective pulse, this argument should be the position offset [cm].
% G         = gradient strength for slice-selective pulse [G/cm];
%
% OUTPUTS:
% out       = simulated spectrum, in FID-A structure format, using pulse-acquire
%             sequence.

function out = sim_onepulse_shaped(n,sw,Bfield,linewidth,sys,RF,tp,phCyc,dfdx,G)

if nargin>9 && G~=0
    simType='g';
else
    simType='f';
end

%Set water to centre
centreFreq=4.65;
for k=1:length(sys)
    sys(k).shifts=sys(k).shifts-centreFreq;
end

%Calculate Hamiltonian matrices and starting density matrix.
[H,d]=sim_Hamiltonian(sys,Bfield);

%BEGIN PULSE SEQUENCE************
if simType=='g'
    d=sim_shapedRF(d,H,RF,tp,90,90+phCyc,dfdx,G);   %slice selective excitation
elseif simType=='f'
    d=sim_shapedRF(d,H,RF,tp,90,90+phCyc,dfdx);     %frequency selective excitation
end
[out,dout]=sim_readout(d,H,n,sw,linewidth,90);      %Readout along y (90 degree phase);
%END PULSE SEQUENCE**************

%Correct the ppm scale:
out.ppm=out.ppm-(4.65-centreFreq);

%Fill in structure header fields:
out.seq='onepulse';
out.te=tp/2;
out.sim='shaped';

%Additional fields for compatibility with FID-A processing tools.
out.sz=size(out.specs);
out.date=date;
out.dims.t=1;
out.dims.coils=0;
out.dims.averages=0;
out.dims.subSpecs=0;
out.dims.extras=0;
out.averages=1;
out.rawAverages=1;
out.subspecs=1;
out.rawSubspecs=1;
out.flags.writtentostruct=1;
out.flags.gotparams=1;
out.flags.leftshifted=0;
out.flags.filtered=0;
out.flags.zeropadded=0;
out.flags.freqcorrected=0;
out.flags.phasecorrected=0;
out.flags.averaged=1;
out.flags.addedrcvrs=1;
out.flags.subtracted=1;
out.flags.writtentotext=0;
out.flags.downsampled=0;
out.flags.isFourSteps=0;


function [report] = TrackDetect(Track, UAV)
%TRACKDETECT 判断航迹是否符合条件(一个agent的)

%% 无人机航迹检测

% 威胁检测
dim = UAV.PointDim;                                         % 仿真环境维度
M = [UAV.Menace.radar; UAV.Menace.other]; % 威胁区

Threat = cell(UAV.num, 1);                                 % 威胁结果树
Angle = cell(UAV.num, 1);                                  % 转角检测结果树
MiniTraj = cell(UAV.num, 1);                              % 最小航迹片段检测结果树
ProbPoint = cell(UAV.num, 1);                           % 问题点

L = cell(UAV.num, 1);                                          % 航迹片段树（累加结构）
Time = cell(UAV.num, 1);                                    % 到达各个点时间树

L_mt = 0;                                                               % 所有无人机航迹之和
totalTime = zeros(UAV.num, 1);                         % 所用时间
totalL = zeros(UAV.num, 1);                                % 每个无人机飞行距离

for i = 1 : UAV.num
      PointNum = UAV.PointNum(i);
      Judge = zeros(1, PointNum+1);
      L_i = zeros(1, PointNum+1);
      Time_i = zeros(1, PointNum+1);
      Angle_i = zeros(1, PointNum+1);
      Traj_i = zeros(1, PointNum+1);
      ProbPoint_i = zeros(1, PointNum+1);
      % 进行检测
      l = 0;
      V = Track.V(i);
      for k = 1 : PointNum
            % 前向检测
            P2 = Track.P{i}(:, k)' ;   % 转置成 1*dim
            if k == 1
                P1 = UAV.S(i, :);
                ZZ = P2 - P1;
                phi0 = atan2(ZZ(2), ZZ(1));   % 偏角检测
                phi1 = phi0;
                d_phi = phi1 - phi0;
                if dim > 2                                % 倾角检测
                    theta0 = atan(ZZ(3) / sqrt(ZZ(1)^2 + ZZ(2)^2));
                    theta1 = theta0;
                    d_theta = theta1 - theta0;
                else
                    d_theta = 0;
                end
            else
                P1 = Track.P{i}(:, k-1)' ;  % 转置成 1*dim
                ZZ = P2 - P1;
                phi1 = atan2(ZZ(2), ZZ(1));
                d_phi = phi1 - phi0;
                phi0 = phi1;
                if dim > 2
                    theta1 = atan(ZZ(3) / sqrt(ZZ(1)^2 + ZZ(2)^2));
                    d_theta = theta1 - theta0;
                    theta0 = theta1;
                else
                    d_theta = 0;
                end
            end

            [across, ~] = CheckThreat(P1, P2, M); % 威胁检测
            Judge(k) = across;

            dl = norm(P1 - P2);
            l = l + dl; % 累加距离
            t = l / V;  % 时间点
            L_i(k) = l;
            Time_i(k) = t;

            if abs(d_phi) > UAV.limt.phi(i, 2)  ||  abs(d_theta) > UAV.limt.theta(i, 2)
                Angle_i(k) = true;
            else
                Angle_i(k) = false;
            end

            if dl < UAV.limt.L(i, 1)
                Traj_i(k) = true;
            else
                Traj_i(k) = false;
            end

            ProbPoint_i(k) = Angle_i(k) | Traj_i(k) | Judge(k); % 问题点

            % 最后一段检测
            if k == PointNum
                P1 = UAV.G(i, :);
                [across, ~] = CheckThreat(P2, P1, M);
                Judge(k+1) = across;

                dl = norm(P1 - P2);
                l = l + dl;
                t = l / V;
                L_i(k+1) = l;
                Time_i(k+1) = t;

                ZZ = P1-P2;
                phi1 = atan2(ZZ(2), ZZ(1));
                d_phi = phi1 - phi0;
                if dim>2
                    theta1 = atan(ZZ(3) / sqrt(ZZ(1)^2 + ZZ(2)^2));
                    d_theta = theta1 - theta0;
                else
                    d_theta = 0;
                end

                if abs(d_phi) > UAV.limt.phi(i, 2)  ||  abs(d_theta) > UAV.limt.theta(i, 2)
                    Angle_i(k+1) = true;
                else
                    Angle_i(k+1) = false;
                end

                if dl < UAV.limt.L(i, 1)
                    Traj_i(k+1) = true;
                else
                    Traj_i(k+1) = false;
                end

                ProbPoint_i(k+1) = Angle_i(k+1) | Traj_i(k+1) | Judge(k+1);

            end
      end
      Threat(i) = {Judge};                % 检测结果（长度比点的数目多一）
      Angle(i) = {Angle_i};               % 转角
      MiniTraj(i) = {Traj_i};              % 航迹片段
      ProbPoint(i) = {ProbPoint_i}; % 问题点

      L(i) = {L_i};                 % 路径长度（累加）
      Time(i) = {Time_i};    % 时间（累加）
      L_mt = L_mt + l;           % 总长度
      totalTime(i) = t;            % 时间
      totalL(i) = l;                   % 长度
end

% 多无人机碰撞检测
d_safe = UAV.ds; % 安全距离
CollideTimes = 0; % 碰撞次数
for i = 2 : UAV.num
    PointNum_i = UAV.PointNum(i);
    for k = 1 : PointNum_i
        P1 = Track.P{i}(:, k)';  % K时刻 i 无人机位置
        t_i = Time{i, 1}(k);                  % K时刻 i 无人机时间
        for j = 1 : i-1
            PointNum_j = UAV.PointNum(j);
            flag = false; %
            % 搜索同一时刻的点
            for kj = 1 : PointNum_j
                if kj ==1
                    t_j_l = 0;
                    P_l =  UAV.S(j, :);
                else
                    t_j_l = Time{j}(kj-1);
                    P_l =  Track.P{j}(:, kj-1)';
                end
                t_j_r = Time{j}(kj);
                P_r =  Track.P{j}(:, kj)';

                if t_i <= t_j_r  &&  t_i >= t_j_l
                    flag = true;
                    P2 =  P_l + (t_i - t_j_l) / (t_j_r - t_j_l) * (P_r - P_l);% K时刻 J 无人机位置
                    % K时刻 j 无人机位置
                end
            end
            % ———————

            if flag  % 查找到P2位置，进行碰撞检测
                collide = CheckCollide(P1, P2, d_safe);
            else     % 没有P2位置，不会碰
                collide = false;
            end
            if collide
                CollideTimes = CollideTimes + 1;
            end
        end
    end
end

% 生成检测报告
report.L_mt = L_mt;                       %总行程之和
report.Threat = Threat;                 %受威胁的航迹点位置
report.AngleProb = Angle;            % 转角不满足的点
report.TrajProb = MiniTraj;           % 不满足最小航迹间隔的点
report.ProbPoint = ProbPoint;     % 有问题的点
report.L = totalL;                            %飞行距离
report.time = totalTime;                %飞行时间
report.col_times = CollideTimes;  %碰撞次数
end

%% 穿越威胁区检测
function [across, across_num] = CheckThreat(P1, P2, M)
    % 威胁区（球或圆区域，不适合圆柱区域）
    O = M(:,1:end-1);  % 圆心
    R = M(:, end);        % 半径

    % 检测线段是否穿过某个障碍区
    total = 0;
    for i = 1 : size(O, 1)
         a = norm(P1 - P2);
         b = norm(P2 - O(i, :));
         c = norm(P1 - O(i, :));
         % P1点是否在圆内
         if c < R(i)
             isHit = true;
         % P2点是否在圆内
         elseif b < R(i)
             isHit = true;
         % P1 P2都不在圆内
         else
             dim = size(O, 2);  % P1： 1*dim 维
             if dim < 3
                 % 平面情况
                 A = P1(2) - P2(2);
                 B = P2(1) - P1(1);
                 C = P1(1)*P2(2) - P2(1)*P1(2);
                 x = O(i, 1);
                 y = O(i, 2);
                 d = abs(A*x + B*y + C) / sqrt(A^2 + B^2);
             else
                 % 空间情况
                 PP = P2 - P1;
                 PO = O(i, :) - P1;
                 d = norm(cross(PP, PO)) / a;
             end
             % 距离判据
             if d >= R(i)
                 isHit = false;  % 不相交
             % 角度判据(距离满足条件时两个角都为锐角时相交)
             elseif d > 0
                 cosP1 = a^2 + c^2 - b^2 / (2*c*a);
                 cosP2 = a^2 + b^2 - c^2 / (2*b*a);
                 if cosP1 > 0  &&  cosP2 > 0
                     isHit = true;
                 else
                     isHit = false;
                 end
             % 两点在外，距离为0情况（共线情况）
             else
                 if a > b  &&  a > c
                    isHit = true;
                 else
                    isHit = false;
                 end
             end
         end

         if isHit
            total = total + 1; %总共碰撞几次
         end
    end

    if total > 0
        across = true;
    else
        across = false;          % 是否穿越禁区
    end
    across_num = total;     % 穿过禁区的个数
end

%% 碰撞检测
function [collide] = CheckCollide(P1, P2, d_safe)
    if norm(P1 - P2) >= d_safe
        collide = false;
    else
        collide = true;
    end
end


function [Forecasteval]=bvarfeval(data_endo_c,data_endo_c_lags,data_exo_c,stringdates3,Fstartdate,Fcenddate,Fcperiods,Fcomp,const,n,p,k,It,Bu,beta_gibbs,sigma_gibbs,forecast_record,forecast_estimates,names,endo,pref)

% function []=bvarfeval(data_endo_c,data_endo_c_lags,data_exo_c,stringdates3,Fstartdate,Fcenddate,Fcperiods,Fcomp,const,n,p,k,It,Bu,beta_gibbs,sigma_gibbs,forecast_estimates,names,endo,datapath)
% calculates, display and saves forecast evaluation results for a BVAR model
% inputs:  - matrix 'data_endo_c': matrix of endogenous data for the forecast evaluation period (i.e. period for which forecast is estimated and actual data exists)
%          - matrix 'data_endo_c_lags': lagged endogenous values to serve as initial conditions to the forecast evaluation period
%          - matrix 'data_exo_c': matrix of predicted data for the forecast period
%          - cell 'stringdates3': date strings for the forecast evaluation period (i.e. period for which forecast is estimated and actual data exists)
%          - string 'Fstartdate': start date of the forecasts
%          - string 'Fcenddate': end date of the forecat evaluation (i.e. period for which forecast is estimated and actual data exists)
%          - integer 'Fcperiods': number of periods for which forecast evaluation can be conducted (i.e.for which forecast is estimated and actual data exists)
%          - integer 'Fcomp': 0-1 value indicating if forecast evaluation is possible
%          - integer 'const': 0-1 value to determine if a constant is included in the model
%          - integer 'n': number of endogenous variables in the BVAR model (defined p 7 of technical guide)
%          - integer 'p': number of lags included in the model (defined p 7 of technical guide)
%          - integer 'k': number of coefficients to estimate for each equation in the BVAR model (defined p 7 of technical guide)
%          - integer 'It': total number of iterations of the Gibbs sampler (defined p 28 of technical guide)
%          - integer 'Bu': number of burn-in iterations of the Gibbs sampler (defined p 28 of technical guide)
%          - matrix 'beta_gibbs': record of the gibbs sampler draws for the beta vector
%          - matrix'sigma_gibbs': record of the gibbs sampler draws for the sigma matrix (vectorised)
%          - cell 'forecast_estimates': lower bound, point estimates, and upper bound for the unconditional forecasts
%          - cell 'names': cell containing the excel spreadsheet labels (names and dates)
%          - cell 'endo': list of endogenous variables of the model
%          - string 'datapath': user-supplied path to excel data spreadsheet
% outputs: none

% changed ad 2017-01-06 to store results
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, from (1.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, from (1.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, from (1.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, from (1.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
   Forecasteval.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));
      Forecasteval.CRPS{ii,1}(1,jj)=score;
      end
   end
   Forecasteval.CRPS_estimates=(cell2mat(Forecasteval.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];
   end

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

   for ll=1:It-Bu
      % step 2: draw beta and sigma
      beta=beta_gibbs(:,ll);
      sigma=reshape(sigma_gibbs(:,ll),n,n);

      % step 3: obtain mu, the mean vector, from (a.8.19)
      temp=bear.forecastsim(data_endo_c_lags,data_exo_c,beta,n,p,k,Fcperiods);
      mu=reshape(temp',n*Fcperiods,1);

      % step 4: obtain upsilon, the covariance matrix, from (a.8.22)
      % recover the A1,A2,... matrices by reshaping beta
      temp=reshape(beta,k,n);
      temp=(temp(1:p*n,:))';
      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
         mean=R*mu;
         % define the variance
         covar=R*upsilon*(R');
         % define the actual value
         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 (a.8.26)
   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);