function gpcf = gpcf_covar(varargin)
%   GPCF_COVAR creates a gpstuff covariance structure for the multivariate
%   Gaussian Process model based on the coregionalization models (LMC).
%   See Gelfand et al. (2004) in nonstationary multivariate process modelling
%   through spatially varying coregionalization. Sociedad de Estadistica e
%   Investigacion Operativa, Test.
%
%   The coregionalization matrix is a covariance matrix and its
%   parametrization is given via variances and the correlation matrix. The
%   correlation matrix is parametrizated with the transformation proposed by
%   Lewandowski et al. (2009) in generating correlation matrices based on
%   vines and extended onion method. Journal of multivariate analysis.
%
%   Description :
%
%   ─ GPCF = GPCF_COVAR('PARAM1',VALUE1,'PARAM2,VALUE2,...)
%     creates correlation matrix structure in which
%     the named parameters have the specified values. Any
%     unspecified parameters are set to default values.
%
%   ─ GPCF = GPCF_COVAR(GPCF,'PARAM1',VALUE1,'PARAM2,VALUE2,...)
%    modify a covariance function structure with the named
%    parameters altered with the specified values.
%
%   ─ Parameters (of the gpstuff structure) for the covariance structure [default]
%     R_prior             - prior for correlation matrix  [prior_corrunif()]
%     V_prior             - prior for the variances. Must be a structure
%                           with each component being also a structure (the
%                           priors). Otherwise, just use prior_fixed.
%     corrFun             - structure containing other covariance
%                           functions. More specifically, each element of
%                           the structure must be a correlation function
%                           (σ²_f = 1). Each element of the structure can
%                           also be a combination of covariance functions,
%                           which will result to a correlation function.
%                           You always need to follow this structure when
%                           using that specific type of covariance function
%                           in the gpstructure (gp_set).
%
%     numberClass         - number of classes (GPs, categories, species, ...)
%                           being modelled (this is required to initialize
%                           other parameters)
%
%     degreeFreedom_prior - prior for degree of freedoms 'nu' [prior_corrunif()]
%     classVariables      - value defining which column of x is used to
%                           identify the class variables. If this is not
%                           given returns an error. They have to be given
%                           increasing downwards, otherwise you get an
%                           error.
%
%   »» Note ««
%   If the prior is 'prior_fixed' then the parameter in
%   question is considered fixed and it is not handled in
%   optimization, grid integration, MCMC etc.
%
% * See also
%   GP_SET, GPCF_*, PRIOR_*
%
% ─────────────── 2018 Marcelo Hartmann.

% If you use this file. Learn to recognize its author. Copyright is good but
% misleading.

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

  if nargin > 0 && ischar(varargin{1}) && ismember(varargin{1}, {'init' 'set'})
    % remove init and set if isfield(gpcf,'metric')
    varargin(1) = [];
  end

  ip = inputParser;
  ip.FunctionName = 'GPCF_COVAR';
  ip.addOptional('gpcf', [], @isstruct);
  ip.addParamValue('R', 0.01, @(x) isvector(x) && ~any(abs(x) > 1));
  ip.addParamValue('V', 1, @(x) isvector(x) && ~any(x < 0));
  ip.addParamValue('R_prior', prior_corrunif(), @(x) isstruct(x) || isempty(x));
  ip.addParamValue('V_prior', prior_t('s2', 2), @(x) isstruct(x) || isempty(x));
  ip.addParamValue('corrFun', {}, @(x) ~isempty(x) && iscell(x));
  ip.addParamValue('classVariables', [], @(x) ~isempty(x) && mod(x, 1) == 0 && x > 0);
  ip.addParamValue('numberClass', [], @(x) mod(x, 1) == 0 && x > 1);
  ip.addParamValue('aValue', 1, @(x) isreal(x) &&  x > 0);

  ip.parse(varargin{:});

  gpcf = ip.Results.gpcf;

  if isempty(gpcf)
      init = true;
      gpcf.type = 'gpcf_covar';
  else
      if ~isfield(gpcf, 'type') && ~isequal(gpcf.type, 'gpcf_covar')
          error('First argument does not seem to be a valid correlation structure')
      end
      init = false;
  end

  if init
      % Set the function handles to the subfunctions
      gpcf.fh.realtoz = @gpcf_covar_realtoz;
      gpcf.fh.ztoreal = @gpcf_covar_ztoreal;
      gpcf.fh.rhotoreal = @gpcf_covar_rhotoreal;
      gpcf.fh.realtorho = @gpcf_covar_realtorho;
      gpcf.fh.pak = @gpcf_covar_pak;
      gpcf.fh.unpak = @gpcf_covar_unpak;
      gpcf.fh.lp = @gpcf_covar_lp;
      gpcf.fh.lpg = @gpcf_covar_lpg;
      gpcf.fh.cfg = @gpcf_covar_cfg;
      gpcf.fh.cov = @gpcf_covar_cov;
      gpcf.fh.sigma = @gpcf_covar_sigma;
      gpcf.fh.trcov = @gpcf_covar_trcov;
      gpcf.fh.trvar = @gpcf_covar_trvar;
      gpcf.fh.recappend = @gpcf_covar_recappend;
  end

  % initialize class variables and selected variables
  if isempty(ip.Results.classVariables) || isempty(ip.Results.numberClass)
      error('class variables and number of classes must be given')
  end
  gpcf.classVariables = ip.Results.classVariables;
  gpcf.numberClass = ip.Results.numberClass;
  gpcf.vectSize = (gpcf.numberClass^2 - gpcf.numberClass)/2;
  gpcf.aValue = ip.Results.aValue;

  % create indexes which will easy our lives from now on
  seq = 1:gpcf.vectSize;
  i = ceil(0.5 + 0.5 * sqrt(1 + 8 * seq)); % ith column
  j = seq - (i - 2).*(i - 1)/2;            % jth column
  ind1 = (j - 1) * gpcf.numberClass + i;   % linear indexes for lower triangular
  ind2 = (i - 1) * gpcf.numberClass + j;   % linear indexes for upper triangular
  gpcf.index = [i; j; ind1; ind2; seq]';

  % Initialize correlations functions
  if init || ~ismember('corrFun', ip.UsingDefaults)
      gpcf.corrFun = ip.Results.corrFun;
  end

  if length(gpcf.corrFun) ~= gpcf.numberClass
      error('the number of correlation functions must match the number of classes')
  end

  % Initialize correlations parameters
  if init && ~ismember('R', ip.UsingDefaults)
      gpcf.R = ip.Results.R;
      if length(gpcf.R) ~= gpcf.vectSize
          error('length of the vector does not agree with the number of classes')
      end
  elseif init
      gpcf.R = (-0.99 + 1.98.*rand(1, gpcf.vectSize)) * 0.001;
  end

  % Initialize variance parameters
  if init && ~ismember('V', ip.UsingDefaults)
      gpcf.V = ip.Results.V;
      if length(gpcf.V) ~= gpcf.numberClass
          error('length of the variance vector does not agree with the number of classes')
      end
  elseif init
      % takes one if nothing is given.
      gpcf.V = ones(1, gpcf.numberClass);
  end

  % initialize prior structure
  if init
      gpcf.p = [];
  end

  % initialize correlations
  if init || ~ismember('R_prior', ip.UsingDefaults)
    gpcf.p.R = ip.Results.R_prior;
    if ~isempty(gpcf.p.R)
        % corrects the prior
        gpcf.p.R.numberClass = gpcf.numberClass;
        gpcf.p.R.vectSize = gpcf.vectSize;
        gpcf.p.R.aValue = gpcf.aValue;
        if gpcf.p.R.numberClass > gpcf.p.R.nu
            error('degrees of freedom must be greater than number of classes')
        end
    end
  end

  % initialize variances
  if init && ~ismember('V_prior', ip.UsingDefaults)
      gpcf.p.V = ip.Results.V_prior;
      if ~isempty(ip.Results.V_prior)
          gpcf.varfields = (fields(gpcf.p.V)');
          if length(structfun(@numel, gpcf.p.V)) ~= gpcf.numberClass
              error('prior missing for some variance')
          end
      end
  else
      var = [repmat('var', gpcf.numberClass , 1), num2str((1:gpcf.numberClass)')];
      for ind = 1:size(var, 1)
          var_aux = var(ind, :);
          var_aux = var_aux(~isspace(var_aux));
          Vpriors.(var_aux).p = ip.Results.V_prior;
          %Vpriors.(var(ind, :)).p = ip.Results.V_prior;
      end
      gpcf.p.V = Vpriors;
      gpcf.varfields = (fields(gpcf.p.V)');
  end

end

function y = gpcf_covar_realtoz(x, a, d)
% Description :
%   · Transforms the real line to the interval (-1, 1) using the shifted
%   logistic function
%
%   · rho = 2 ./ (1 + exp(-a*x)) - 1;
%   · x is the vector on the real line
%   · d equals to [], 1 or 2, and indicates whether the output corresponds
%     no derivatives, first or second derivative.

if isempty(d)
    % modified logistic distribution function
    y = 2 ./ (1 + exp(-a*x)) - 1;
elseif d == 1
    % repeated terms
    eminax = exp(-a*x);
    % 1st derivative, drho/dx
    y = 2 * a * (1 + eminax).^(-2) .* eminax;
elseif d == 2
    % repeated terms
    a2 = a ^ 2;
    eminax = exp(-a*x);
    % 2nd derivative, d2rho/dx2
    y = 4 * a2 * (1 + eminax).^(-3) .* eminax.^2 - 2 * a2 * (1 + eminax).^(-2) .* eminax;
else
    error('Something is wrong. Maybe d should be [], 1 or 2')
end
end

function y = gpcf_covar_ztoreal(x, a)
%  Description :
%   · Transforms the interval (-1, 1) to the real line, using the inverse
%   hyperbolic tangent function
%
%   · y = - 1/a * log(2/(x+1) - 1)
%   · x in (-1, 1)^(dim(x))

if any(abs(x) >= 1)
    error('domain error');
else
    y = -1/a * log(2./(x+1) - 1);
end
end

function y = gpcf_covar_rhotoreal(gpcf)
%  Description :
%   · Transforms the matrix R to the matrix Z (composed by the elements
%   gamma on the real line)

ind1 = gpcf.index(:, 3);
ind2 = gpcf.index(:, 4);

Z = zeros(gpcf.numberClass, gpcf.numberClass);
R = eye(gpcf.numberClass);
R([ind1, ind2]) = [gpcf.R'; gpcf.R'];

W = chol(R);
Z(1, 2:gpcf.numberClass) = W(1, 2:gpcf.numberClass);
for i1 = 2:(gpcf.numberClass-1)
    for j1 = (i1+1):gpcf.numberClass
        Z(i1, j1) = W(i1, j1) .* exp(-0.5*sum(log(1-Z(1:(i1-1), j1).^2)));
        % Z(i1, j1) = W(i1, j1) .* 1/sqrt(prod(1-Z(1:(i1-1), j1).^2));
    end
end

y = gpcf.fh.ztoreal(Z(ind2), gpcf.aValue);
end

function y = gpcf_covar_realtorho(gpcf, w, d)
%  Description :
%   · Transforms the vector gamma on the real line to the matrix R (in a vector);

%   · z = 2 ./ (1 + exp(-a*x)) - 1;
%   · x is the vector on the real line
%   · d equals to [], 1 or 2, and indicates whether the output corresponds
%     no derivatives, first or second derivative.

i = gpcf.index(:, 1);
j = gpcf.index(:, 2);
ind2 = gpcf.index(:, 4);
seq = gpcf.index(:, end);
i2 = gpcf.vectSize;

Z = zeros(gpcf.numberClass, gpcf.numberClass);
W = eye(gpcf.numberClass, gpcf.numberClass);

Z(ind2) = gpcf.fh.realtoz(w(1:i2), gpcf.aValue, []);
W(1, 2:gpcf.numberClass) = Z(1, 2:gpcf.numberClass);

for i1 = 2:gpcf.numberClass
    for j1 = i1:gpcf.numberClass
        ztmp = 0.5.*sum(log(1 - Z(1:(i1-1), j1).^2));
        W(i1, j1) = ((i1 == j1) + (i1 ~= j1)*Z(i1, j1)) * exp(ztmp);
        % ztmp = sqrt(1-Z(1:(i1-1), j1).^2);
        % W(i1, j1) = ((i1 == j1) + ~(i1 == j1)*Z(i1, j1)) * prod(ztmp);
    end
end

if isempty(d)
    % correlation matrix
    R = W'*W;
    y = R(ind2);

elseif d == 1
    % indexes
    ind3 = [j i seq];

    % derivatives of the cholesk decomposition w.r.t values on the real line
    dW = zeros(gpcf.numberClass, gpcf.numberClass, i2);
    dL = zeros(gpcf.numberClass, gpcf.numberClass, i2);
    z = Z(ind2);
    dz = gpcf.fh.realtoz(w, gpcf.aValue, 1);

    fz = sqrt(1 - z.^2);
    dfz = - 1./sqrt(1 - z.^2) .*  z .* dz;

    for k = 1:i2
        % take the column
        j1 = ind3(ind3(:, 3) == k, 2);

        % take the indexes for that column
        ind4 = ind3(ind3(:, 2) == j1, end);

        fzaux = fz(ind4);
        dfzaux = dfz(ind4);

        for i1 = 1:j1
            if i1 == 1
                dW(i1, j1, k) = (k == ind4(1))*dz(k);

            elseif i1 ~= j1
                kaux = ind4(i1);

                if kaux < k
                    continue
                else
                    ind5 = ind4(1:(i1-1));
                    fzaux2 = fz(ind5);
                    dfzaux2 = dfz(ind5);
                    e = (k == ind5);

                    if  kaux == k
                        %dW(i1, j1, k) = dz(k).*prod(fzaux2(~e));
                        dW(i1, j1, k) = dz(k).*exp(sum(log(fzaux2(~e))));
                    end

                    if kaux > k
                        %dW(i1, j1, k) = z(kaux).*dfzaux2(e)*prod(fzaux2(~e));
                        dW(i1, j1, k) = z(kaux).*dfzaux2(e)*exp(sum(log(fzaux2(~e))));
                    end
                end

            elseif i1 == j1
                e = (k == ind4);
                %dW(i1, j1, k) = dfzaux(e)*prod(fzaux(~e));
                dW(i1, j1, k) = dfzaux(e)*exp(sum(log(fzaux(~e))));

            end
        end
        dL(:, :, k) = dW(:, :, k)';

    end
    y = dL;

elseif d == 2
    % lower cholesk decompostion of R
    y = W';

end
end

function [w, s, h] = gpcf_covar_pak(gpcf, w)
%  GPCF_COVAR_PAK Combine GP correlation matrix values into one vector
%   keeping the order of input below.
%
%  Description :
%   · W = GPCF_COVAR_PAK(GPCF) takes the non-diagonal elements of
%     the correlation matrix and put them into a single row vector w
%     This is a mandatory subfunction used for example
%     in energy and gradient computations.
%
%   · w = [rho_(1, 2), ..., rho_(1, J), rho_(2, 3), ..., rho_(2, J), ..., rho_((J-1), J)]
%
%  * See also
%    GPCF_covar_UNPAK

  w = []; s = {}; h = [];

  % pak parameters of correlation functions
  for i = 1:gpcf.numberClass
      [wh, sh, hh] = gpcf.corrFun{i}.fh.pak(gpcf.corrFun{i});
      w = [w wh];
      s = [s; sh];
      h = [h 1+hh];
  end

  % pak correlations
  if ~isempty(gpcf.p.R)
      i = gpcf.index(:, 1);
      j = gpcf.index(:, 2);
      waux = gpcf.fh.rhotoreal(gpcf);
      w = [w waux'];
      S = [repmat('realcorr.', gpcf.vectSize, 1), num2str(j), num2str(i)];
      s = [s; cellstr(S)];
      h = [h 1];

      % pak hyperparameters of R
      [wh, sh, hh] = gpcf.p.R.fh.pak(gpcf.p.R);
      sh = strcat(repmat('prior-', size(sh, 1), 1), sh);
      w = [w wh];
      s = [s; sh];
      h = [h 1+hh];
  end

  % pak variances
  if ~isempty(gpcf.p.V)
    w = [w log(gpcf.V)];
    S = [repmat('log var.', gpcf.numberClass, 1), num2str((1:gpcf.numberClass)')];
    s = [s; cellstr(S)];
    h = [h 1];

    % pak hyperparameters of V
    for i = 1:gpcf.numberClass
        cfvf = gpcf.varfields{i};
        [wh, sh, hh] = gpcf.p.V.(cfvf).p.fh.pak(gpcf.p.V.(cfvf).p);
        sh = strcat(repmat('prior-', size(sh, 1), 1), sh);
        w = [w wh];
        s = [s; sh];
        h = [h 1+hh];
    end

   end
end

function [gpcf, w] = gpcf_covar_unpak(gpcf, w)
%  GPCF_covar_UNPAK Sets the covariance function parameters into the
%   structure
%
%  Description :
%   · [GPCF, W] = GPCF_COVAR_UNPAK(GPCF, W) takes a covariance
%     function structure GPCF and a hyperparameter vector W,
%     and returns a covariance function structure identical to
%     the input, except that the covariance hyperparameters have
%     been set to the values in W. Deletes the values set to GPCF
%     from W and returns the modified W. This is a mandatory
%     subfunction used for example in energy and gradient computations.
%
%  * See also
%    GPCF_covar_PAK

  % unpak parameters of correlations functions
  for i = 1:gpcf.numberClass
      cf = gpcf.corrFun{i};
      [gpcf.corrFun{i}, w] = cf.fh.unpak(cf, w);
  end

  gpp = gpcf.p;

  % unpak for Σ
  if ~ isempty(gpp.R)
      gpcf.R = gpcf.fh.realtorho(gpcf, w(1:gpcf.vectSize), [])';
      w = w((gpcf.vectSize + 1):end);

      % unpak hyperparameters of R
      [p, w] = gpcf.p.R.fh.unpak(gpcf.p.R, w);
      gpcf.p.R = p;
  end

  if ~ isempty(gpp.V)
      i3 = gpcf.numberClass;
      gpcf.V = exp(w(1:i3));
      w = w((i3 + 1):end);

      % unpak hyperparameters of V
      % loop for the hyperpriors
      for i = 1:gpcf.numberClass
          cfvf = gpcf.varfields{i};
          [p, w] = gpcf.p.V.(cfvf).p.fh.unpak(gpcf.p.V.(cfvf).p, w);
          gpcf.p.V.(cfvf).p = p;
      end
  end
end

function lp = gpcf_covar_lp(gpcf)
%  GPCF_COVAR_LP Evaluate the log prior of covariance function parameters
%
%  Description :
%   · LP = GPCF_COVAR_LP(GPCF, X, T) takes a correlation function
%     structure GPCF and evaluates log-prior(R)
%
%  * See also
%    GPCF_COVAR_PAK, GPCF_covar_UNPAK, GPCF_covar_LPG, GP_E

  lp = 0;

  % add log-prior for correlations functions parameters
  for i = 1:gpcf.numberClass
      lp = lp + gpcf.corrFun{i}.fh.lp(gpcf.corrFun{i});
  end

  gpp = gpcf.p;

  % log-prior for correlations
  if ~isempty(gpp.R)
     % mapping to the real line
     x  = gpcf.fh.rhotoreal(gpcf);
     z  = gpcf.fh.realtoz(x, gpcf.aValue, []);
     dz = gpcf.fh.realtoz(x, gpcf.aValue, 1);

     % add log-prior on rho parameterization
     lp = lp + gpp.R.fh.lp(gpcf.R, gpp.R);

     % add log(|det J|) on the real line parameterization. Equation 11 in
     % Lewandowski et al (2009).
     pwr = gpcf.numberClass - gpcf.index(:, 2) - 1;
     lp = lp + 0.5 .* sum(pwr .* log(1 - z.^2)) + sum(log(dz));

  end

  % log-prior for variances
  if ~isempty(gpp.V)
      for i = 1:gpcf.numberClass
          cfvf = gpcf.varfields{i};

          lp = lp + gpp.V.(cfvf).p.fh.lp(gpcf.V(i), ...
              gpp.V.(cfvf).p) + log(gpcf.V(i));
      end
  end
end

function lpg = gpcf_covar_lpg(gpcf)
%  GPCF_COVAR_LPG Evaluate gradient of the log prior with respect
%   to the parameters.
%
%  Description :
%   · LPG = GPCF_COVAR_LPG(GPCF) takes a covariance function
%     structure GPCF and returns LPG = d log (p(th))/dth, where th
%     is the parametric vector of correlations and variances.
%     This is a mandatory subfunction used for example in gradient
%     computations.
%
%  * See also
%    GPCF_covar_PAK, GPCF_covar_UNPAK, GPCF_covar_LP, GP_G

  lpg = [];

  % stack grad vector of the correlation functions parameters
  for i = 1:gpcf.numberClass
      lpg = [lpg gpcf.corrFun{i}.fh.lpg(gpcf.corrFun{i})];
  end

  gpp = gpcf.p;

  % for correlations
  if ~isempty(gpcf.p.R)
      % mapping to the real line
      real = gpcf.fh.rhotoreal(gpcf);
      z   = gpcf.fh.realtoz(real, gpcf.aValue, [])';
      dz  = gpcf.fh.realtoz(real, gpcf.aValue, 1)';
      d2z = gpcf.fh.realtoz(real, gpcf.aValue, 2)';
      pwr = (gpcf.numberClass - gpcf.index(:, 2) - 1)';

      % cholesk decomposition
      L = gpcf.fh.realtorho(gpcf, real, 2);

      % transforming to first derivative of L w.r.t gammas
      dL = gpcf.fh.realtorho(gpcf, real, 1);

      b = zeros(gpcf.vectSize, gpcf.vectSize);
      for k = 1:size(dL, 3)
          dRaux = dL(:, :, k)*L' + L*dL(:, :, k)';
          b(:, k) = dRaux(gpcf.index(:, 4));
      end

      % dlogp(R(gamma))/dgamma
      dlpdgm = gpp.R.fh.lpg(gpcf.R, gpp.R) * b;

      lpgs = dlpdgm - pwr.*(z.*dz./(1 - z.^2)) + d2z./dz;
      lpg = [lpg lpgs];
  end

  % for variances
  if ~isempty(gpp.V)
      % building the grad vector on the real line parameterization
      for i = 1:gpcf.numberClass
          cfvf = gpcf.varfields{i};
          lpgs = gpp.V.(cfvf).p.fh.lpg(gpcf.V(i), gpp.V.(cfvf).p);
          lpg = [lpg lpgs(1).*gpcf.V(i)+1 lpgs(2:end)];
      end
  end
end

function C = gpcf_covar_sigma(gpcf, type)
%  GP_COVAR_COV  Evaluate the covariance matrix Σ of the linear model of
%  coregionalization and its Cholesky decomposition.
%
%  Description :
%   · C = GP_COVAR_COV(GP, i1) takes the gpcf structure and
%     returns the covariance matrix Σ and its Cholesky decomposition.
%     Every element (i, j) of Σ contains the covariance (parameterised by
%     correlation and variances) between class i and class j. This is a
%     mandatory subfunction.
%
%  * See also
%    GPCF_COVAR_TRCOV, GPCF_COVAR_TRVAR, GP_COV, GP_TRCOV

% linear indexes
ind1 = gpcf.index(:, 3);
ind2 = gpcf.index(:, 4);

% corr matrix
R = eye(gpcf.numberClass);

% filling elements in lower and upper part
R([ind1, ind2]) = [gpcf.R, gpcf.R];

% matrix Σ
Sig = bsxfun(@times, bsxfun(@times, sqrt(gpcf.V)', R), sqrt(gpcf.V));
% Sig = S * R * S;

% other options
if nargin == 2
    switch type
        case 'cov'
            % empty matrix
            C{1} = Sig;

            % calcule cholesk decomposition of Sigma
            [L, notpositivedefinite] = chol(Sig, 'lower');
            % test whether is positive-definite or not
            if notpositivedefinite
                L = NaN(gpcf.numberClass, gpcf.numberClass);
                L(ind2) = 0;
            end

            % take cholesky decomposition
            C{2} = L;

        case 'corr'
            x = gpcf.fh.rhotoreal(gpcf);
            L = gpcf.fh.realtorho(gpcf, x, 2);

            C{1} = L*L';
            C{2} = L;
    end
else
    C = Sig;

end
end

function C = gpcf_covar_cov(gpcf, x1, x2)
%  GP_COVAR_COV  Evaluate the covariance matrix between two input vectors
%
%  Description
%   · C = GP_COVAR_COV(GP, TX, X) takes in correlation structure
%     and two matrixes TX and X that contain input vectors to GP.
%     Returns covariance matrix C.
%     Every element ij of C contains correlation between inputs i
%     in TX and j in X. This is a mandatory subfunction used for
%     example in prediction and energy computations.
%
%
%  * See also
%    GPCF_COVAR_TRCOV, GPCF_COVAR_TRVAR, GP_COV, GP_TRCOV

 if isempty(x2)
     x2 = x1;
 end

 if size(x1, 2) ~= size(x2, 2)
     error('the number of columns of X1 and X2 has to be same')
 end

 xClass1 = x1(:, gpcf.classVariables);
 xClass2 = x2(:, gpcf.classVariables);

 if ~issorted(xClass1) || ~issorted(xClass2)
     error('you need to give the class variable increasing downwards');
 end

 if max(xClass1) > gpcf.numberClass || max(xClass2) > gpcf.numberClass
     error('more classes than the given number of classes');
 end

 n1 = size(xClass1, 1);
 n2 = size(xClass2, 1);

 % information of the classes in the data
 ind1 = unique(xClass1)'; nind1 = size(ind1, 2);
 ind2 = unique(xClass2)'; nind2 = size(ind2, 2);

 % positions of observations in each vector
 nb1 = find(diff([-inf xClass1' inf]));
 nb2 = find(diff([-inf xClass2' inf]));

 % chol(Σ)
 M = gpcf.fh.sigma(gpcf, 'cov');
 L = M{2};

 % M = gpcf.fh.sigma(gpcf, 'corr');
 % L = bsxfun(@times, sqrt(gpcf.V)', M{2});

 C = zeros(n1, n2);
 for k = 1:gpcf.numberClass

     % kth column of the cholesk decomposition of Σ = SRS
     T = L(:, k) * L(:, k)';

     for i = 1:nind1
         for j = 1:nind2
             inb1 = nb1(i) : nb1(i + 1) - 1;
             inb2 = nb2(j) : nb2(j + 1) - 1;
             K(inb1, inb2) = T(ind1(i), ind2(j));
         end
     end

     cf = gpcf.corrFun{k};
     C = C + cf.fh.cov(cf, x1, x2) .* K;
end

 C(abs(C) < eps) = 0;
end

function dCRV = gpcf_covar_cfg(gpcf, x, x2, mask, i1)
%  GPCF_COVAR_CFG  Evaluate the derivarive of the covar matrix
%
%  Description :
%   · dRff = GPCF_COVAR_CFG(GPCF, X) takes a
%     covariance structure GPCF, a matrix of inputs
%     vectors and returns dSRS/drho and/or dSRS/dV, the gradients of
%     SRS matrix
%
%   · dRff = GPCF_COVAR_CFG(GPCF, X, X2) takes a
%     covariance structure GPCF, a matrix X of input
%     vectors and returns  dSRS/drho and/or dSRS/dV, the gradients of
%     SRS matrix with respect to rho (cell array with matrix
%     elements). This subfunction is needed when using sparse
%     approximations (e.g. FIC).
%
%  * See also
%    GPCF_COVAR_PAK, GPCF_COVAR_UNPAK, GPCF_COVAR_LP, GP_G

dCRV = {};

% covariance matrix Σ and chol(Σ)
M = gpcf.fh.sigma(gpcf, 'corr'); % R = M{1};
L = M{2};

% covariance matrix
%Sig = sVL * sVL';

% \sqrt(diag(var)) times L
sVL = bsxfun(@times, sqrt(gpcf.V)', L);

% take class variables
xC = x(:, gpcf.classVariables);

% positions of the observations
nb = diff([0 xC' xC(end) + 1]) .* (1:(length(xC) + 1));
nb = nb(nb ~= 0);

% number of observation in each class
diffnb = diff(nb);

% number of classes
nc = gpcf.numberClass;

% number of observations
n = length(xC);

% use savememory option
savememory = nargin == 5;

if nargin == 2 || (isempty(x2) && isempty(mask))

    % Evaluate the values (dSRS/dgamma_12, dSRS/dgamma_13, ..., dSRS/dgamma_(J-1)J
    % and/or dSRS/dV_1 ...  dSRS/dV_J

    % build the big Tj's
    T = cell(1, gpcf.numberClass);

    for  k = 1:gpcf.numberClass
        Taux = sVL(:, k) * sVL(:, k)';

        for i = 1:gpcf.numberClass
            inb1 = nb(i) : nb(i + 1) - 1;
            T{k}(inb1, inb1) = Taux(i, i);

            for j = (i + 1):gpcf.numberClass
                inb2 = nb(j):nb(j + 1) - 1;

                T{k}(inb1, inb2) = Taux(i, j);
                T{k}(inb2, inb1) = Taux(j, i);
            end
        end
    end

    % correlations matrices
    dC = {};

    for i1 = 1:nc
        dKK = gpcf.corrFun{i1}.fh.cfg(gpcf.corrFun{i1}, x);

        if ~isempty(dKK)
            % number of derivatives of the correlation function
            i2 = length(dKK);

            % derivatives of the specific correlation function
            for i3 = 1:i2
                dC{end + 1} = dKK{i3} .* T{i1};
            end
        end
    end

    % correlations and variances in Σ = sqrt(S) L L sqrt(S)
    sVdL = [];
    dsVL = [];

    if ~isempty(gpcf.p.R) || ~isempty(gpcf.p.V)

        % for gammas in Σ
        if ~isempty(gpcf.p.R)
            % transforming to the real line
            real = gpcf.fh.rhotoreal(gpcf);

            % transforming to first derivative of L w.r.t gammas
            dL = gpcf.fh.realtorho(gpcf, real, 1);

            % variances times first derivatives w.r.t correlations
            % \sqrt(diag(var)) (dLj/dΘ)
            sVdL = bsxfun(@times, sqrt(gpcf.V)', dL);
        end

        % for variances in Σ
        if ~isempty(gpcf.p.V)
            dV = 0.5 .* sqrt(gpcf.V);
            dsVLm = bsxfun(@times, dV', L); % take the lines ...
            dsVL = zeros(nc, nc, nc);

            for k = 1:nc
                dsVL(k, :, k) = dsVLm(k, :);
            end
        end

        % derivatives w.r.t to correlations or/and variances
        dSig = cat(3, sVdL, dsVL);

        if ~isempty(dSig)

            nSig = length(dSig);
            dL = cell(1, nSig);
            % dCorr = zeros(n, n);

            for k = 1:nSig
                % sum_j R_j kron (daj/dΘ * aj' + aj * daj/dΘ')
                dL{k} = zeros(n, n);

                for j = 1:nc
                    aj  = repelem(sVL(:, j).', diffnb);
                    daj = repelem(dSig(:, j, k), diffnb);
                    dd = daj * aj;

                    cf = gpcf.corrFun{j};
                    dCorr = cf.fh.trcov(cf, x) .* (dd + dd.');
                    dL{k} = dL{k} + dCorr;
                end
            end
        end
    end

    % return matrices
    dCRV = [dC dL];

% related to sparse approximations
elseif nargin == 3 || isempty(mask)
    error('nargin == 3 || isempty(mask) not implemented yet');

elseif nargin == 4 || nargin == 5
    error('nargin == 4 || nargin == 5 not implemented yet');

end
end

function C = gpcf_covar_trcov(gpcf, x)
%  GP_COVAR_TRCOV  Evaluate training covariance matrix of inputs
%
%  Description :
%   · C = GP_COVAR_TRCOV(GP, TX) takes in correlation structure
%     matrix TX that contains training input vectors.
%     Returns covariance matrix C. Every element ij of C contains
%     the correlation between inputs i and j in TX.
%     This is a mandatory subfunction used for example in
%     prediction and energy computations.
%
%  * See also
%    GPCF_COVAR_COV, GPCF_COVAR_TRVAR, GP_COV, GP_TRCOV
%
% HERE IS TO TAKE ADVANTAGE OF THE SYMMETRIC MATRIX

% takes the class variables
xC = x(:, gpcf.classVariables);

% checking
if ~issorted(xC)
    error('class variable should increasing downwards');
end

% number of observations
n = size(xC, 1);

% getting the information of the classes in the data
a = unique(xC); % na = size(a, 1);

% checking
if max(a) > gpcf.numberClass
% if na ~= gpcf.numberClass || max(a) ~= gpcf.numberClass
    error('more or less classes than given to the model');
end

% getting the information of the classes in the data
ind = unique(xC)'; nind = size(ind, 2);

% number of observations in each class
nb = find(diff([-inf xC' inf]));

% Try to use the C implementation
% C = trcov_corr(gpcf, x1);

C = NaN;

if isnan(C)
    % take chol(Σ)
    M = gpcf.fh.sigma(gpcf, 'cov');
    L = M{2};

    % full matrices
    C = zeros(n, n);  K = zeros(n, n);

    for k = 1:gpcf.numberClass
        % T_k = a_k * a_k' matrices,
        T =  L(:, k) * L(:, k)';

        for i = 1:nind
            inb1 = nb(i) : nb(i + 1) - 1;
            K(inb1, inb1) = T(ind(i), ind(i));

            for j = (i + 1):nind
                inb2 = nb(j) : nb(j + 1) - 1;

                K(inb1, inb2) = T(ind(i), ind(j));
                K(inb2, inb1) = T(ind(j), ind(i));
            end
        end

        % corrFun kronecker T_k
        cf = gpcf.corrFun{k};
        C = C + cf.fh.trcov(cf, x) .* K;
    end

    C(abs(C) < eps) = 0;
end
end

function C = gpcf_covar_trvar(gpcf, x)
%  GP_COVAR_VECTOR  Evaluate training variance vector
%
%  Description:
%   · C = GP_COVAR_TRVAR(GPCF, TX) takes in correlation structure
%     of and matrix TX that contains training inputs.
%     Returns correlation vector C, which are ones.
%     Every element i of C contains the correlation of the input in TX.
%     This is a mandatory subfunction used for example in prediction and
%     energy computations.
%
%  * See also:
%    GPCF_COVAR_COV, GP_COV, GP_TRCOV

 % take indicator class
 x = x(:, gpcf.classVariables);

 % information of the classes in the data
 a = unique(x);

 % locate each class
 nb = find(diff([-inf x' inf]));

 % number of observation in each class
 diffnb = diff(nb);

 C = repelem(gpcf.V(a), diffnb)';
 end

function reccf = gpcf_covar_recappend(reccf, ri, gpcf)
%  RECAPPEND  Record append
%
%  Description:
%   · RECCF = GPCF_COVAR_RECAPPEND(RECCF, RI, GPCF) takes a
%     correlation structure record structure RECCF, record index RI
%     and correlation structure GPCF with the current MCMC
%     samples of the parameters. Returns RECCF which contains
%     all the old samples and the current samples from GPCF.
%     This subfunction is needed when using MCMC sampling (gp_mc).
%
%  * See also:
%    GP_MC and GP_MC -> RECAPPEND

  if nargin == 2
      % initialize the record
      reccf.type = 'gpcf_covar';

      % initialize parameters for correlations functions
      for i = 1:ri.numberClass
          reccf.corrFun{i} = ri.corrFun{i}.fh.recappend([], ri.corrFun{i});
      end

      % Initialize parameters
      reccf.R = [];
      reccf.V = [];

      % Set the function handles
      reccf.fh.realtoz = @gpcf_covar_realtoz;
      reccf.fh.ztoreal = @gpcf_covar_ztoreal;
      reccf.fh.rhotoreal = @gpcf_covar_rhotoreal;
      reccf.fh.realtorho = @gpcf_covar_realtorho;
      reccf.fh.pak = @gpcf_covar_pak;
      reccf.fh.unpak = @gpcf_covar_unpak;
      reccf.fh.lp = @gpcf_covar_lp;
      reccf.fh.lpg = @gpcf_covar_lpg;
      reccf.fh.cfg = @gpcf_covar_cfg;
      reccf.fh.cov = @gpcf_covar_cov;
      reccf.fh.sigma = @gpcf_covar_sigma;
      reccf.fh.trcov = @gpcf_covar_trcov;
      reccf.fh.trvar = @gpcf_covar_trvar;
      reccf.fh.recappend = @gpcf_covar_recappend;
      reccf.p = [];
      reccf.p.R = [];
      reccf.p.V = [];

      if isfield(ri.p, 'R') && ~isempty(ri.p.R)
          reccf.p.R = ri.p.R;
      end

      if isfield(ri.p, 'V') && ~isempty(ri.p.V)
          for i = 1:ri.numberClass
              reccf.p.V.(ri.varfields{i}).p = ri.p.V.(ri.varfields{i}).p;
          end
      end

      if isfield(ri, 'classVariables')
          reccf.classVariables = ri.classVariables;
          reccf.numberClass = ri.numberClass;
          reccf.vectSize = ri.vectSize;
          reccf.aValue = ri.aValue;
          reccf.varfields = ri.varfields;
          reccf.index = ri.index;
      end

  else
      % Append to the record
      gpp = gpcf.p;

      if ~ isfield(gpcf,'metric')
          % record correlation function elements
          for i = 1:gpcf.numberClass
              cf = gpcf.corrFun{i};
              reccf.corrFun{i} = cf.fh.recappend(reccf.corrFun{i}, ri, cf);
          end

          % record R elements
          reccf.R(ri, :) = gpcf.R;
          if isfield(gpp, 'R') && ~isempty(gpp.R)
              reccf.p.R = gpp.R.fh.recappend(reccf.p.R, ri, gpcf.p.R);
          end

          % record V elements
          reccf.V(ri, :) = gpcf.V;
          if isfield(gpp, 'V') && ~isempty(gpp.V)
              for i = 1:gpcf.numberClass
                  cfvf = gpcf.varfields{i};
                  reccf.p.V.(cfvf).p = ...
                      gpp.V.(cfvf).p.fh.recappend(reccf.p.V.(cfvf).p, ...
                      ri, gpcf.p.V.(cfvf).p);
              end
          end
      end

  end
end


function [xyVec] = createXYCoordVec(start,pix,dim,dir)
% createXYCoordVec
%
% Create one dimensional vector

if strcmpi(dir,'pos')% positive direction
    for i=1:dim
        xyVec(i)=start+(pix.*i)-pix;
    end

elseif strcmpi(dir,'neg')% negative direction
    for i=1:dim
        xyVec(i)=start-(pix.*i)+pix;
    end
end


function tcell_flow_png ( )

%*****************************************************************************80
%
%% TCELL_FLOW_PNG plots TCELL data.
%
%  Discussion:
%
%    This MATLAB script file reads the T-Cell flow data:
%
%      geometry (XY values at nodes, assumed to be in 'xy.txt')
%      flow (UV values at nodes, in a sequence of files starting with 'up001.txt')
%
%    and plots the velocity vectors (U,V)(X,Y), and saves each plot as a
%    PNG file, presumably so the PNG files can be gathered into an
%    animation.
%
%    The file plots either the velocity vector field, or the velocity
%    direction field, depending on the value of the internal logical
%    parameter "normalized".
%
%    The MATLAB routine quiver internally scales the vectors, but this can be
%    adjusted by using a value of SCALE that is not 1.
%
%    For the unnormalized case, the velocity field is computed to a fixed
%    scale determined by finding the largest velocity vector over time.
%
%    This routine requires some auxiliary routines in order to "increment"
%    the name of the current velocity data file to get the name of the next
%    one, with the main such routine being called FILE_NAME_INC.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 July 2003
%
%  Author:
%
%    John Burkardt
%
  FALSE = 0;
  TRUE = 1;
  scale = 1.0;
  normalized = TRUE;

  nframes = 500;
%
%  Get the XY coordinates of the nodes.
%
  load xy.txt;

  x = xy(:,1);
  y = xy(:,2);
%
%  Thin the data.
%
  thin_factor = 4;
  thin_dex = thin_index ( x, y, thin_factor );
  thin_num = length ( thin_dex );
  x = x(thin_dex);
  y = y(thin_dex);
%
%  For unnormalized plots, you need to make sure that a fixed scale is preserved from
%  step to step.  The only way I can see to do this requires that I determine the
%  maximum velocity over all time, and then append one extract node and velocity
%  to the data structure.
%
  if ( normalized == FALSE )

    upfile = 'up000.txt';
    vnorm_max = 0.0;

    for i = 1 : nframes
      fprintf ( 1, '  Checking file %d.\n', i );
      upfile = file_name_inc ( upfile );
      uv = load ( upfile );
      u = uv(thin_dex,1);
      v = uv(thin_dex,2);
      norm = sqrt ( u.^2 + v.^2 );
      vnorm_max = max ( vnorm_max, max ( norm ) );
    end

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Maximum visible velocity magnitude is %f\n', vnorm_max );

  end
%
%  Set the coordinates of the boundary lines, and an extra set of
%  lines that will be invisible, but which make some space for the
%  file name to be displayed within the plot area.
%
  bx1 = [ 0.00, 1.00, 1.00, 0.00, 0.00, 0.00 ];
  by1 = [ 1.00, 1.00, 0.99, 0.99, 0.99, 1.00 ];

  bx2 = [ 0.00, 0.25, 0.25, 0.75, 0.75, 1.00, 1.00, 0.74, 0.74, 0.26, 0.26, 0.00, 0.00];
  by2 = [ 0.50, 0.50, 0.00, 0.00, 0.50, 0.50, 0.51, 0.51, 0.01, 0.01, 0.51, 0.51, 0.50];

  bx3 = [ -0.10,  1.10, 1.10, -0.10, -0.10 ];
  by3 = [ -0.10, -0.10, 1.10,  1.10, -0.10 ];
%
%  Set the name of the 0th (nonexistent) velocity file.
%  All the velocity files with have this format, with the
%  numeric part of the name incremented to get the next one.
%  In particular, the first velocity file is called UP001.TXT.
%
  upfile = 'up000.txt';
  pngfile = 'up000.png';

  for i = 1 : nframes

    fprintf ( 1, '  Converting file %d.\n', i );

    upfile = file_name_inc ( upfile );
    pngfile = file_name_inc ( pngfile );
    uv = load ( upfile );
    u = uv(thin_dex,1);
    v = uv(thin_dex,2);

    if ( normalized == TRUE )
      norm = sqrt ( u.^2 + v.^2 );
      nonzero = find ( norm ~= 0.0 );
      u(nonzero) = u(nonzero) ./ norm(nonzero);
      v(nonzero) = v(nonzero) ./ norm(nonzero);
      vnorm_max = 1.0;
    end

    x(thin_num+1) = 0.00;
    y(thin_num+1) = 1.05;
    u(thin_num+1) = vnorm_max;
    v(thin_num+1) = 0.0;

    quiver ( x, y, u, v, scale )
%
%  Draw the boundary lines, and the invisible bounding line.
%
    line ( bx1, by1, 'color', 'r' )
    line ( bx2, by2, 'color', 'r' )
    line ( bx3, by3, 'color', 'w' )

    axis equal
    if ( normalized == TRUE )
      title ( 'T-Cell Direction Field' )
    else
      title ( 'T-Cell Flow Field' )
    end
    text ( 0.420, 1.03, upfile )
%
%  Here's where we take a snapshot of the current image, and save it to
%  a PNG file.
%
    F = getframe;
    [X,map] = frame2im ( F );
    imwrite ( X, pngfile, 'PNG' );

  end

  return
end
function file_name = file_name_inc ( file_name )

%*****************************************************************************80
%
%% FILE_NAME_INC generates the next filename in a series.
%
%  Discussion:
%
%    It is assumed that the digits in the name, whether scattered or
%    connected, represent a number that is to be increased by 1 on
%    each call.  If this number is all 9's on input, the output number
%    is all 0's.  Non-numeric letters of the name are unaffected..
%
%    If the name is empty, then the routine stops.
%
%    If the name contains no digits, the empty string is returned.
%
%  Example:
%
%      Input            Output
%      -----            ------
%      'a7to11.txt'     'a7to12.txt'  (typical case.  Last digit incremented)
%      'a7to99.txt'     'a8to00.txt'  (last digit incremented, with carry.)
%      'a9to99.txt'     'a0to00.txt'  (wrap around)
%      'cat.txt'        ' '           (no digits in input name.)
%      ' '              STOP!         (error.)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 September 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string FILE_NAME, the string to be incremented.
%
%    Output, string FILE_NAME, the incremented string.
%
  lens = length ( file_name );

  if ( lens <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'FILE_NAME_INC - Fatal error!\n' );
    fprintf ( 1, '  The input filename is empty.\n' );
    error ( 'FILE_NAME_INC - Fatal error!' );
  end

  change = 0;

  for i = lens : -1 : 1

    c = file_name(i);

    if ( '0' <= c & c <= '8' )

      change = change + 1;

      c = c + 1;

      file_name(i) = c;

      return

    elseif ( c == '9' )

      change = change + 1;

      c = '0';

      file_name(i) = c;

    end

  end

  if ( change == 0 )
    file_name = ' ';
  end

  return
end
function thin_dex = thin_index ( x, y, thin_factor )

%*****************************************************************************80
%
%%  THIN_INDEX determines thinning indices for a X, Y data.
%
%  Discussion:
%
%    A set of X, Y data is given, that is presumably, not too far off
%    from being on a rectangular grid.
%
%    The input value of THIN_FACTOR indicates by how much the data should
%    be thinned.
%
%    The X and Y ranges are computed, and only those data points are
%    retained for which both X and Y lie in an appropriate subrange.
%
%    For instance, a THIN_FACTOR of 2 would essentially save data
%    that lay in the black squares of a checkerboard.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 June
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X(NODE_NUM), Y(NODE_NUM), the X and Y coordinates
%    of the nodes.
%
%    Input, integer THIN_FACTOR, the thinning factor.
%
%    Output, integer THIN_DEX(NODE_NUM), contains in (1:THIN_NUM) the
%    indices into X and Y of the vectors to be retained after thinning.
%
  TRUE = 1;
  FALSE = 0;

  node_num = length ( x );

  x_unique_num = 0;

  for i = 1 : node_num

    unique = TRUE;

    for j = 1 : x_unique_num
      if ( x(i) == x_unique(j) )
        unique = FALSE;
        break;
      end
    end

    if ( unique )
      x_unique_num = x_unique_num + 1;
      x_unique(x_unique_num) = x(i);
    end

  end

  sort ( x_unique );

  y_unique_num = 0;

  for i = 1 : node_num

    unique = TRUE;

    for j = 1 : y_unique_num
      if ( y(i) == y_unique(j) )
        unique = FALSE;
        break;
      end
    end

    if ( unique )
      y_unique_num = y_unique_num + 1;
      y_unique(y_unique_num) = y(i);
    end

  end

  sort ( y_unique );

  thin_num = 0;

  for i = 1 : node_num

    for j = 1 : x_unique_num-1
      if ( x_unique(j) <= x(i) & x(i) <= x_unique(j+1) )
        x_bin = j;
        break;
      end
    end

    for j = 1 : y_unique_num-1
      if ( y_unique(j) <= y(i) & y(i) <= y_unique(j+1) )
        y_bin = j;
        break;
      end
    end

    if ( mod ( y_bin, thin_factor ) == thin_factor / 2 & ...
         mod ( x_bin, thin_factor ) == thin_factor / 2 )

      thin_num = thin_num + 1;
      thin_dex(thin_num) = i;

    end

  end

  return
end


function out = cfg_example_run_cumsum2(job)
% Example function that returns the cumulative sum of an vector given in
% job.a in out. The output is referenced as out(:), this is defined in
% cfg_example_vout_cumsum1.
%
% This code is part of a batch job configuration system for MATLAB. See
%      help matlabbatch
% for a general overview.
%_______________________________________________________________________
% Copyright (C) 2007 Freiburg Brain Imaging

% Volkmar Glauche
% $Id: cfg_example_run_cumsum2.m 1716 2008-05-23 08:18:45Z volkmar $

rev = '$Rev: 1716 $'; %#ok

% The harvested cfg_repeat will return a cell array of numbers. These need
% to be cat'ed together to be useful as input to cumsum.
out.cs = cumsum(cat(1,job.a{:}));


function R=ImportSurferGrd(filename)
% ImportSurferGrid reads an ASCII format surfer .GRD file and returns the
% data matrix in R. NaN values, which are written as 1.7308+38 in surfer
% are replaced with NaN to prevent crashing.
% Input:
%      filename = name of the file to be read, including ".grd" extension
% Output:
% R - matrix containing data (e.g elevation)
%James Ramm 2011
if exist(filename,'file')
grdfile=fopen(filename,'r');    % Open file
else
    error('ImportGrid:NoFile','File does not exist');
end

code=fgetl(grdfile);            % Reads surfer code 'DSAA'
% Grid dimensions (number of nodes, 2nd line of surfer file)
aux=str2num(fgetl(grdfile)); nx=aux(1); % columns
ny=aux(2); % rows
% X limits, 3rd line of .grd file
aux=(fgetl(grdfile)); %xmin=aux(1); xmax=aux(2);
% Y limits, 4th line of .grd file
aux=(fgetl(grdfile)); %ymin=aux(1); ymax=aux(2);
% Z limits, 5th line
aux=(fgetl(grdfile)); %zmin=aux(1); aux(2);
% Reads Z matrix
[R] = fscanf(grdfile, '%f', [nx,ny]);

% transpose matrix as matlab indices are (rows, columns) not (columns, rows).
R = R';
fclose(grdfile);
% Get rid of those crappy surfer nan values and replace with NaN
[I,J]= size(R);

for i = 1:I
    for j=1:J
        if R(i,j)>100000
            R(i,j) = NaN;
        end
    end
end
end


function M = fixedrankfactory_2factors_preconditioned(m, n, k)
% Manifold of m-by-n matrices of rank k with two factor quotient geometry.
%
% function M = fixedrankfactory_2factors_preconditioned(m, n, k)
%
% This geometry is tuned to least-squares problems such as low-rank matrix
% completion with ell-2 loss.
%
% A point X on the manifold is represented as a structure with two
% fields: L and R. The matrices L (m-by-k) and R (n-by-k) are
% full column-rank matrices such that X = L*R'.
%
% Tangent vectors are represented as a structure with two fields: L, R.
%
% Please cite the Manopt paper as well as the research paper:
%     @Techreport{mishra2012optimized,
%       Title   = {A {R}iemannian geometry for low-rank matrix completion},
%       Author  = {Mishra, B. and Adithya Apuroop, K. and Sepulchre, R.},
%       Journal = {Arxiv preprint arXiv:1211.1550},
%       Year    = {2012}
%     }
%
%
% See also: fixedrankembeddedfactory fixedrankfactory_2factors fixedrankfactory_3factors_preconditioned

% This file is part of Manopt: www.manopt.org.
% Original author: Bamdev Mishra, Dec. 30, 2012.
% Contributors:
% Change log:
%
%	April 04, 2015 (BM):
%      Cosmetic changes including avoiding storing the inverse of a
%       k-by-k matrix.

    M.name = @() sprintf('LR''(tuned to least square problems) quotient manifold of %dx%d matrices of rank %d', m, n, k);

    M.dim = @() (m+n-k)*k;

    % Some precomputations at the point X to be used in the inner product
    % (and pretty much everywhere else).
    function X = prepare(X)
        if ~all(isfield(X,{'LtL','RtR'}))
            L = X.L;
            R = X.R;
            X.LtL = L'*L;
            X.RtR = R'*R;
        end
    end

    % The choice of metric is motivated by symmetry and
    % tuned to least-squares cost function.
    M.inner = @iproduct;
    function ip = iproduct(X, eta, zeta)
        X = prepare(X);
        ip = trace(X.RtR*(eta.L'*zeta.L)) + trace(X.LtL*(eta.R'*zeta.R)); % Scaled metric
    end

    M.norm = @(X, eta) sqrt(M.inner(X, eta, eta));

    M.dist = @(x, y) error('fixedrankfactory_2factors_preconditioned.dist not implemented yet.');

    M.typicaldist = @() 10*k;

    M.egrad2rgrad = @egrad2rgrad;
    function rgrad = egrad2rgrad(X, egrad)
        X = prepare(X);

        % Riemannian gradient
        rgrad.L = egrad.L/X.RtR;
        rgrad.R = egrad.R/X.LtL;
    end

    M.ehess2rhess = @ehess2rhess;
    function Hess = ehess2rhess(X, egrad, ehess, eta)
        X = prepare(X);

        % Riemannian gradient.
        rgrad = egrad2rgrad(X, egrad);

        % Directional derivative of the Riemannian gradient.
        Hess.L = ehess.L/X.RtR - 2*egrad.L*(X.RtR \ (symm(eta.R'*X.R) / X.RtR));
        Hess.R = ehess.R/X.LtL - 2*egrad.R*(X.LtL \ (symm(eta.L'*X.L) / X.LtL));

        % We still need a correction factor for the non-constant metric.
        Hess.L = Hess.L + rgrad.L*(symm(eta.R'*X.R)/X.RtR) + eta.L*(symm(rgrad.R'*X.R)/X.RtR) - X.L*(symm(eta.R'*rgrad.R)/X.RtR);
        Hess.R = Hess.R + rgrad.R*(symm(eta.L'*X.L)/X.LtL) + eta.R*(symm(rgrad.L'*X.L)/X.LtL) - X.R*(symm(eta.L'*rgrad.L)/X.LtL);

        % Project on the horizontal space.
        Hess = M.proj(X, Hess);
    end

    M.proj = @projection;
    function etaproj = projection(X, eta)
        X = prepare(X);

        % Projection onto the horizontal space.
        Lambda = 0.5*((eta.R'*X.R)/X.RtR  -   X.LtL\(X.L'*eta.L));
        etaproj.L = eta.L + X.L*Lambda;
        etaproj.R = eta.R - X.R*Lambda';
    end

    M.tangent = M.proj;

    M.tangent2ambient = @(X, eta) eta;

    M.retr = @retraction;
    function Y = retraction(X, eta, t)
        if nargin < 3
            t = 1.0;
        end
        Y.L = X.L + t*eta.L;
        Y.R = X.R + t*eta.R;

        % Numerical conditioning step: a simpler version.
        % We need to ensure that L and R are do not have very relative
        % skewed norms.

        scaling = norm(X.L, 'fro')/norm(X.R, 'fro');
        scaling = sqrt(scaling);
        Y.L = Y.L / scaling;
        Y.R = Y.R * scaling;

        % These are reused in the computations of gradient and Hessian.
        Y = prepare(Y);
    end

    M.exp = @exponential;
    function Y = exponential(X, eta, t)
        if nargin < 3
            t = 1.0;
        end

        Y = retraction(X, eta, t);
        warning('manopt:fixedrankfactory_2factors_preconditioned:exp', ...
            ['Exponential for fixed rank ' ...
            'manifold not implemented yet. Used retraction instead.']);
    end

    M.hash = @(X) ['z' hashmd5([X.L(:) ; X.R(:)])];

    M.rand = @random;

    function X = random()
        X.L = randn(m, k);
        X.R = randn(n, k);
    end

    M.randvec = @randomvec;
    function eta = randomvec(X)
        eta.L = randn(m, k);
        eta.R = randn(n, k);
        eta = projection(X, eta);
        nrm = M.norm(X, eta);
        eta.L = eta.L / nrm;
        eta.R = eta.R / nrm;
    end

    M.lincomb = @lincomb;

    M.zerovec = @(X) struct('L', zeros(m, k),'R', zeros(n, k));

    M.transp = @(x1, x2, d) projection(x2, d);

    % vec and mat are not isometries, because of the scaled inner metric.
    M.vec = @(X, U) [U.L(:) ; U.R(:)];

    M.mat = @(X, u) struct('L', reshape(u(1:(m*k)), m, k), ...
        'R', reshape(u((m*k+1):end), n, k));

    M.vecmatareisometries = @() false;

    % Auxiliary functions
    symm = @(M) .5*(M+M');
end

% Linear combination of tangent vectors.
function d = lincomb(x, a1, d1, a2, d2) %#ok<INUSL>

    if nargin == 3
        d.L = a1*d1.L;
        d.R = a1*d1.R;
    elseif nargin == 5
        d.L = a1*d1.L + a2*d2.L;
        d.R = a1*d1.R + a2*d2.R;
    else
        error('Bad use of fixedrankfactory_2factors_preconditioned.lincomb.');
    end

end


function plin = linearize(p)
% LINEARIZE Linearize SDPVAR object
%
% h = LINEARIZE(p)
%
% Returns linearization p(double(x)) + dp(double(x))*(x-double(x))
% where x is the SDPVAR variables defining the polynomial p(x)
%
% See also SDPVAR, JACOBIAN

if isa(p,'double')
    plin = zeros(size(p));
    return
end

if is(p,'linear') & ~is(p,'compound')
    plin = p;
    return
end

x = recover(depends(p));
x0 = double(x);
p0 = double(p);

n = size(p,1);
m = size(p,2);

if min(n,m)>1
    plin = [];
    for i = 1:m
        plin = [plin p0(:,i)+double(jacobian(p(:,i),x))*(x-x0)];
    end
else
    plin = p0+double(jacobian(p,x))*(x-x0);
end


function test_bug2784

% MEM 2gb
% WALLTIME 00:10:00
% DEPENDENCY ft_mvaranalysis

data = [];
data.trial{1} = randn(2,100);
data.trial{2} = randn(2,101);
data.time{1}  = (0:99)./100+0.001;
data.time{2}  = (0:100)./100;
data.label    = {'chan01';'chan02'};

try
  cfg = [];
  ft_mvaranalysis(cfg,data);
catch me
  if strcmp(me.message, 'time axes of all trials should be identical')
    fprintf('error caught by function\n');
  end
end

cfg.t_ftimwin = 0.5;
cfg.toi       = 0.25;
ft_mvaranalysis(cfg,data);

% load in the data provided by Tyler Grummett
filename = dccnpath('/home/common/matlab/fieldtrip/data/test/bug2784.mat');
load(filename);

cfg = [];
mdata = ft_mvaranalysis(cfg, temp);

% the issue Tyler had could be reproduced and was caused by a combination
% of things: the tfwin being 501 samples long, as well as the time axes of
% the trials being different. ft_mvaranalysis has been adjusted to more
% robustly deal with these cases.


function calendar_nyt_test04 ( )

%*****************************************************************************80
%
%% CALENDAR_NYT_TEST04 compares JED_TO_NYT and JED_TO_NYT_ISSUE_IDEAL.
%
%  Discussion:
%
%    JED_TO_NYT returns the issue number printed on the New York Times.
%    There were many "mistakes" and "accidents" and "corrections" in this system.
%
%    JED_TO_NYT_ISSUE_IDEAL returns an "ideal" issue number which keeps track
%    of every issue, in a sensible, usable way.
%
%  Volume  Issue   D M         Y
%  ------  -----  -- --------- ----
%       1      1  18 September 1851
%       1     92   3 January   1852
%       2    404   3 January   1853
%       2    416  17 January   1853
%       3    856  15 June      1854
%
%       4   1210   4 August    1855
%       5   1259   1 October   1855
%       5   1491  28 June      1856
%       6   1706   9 March     1857
%       7   2155  17 August    1858
%
%       8   2421  23 June      1859
%       9   2586   4 January   1860
%      10   2897   3 January   1861
%      10   3000   1 May       1861
%      12   3432  23 September 1862
%
%      13   3794  20 November  1863
%      14   4130  19 December  1864
%      14   4230  16 April     1865
%      15   4576  26 May       1866
%      17   5034  13 November  1867
%
%      17   5250  22 July      1868
%      20   6189  22 July      1871
%      47  14499   5 February  1898
%      47  15000   7 February  1898
%      52  16579  24 February  1903
%
%      53  16909  15 March     1904
%      54  17292  29 May       1905
%      55  17561  22 February  1906
%      57  18164  18 October   1907
%      58  18856   9 September 1909
%
%      66  21619   3 April     1917
%      74  24651  22 July      1925
%      89  29827  23 September 1939
%      89  30000  14 March     1940
%      94  31881   8 May       1945
%
%      94  31980  15 August    1945
%     113  38864  20 June      1964
%     114  39317  16 September 1965
%     117  40076  15 October   1967
%     118  40721  21 July      1969
%
%     120  41418  18 June      1971
%     127  44027   9 August    1978
%     128  44028   6 November  1978
%     141  48939  17 April     1992
%     144  50000  14 March     1995
%
%     147  50939   8 October   1997
%     149  51753  31 December  1999
%     149  51254   1 January   2000
%     150  51599  11 December  2000
%     154  53108  28 January   2005
%
%     157  54136  22 November  2007
%     157  54267  01 April     2008
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%   04 February 2009
%
%  Author:
%
%    John Burkardt
%
  test_num = 52;
  issue_test = [ ...
        1,    92,   404,   416,   856, ...
     1210,  1259,  1491,  1706,  2155, ...
     2421,  2586,  2897,  3000,  3432, ...
     3794,  4130,  4230,  4576,  5034, ...
     5250,  6189, 14499, 15000, 16579, ...
    16909, 17292, 17561, 18164, 18856, ...
    21619, 24651, 29827, 30000, 31881, ...
    31980, 38864, 39317, 40076, 40721, ...
    41418, 44027, 44028, 48939, 50000, ...
    50939, 51753, 51254, 51599, 53108, ...
    54136, 54267 ];
  jed_test = [ ...
   2397383.50, 2397490.50, 2397856.50, 2397870.50, 2398384.50, ...
   2398799.50, 2398857.50, 2399128.50, 2399382.50, 2399908.50, ...
   2400218.50, 2400413.50, 2400778.50, 2400896.50, 2401406.50, ...
   2401829.50, 2402224.50, 2402342.50, 2402747.50, 2403283.50, ...
   2403535.50, 2404630.50, 2414325.50, 2414327.50, 2416169.50, ...
   2416554.50, 2416994.50, 2417263.50, 2417866.50, 2418558.50, ...
   2421321.50, 2424353.50, 2429529.50, 2429702.50, 2431583.50, ...
   2431682.50, 2438566.50, 2439019.50, 2439778.50, 2440423.50, ...
   2441120.50, 2443729.50, 2443818.50, 2448729.50, 2449790.50, ...
   2450729.50, 2451543.50, 2451544.50, 2451889.50, 2453398.50, ...
   2454426.50, 2454557.50 ];
  volume_test = [ ...
       1,    1,    2,    2,    3, ...
       4,    5,    5,    6,    7, ...
       8,    9,   10,   10,   12, ...
      13,   14,   14,   15,   17, ...
      17,   20,   47,   47,   52, ...
      53,   54,   55,   57,   58, ...
      66,   74,   89,   89,   94, ...
      94,  113,  114,  117,  118, ...
     120,  127,  128,  141,  144, ...
     147,  149,  149,  150,  154, ...
     157,  157 ];

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CALENDAR_NYT_TEST04\n' );
  fprintf ( 1, '  For the New York Times issue date:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  JED -> NYT1 by historical record.\n' );
  fprintf ( 1, '  JED -> NYT2 by "JED_TO_NYT"\n' );
  fprintf ( 1, '  JED -> NYT3 by "JED_TO_NYT_ISSUE_IDEAL"\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '       JED                 Date             Volume     Issue     Issue     Issue\n' );
  fprintf ( 1, '                                                     (lookup)  (compute)  (Ideal)\n' );
  fprintf ( 1, '\n' );

  for test = 1 : test_num

    jed = jed_test(test);

    [ y, m, d, f ] = jed_to_ymdf_common ( jed );
    s = ymdf_to_s_common ( y, m, d, f );

    issue1 = issue_test(test);
    volume1 = volume_test(test);

    [ volume2, issue2 ] = jed_to_nyt ( jed );

    issue3 = jed_to_nyt_issue_ideal ( jed );

    fprintf ( 1, '  %11.2f  %25s  %8d  %8d  %8d  %8d\n', ...
      jed, s, volume1, issue1, issue2, issue3 );

  end

  return
end


function nx = setNull(x,e)
% set approx zero to exact zero

if nargin == 1
    e = -10;
end

epsilon = exp(log(10)*e);
nx = x;
nx(abs(nx) < epsilon) = 0;


function varargout = ubcmatch(varargin)
% VL_UBCMATCH  Match SIFT features
%   MATCHES = VL_UBCMATCH(DESCR1, DESCR2) matches the two sets of SIFT
%   descriptors DESCR1 and DESCR2.
%
%   [MATCHES,SCORES] = VL_UBCMATCH(DESCR1, DESCR2) retuns the matches and
%   also the squared Euclidean distance between the matches.
%
%   The function uses the algorithm suggested by D. Lowe [1] to reject
%   matches that are too ambiguous.
%
%   VL_UBCMATCH(DESCR1, DESCR2, THRESH) uses the specified threshold
%   THRESH. A descriptor D1 is matched to a descriptor D2 only if the
%   distance d(D1,D2) multiplied by THRESH is not greater than the
%   distance of D1 to all other descriptors. The default value of
%   THRESH is 1.5.
%
%   The storage class of the descriptors can be either DOUBLE, FLOAT,
%   INT8 or UINT8. Usually integer classes are faster.
%
%   REFERENCES::
%     [1] D. G. Lowe, Distinctive image features from scale-invariant
%     keypoints. IJCV, vol. 2, no. 60, pp. 91-110, 2004.
%
%   See also: VL_HELP(), VL_SIFT().
[varargout{1:nargout}] = vl_ubcmatch(varargin{:});


function [params, names] = gammaPriorExtractParam(prior)

% GAMMAPRIOREXTRACTPARAM Extract params from gamma prior structure.

% PRIOR

params = [prior.a prior.b];
if nargout > 1
  names = {'gamma a', 'gamma b'};
end


% This example loads the 'nps plus' image set of patterns, and examines the
% relationships between these patterns and the standard Buckner Lab
% networks.

% Layer 1 of the river plot is the Wager lab patterns, and Layer 2 is the
% Buckner Lab networks

%% Load NPSplus

[npsplus_obj, layer1names, imgnames] = load_image_set('npsplus');

wh = 1:4;
npsplus_obj = get_wh_image(npsplus_obj, wh);
layer1names = layer1names(wh);

layer1names

%% Get rectangular similarity matrix

stats = image_similarity_plot(npsplus_obj, 'bucknerlab', 'noplot', 'cosine_similarity');
% Similarity matrix is stats.r

% Threshold to look at positive associations
sim_matrix = stats.r;
sim_matrix(sim_matrix < 0) = 0;

layer2names = stats.networknames;

[n2, n1] = size(sim_matrix);

%layer1colors = scn_standard_colors(n1);
%layer1colors = seaborn_colors(n1);

layer1colors = {[1 .4 .2] [.3 .3 1] [.7 .3 1] [.3 .7 .5]};

layer2colors = seaborn_colors(n2);

%% Reorder rows and columns for display, as needed

% Custom REORDERING of layer 1 for aesthetics
neworder = [1 3 2 4];
sim_matrix = sim_matrix(:, neworder);
layer1names = layer1names(neworder);
layer1colors = layer1colors(neworder);

%  REORDER rows here based on similarity to make plot look cleaner
[sim_matrix, layer2names, layer2colors] = riverplot_reorder_matrix(sim_matrix, layer2names, layer2colors);

% Custom REORDERING of layer 2 because the automatic one isn't super
neworder = [1 7 2 3 4 5 6];
sim_matrix = sim_matrix(neworder, :);
layer2names = layer2names(neworder);
layer2colors = layer2colors(neworder);

%% Create the plot

create_figure('riverplot');
set(gca, 'YDir', 'reverse');
layer1 = riverplot_draw_layer(2, n1, 'colors', layer1colors, 'y_loc', 1.5);
layer2 = riverplot_draw_layer(5, n2, 'colors', layer2colors);

ribbons = riverplot_ribbon_matrix(layer1, layer2, sim_matrix, 'colors', layer1colors, 'layer1fullcoverage', 'steepness', 0);

% Turn off lines
riverplot_toggle_lines(ribbons);

% Increase opacity
riverplot_set_ribbon_property(ribbons, 'FaceAlpha', .6);

set(gca, 'XLim', [0 8]);

% Add names
layer1 = riverplot_layer_names(layer1, layer1names);
layer2 = riverplot_layer_names(layer2, layer2names, 'right');

axis off


function y = loadPcwConst(type, nSamples)
%LOADPCWCONST Creates a piecewise constant signal or image

% written by M. Storath
% $Date: 2014-09-02 14:56:23 +0200 (Di, 02. Sep 2014) $	$Revision: 105 $

if not(exist('nSamples', 'var'))
    nSamples = 2^8;
end

x = linspace(-1, 1, nSamples)';
y = zeros(size(x));

switch type
    case 'rect'
        y = (-0.5 < x) & ( x < 0.5);
    case {'step', 'heaviside'}
        y = x >= 0;
    case 'jumps'
        % two jumps
        y = (-0.3 < x) & ( x < -0.1) + 2 * ((0.1 < x) & ( x < 0.4));
    case 'equidistant'
        n = 8; % number of partitions
        step = nSamples / n;
        heights = [3, 1, 7, 6, 5, 0,6,4];
        for i = 1:n
            iv = ((i-1) * step + 1) : (i * step);
            y(iv) = heights(i) / max(heights);
        end

    case 'sampleDec'
        heights = [3, 1, 7, 6, 5, 0,6,6];
        jumps = diff(heights);
        jumpDist = [30, 40, 32, 32, 32, 50, 60];
        jumpIdx = cumsum(jumpDist);
        y(jumpIdx) = jumps;
        y = cumsum(y);
        y = mat2gray(y);

    case 'sampleDec2'
        heights = [3, 1, 7, 6, 5, 0,6,6];
        jumps = diff(heights);
        jumpDist = [30, 40, 32, 32, 32, 50, 40];
        jumpIdx = cumsum(jumpDist);
        y(jumpIdx) = jumps;
        y = cumsum(y);
        y = mat2gray(y);

    case 'equidistant2'
        n = 8; % number of partitions
        step = nSamples / n;
        heights = [3, 1, 7, 6, 5, 4,6,4];
        for i = 1:n
            iv = ((i-1) * step + 1) : (i * step);
            y(iv) = heights(i) / max(heights);
        end

    case 'sample1'
        steps = [2, 4, 3, 3, 2, 5, 3, 3, 3];
        steps = (cumsum(steps ./ sum(steps)) - 0.5) * 2;
        heights = [3, 1, 7, 5.5, 4.5, 3.5,6,0];
        n= numel(heights);
        for i = 1:n
            idx = find((steps(i) < x) & (x <= steps(i+1)));
            y( idx ) = heights(i) / max(heights);
        end

    case 'sample2'
        steps = [2, 4, 3, 3, 2, 5, 2, 3, 3,6,4,2,8,3,4, 4]; %15
        steps = (cumsum(steps ./ sum(steps)) - 0.5) * 2;
        heights = [3, 1, 7, 6, 5, 4,6,0, 2, 4, 3, 1.5, 7, 9, 2];
        n= numel(heights);
        for i = 1:n
            idx = find((steps(i) < x) & (x <= steps(i+1)));
            y( idx ) = heights(i) / max(heights);
        end

   case 'sample3'
        steps = [2, 4, 3, 6, 4, 5, 4, 3, 3];
        steps = (cumsum(steps ./ sum(steps)) - 0.5) * 2;
        heights = [3, 1, 7, 6, 5, 4,6,0];
        n= numel(heights);
        for i = 1:n
            idx = find((steps(i) < x) & (x <= steps(i+1)));
            y( idx ) = heights(i) / max(heights);
        end

   case 'sample4'
        %steps = [3, 2, 5, 10, 4, 3, 3];
        %steps = [ -Inf, (cumsum(steps ./ sum(steps)) - 0.5) * 2];
        %heights = [4, 2,3, 3.1, 3.05, 1, 1];
        steps = [2, 4, 2, 2, 1, 1];
        steps = [ -Inf, (cumsum(steps ./ sum(steps)) - 0.5) * 2];
        heights = [0.0, 0.1, 0.05, 1, 0.5, 0];
        n= numel(heights);
        for i = 1:n
            idx = find((steps(i) < x) & (x <= steps(i+1)));
            y( idx ) = heights(i) / max(abs(heights));
        end

   case 'sample5'
        steps = [3, 5, 3, 4, 6, 4, 3, 5, 3];
        steps = (cumsum(steps ./ sum(steps)) - 0.5) * 2;
        heights = [3, 7, 2, 4, 5, 2,6,0];
        n= numel(heights);
        for i = 1:n
            idx = find((steps(i) < x) & (x <= steps(i+1)));
            y( idx ) = heights(i) / max(heights);
        end

    case 'geo1'
        y = double(imread('geo1.png'))/255;

    case 'geo2'
        y = double(imread('geo2.png'))/255;

    case 'geo3'
        y = double(imread('geo3.png'))/255;

    case 'geo4'
        y = double(imread('geo4.png'))/255;

    case 'geo5'
        y = double(imread('geo5.png'))/255;

    case 'geo6'
        y = double(imread('geo6.png'))/255;

    case 'geo7'
        y = double(imread('geo7.png'))/255;

    case 'geo8'
        y = double(imread('geo8.png'))/255;

    case 'overlay'
        I1 = double(imread('rectangle.png'));
        I2 = double(imread('octo.png'));
        y =  (I1 + 2*I2)/ (3*255);

    otherwise
        error('This option does not exist.')
end

% cast to double
y = double(y);

end


function [ r, lchar, ierror ] = s_to_r8 ( s )

%*****************************************************************************80
%
%% S_TO_R8 reads an R8 from a string.
%
%  Discussion:
%
%    This routine will read as many characters as possible until it reaches
%    the end of the string, or encounters a character which cannot be
%    part of the real number.
%
%    Legal input is:
%
%       1 blanks,
%       2 '+' or '-' sign,
%       2.5 spaces
%       3 integer part,
%       4 decimal point,
%       5 fraction part,
%       6 'E' or 'e' or 'D' or 'd', exponent marker,
%       7 exponent sign,
%       8 exponent integer part,
%       9 exponent decimal point,
%      10 exponent fraction part,
%      11 blanks,
%      12 final comma or semicolon.
%
%    with most quantities optional.
%
%  Example:
%
%    S                 R
%
%    '1'               1.0
%    '     1   '       1.0
%    '1A'              1.0
%    '12,34,56'        12.0
%    '  34 7'          34.0
%    '-1E2ABCD'        -100.0
%    '-1X2ABCD'        -1.0
%    ' 2E-1'           0.2
%    '23.45'           23.45
%    '-4.2E+2'         -420.0
%    '17d2'            1700.0
%    '-14e-2'         -0.14
%    'e2'              100.0
%    '-12.73e-9.23'   -12.73 * 10.0**(-9.23)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 November 2003
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string S, the string containing the
%    data to be read.  Reading will begin at position 1 and
%    terminate at the end of the string, or when no more
%    characters can be read to form a legal real.  Blanks,
%    commas, or other nonnumeric data will, in particular,
%    cause the conversion to halt.
%
%    Output, real R, the value that was read from the string.
%
%    Output, integer LCHAR, the number of characters of S that were used to form R.
%
%    Output, integer IERROR, is 0 if no error occurred.
%
  s_length = s_len_trim ( s );
  ierror = 0;
  r = 0.0;
  lchar = -1;
  isgn = 1;
  rtop = 0.0;
  rbot = 1.0;
  jsgn = 1;
  jtop = 0;
  jbot = 1;
  ihave = 1;
  iterm = 0;

  while ( 1 )

    lchar = lchar + 1;
    c = s(lchar+1);
%
%  Blank character.
%
    if ( c == ' ' )

      if ( ihave == 2 )

      elseif ( ihave == 6 | ihave == 7 )
        iterm = 1;
      elseif ( 1 < ihave )
        ihave = 11;
      end
%
%  Comma.
%
    elseif ( c == ',' | c == ';' )

      if ( ihave ~= 1 )
        iterm = 1;
        ihave = 12;
        lchar = lchar + 1;
      end
%
%  Minus sign.
%
    elseif ( c == '-' )

      if ( ihave == 1 );
        ihave = 2;
        isgn = -1;
      elseif ( ihave == 6 )
        ihave = 7;
        jsgn = -1;
      else
        iterm = 1;
      end
%
%  Plus sign.
%
    elseif ( c == '+' )

      if ( ihave == 1 )
        ihave = 2;
      elseif ( ihave == 6 )
        ihave = 7;
      else
        iterm = 1;
      end
%
%  Decimal point.
%
    elseif ( c == '.' )

      if ( ihave < 4 )
        ihave = 4;
      elseif ( 6 <= ihave & ihave <= 8 )
        ihave = 9;
      else
        iterm = 1;
      end
%
%  Exponent marker.
%
    elseif ( ch_eqi ( c, 'E' ) | ch_eqi ( c, 'D' ) )

      if ( ihave < 6 )
        ihave = 6;
      else
        iterm = 1;
      end
%
%  Digit.
%
    elseif ( ihave < 11 & ch_is_digit ( c ) )

      if ( ihave <= 2 )
        ihave = 3;
      elseif ( ihave == 4 )
        ihave = 5;
      elseif ( ihave == 6 | ihave == 7 )
        ihave = 8;
      elseif ( ihave == 9 )
        ihave = 10;
      end

      d = ch_to_digit ( c );

      if ( ihave == 3 )
        rtop = 10.0 * rtop + d;
      elseif ( ihave == 5 )
        rtop = 10.0 * rtop + d;
        rbot = 10.0 * rbot;
      elseif ( ihave == 8 )
        jtop = 10 * jtop + d;
      elseif ( ihave == 10 )
        jtop = 10 * jtop + d;
        jbot = 10 * jbot;
      end
%
%  Anything else is regarded as a terminator.
%
    else
      iterm = 1;
    end
%
%  If we haven't seen a terminator, and we haven't examined the
%  entire string, go get the next character.
%
    if ( iterm == 1 | s_length <= lchar + 1 )
      break;
    end

  end
%
%  If we haven't seen a terminator, and we have examined the
%  entire string, then we're done, and LCHAR is equal to S_LENGTH.
%
  if ( iterm ~= 1 & lchar + 1 == s_length )
    lchar = s_length;
  end
%
%  Number seems to have terminated.  Have we got a legal number?
%  Not if we terminated in states 1, 2, 6 or 7!
%
  if ( ihave == 1 | ihave == 2 | ihave == 6 | ihave == 7 )
    ierror = ihave;
    fprintf ( 1, '\n' );
    fprintf ( 1, 'S_TO_R8 - Fatal error!\n' );
    fprintf ( 1, '  IHAVE = %d\n', ihave );
    error ( 'S_TO_R8 - Fatal error!' );
  end
%
%  Number seems OK.  Form it.
%
  if ( jtop == 0 )
    rexp = 1.0;
  else

    if ( jbot == 1 )
      rexp = 10.0^( jsgn * jtop );
    else
      rexp = jsgn * jtop;
      rexp = rexp / jbot;
      rexp = 10.0^rexp;
    end

  end

  r = isgn * rexp * rtop / rbot;

  return
end


function symb_pvec = sdisplay(pvec,symbolicname)
%SDISPLAY Symbolic display of SDPVAR expression
%
% Note that the symbolic display only work if all
% involved variables are explicitely defined as
% scalar variables.
%
% Variables that not are defined as scalars
% will be given the name ryv(i). ryv means
% recovered YALMIP variables, i indicates the
% index in YALMIP (i.e. the result from getvariables)
%
% If you want to change the generic name ryv, just
% pass a second string argument
%
% EXAMPLES
%  sdpvar x y
%  sdisplay(x^2+y^2)
%    ans =
%       'x^2+y^2'
%
%  t = sdpvar(2,1);
%  sdisplay(x^2+y^2+t'*t)
%    ans =
%      'x^2+y^2+ryv(5)^2+ryv(6)^2'

% Author Johan L�fberg
% $Id: sym.m,v 1.1 2005-02-22 16:50:11 johanl Exp $
allnames = {};
for pi = 1:size(pvec,1)
    for pj = 1:size(pvec,2)
        Y.type = '()';
        Y.subs = [{pi} {pj}];
        p = subsref(pvec,Y);

        if isnumeric(p)
            symb_p = num2str(p);
        else
            LinearVariables = depends(p);
            x = recover(LinearVariables);
            exponent_p = full(exponents(p,x));
            names = cell(length(x),1);
            for i = 1:length(names)
                names{i} = ['x' num2str(LinearVariables(i))];
                allnames{end+1} = names{i};
            end

            symb_p = '';
            if all(exponent_p(1,:)==0)
                symb_p = num2str(full(getbasematrix(p,0)));
                exponent_p = exponent_p(2:end,:);
            end

            for i = 1:size(exponent_p,1)
                coeff = full(getbasematrixwithoutcheck(p,i));
                switch coeff
                    case 1
                        coeff='+';
                    case -1
                        coeff = '-';
                    otherwise
                        if isreal(coeff)
                        if coeff >0
                            coeff = ['+' num2str2(coeff)];
                        else
                            coeff=[num2str2(coeff)];
                        end
                        else
                            coeff = ['+' '(' num2str2(coeff) ')' ];
                        end
                end
                symb_p = [symb_p coeff symbmonom(names,exponent_p(i,:))];
            end
            if symb_p(1)=='+'
                symb_p = symb_p(2:end);
            end
        end
        symb_pvec{pi,pj} = symb_p;
    end
end
allnames = unique(allnames);
for i = 1:length(allnames)
    evalin('caller',['syms ' allnames{i}]);
end

S = '';
for pi = 1:size(pvec,1)
    ss = '';
    for pj = 1:size(pvec,2)
        ss = [ss ' ' symb_pvec{pi,pj} ','];
    end
    S = [S ss ';'];
end
S = ['[' S ']']   ;
symb_pvec = evalin('caller',S);

function s = symbmonom(names,monom)
s = '';
for j = 1:length(monom)
    if abs( monom(j))>0
        s = [s names{j}];
        if monom(j)~=1
            s = [s '^' num2str(monom(j))];
        end
        s =[s '*'];
    end

end
if isequal(s(end),'*')
    s = s(1:end-1);
end

function s = num2str2(x)
s = num2str(full(x));
if isequal(s,'1')
    s = '';
end
if isequal(s,'-1')
    s = '-';
end


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

%% -*- texinfo -*-
%% @documentencoding UTF-8
%% @defmethod @@sym gamma (@var{x})
%% Symbolic gamma function.
%%
%% Example:
%% @example
%% @group
%% syms x
%% y = gamma (x)
%%   @result{} y = (sym) Γ(x)
%% @end group
%% @end example
%%
%% Note: this file is autogenerated: if you want to edit it, you might
%% want to make changes to 'generate_functions.py' instead.
%%
%% @end defmethod

function y = gamma(x)
  if (nargin ~= 1)
    print_usage ();
  end
  y = elementwise_op ('gamma', x);
end

%!error gamma (sym(1), 2)
%!assert (isequaln (gamma (sym(nan)), sym(nan)))

%!shared x, d
%! d = 1;
%! x = sym('1');

%!test
%! f1 = gamma(x);
%! f2 = gamma(d);
%! assert( abs(double(f1) - f2) < 1e-15 )

%!test
%! D = [d d; d d];
%! A = [x x; x x];
%! f1 = gamma(A);
%! f2 = gamma(D);
%! assert( all(all( abs(double(f1) - f2) < 1e-15 )))

%!test
%! % round trip
%! y = sym('y');
%! A = gamma (d);
%! f = gamma (y);
%! h = function_handle (f);
%! B = h (d);
%! assert (A, B, -eps)


function triangulation_test16 ( )

%*****************************************************************************80
%
%% TEST16 tests TRIANGULATION_ORDER3_CHECK.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 August 2006
%
%  Author:
%
%    John Burkardt
%
  triangle_num = 16;
  node_num = 13;

  triangle_node(1:3,1:triangle_num) = [ ...
     3,   4,   1; ...
     3,   1,   2; ...
     3,   2,   8; ...
     2,   1,   5; ...
     8,   2,  13; ...
     8,  13,   9; ...
     3,   8,   9; ...
    13,   2,   5; ...
     9,  13,   7; ...
     7,  13,   5; ...
     6,   7,   5; ...
     9,   7,   6; ...
    10,   9,   6; ...
     6,   5,  12; ...
    11,   6,  12; ...
    10,   6,  11 ]';

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST16\n' );
  fprintf ( 1, '  For a triangulation of a set of nodes,\n' );
  fprintf ( 1, '  TRIANGULATION_ORDER3_CHECK checks the triangulation.\n' );

  i4mat_transpose_print ( 3, triangle_num, triangle_node, '  Triangles:' );
%
%  Pass all tests.
%
  ierror = triangulation_order3_check ( node_num, triangle_num, triangle_node );

  fprintf ( 1, '  Error code = %d\n', ierror );
%
%  Fail test 1.
%
  node_num2 = 2;

  ierror = triangulation_order3_check ( node_num2, triangle_num, ...
    triangle_node );

  fprintf ( 1, '  Error code = %d\n', ierror );
%
%  Fail test 2.
%
  triangle_num2 = 0;

  ierror = triangulation_order3_check ( node_num, triangle_num2, ...
    triangle_node );

  fprintf ( 1, '  Error code = %d\n', ierror );
%
%  Fail test 3.
%
  isave = triangle_node(2,5);
  triangle_node(2,5) = 0;

  ierror = triangulation_order3_check ( node_num, triangle_num, triangle_node );

  fprintf ( 1, '  Error code = %d\n', ierror );
  triangle_node(2,5) = isave;
%
%  Fail test 4.
%
  isave = triangle_node(3,10);
  triangle_node(3,10) = 2 * node_num + 1;

  ierror = triangulation_order3_check ( node_num, triangle_num, triangle_node );

  fprintf ( 1, '  Error code = %d\n', ierror );
  triangle_node(3,10) = isave;
%
%  Fail test 5.
%
  triangle_node(3,4) = 3;
  triangle_node(3,8) = 3;
  triangle_node(3,10) = 3;
  triangle_node(3,11) = 3;
  triangle_node(2,14) = 3;

  ierror = triangulation_order3_check ( node_num, triangle_num, triangle_node );

  fprintf ( 1, '  Error code = %d\n', ierror );

  triangle_node(3,4) = 5;
  triangle_node(3,8) = 5;
  triangle_node(3,10) = 5;
  triangle_node(3,11) = 5;
  triangle_node(2,14) = 5;
%
%  Fail test 6.
%
  triangle_node(1,9) = 7;
  ierror = triangulation_order3_check ( node_num, triangle_num, triangle_node );

  fprintf ( 1, '  Error code = %d\n', ierror );
  triangle_node(1,9) = 9;
%
%  Fail test 7.
%
  triangle_node(3,7) = 2;
  ierror = triangulation_order3_check ( node_num, triangle_num, triangle_node );

  fprintf ( 1, '  Error code = %d\n', ierror );
  triangle_node(3,7) = 9;

  return
end


function im_warp = warpImageFast(im,XXdense, YYdense)

%{
Citation:
J. Xiao, K. A. Ehinger, A. Oliva and A. Torralba.
Recognizing Scene Viewpoint using Panoramic Place Representation.
Proceedings of 25th IEEE Conference on Computer Vision and Pattern Recognition, 2012.
http://sun360.mit.edu
%}

minX = max(1,floor(min(min(XXdense)))-1);
minY = max(1,floor(min(min(YYdense)))-1);

maxX = min(size(im,2),ceil(max(max(XXdense)))+1);
maxY = min(size(im,1),ceil(max(max(YYdense)))+1);

im = im(minY:maxY,minX:maxX,:);

for c=1:size(im,3)
    % im_warp(:,:,c) = uint8(interp2(double(im(:,:,c)), XXdense-minX+1, YYdense-minY+1,'*cubic'));
    %im_warp(:,:,c) = interp2(im(:,:,c), XXdense-minX+1, YYdense-minY+1,'*cubic');
    im_warp(:,:,c) = interp2(im(:,:,c), XXdense-minX+1, YYdense-minY+1,'*linear');
end


function g = gaussianKernGradient(kern, x, varargin)

% GAUSSIANKERNGRADIENT Gradient of gaussian kernel's parameters.
% FORMAT
% DESC computes the gradient of
%	functions with respect to the gaussian kernel's
%	parameters. As well as the kernel structure and the input positions,
%	the user provides a matrix PARTIAL which gives the partial
%	derivatives of the function with respect to the relevant elements of
%	the kernel matrix.
% RETURN g:  gradients of the function of interest with respect to the
%	   kernel parameters. The ordering of the vector should match that
%	   provided by the function kernExtractParam.
% ARG kern : the kernel structure for which the gradients are being
%	   computed.
% ARG x : the input locations for which the gradients are being
%	   computed.
% ARG partial : matrix of partial derivatives of the function of
%	   interest with respect to the kernel matrix. The argument takes the
%	   form of a square matrix of dimension  numData, where numData is
%	   the number of rows in X.
%
% FORMAT
% DESC  computes the derivatives
%	as above, but input locations are now provided in two matrices
%	associated with rows and columns of the kernel matrix.
% RETURN g : gradients of the function of interest with respect to the
%	   kernel parameters.
% ARG kern : the kernel structure for which the gradients are being
%	   computed.
% ARG x1 : the input locations associated with the rows of the kernel
%	   matrix.
% ARG x2 : the input locations associated with the columns of the kernel
%	   matrix.
% ARG partial : matrix of partial derivatives of the function of
%	   interest with respect to the kernel matrix. The matrix should have
%	   the same number of rows as X1 and the same number of columns as X2
%	   has rows.
%
% SEEALSO : gaussianKernParamInit, kernGradient,
% gaussianKernDiagGradient, kernGradX
%
% COPYRIGHT : Mauricio A. Alvarez and Neil D. Lawrence, 2008
%
% MODIFICATIONS : Mauricio A. Alvarez, 2009.

% KERN

if nargin < 4
    x2 = x;
    covPar = varargin{1};
else
    x2 = varargin{1};
    covPar = varargin{2};
end

if kern.isArd
    [K, Kbase] = gaussianKernCompute(kern, x, x2);
    matGrad = zeros(kern.inputDimension,1);
    temp = 0.5*covPar.*K;
    for i = 1:kern.inputDimension,
        pX = x(:,i);
        X = pX(:, ones(1, size(x2,1)));
        pX2 = x2(:,i)';
        X2 = pX2(ones(size(x,1),1), :);
%         X = repmat(x(:,i),1, size(x2,1));
%         X2 = repmat(x2(:,i)',size(x,1),1);
        X_X2 = X - X2;
        matGrad(i) = -sum(sum(temp.*X_X2.*X_X2));
    end
else
    [K, Kbase, n2] = gaussianKernCompute(kern, x, x2);
    matGrad = - 0.5*sum(sum(covPar.*K.*n2,2));

end
g = [matGrad(:)' sum(sum(covPar.*Kbase))];


function [bnet, Qnodes, Fnodes, Onode] = mk_motif_hhmm(varargin)
% [bnet, Qnodes, Fnodes, Onode] = mk_motif_hhmm(...)
%
% Make the following HHMM
%
%    S2 <----------------------> S1
%    |                           |
%    |                           |
%   M1 -> M2 -> M3 -> end        B1 -> end
%
% where Mi represents the i'th letter in the motif
% and B is the background state.
% Si chooses between running the motif or the background.
% The Si and B states have self loops (not shown).
%
% The transition params are defined to respect the above topology.
% The background is uniform; each motif state has a random obs. distribution.
%
% Optional params:
% motif_length  - required, unless we specify motif_pattern
% motif_pattern - if specified, we make the motif submodel deterministically
%                  emit this pattern
% background    - if specified, we make the background submodel
%                  deterministically emit this (makes the motif easier to see!)

args = varargin;
nargs = length(args);

% extract pattern, if any
motif_pattern = [];
for i=1:2:nargs
  switch args{i},
   case 'motif_pattern', motif_pattern = args{i+1};
  end
end

% set defaults
motif_length = length(motif_pattern);
background_char = [];

% get params
for i=1:2:nargs
  switch args{i},
   case 'motif_length', motif_length = args{i+1};
   case 'background', background_char = args{i+1};
  end
end

chars = ['a', 'c', 'g', 't'];
Osize = length(chars);

Qsize = [2 motif_length];
Qnodes = 1:2;
D = 2;
transprob = cell(1,D);
termprob = cell(1,D);
startprob = cell(1,D);

% startprob{d}(k,j), startprob{1}(1,j)
% transprob{d}(i,k,j), transprob{1}(i,j)
% termprob{d}(k,j)

% LEVEL 1

startprob{1} = zeros(1, 2);
startprob{1} = [1 0]; % always start in the background model

% When in the background state, we stay there with high prob
% When in the motif state, we immediately return to the background state.
transprob{1} = [0.8 0.2;
		1.0 0.0];

% LEVEL 2
startprob{2} = 'leftstart'; % both submodels start in substate 1
transprob{2} = zeros(motif_length, 2, motif_length);
termprob{2} = zeros(2, motif_length);

% In the background model, we only use state 1.
transprob{2}(1,1,1) = 1; % self loop
termprob{2}(1,1) = 0.2; % prob transition to end state

% Motif model
transprob{2}(:,2,:) = mk_leftright_transmat(motif_length, 0); % no self loops
termprob{2}(2,end) = 1.0; % last state immediately terminates

% OBS LEVEl

obsprob = zeros([Qsize Osize]);
if isempty(background_char)
  % uniform background model
  %obsprob(1,1,:) = normalise(ones(Osize,1));
  obsprob(1,1,:) = normalise(rand(Osize,1));
else
  % deterministic background model (easy to see!)
  m = find(chars==background_char);
  obsprob(1,1,m) = 1.0;
end

if ~isempty(motif_pattern)
  % initialise with true motif (cheating)
  for i=1:motif_length
    m = find(chars == motif_pattern(i));
    obsprob(2,i,m) = 1.0;
  end
else
  obsprob(2,:,:) = mk_stochastic(rand(motif_length, Osize));
end

if 0
  Oargs = {'CPT', obsprob};
else
  % We use a minent prior for the emission distribution for the states in the motif model
  % (but not the background model). This encourages nearly deterministic distributions.
  % We create an index matrix  (where M = motif length)
  %  [2 1
  %   2 2
  %   ...
  %   2 M]
  % and then convert this to a list of integers, which
  % specifies when to use the minent prior (Q1=2 specifies motif model).
  M = motif_length;
  ndx = [2*ones(M,1) (1:M)'];
  pcases = subv2ind([2 motif_length], ndx);
  Oargs = {'CPT', obsprob, 'prior_type', 'entropic', 'entropic_pcases', pcases};
end

[bnet, Qnodes, Fnodes, Onode] = mk_hhmm('Qsizes', Qsize, 'Osize', Osize, 'discrete_obs', 1, ...
	       'Oargs', Oargs, 'Ops', Qnodes(1:2), ...
	       'startprob', startprob, 'transprob', transprob, 'termprob', termprob);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% fbestloc.m %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% checks whether a new point in the shopping basket (found by local
% search) is better than the current best point; in that case it
% updates the best point and its function value
% Uses the following m-file:
% chrelerr.m

if fmi(nbasket0) < fbest
  fbest = fmi(nbasket0)
  xbest = xmin(:,nbasket0)
  chrelerr;
end


function [D,DJacob,DHess,pDpt]=DPolarLin2D(x,q0Dir,param4)
%%DPOLARCV2D The diffusion matrix for a continuous-time constant heading
%            motion model where the target state is given in terms of
%            position, direction of velocity in radians counterclockwise
%            from the x-axis, and speed.
%
%INPUTS: x The 4XN set of target state vectors in 2D space in the order of
%          [2D position;direction angle;speed] for each column where the
%          heading angle is measured in radians counterclockwise from the
%          x-axis. Alternatively, a 5XN state vector of
%          [2D position;direction angle;speed; speed derivative] can be
%          used to cover a linearlly accelerating target. Only the
%          dimensionality of x matters; the components are ignored. The
%          dimensionality is used to determine whether linear acceleration
%          is modeled and what N is.
%    q0Dir The power spectral density of the process noise for the heading
%          direction. The units are rad^2/s.
%   param4 If x is only 4D, that is, does not model a constant linear
%          acceleration, then param4 is the power spectral density of the
%          process noise for the speed. The units are m^2/s^3. Otherwise,
%          param4 is the power spectral density of the noise for the
%          linear acceleration and its units are m^2/s^5.
%
%OUTPUT: D The 4X2XN set of N diffusion matrices of a continuous-time
%          linear additive noise model where the velocity is given in terms
%          of direction and speed and the noise is only added to the
%          direction and speed components or the 5X2XN set of matrices
%          where noise is added to the direction and acceleration
%          components if a linear acceleration is assumed.
%   DJacob, DHess The xDimX2XxDim, xDimX2XxDimXxDim matrices of first and
%           second partial derivatives of the elements of D with respect to
%           x. These are all zero, since D is a constant. it is the same
%           for all D and is not repeated N times.
%      pDpt The xDimX2 partial derivative of D with respect to time. This
%           is all zeros, because D is a constant.
%
%Ideas for setting q0Dir  can be obtained using processNoiseSuggest with
%'PolyKal-ROT' and order 0. Ideas for setting param4 can
%be obtained using processNoiseSuggest with 'PolyKal-ROT' and order 1 or 2
%respectively depending on whether x is constant velocity or constant
%acceleration.
%
%The idea of decomposing the velocity into direction and speed components
%is presented in [1], where the direction component is differently defined
%and additional derivatives are used.
%
%This diffusion matrix goes with the drift function aPolarLin2D.
%
%REFERENCES:
%[1] J. L. Gertz, "Multisensor surveillance for improved aircraft
%    tracking," The Lincoln Laboratory Journal, vol. 2, no. 3, pp. 381-396,
%    1989.
%
%July 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

N=size(x,2);

q0DirR=sqrt(q0Dir);
param4R=sqrt(param4);

numDim=size(x,1);

if(numDim==4)%If it is a constant velocity model
    D=repmat([0,      0;
                0,      0;
                q0DirR, 0;
                0,     param4R],[1,1,N]);
elseif(numDim==5)%If it is a constant acceleration model.
    D=repmat([0,      0;
                0,      0;
                q0DirR, 0;
                0,      0;
                0,      param4R],[1,1,N]);
else
    error('The length of x is neither 4 nor 5.');
end

if(nargout>1)
    DJacob=zeros(numDim,2,numDim);
    if(nargout>2)
        DHess=zeros(numDim,2,numDim,numDim);
        if(nargout>3)
            pDpt=zeros(numDim,2);
        end
    end
end
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function varargout = iff_internal(varargin)
X = varargin{1};
Y = varargin{2};

if nargin == 2
    zero_tolerance = 1e-5;
else
    zero_tolerance = abs(varargin{3});
end

% Normalize data
if isa(Y,'constraint')
    Y=lmi(Y,[],[],1);
end
if isa(X,'constraint')
    X=lmi(X,[],[],1);
end
if isa(X,'lmi') & isa(Y,'sdpvar')
    temp = X;
    X = Y;
    Y = temp;
end

if isa(X,'sdpvar') & isa(Y,'sdpvar')
    % user presumably works with binary variables
    varargout{1} = [Y == X];

elseif (isa(X,'sdpvar') & isa(Y,'lmi'))
    % Binary variable iff a constraint holds
    switch settype(Y)
        case 'elementwise'  % binary X <--> Y(:)>=0
            varargout{1} = binary_iff_lp(X,-sdpvar(Y),zero_tolerance);
        case 'equality'
            varargout{1} = binary_iff_equality(X,sdpvar(Y),zero_tolerance);
        case 'multiple'
            Y1 = Y(find(is(Y,'equality')));
            Y2 = Y(find(is(Y,'elementwise')));
            % Glue using intermediate variables holding truth of each set
            % of constraints. X is true iff both di are true and v.v.
            % Hence, we make a recursive call with two new models
            binvar d1 d2
            C1 = iff_internal(d1,Y1,zero_tolerance);
            C2 = iff_internal(d2,Y2,zero_tolerance);
            C3 = [X <= d1,X <= d2,X >= d1+d2-1];
            varargout{1} = [C1, C2, C3];
          %  varargout{1} = [iff_internal(X,Y1),iff_internal(X,Y2)];
        otherwise
            error('IFF not implemented for this case');
    end
elseif isa(X,'lmi') & isa(Y,'lmi')
    % Constraint holds iff constraint holds.
    % Glue using an intermediate variable
    binvar d
    C1 =  iff_internal(d,X,zero_tolerance);
    C2 =  iff_internal(d,Y,zero_tolerance);
    varargout{1} = [C1,C2];
end

function F = binary_iff_lp(X,f,zero_tolerance)
% X == 1    <=>   f<=0
[M,m,infbound] = derivebounds(f);
if infbound
    warning('You have unbounded variables in IFF leading to a lousy big-M relaxation.');
end

[nf,mf]=size(f);
if nf*mf==1
    F = linearnegativeconstraint_iff_binary(f,X,M,m,zero_tolerance);
else
    f = reshape(f,nf*mf,1);
    di = binvar(nf*mf,1);
    F = linearnegativeconstraint_iff_binary(f,di,M,m,zero_tolerance);
    if length(X)==1
        % X is true if any di
        F = [F, X>=sum(di)-length(di)+1, X <= di];
    else
        % This must be a vectorized X(i) iff f(i)
        F = [F, di == X];
    end

    % di=0 means the ith hypeplane is violated
    % X=1 means we are in the polytope
    % F  = [f <= M*(1-X), f>=eps+(m-eps).*di, X>=sum(di)-length(di)+1, X <= di];

    % Add some cuts for c < a'x+b < d
    [bA] = getbase(f);
    b = bA(:,1);
    A = bA(:,2:end);
    S = zeros(0,length(di));
    for i = 1:length(b)
        j = findrows(abs(A),abs(A(i,:)));
        j = j(j > i);
        if length(j)==1
            S(end+1,[i j]) = 1;
        end
    end
    if size(S,1) > 0
        % Add cut cannot be outside both constraints
        F = F + (S*di >= 1);
    end
end

function F = binary_iff_equality(X,Y,zero_tolerance)

% Things like iff(x,y==1) is sent as X, 1-Y
if isLogicalVector(X) && isLogicalVector(Y)
    if isequal(getbase(Y),[0 -1]) | isequal(getbase(Y),[-1 1])
        Y = -Y;
    end
     F = [1-X == Y];
     return
 end
Y = Y(:);
d = binvar(length(Y),3);
% We have to model every single line of equality.
% di1 : Yi < -eps
% di2 : -eps < Yi < eps
% di3 : eps < Yi
C = sum(d,2)==1;
[M,m,infbounds] = derivebounds(Y);
for i = 1:length(Y)
    if all(ismember(depends(Y(i)),yalmip('binvariables'))) & all(getbase(Y(i))==fix(getbase(Y(i))))
        eps = 0.5;
    else
        eps = 0;
    end
    C1 =  binary_iff_lp(d(i,1),Y(i)+eps,abs(zero_tolerance));             % Y <-eps
    C2 =  binary_iff_lp(d(i,2),[Y(i)-eps;-eps-Y(i)],abs(zero_tolerance)); % -eps < Y < eps
    C3 =  binary_iff_lp(d(i,3),eps-Y(i),abs(zero_tolerance));             % eps < Y
    C = [C, C1,C2,C3];
    % Cut off some trivial cases
    if m(i)>=0
        C = [C,d(i,1)==0];
    elseif M(i)<=0
        C = [C,d(i,3)==0];
    end
end
% X is true if all di2 are true and v.v
F = [C,X <= d(:,2), X >= sum(d(:,2))-length(Y)+1];

function yes = isLogicalVector(X)
yes = 0;
X = sdpvar(X);
B = getbase(X);
if all(ismember(depends(X),yalmip('binvariables')))
    b = B(:,1);
    if all(b==0 | b==1)
        A = B(:,2:end);
        if all(A == 0 | A == 1 | A == -1)
            if all(sum(A | A,2)==1)
                if all(sum(B,2)<= 1)
                    yes = 1;
                end
            end
        end
    end
end


function vol=smoothbinvol(vol,layer)
%
% vol=smoothbinvol(vol,layer)
%
% perform a memory-limited 3D image smoothing
%
% author: Qianqian Fang <q.fang at neu.edu>
%
% input:
%     vol: a 3D volumetric image to be smoothed
%     layer: number of iterations for the smoothing
%
% output:
%     vol: the volumetric image after smoothing
%
% -- this function is part of iso2mesh toolbox (http://iso2mesh.sf.net)
%

dim=size(vol);
dxy=dim(1)*dim(2);
fulllen=prod(dim);

weight=1./6.;
step=4000;

% in case vol is a logical
vol=double(vol);
offs=[1,-1,dim(1), -dim(1),dxy, -dxy];

for i=1:layer
  % find all non-zero values
  idx=find(vol);
  % get the neighbors of all the non-zero values
  % this may cause wrapping -- TODO
  val=vol(idx);
  for k=1:6
    nextidx=idx+offs(k);
    % find all 1-valued voxels that are located within the domain
	  goodidx=find(nextidx>0 & nextidx<fulllen);
    % for all neighboring voxels, add a fraction from the non-0 voxels
    % problematic when running in parallel (racing)
    len=length(goodidx);
    % control granualarity with step
    if(len>step)
        for j=1:step:len-step
            vol(nextidx(goodidx(j:j+step-1)))=vol(nextidx(goodidx(j:j+step-1)))+weight*val(goodidx(j:j+step-1));
        end
        vol(nextidx(goodidx(j+step:end)))=vol(nextidx(goodidx(j+step:end)))+weight*val(goodidx(j+step:end));
    else
        vol(nextidx(goodidx))=vol(nextidx(goodidx))+weight*val(goodidx);
    end
    % the above line may change the values of the non-zero voxels, recover
    % them
  end
  vol(idx)=val;
end


function varargout = twister(varargin)
% VL_TWISTER Random number generator
%    VL_TWISTER() is essentially equivalent to MATLAB native RAND()
%    when using the Twister random number generator. VL_TWISTER(),
%    VL_TWISTER(M,N,P,...)  and VL_TWISTER([M N P ...]) are equivalent
%    to RAND(), RAND(M,N,P,...)  and RAND([M N P ...]) respectively.
%
%    The state of the random generator can be seeded by
%    VL_TWISTER('STATE', X), where X is a DOUBLE scalar (this is
%    equivalent to RAND('TWISTER', X)). The state can be read by
%    VL_TWISTER('STATE') (equivalent to RAND('TWISTER')) and set by
%    VL_TWISTER('STATE', STATE) (equivalent to RAND('TWISTER',
%    STATE)). Here STATE is a vector of 625 elements of class
%    UINT32. Finally VL_TWISTER('STATE',KEY) seeds the generator by a
%    vector of DOUBLE of length not greater than 624.
%
%    VL_TWISTER() is slightly faster than RAND(). Moreover it can be
%    used to control the state of the random number generator used by
%    all VLFEAT functions.
%
%    See also: VL_HELP().
[varargout{1:nargout}] = vl_twister(varargin{:});


function [ptheta] = tapas_sem_mixed_validate_ptheta(ptheta)
%% Make sute that the input is consistent.
%
% Input
%
% Output
%

% aponteeduardo@gmail.com
% copyright (C) 2017
%

if ~isfield(ptheta, 'x')
    error('tapas:sem:mixed:ptheta', 'Design matrix ptheta.m is not specified');
end

if ~isfield(ptheta, 'jm')
    error('tapas:sem:mixed:ptheta', ...
        'Reduction matrix ptheta.jm is not specified');
end

if ~isfield(ptheta, 'mixed')
    error('tapas:sem:mixed:ptheta', ...
        'Random effects ptheta.mixed is not specified');
end

% Now make sure that the every thing is consistent

jm1, jm2 = size(ptheta.jm);
x1, x2 = size(ptheta.x);
m1, m2 = size(ptheta.mixed);

test_matrix(ptheta.jm, jm1, jm2, 'jm');
test_matrix(ptheta.x, jm2, x2, 'x');
test_matrix(ptheta.mixed, jm2, m2, 'mixed');

ptheta.x = logical(ptheta.x);

end

function test_matrix(amatrix, d1, d2, name)
% Make standard test on a matrix

if ~isnumeric(amatrix)
    error(sprintf('tapas:sem:mixed:ptheta:%s', name), ...
        '%s not numeric', name);

end

if ~all(size(amatrix) == [d1, d2])
    error(sprintf('tapas:sem:mixed:ptheta:%s', name), ...
        '%s dimension are %d, %d, instead %d, %d', ...
        name, d1, d2, size(amatrix, 1), size(amatrix, 2));
end

end


% Replace zero values in a vector with arbitrary small values
%
% Description
%  This is a convenient function to avoid methods crashing because
%  of zero values in a signal.
%
% Input
%  x    : A vector containing possibly zero values.
%
% Output
%  y    : The same vector as x with zero values replaced.
%
% References
%  https://github.com/covarep/covarep/issues/75
%
% Copyright (c) 2011 University of Crete - Computer Science Department
%
% License
%  This file is under the LGPL license,  you can
%  redistribute it and/or modify it under the terms of the GNU Lesser General
%  Public License as published by the Free Software Foundation, either version 3
%  of the License, or (at your option) any later version. This file is
%  distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
%  without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
%  PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
%  details.
%
% This function is part of the Covarep project: http://covarep.github.io/covarep
%
% Author
%  Gilles Degottex <gad27@cam.ac.uk>

function y = replacezeros(x)

    y = x;
    idx = find(x==0);
    y(idx) = eps*rand(1,length(idx));


function [summary] = tapas_sem_posterior_summary(posterior, time)
%% Generate a summary of the posterior estimates.
%
% Input
%       posterior       -- Estimate from the model.
% Output
%       summary         -- Summary structue computed from the posterior.
%

% aponteeduardo@gmail.com
% copyright (C) 2019
%

n = 1;

n = n + 1;
if nargin < n
    time = linspace(0, 10.0, 1200);
end

% Plot the data.
data = posterior.data;
try
    model = posterior.model.graph{1}.htheta.model;
catch err
    try
        model = posterior.ptheta;
    catch
        error('tapas:sem:display_posterior', 'Improper input')
    end
end

ns = numel(data);

summary = struct('summaries', [], 'fits', []);

summaries = cell(ns, 1);
fits = cell(ns, 1);

for i = 1:ns
    samples = posterior.ps_theta(i, :);
    fits{i} = tapas_sem_generate_fits(data(i), samples, model, time);
    summaries{i} = tapas_sem_generate_summary(data(i), samples, model, i);
end

summary.fits = fits;
summary.summaries = vertcat(summaries{:});

end