function gpcf = gpcf_periodic(varargin)
%GPCF_PERIODIC  Create a periodic covariance function for Gaussian Process
%
%  Description
%    GPCF = GPCF_PERIODIC('PARAM1',VALUE1,'PARAM2,VALUE2,...)
%    creates periodic covariance function structure in which the
%    named parameters have the specified values. Any unspecified
%    parameters are set to default values.
%
%    GPCF = GPCF_PERIODIC(GPCF,'PARAM1',VALUE1,'PARAM2,VALUE2,...)
%    modify a covariance function structure with the named
%    parameters altered with the specified values.
%
%    Periodic covariance function with squared exponential decay
%    part as in Rasmussen & Williams (2006) Gaussian processes for
%    Machine Learning.
%
%    Parameters for periodic covariance function [default]
%      magnSigma2             - magnitude (squared) [0.1]
%      lengthScale            - length scale for each input [10]
%                               This can be either scalar
%                               corresponding isotropic or vector
%                               corresponding ARD
%      period                 - length of the periodic component(s) [1]
%      lengthScale_sexp       - length scale for the squared exponential
%                               component [10] This can be either scalar
%                               corresponding isotropic or vector
%                               corresponding ARD.
%      decay                  - determines whether the squared exponential
%                               decay term is used (1) or not (0).
%                               Not a hyperparameter for the function.
%      kalman_deg             - degree of approximation in type 'KALMAN' [6]
%      kalman_deg_sexp        - degree of sexp approximation in type
%                               'KALMAN' [6]
%      kalman_valid           - determines whether Bessel function is
%                               used (1) or not (0) in type 'KALMAN' [0]
%      magnSigma2_prior       - prior structure for magnSigma2 [prior_logunif]
%      lengthScale_prior      - prior structure for lengthScale [prior_t]
%      lengthScale_sexp_prior - prior structure for lengthScale_sexp
%                               [prior_fixed]
%      period_prior           - prior structure for period [prior_fixed]
%
%    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_*
%
% Copyright (c) 2009-2010 Heikki Peura
% Copyright (c) 2010 Aki Vehtari
% Copyright (c) 2014 Arno Solin and Jukka Koskenranta

% 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.

  ip=inputParser;
  ip.FunctionName = 'GPCF_PERIODIC';
  ip.addOptional('gpcf', [], @isstruct);
  ip.addParamValue('magnSigma2',0.1, @(x) isscalar(x) && x>0);
  ip.addParamValue('lengthScale',10, @(x) isvector(x) && all(x>0));
  ip.addParamValue('period',1, @(x) isscalar(x) && x>0);
  ip.addParamValue('lengthScale_sexp',10, @(x) isvector(x) && all(x>0));
  ip.addParamValue('decay',0, @(x) isscalar(x) && (x==0||x==1));
  ip.addParamValue('kalman_deg',[], @(x) isscalar(x) && mod(x,1)==0);
  ip.addParamValue('kalman_deg_sexp',[], @(x) isscalar(x) && mod(x,1)==0);
  ip.addParamValue('kalman_valid',[], @(x) isscalar(x) && (x==0||x==1));
  ip.addParamValue('magnSigma2_prior',prior_logunif, @(x) isstruct(x) || isempty(x));
  ip.addParamValue('lengthScale_prior',prior_t, @(x) isstruct(x) || isempty(x));
  ip.addParamValue('lengthScale_sexp_prior',[], @(x) isstruct(x) || isempty(x));
  ip.addParamValue('period_prior',[], @(x) isstruct(x) || isempty(x));
  ip.addParamValue('selectedVariables',[], @(x) isempty(x) || ...
                   (isvector(x) && all(x>0)));
  ip.parse(varargin{:});
  gpcf=ip.Results.gpcf;

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

  if init || ~ismember('magnSigma2',ip.UsingDefaults)
    gpcf.magnSigma2 = ip.Results.magnSigma2;
  end
  if init || ~ismember('lengthScale',ip.UsingDefaults)
    gpcf.lengthScale = ip.Results.lengthScale;
  end
  if init || ~ismember('period',ip.UsingDefaults)
    gpcf.period = ip.Results.period;
  end
  if init || ~ismember('lengthScale_sexp',ip.UsingDefaults)
    gpcf.lengthScale_sexp=ip . Results.lengthScale_sexp;
  end
  if init || ~ismember('decay',ip.UsingDefaults)
    gpcf.decay = ip.Results.decay;
  end
  if init || ~ismember('kalman_deg',ip.UsingDefaults)
      if ~isempty(ip.Results.kalman_deg)
          gpcf.kalman_deg = ip.Results.kalman_deg;
      end
  end
  if init || ~ismember('kalman_deg_sexp',ip.UsingDefaults)
      if ~isempty(ip.Results.kalman_deg_sexp)
          gpcf.kalman_deg_sexp = ip.Results.kalman_deg_sexp;
      end
  end
  if init || ~ismember('kalman_valid',ip.UsingDefaults)
      if ~isempty(ip.Results.kalman_valid)
          gpcf.kalman_valid = ip.Results.kalman_valid;
      end
  end
  if init || ~ismember('magnSigma2_prior',ip.UsingDefaults)
    gpcf.p.magnSigma2 = ip.Results.magnSigma2_prior;
  end
  if init || ~ismember('lengthScale_prior',ip.UsingDefaults)
    gpcf.p.lengthScale = ip.Results.lengthScale_prior;
  end
  if init || ~ismember('lengthScale_sexp_prior',ip.UsingDefaults)
    gpcf.p.lengthScale_sexp = ip.Results.lengthScale_sexp_prior;
  end
  if init || ~ismember('period_prior',ip.UsingDefaults)
    gpcf.p.period = ip.Results.period_prior;
  end

  if ~ismember('selectedVariables',ip.UsingDefaults)
    gpcf.selectedVariables = ip.Results.selectedVariables;
  end

  if init
    % Set the function handles to the subfunctions
    gpcf.fh.pak = @gpcf_periodic_pak;
    gpcf.fh.unpak = @gpcf_periodic_unpak;
    gpcf.fh.lp = @gpcf_periodic_lp;
    gpcf.fh.lpg = @gpcf_periodic_lpg;
    gpcf.fh.cfg = @gpcf_periodic_cfg;
    gpcf.fh.ginput = @gpcf_periodic_ginput;
    gpcf.fh.cov = @gpcf_periodic_cov;
    gpcf.fh.covvec = @gpcf_periodic_covvec;
    gpcf.fh.trcov  = @gpcf_periodic_trcov;
    gpcf.fh.trvar  = @gpcf_periodic_trvar;
    gpcf.fh.recappend = @gpcf_periodic_recappend;
    gpcf.fh.cf2ss = @gpcf_periodic_cf2ss;
  end

end

function [w, s, h] = gpcf_periodic_pak(gpcf)
%GPCF_PERIODIC_PAK  Combine GP covariance function parameters into
%                   one vector
%
%  Description
%    W = GPCF_PERIODIC_PAK(GPCF) takes a covariance function
%    structure GPCF and combines the covariance function
%    parameters and their hyperparameters into a single row
%    vector W. This is a mandatory subfunction used for example
%    in energy and gradient computations.
%
%       w = [ log(gpcf.magnSigma2)
%             (hyperparameters of gpcf.magnSigma2)
%             log(gpcf.lengthScale(:))
%             (hyperparameters of gpcf.lengthScale)
%             log(gpcf.lengthScale_sexp)
%             (hyperparameters of gpcf.lengthScale_sexp)
%             log(gpcf.period)
%             (hyperparameters of gpcf.period)]'
%
%  See also
%    GPCF_PERIODIC_UNPAK

  if isfield(gpcf,'metric')
    error('Periodic covariance function not compatible with metrics.');
  else
    i1=0;i2=1;
    w = []; s = {}; h=[];

    if ~isempty(gpcf.p.magnSigma2)
      w = [w log(gpcf.magnSigma2)];
      s = [s; 'log(periodic.magnSigma2)'];
      h = [h 1];

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

    if ~isempty(gpcf.p.lengthScale)
      w = [w log(gpcf.lengthScale)];
      s = [s; 'log(periodic.lengthScale)'];
      h = [h ones(1,numel(gpcf.lengthScale))];

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

    if gpcf.decay == 1 && ~isempty(gpcf.p.lengthScale_sexp)
      w = [w log(gpcf.lengthScale_sexp)];
      s = [s; 'log(periodic.lengthScale_sexp)'];
      h = [h ones(1,numel(gpcf.lengthScale_sexp))];

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

    if ~isempty(gpcf.p.period)
      w = [w log(gpcf.period)];
      s = [s; 'log(periodic.period)'];
      h = [h 1];

      % Hyperparameters of period
      [wh, sh, hh] = gpcf.p.period.fh.pak(gpcf.p.period);
      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_periodic_unpak(gpcf, w)
%GPCF_PERIODIC_UNPAK  Sets the covariance function parameters into
%                     the structure
%
%  Description
%    [GPCF, W] = GPCF_PERIODIC_UNPAK(GPCF, W) takes a covariance
%    function structure GPCF and a hyper-parameter vector W, and
%    returns a covariance function structure identical to the
%    input, except that the covariance hyper-parameters 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.
%
%    Assignment is inverse of
%       w = [ log(gpcf.magnSigma2)
%             (hyperparameters of gpcf.magnSigma2)
%             log(gpcf.lengthScale(:))
%             (hyperparameters of gpcf.lengthScale)
%             log(gpcf.lengthScale_sexp)
%             (hyperparameters of gpcf.lengthScale_sexp)
%             log(gpcf.period)
%             (hyperparameters of gpcf.period)]'
%
%  See also
%    GPCF_PERIODIC_PAK

  if isfield(gpcf,'metric')
    error('Covariance function not compatible with metrics');
  else
    gpp=gpcf.p;
    if ~isempty(gpp.magnSigma2)
      i1=1;
      gpcf.magnSigma2 = exp(w(i1));
      w = w(i1+1:end);
    end
    if ~isempty(gpp.lengthScale)
      i2=length(gpcf.lengthScale);
      i1=1;
      gpcf.lengthScale = exp(w(i1:i2));
      w = w(i2+1:end);
    end
    if ~isempty(gpp.lengthScale_sexp) && gpcf.decay == 1
      i2=length(gpcf.lengthScale_sexp);
      i1=1;
      gpcf.lengthScale_sexp = exp(w(i1:i2));
      w = w(i2+1:end);
    end
    if ~isempty(gpp.period)
      i2=length(gpcf.period);
      i1=1;
      gpcf.period = exp(w(i1:i2));
      w = w(i2+1:end);
    end
    % hyperparameters
    if ~isempty(gpp.magnSigma2)
      [p, w] = gpcf.p.magnSigma2.fh.unpak(gpcf.p.magnSigma2, w);
      gpcf.p.magnSigma2 = p;
    end
    if ~isempty(gpp.lengthScale)
      [p, w] = gpcf.p.lengthScale.fh.unpak(gpcf.p.lengthScale, w);
      gpcf.p.lengthScale = p;
    end
    if ~isempty(gpp.lengthScale_sexp)
      [p, w] = gpcf.p.lengthScale_sexp.fh.unpak(gpcf.p.lengthScale_sexp, w);
      gpcf.p.lengthScale_sexp = p;
    end
    if ~isempty(gpp.period)
      [p, w] = gpcf.p.period.fh.unpak(gpcf.p.period, w);
      gpcf.p.period = p;
    end

  end
end

function lp = gpcf_periodic_lp(gpcf)
%GPCF_PERIODIC_LP  Evaluate the log prior of covariance function parameters
%
%  Description
%    LP = GPCF_PERIODIC_LP(GPCF) takes a covariance function
%    structure GPCF and returns log(p(th)), where th collects the
%    parameters. This is a mandatory subfunction used for example
%    in energy computations.
%
%    Also the log prior of the hyperparameters of the covariance
%    function parameters is added to E if hyperprior is
%    defined.
%
%  See also
%    GPCF_PERIODIC_PAK, GPCF_PERIODIC_UNPAK, GPCF_PERIODIC_LPG, GP_E

  lp = 0;
  gpp=gpcf.p;

  if isfield(gpcf,'metric')
    error('Covariance function not compatible with metrics');
  else
    % Evaluate the prior contribution to the error. The parameters that
    % are sampled are from space W = log(w) where w is all the "real" samples.
    % On the other hand errors are evaluated in the W-space so we need take
    % into account also the  Jacobian of transformation W -> w = exp(W).
    % See Gelman et al. (2013), Bayesian Data Analysis, third edition, p. 21.

    if ~isempty(gpcf.p.magnSigma2)
      lp = gpp.magnSigma2.fh.lp(gpcf.magnSigma2, gpp.magnSigma2) +log(gpcf.magnSigma2);
    end
    if ~isempty(gpp.lengthScale)
      lp = lp +gpp.lengthScale.fh.lp(gpcf.lengthScale, gpp.lengthScale) +sum(log(gpcf.lengthScale));
    end

    if ~isempty(gpp.lengthScale_sexp) && gpcf.decay == 1
      lp = lp +gpp.lengthScale_sexp.fh.lp(gpcf.lengthScale_sexp, gpp.lengthScale_sexp) +sum(log(gpcf.lengthScale_sexp));
    end
    if ~isempty(gpcf.p.period)
      lp = gpp.period.fh.lp(gpcf.period, gpp.period) +sum(log(gpcf.period));
    end
  end

end

function lpg = gpcf_periodic_lpg(gpcf)
%GPCF_PERIODIC_LPG  Evaluate gradient of the log prior with respect
%               to the parameters.
%
%  Description
%    LPG = GPCF_PERIODIC_LPG(GPCF) takes a covariance function
%    structure GPCF and returns LPG = d log (p(th))/dth, where th
%    is the vector of parameters. This is a mandatory subfunction
%    used for example in gradient computations.
%
%  See also
%    GPCF_PERIODIC_PAK, GPCF_PERIODIC_UNPAK, GPCF_PERIODIC_LP, GP_G

  lpg = [];
  gpp=gpcf.p;
  if isfield(gpcf,'metric')
    error('Covariance function not compatible with metrics');
  end
  if ~isempty(gpcf.p.magnSigma2)
    lpgs = gpp.magnSigma2.fh.lpg(gpcf.magnSigma2, gpp.magnSigma2);
    lpg = [lpg lpgs(1).*gpcf.magnSigma2+1 lpgs(2:end)];
  end
  if ~isempty(gpcf.p.lengthScale)
    lll = length(gpcf.lengthScale);
    lpgs = gpp.lengthScale.fh.lpg(gpcf.lengthScale, gpp.lengthScale);
    lpg = [lpg lpgs(1:lll).*gpcf.lengthScale+1 lpgs(lll+1:end)];
  end
  if gpcf.decay == 1 && ~isempty(gpcf.p.lengthScale_sexp)
    lll = length(gpcf.lengthScale_sexp);
    lpgs = gpp.lengthScale_sexp.fh.lpg(gpcf.lengthScale_sexp, gpp.lengthScale_sexp);
    lpg = [lpg lpgs(1:lll).*gpcf.lengthScale_sexp+1 lpgs(lll+1:end)];
  end
  if ~isempty(gpcf.p.period)
    lpgs = gpp.period.fh.lpg(gpcf.period, gpp.period);
    lpg = [lpg lpgs(1).*gpcf.period+1 lpgs(2:end)];
  end
end

function DKff = gpcf_periodic_cfg(gpcf, x, x2, mask, i1)
%GPCF_PERIODIC_CFG  Evaluate gradient of covariance function
%                   with respect to the parameters
%
%  Description
%    DKff = GPCF_PERIODIC_CFG(GPCF, X) takes a covariance
%    function structure GPCF, a matrix X of input vectors and
%    returns DKff, the gradients of covariance matrix Kff =
%    k(X,X) with respect to th (cell array with matrix elements).
%    This is a mandatory subfunction used in gradient computations.
%
%    DKff = GPCF_PERIODIC_CFG(GPCF, X, X2) takes a covariance
%    function structure GPCF, a matrix X of input vectors and
%    returns DKff, the gradients of covariance matrix Kff =
%    k(X,X2) with respect to th (cell array with matrix
%    elements). This subfunction is needed when using sparse
%    approximations (e.g. FIC).
%
%    DKff = GPCF_PERIODIC_CFG(GPCF, X, [], MASK) takes a
%    covariance function structure GPCF, a matrix X of input
%    vectors and returns DKff, the diagonal of gradients of
%    covariance matrix Kff = k(X,X2) with respect to th (cell
%    array with matrix elements). This subfunction is needed
%    when using sparse approximations (e.g. FIC).
%
%    DKff = GPCF_PERIODIC_CFG(GPCF, X, X2, [], i) takes a covariance
%    function structure GPCF, a matrix X of input vectors and
%    returns DKff, the gradients of covariance matrix Kff =
%    k(X,X2), or k(X,X) if X2 is empty, with respect to ith
%    hyperparameter. This subfunction is needed when using memory
%    save option in gp_set.
%
%  See also
%    GPCF_PERIODIC_PAK, GPCF_PERIODIC_UNPAK, GPCF_PERIODIC_LP, GP_G

  gpp=gpcf.p;

  i2=1;
  gp_period=gpcf.period;
  DKff={};
  gprior=[];

  if nargin==5
    % Use memory save option
    savememory=1;
    if i1==0
      % Return number of hyperparameters
      i=0;
      if ~isempty(gpcf.p.magnSigma2)
        i=1;
      end
      if ~isempty(gpcf.p.lengthScale)
        i=i+length(gpcf.lengthScale);
      end
      if gpcf.decay==1 && ~isempty(gpcf.p.lengthScale_sexp)
        i=i+length(gpcf.lengthScale_sexp);
      end
      if ~isempty(gpcf.p.period)
        i=i+length(gpcf.lengthScale);
      end
      DKff=i;
      return
    end
  else
    savememory=0;
  end

  % Evaluate: DKff{1} = d Kff / d magnSigma2
  %           DKff{2} = d Kff / d lengthScale
  % NOTE! Here we have already taken into account that the parameters are transformed
  % through log() and thus dK/dlog(p) = p * dK/dp

  % evaluate the gradient for training covariance
  if nargin == 2 || (isempty(x2) && isempty(mask))
    Cdm = gpcf_periodic_trcov(gpcf, x);

    ii1=0;
    if ~isempty(gpcf.p.magnSigma2) && (~savememory || all(i1==1));
        ii1=1;
        DKff{ii1} = Cdm;
    end

    if isfield(gpcf,'metric')
      error('Covariance function not compatible with metrics');
    else
      if isfield(gpcf,'selectedVariables')
        x = x(:,gpcf.selectedVariables);
      end
      [n, m] =size(x);
      if ~savememory
        i1=1:m;
      else
        if i1==1
          DKff=DKff{1};
          return
        else
          i1=i1-1;
        end
      end

      if ~isempty(gpcf.p.lengthScale) && (~savememory || all(i1 <= length(gpcf.lengthScale)))
        % loop over all the lengthScales
        if length(gpcf.lengthScale) == 1
          % In the case of isotropic PERIODIC
          s = 2./gpcf.lengthScale.^2;
          dist = 0;
          for i=1:m
            D = sin(pi.*bsxfun(@minus,x(:,i),x(:,i)')./gp_period);
            dist = dist + 2.*D.^2;
          end
          D = Cdm.*s.*dist;

          ii1 = ii1+1;
          DKff{ii1} = D;
        else
          % In the case ARD is used
          for i=i1
            s = 2./gpcf.lengthScale(i).^2;
            dist = sin(pi.*bsxfun(@minus,x(:,i),x(:,i)')./gp_period);
            D = Cdm.*s.*2.*dist.^2;

            ii1 = ii1+1;
            DKff{ii1} = D;
          end
        end
      end
      if savememory
        if length(DKff) == 1
          DKff=DKff{1};
          return
        end
        i1=i1-length(gpcf.lengthScale);
      end
      if gpcf.decay == 1
        if ~isempty(gpcf.p.lengthScale_sexp) && (~savememory || all(i1 <= length(gpcf.lengthScale_sexp)))
          if length(gpcf.lengthScale_sexp) == 1
            % In the case of isotropic PERIODIC
            s = 1./gpcf.lengthScale_sexp.^2;
            dist = 0;
            for i=1:m
              D = bsxfun(@minus,x(:,i),x(:,i)');
              dist = dist + D.^2;
            end
            D = Cdm.*s.*dist;

            ii1 = ii1+1;
            DKff{ii1} = D;
          else
            % In the case ARD is used
            for i=i1
              s = 1./gpcf.lengthScale_sexp(i).^2;
              dist = bsxfun(@minus,x(:,i),x(:,i)');
              D = Cdm.*s.*dist.^2;

              ii1 = ii1+1;
              DKff{ii1} = D;
            end
          end
        end
      end
      if savememory
        if length(DKff) == 1
          DKff=DKff{1};
          return
        end
        i1=i1-length(gpcf.lengthScale_sexp);
      end

      if ~isempty(gpcf.p.period)
        % Evaluate help matrix for calculations of derivatives
        % with respect to the period
        if length(gpcf.lengthScale) == 1
          % In the case of an isotropic PERIODIC
          s = repmat(1./gpcf.lengthScale.^2, 1, m);

          dist = 0;
          for i=1:m
            dist = dist + 2.*pi./gp_period.*sin(2.*pi.*bsxfun(@minus,x(:,i),x(:,i)')./gp_period).*bsxfun(@minus,x(:,i),x(:,i)').*s(i);
          end
          D = Cdm.*dist;
          ii1=ii1+1;
          DKff{ii1} = D;
        else
          % In the case ARD is used
          for i=i1
            s = 1./gpcf.lengthScale(i).^2;        % set the length
            dist = 2.*pi./gp_period.*sin(2.*pi.*bsxfun(@minus,x(:,i),x(:,i)')./gp_period).*bsxfun(@minus,x(:,i),x(:,i)');
            D = Cdm.*s.*dist;

            ii1=ii1+1;
            DKff{ii1} = D;
          end
        end
      end

    end
    % Evaluate the gradient of non-symmetric covariance (e.g. K_fu)
  elseif nargin == 3 || isempty(mask)
    if size(x,2) ~= size(x2,2)
      error('gpcf_periodic -> _ghyper: The number of columns in x and x2 has to be the same. ')
    end

    K = gpcf.fh.cov(gpcf, x, x2);
    ii1=0;
    if ~isempty(gpcf.p.magnSigma2) && (~savememory || all(i1==1))
        ii1=1;
        DKff{ii1} = K;
    end

    if isfield(gpcf,'metric')
      error('Covariance function not compatible with metrics');
    else
      if isfield(gpcf,'selectedVariables')
        x = x(:,gpcf.selectedVariables);
        x2 = x2(:,gpcf.selectedVariables);
      end
      [n, m] =size(x);
      if ~savememory
        i1=1:m;
      else
        if i1==1
          DKff=DKff{1};
          return
        end
        i1=i1-1;
      end
      % Evaluate help matrix for calculations of derivatives with respect to the lengthScale
      if ~isempty(gpcf.p.lengthScale) && (~savememory || all(i1 <= length(gpcf.lengthScale)))
        if length(gpcf.lengthScale) == 1
          % In the case of an isotropic PERIODIC
          s = 1./gpcf.lengthScale.^2;
          dist = 0; dist2 = 0;
          for i=1:m
            dist = dist + 2.*sin(pi.*bsxfun(@minus,x(:,i),x2(:,i)')./gp_period).^2;
          end
          DK_l = 2.*s.*K.*dist;

          ii1=ii1+1;
          DKff{ii1} = DK_l;
        else
          % In the case ARD is used
          for i=i1
            s = 1./gpcf.lengthScale(i).^2;        % set the length
            dist = 2.*sin(pi.*bsxfun(@minus,x(:,i),x2(:,i)')./gp_period);
            DK_l = 2.*s.*K.*dist.^2;

            ii1=ii1+1;
            DKff{ii1} = DK_l;
          end
        end
      end
      if savememory
        if length(DKff) == 1
          DKff=DKff{1};
          return
        end
        i1=i1-length(gpcf.lengthScale);
      end

      if gpcf.decay == 1 && (~savememory || all(i1 <= length(gpcf.lengthScale_sexp)))
        % Evaluate help matrix for calculations of derivatives with
        % respect to the lengthScale_sexp
        if length(gpcf.lengthScale_sexp) == 1
          % In the case of an isotropic PERIODIC
          s = 1./gpcf.lengthScale_sexp.^2;
          dist = 0; dist2 = 0;
          for i=1:m
            dist = dist + bsxfun(@minus,x(:,i),x2(:,i)').^2;
          end
          DK_l = s.*K.*dist;

          ii1=ii1+1;
          DKff{ii1} = DK_l;
        else
          % In the case ARD is used
          for i=i1
            s = 1./gpcf.lengthScale_sexp(i).^2;        % set the length
            dist = bsxfun(@minus,x(:,i),x2(:,i)');
            DK_l = s.*K.*dist.^2;

            ii1=ii1+1;
            DKff{ii1} = DK_l;
          end
        end
      end
      if savememory
        if length(DKff) == 1
          DKff=DKff{1};
          return
        end
        i1=i1-length(gpcf.lengthScale_sexp);
      end

      if ~isempty(gpcf.p.period)
        % Evaluate help matrix for calculations of derivatives
        % with respect to the period
        if length(gpcf.lengthScale) == 1
          % In the case of an isotropic PERIODIC
          s = repmat(1./gpcf.lengthScale.^2, 1, m);
          dist = 0; dist2 = 0;
          for i=1:m
            dist = dist + 2.*pi./gp_period.*sin(2.*pi.*bsxfun(@minus,x(:,i),x2(:,i)')./gp_period).*bsxfun(@minus,x(:,i),x2(:,i)').*s(i);
          end
          DK_l = K.*dist;

          ii1=ii1+1;
          DKff{ii1} = DK_l;
        else
          % In the case ARD is used
          for i=i1
            s = 1./gpcf.lengthScale(i).^2;        % set the length
            dist = 2.*pi./gp_period.*sin(2.*pi.*bsxfun(@minus,x(:,i),x2(:,i)')./gp_period).*bsxfun(@minus,x(:,i),x2(:,i)');
            DK_l = s.*K.*dist;

            ii1=ii1+1;
            DKff{ii1} = DK_l;
          end
        end
      end
    end
    % Evaluate: DKff{1}    = d mask(Kff,I) / d magnSigma2
    %           DKff{2...} = d mask(Kff,I) / d lengthScale etc.
  elseif nargin == 4 || nargin == 5
    [n, m] =size(x);
    if isfield(gpcf,'metric')
      error('Covariance function not compatible with metrics');
    else
        ii1=0;
        if ~isempty(gpcf.p.magnSigma2) && (~savememory || all(i1==1))
            ii1=1;
            DKff{ii1} = gpcf.fh.trvar(gpcf, x);   % d mask(Kff,I) / d magnSigma2
        end
      for i2=1:length(gpcf.lengthScale)
        ii1 = ii1+1;
        DKff{ii1}  = 0;                          % d mask(Kff,I) / d lengthScale
      end
      if gpcf.decay == 1
        for i2=1:length(gpcf.lengthScale_sexp)
          ii1 = ii1+1;
          DKff{ii1}  = 0;                      % d mask(Kff,I) / d lengthScale_sexp
        end
      end
      if ~isempty(gpcf.p.period)
        ii1 = ii1+1;                             % d mask(Kff,I) / d period
        DKff{ii1}  = 0;
      end
    end
  end
  if savememory
    DKff=DKff{1};
  end
end

function DKff = gpcf_periodic_ginput(gpcf, x, x2, i1)
%GPCF_PERIODIC_GINPUT  Evaluate gradient of covariance function with
%                      respect to x
%
%  Description
%    DKff = GPCF_PERIODIC_GINPUT(GPCF, X) takes a covariance
%    function structure GPCF, a matrix X of input vectors and
%    returns DKff, the gradients of covariance matrix Kff =
%    k(X,X) with respect to X (cell array with matrix elements).
%    This subfunction is needed when computing gradients with
%    respect to inducing inputs in sparse approximations.
%
%    DKff = GPCF_PERIODIC_GINPUT(GPCF, X, X2) takes a covariance
%    function structure GPCF, a matrix X of input vectors and
%    returns DKff, the gradients of covariance matrix Kff =
%    k(X,X2) with respect to X (cell array with matrix elements).
%    This subfunction is needed when computing gradients with
%    respect to inducing inputs in sparse approximations.
%
%    DKff = GPCF_PERIODIC_GINPUT(GPCF, X, X2, i) takes a covariance
%    function structure GPCF, a matrix X of input vectors and
%    returns DKff, the gradients of covariance matrix Kff =
%    k(X,X2), or k(X,X) if X2 is empty, with respect to ith
%    covariate in X. This subfunction is needed when using memory
%    save option in gp_set.
%
%  See also
%    GPCF_PERIODIC_PAK, GPCF_PERIODIC_UNPAK, GPCF_PERIODIC_LP, GP_G

  [n, m] =size(x);
  gp_period=gpcf.period;
  ii1 = 0;
  if length(gpcf.lengthScale) == 1
    % In the case of an isotropic PERIODIC
    s = repmat(1./gpcf.lengthScale.^2, 1, m);
    %gp_period = repmat(1./gp_period, 1, m);
  else
    s = 1./gpcf.lengthScale.^2;
  end
  if gpcf.decay == 1
    if length(gpcf.lengthScale_sexp) == 1
      % In the case of an isotropic PERIODIC
      s_sexp = repmat(1./gpcf.lengthScale_sexp.^2, 1, m);
    else
      s_sexp = 1./gpcf.lengthScale_sexp.^2;
    end
  end
  if nargin<4
    i1=1:m;
  else
    % Use memory save option
    if i1==0
      % Return number of covariates
      if isfield(gpcf,'selectedVariables')
        DKff=length(gpcf.selectedVariables);
      else
        DKff=m;
      end
      return
    end
  end

  if nargin == 2 || isempty(x2)
    K = gpcf.fh.trcov(gpcf, x);
    if isfield(gpcf,'metric')
      error('Covariance function not compatible with metrics');
    else
      for j = 1:n
        for i=i1
          DK = zeros(size(K));
          DK(j,:) = -s(i).*2.*pi./gp_period.*sin(2.*pi.*bsxfun(@minus,x(j,i),x(:,i)')./gp_period);
          if gpcf.decay == 1
            DK(j,:) = DK(j,:)-s_sexp(i).*bsxfun(@minus,x(j,i),x(:,i)');
          end
          DK = DK + DK';

          DK = DK.*K;      % dist2 = dist2 + dist2' - diag(diag(dist2));

          ii1 = ii1 + 1;
          DKff{ii1} = DK;
        end
      end
    end

  elseif nargin == 3
    K = gpcf.fh.cov(gpcf, x, x2);

    if isfield(gpcf,'metric')
      error('Covariance function not compatible with metrics');
    else
      ii1 = 0;
      for i=i1
        for j = 1:n
          DK= zeros(size(K));
          if gpcf.decay == 1
            DK(j,:) = -s(i).*2.*pi./gp_period.*sin(2.*pi.*bsxfun(@minus,x(j,i),x2(:,i)')./gp_period)-s_sexp(i).*bsxfun(@minus,x(j,i),x2(:,i)');
          else
            DK(j,:) = -s(i).*2.*pi./gp_period.*sin(2.*pi.*bsxfun(@minus,x(j,i),x2(:,i)')./gp_period);
          end
          DK = DK.*K;

          ii1 = ii1 + 1;
          DKff{ii1} = DK;
        end
      end
    end
  end
end

function C = gpcf_periodic_cov(gpcf, x1, x2)
%GP_PERIODIC_COV  Evaluate covariance matrix between two input vectors
%
%  Description
%    C = GP_PERIODIC_COV(GP, TX, X) takes in covariance function
%    of a Gaussian process GP and two matrixes TX and X that
%    contain input vectors to GP. Returns covariance matrix C.
%    Every element ij of C contains covariance 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_PERIODIC_TRCOV, GPCF_PERIODIC_TRVAR, GP_COV, GP_TRCOV

  if isempty(x2)
    x2=x1;
  end
%   [n1,m1]=size(x1);
%   [n2,m2]=size(x2);
  gp_period=gpcf.period;

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

  if isfield(gpcf,'metric')
    error('Covariance function not compatible with metrics');
  else
    if isfield(gpcf,'selectedVariables')
      x1 = x1(:,gpcf.selectedVariables);
      x2 = x2(:,gpcf.selectedVariables);
    end
    [n1,m1]=size(x1);
    [n2,m2]=size(x2);

    C=zeros(n1,n2);
    ma2 = gpcf.magnSigma2;

    % Evaluate the covariance
    if ~isempty(gpcf.lengthScale)
      s = 1./gpcf.lengthScale.^2;
      if gpcf.decay == 1
        s_sexp = 1./gpcf.lengthScale_sexp.^2;
      end
      if m1==1 && m2==1
        dd = bsxfun(@minus,x1,x2');
        dist=2.*sin(pi.*dd./gp_period).^2.*s;
        if gpcf.decay == 1
          dist = dist + dd.^2.*s_sexp./2;
        end
      else
        % If ARD is not used make s a vector of
        % equal elements
        if size(s)==1
          s = repmat(s,1,m1);
        end
        if gpcf.decay == 1
          if size(s_sexp)==1
            s_sexp = repmat(s_sexp,1,m1);
          end
        end

        dist=zeros(n1,n2);
        for j=1:m1
          dd = bsxfun(@minus,x1(:,j),x2(:,j)');
          dist = dist + 2.*sin(pi.*dd./gp_period).^2.*s(:,j);
          if gpcf.decay == 1
            dist = dist +dd.^2.*s_sexp(:,j)./2;
          end
        end
      end
      dist(dist<eps) = 0;
      C = ma2.*exp(-dist);
    end

  end
end

function C = gpcf_periodic_trcov(gpcf, x)
%GP_PERIODIC_TRCOV  Evaluate training covariance matrix of inputs
%
%  Description
%    C = GP_PERIODIC_TRCOV(GP, TX) takes in covariance function
%    of a Gaussian process GP and matrix TX that contains
%    training input vectors. Returns covariance matrix C. Every
%    element ij of C contains covariance between inputs i and j
%    in TX. This is a mandatory subfunction used for example in
%    prediction and energy computations.
%
%  See also
%    GPCF_PERIODIC_COV, GPCF_PERIODIC_TRVAR, GP_COV, GP_TRCOV

  if isfield(gpcf,'metric')
    error('Covariance function not compatible with metrics');
  else

    % Try to use the C-implementation
    C=trcov(gpcf, x);
    %     C = NaN;
    if isnan(C)
      % If there wasn't C-implementation do here
      if isfield(gpcf,'selectedVariables')
        x = x(:,gpcf.selectedVariables);
      end
      [n, m] =size(x);
      gp_period=gpcf.period;

      s = 1./(gpcf.lengthScale);
      s2 = s.^2;
      if size(s)==1
        s2 = repmat(s2,1,m);
        gp_period = repmat(gp_period,1,m);
      end
      if gpcf.decay == 1
        s_sexp = 1./(gpcf.lengthScale_sexp);
        s_sexp2 = s_sexp.^2;
        if size(s_sexp)==1
          s_sexp2 = repmat(s_sexp2,1,m);
        end
      end

      ma = gpcf.magnSigma2;

      C = zeros(n,n);
      for ii1=1:n-1
        d = zeros(n-ii1,1);
        col_ind = ii1+1:n;
        for ii2=1:m
          d = d+2.*s2(ii2).*sin(pi.*(x(col_ind,ii2)-x(ii1,ii2))./gp_period(ii2)).^2;
          if gpcf.decay == 1
            d=d+s_sexp2(ii2)./2.*(x(col_ind,ii2)-x(ii1,ii2)).^2;
          end
        end
        C(col_ind,ii1) = d;

      end
      C(C<eps) = 0;
      C = C+C';
      C = ma.*exp(-C);
    end
  end
end

function C = gpcf_periodic_trvar(gpcf, x)
%GP_PERIODIC_TRVAR  Evaluate training variance vector
%
%  Description
%    C = GP_PERIODIC_TRVAR(GPCF, TX) takes in covariance function
%    of a Gaussian process GPCF and matrix TX that contains
%    training inputs. Returns variance vector C. Every
%    element i of C contains variance of input i in TX. This is a
%    mandatory subfunction used for example in prediction and
%    energy computations.
%
%  See also
%    GPCF_PERIODIC_COV, GP_COV, GP_TRCOV

  [n, m] =size(x);

  C = ones(n,1)*gpcf.magnSigma2;
  C(C<eps)=0;
end

function reccf = gpcf_periodic_recappend(reccf, ri, gpcf)
%RECAPPEND Record append
%
%  Description
%    RECCF = GPCF_PERIODIC_RECAPPEND(RECCF, RI, GPCF) takes a
%    covariance function record structure RECCF, record index RI
%    and covariance function 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_periodic';

    % Initialize parameters
    reccf.lengthScale= [];
    reccf.magnSigma2 = [];
    reccf.lengthScale_sexp = [];
    reccf.period = [];

    % Set the function handles
    reccf.fh.pak = @gpcf_periodic_pak;
    reccf.fh.unpak = @gpcf_periodic_unpak;
    reccf.fh.lp = @gpcf_periodic_lp;
    reccf.fh.lpg = @gpcf_periodic_lpg;
    reccf.fh.cfg = @gpcf_periodic_cfg;
    reccf.fh.cov = @gpcf_periodic_cov;
    reccf.fh.trcov  = @gpcf_periodic_trcov;
    reccf.fh.trvar  = @gpcf_periodic_trvar;
    reccf.fh.recappend = @gpcf_periodic_recappend;
    reccf.p=[];
    reccf.p.lengthScale=[];
    reccf.p.magnSigma2=[];
    if ri.decay == 1
      reccf.p.lengthScale_sexp=[];
      if ~isempty(ri.p.lengthScale_sexp)
        reccf.p.lengthScale_sexp = ri.p.lengthScale_sexp;
      end
    end

    reccf.p.period=[];
    if ~isempty(ri.p.period)
      reccf.p.period= ri.p.period;
    end
    if isfield(ri.p,'lengthScale') && ~isempty(ri.p.lengthScale)
      reccf.p.lengthScale = ri.p.lengthScale;
    end
    if ~isempty(ri.p.magnSigma2)
      reccf.p.magnSigma2 = ri.p.magnSigma2;
    end
  else
    % Append to the record

    gpp = gpcf.p;

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

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

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

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

    % record decay
    if ~isempty(gpcf.decay)
      reccf.decay(ri,:)=gpcf.decay;
    end

  end
end

function [F,L,Qc,H,Pinf,dF,dQc,dPinf,params] = gpcf_periodic_cf2ss(gpcf,x)
%GPCF_PERIODIC_CF2SS Convert the covariance function to state space form
%
%  Description
%    Convert the covariance function to state space form such that
%    the process can be described by the stochastic differential equation
%    of the form:
%      df(t)/dt = F f(t) + L w(t),
%    where w(t) is a white noise process. The observation model now
%    corresponds to y_k = H f(t_k) + r_k, where r_k ~ N(0,sigma2).
%
%  References:
%    Arno Solin and Simo Sarkka (2014). Explicit link between periodic
%    covariance functions and state space models. Accepted for
%    publication in Proceedings of the Seventeenth International
%    Conference on Artifcial Intelligence and Statistics (AISTATS 2014).
%

  % Check arguments
  if nargin < 2, x = []; end

  % Check if kalman_deg is given
  if isfield(gpcf,'kalman_deg')
      kalman_deg = gpcf.kalman_deg;
  else
      kalman_deg = [];
  end

  % Check if kalman_deg_sexp is given
  if isfield(gpcf,'kalman_deg_sexp')
      kalman_deg_sexp = gpcf.kalman_deg_sexp;
  else
      kalman_deg_sexp = [];
  end

  % Check if kalman_valid is given
  if isfield(gpcf,'kalman_valid')
      kalman_valid = gpcf.kalman_valid;
  else
      kalman_valid = [];
  end

  % Case squared exponential (i.e. quasi-periodic)
  if gpcf.decay

      % Return model matrices, derivatives and parameter information
      [F,L,Qc,H,Pinf,dF,dQc,dPinf,params] = ...
          cf_quasiperiodic_to_ss(gpcf.magnSigma2,gpcf.lengthScale, ...
          gpcf.period,gpcf.lengthScale_sexp,kalman_deg, ...
          inf,kalman_deg_sexp,kalman_valid);

      % Balance matrices for numerical stability
      [F,L,Qc,H,Pinf,dF,dQc,dPinf] = ...
          ss_balance(F,L,Qc,H,Pinf,dF,dQc,dPinf);

      % Check optimized parameters
      if isempty(gpcf.p.magnSigma2), ind(1) = false; else ind(1) = true; end
      if isempty(gpcf.p.lengthScale), ind(2) = false; else ind(2) = true; end
      if isempty(gpcf.p.lengthScale_sexp), ind(3) = false; else ind(3) = true; end
      if isempty(gpcf.p.period), ind(4) = false; else ind(4) = true; end

      % Change order: lengthScale_sexp <--> period
      % Use only optimized parameter gradients
      dF(:,:,[3,4])    = dF(:,:,[4,3]);
      dQc(:,:,[3,4])   = dQc(:,:,[4,3]);
      dPinf(:,:,[3,4]) = dPinf(:,:,[4,3]);

  % Case without squared exponential (i.e. purely periodic)
  else

      % Return model matrices, derivatives and parameter information
      [F,L,Qc,H,Pinf,dF,dQc,dPinf,params] = ...
          cf_periodic_to_ss(gpcf.magnSigma2,gpcf.lengthScale, ...
            gpcf.period,kalman_deg,kalman_valid);

      % Check optimization parameters
      if isempty(gpcf.p.magnSigma2), ind(1) = false; else ind(1) = true; end
      if isempty(gpcf.p.lengthScale), ind(2) = false; else ind(2) = true; end
      if isempty(gpcf.p.period), ind(3) = false; else ind(3) = true; end

  end

  % Return only those derivatives that are needed
  dF    = dF(:,:,ind);
  dQc   = dQc(:,:,ind);
  dPinf = dPinf(:,:,ind);

end


function e=newoenvel(f,u,j,i);
%
% Finds the upper envelope of a 'function' f(u) between u(i) and u(j)
%
if i>j,
  [j i]
  error('i>j');
end;
if i==j,
  e=[i]; 	    	%Terminate with a 'shock'
elseif i+1==j,
  e=[i j]; 		%Terminate with a 'rarefaction'
else
  du=u(i)-u(i+1:j);
  df=f(i)-f(i+1:j);
  s=df./du;
  [m k]=max(s);			%New point on envelope
  e=[i newoenvel(f,u,j,i+k)];	%Find the rest of the envelope
end;


%--------------------------------------------------------------------------
%this function is to transfer image to data suitable for transmission
%
%Chen Zhifeng
%2007-05-19
%zhifeng@ecel.ufl.edu
%%I left this function as is since I'm clueless and a neophyte in image
%processing. JC 7/16/08
%--------------------------------------------------------------------------

function [data, row_im, col_im, third_im] = image2data(im, M)
% im = imread('photo.bmp');
% M =4;
[row_im, col_im, third_im] =size(im);
V_im = zeros(1, row_im*col_im*third_im);
for i=1:third_im,
    for j=1:col_im,
        V_im((i-1)*row_im*col_im+(j-1)*row_im +1 : (i-1)*row_im*col_im+j*row_im) = im(:,j,i);
    end
end

%I use dec2base here, then if M>8, for example M=16, the result string may
%include characters, this is not appropriate to use str2num below. However,
%due to the time limit, I will not add function here to deal with this.
%Actually, this may be done by dec2bin function.
Tm = dec2base(V_im,M);
[row, col] = size(Tm);
% data = [];
% for i = 1:col,
%     data = [data; Tm(:,i)];
% end
%
%data=[];
for i =1:col,
    data(row*(i-1)+1:row*i)=str2num(Tm(:,i));   %very important to add str2num here
end
%Tm = str2num(Tm_char);

%data = str2num(data);


function intervals = ToIntervals(x,in)

%ToIntervals - Convert logical vector to a list of intervals.
%
%  USAGE
%
%    intervals = ToIntervals(x,in)
%
%    x              values, e.g. timestamps
%    in             logical vector (1 = x value is inside, 0 = x value is outside)
%
%  NOTE
%
%    Values can also be omitted, in which case the intervals are defined in terms
%    of indices in the logical vector (see Example below).
%
%  EXAMPLES
%
%
%    x = [0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1.0 1.1];
%    in = [0 0 1 1 1 0 0 0 1 1 0];
%    ToIntervals(x,in)
%
%     ans =
%               0.3      0.5
%               0.9        1
%
%    ToIntervals(in)
%
%     ans =
%               3      5
%               9     10
%
%  SEE
%
%    See also ConsolidateIntervals, ExcludeIntervals, InIntervals, Restrict,
%    FindInInterval, CountInIntervals, PlotIntervals.
%

% Copyright (C) 2010-2011 by Michaël Zugaro
%
% This program 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.

if nargin < 1,
	error('Incorrect number of parameters (type ''help ToIntervals'' for details).');
end

if nargin == 1,
	in = x;
end

if ~islvector(in),
	error('Incorrect logical vector (type ''help ToIntervals'' for details).');
end

if in(end) == 1, in(end+1) = 0; end
in = in(:);
din = diff([0;in]);

start = din == 1;
stop = din == -1;

intervals = [find(start) find(stop)-1];

if nargin >=2,
	if ~isdvector(x),
		error('Incorrect x values (type ''help ToIntervals'' for details).');
	end
	intervals = x(intervals);
end


function [x,fval,exitflag,info] = opti_sedumi(At,b,c,K,opts)
%OPTI_SEDUMI Solve a SDP using SEDUMI
%
%   x = opti_sedumi(At,b,c,K) solves a LP or SDP in SeDuMi format.
%
%   x = opti_sedumi(At,b,c,K,opts) uses opts to pass optiset options to the
%   solver.
%
%   [x,fval,exitflag,info] = opti_sedumi(...) returns the objective value at
%   the solution, together with the solver exitflag, and an information
%   structure.
%
%   THIS IS A WRAPPER FOR SEDUMI (GPL)

%   Copyright (C) 2013 Jonathan Currie (IPL)

if(nargin < 5), opts = optiset; end
if(nargin < 4), K = []; end
if(nargin < 3), error('This function expects at least 3 arguments!'); end

%Set SEDUMI options
pars = opts.solverOpts;
pars.fid = min(dispLevel(opts.display),1);
pars.maxiter = opts.maxiter;
pars.eps = opts.tolrfun;

t = tic;
%Run Solver
[X,x,sinfo] = sedumi(At,b,c,K,pars);

%Assign primal fval
fval = -b'*x;

%Assign Outputs
info.Iterations = sinfo.iter;
info.Time = toc(t);
info.Algorithm = 'SeDuMi: Self-Dual-Minimization SDP Solver';

%Assign Exit Flag
if(sinfo.iter >= opts.maxiter)
    info.Status = 'Exceeded Maximum Iterations';
    exitflag = 0;
elseif(sinfo.numerr > 0)
    info.Status = 'Numerical Errors';
    exitflag = -2;
elseif(sinfo.pinf==1 || sinfo.dinf==1)
    info.Status = 'Infeasible';
    exitflag = -1;
elseif(sinfo.pinf==0 && sinfo.dinf==0)
    info.Status = 'Optimal';
    exitflag = 1;
end
info.DualObjective = c'*X;
info.X = X;


function v = so3_hatinv(xi)
v = [xi(3,2); xi(1,3); xi(2,1)];


classdef TestMultiply
    %TestMultiply

    methods (Static)
        function test_rbg_image
            img = cv.imread(fullfile(mexopencv.root(),'test','img001.jpg'), 'ReduceScale',2);
            img = img/2;

            out = cv.multiply(img, img);
            validateattributes(out, {class(img)}, {'size',size(img)});
            if mexopencv.require('images')
                expected = my_multiply(img, img);
                assert(isequal(out, expected));
            end

            out = cv.multiply(img, img, 'DType','uint16');
            validateattributes(out, {'uint16'}, {'size',size(img)});
            if mexopencv.require('images')
                expected = my_multiply(img, img, 'uint16');
                assert(isequal(out, expected));
            end
        end

        function test_2d_matrix
            A = uint8([10 35; 20 25]);
            B = uint8([10 10; 20 10]);

            out = cv.multiply(A, B);
            validateattributes(out, {'uint8'}, {'size',size(A)});
            expected = uint8([100 255; 255 250]);  % A .* B
            assert(isequal(out, expected));

            out = cv.multiply(A, 2);
            validateattributes(out, {'uint8'}, {'size',size(A)});
            expected = A .* 2;
            assert(isequal(out, expected));

            out = cv.multiply(2, A);
            validateattributes(out, {'uint8'}, {'size',size(A)});
            expected = 2 .* A;
            assert(isequal(out, expected));

            out = cv.multiply(2, 2);
            validateattributes(out, {'double'}, {'scalar'});
            expected = 2 * 2;
            assert(isequal(out, expected));

            out = cv.multiply(A, B, 'Scale',2);
            validateattributes(out, {'uint8'}, {'size',size(A)});
            expected = uint8([200 255; 255 255]);  % 2*A .* B
            assert(isequal(out, expected));
        end

        function test_output_depth
            out = cv.multiply(int16(20), uint8(20), 'DType','single');
            validateattributes(out, {'single'}, {'scalar'});
            assert(isequal(out, 400));
        end

        function test_saturation
            out = cv.multiply(uint8(20), uint8(30), 'DType','uint8');
            validateattributes(out, {'uint8'}, {'scalar'});
            assert(isequal(out, 255));

            out = cv.multiply(uint8(20), uint8(30), 'DType','uint16');
            validateattributes(out, {'uint16'}, {'scalar'});
            assert(isequal(out, 600));

            % saturation not applied for int32 (result overflows)
            out = cv.multiply(int32(50000), int32(50000), 'DType','int32');
            %validateattributes(out, {'int32'}, {'scalar', '<',0});

            % unlike MATLAB where result saturates
            %out = int32(50000) * int32(50000);
            %assert(isequal(out, intmax('int32')));
        end

        function test_error_argnum
            try
                cv.multiply();
                throw('UnitTest:Fail');
            catch e
                assert(strcmp(e.identifier,'mexopencv:error'));
            end
        end
    end

end

function out = my_multiply(src1, src2, dtype)
    %MY_MULTIPLY  Similar to cv.multiply using immultiply from IPT

    if nargin < 3, dtype = class(src1); end

    % multiply two images with specified output class
    out = immultiply(cast(src1, dtype), cast(src2, dtype));
end


% hungarianLSAPE.m Help file for hungarianLSAP MEX-file.
%  hungarianLSAPE.cpp - Compute a solution to (symmetric or asymmetric) LSAP with Hungarian algorithm
%
%      [rho,varrho] = hungarianLSAPE(C,init_type,forb)
%         [rho,u,v] = hungarianLSAPE(C,init_type,forb)
%  [rho,varrho,u,v] = hungarianLSAPE(C,init_type,forb)
%
%  Given a nxm cost matrix C (integer ou floatting values)
%  with last column encoding removal costs and last row insertion costs, it computes:
%
%  - a solution rho to the LSAPE, i.e. a mapping from the rows of C to its columns,
%    and the mapping varrho from the columns to the rows
%
%    rho is represented by a (n-1)x1 matrix so that rho(i)=j means that i is assigned to j
%    varrho is a 1x(m-1) matrix so that varrho(j)=i means that j is assigned to i
%    rho(i)=m or varrho(j)=n encode assignments to the null element (removal or insertion)
%
%  - a solution (u,v) to its dual problem (labeling problem)
%
%  optional init_type:
%    0: no initialization (u=v=0)
%    1 (default): classical initialization (u=min(C) on rows, v=min(C-u) on columns)
%
%  optional boolean parameter forb:
%    true  -> forbidden assignments are represented by negative cost values
%    false -> no forbidden assignments (by default)
%
%  This is a MEX-file for MATLAB.
%  This file is part of LSAPE.
%  LSAPE is free software: you can redistribute it and/or modify it
%  under the terms of the CeCILL-C License. See README for more details.
%
%     Copyright 2015-2017
%      authors: Sebastien Bougleux
%  institution: Normandie Univ, CNRS - ENSICAEN - UNICAEN, GREYC, France
%   last modif: July 5 2017
%


function [dimensions,x1,x2,x3] = xyz_axes_selection(x,y,z)
%XYZ_AXES_SELECTION returns the first non-singleton axes and a vector
%indicating which axes are selected
%
%   Usage: [dimensions,x1,x2,x3] = xyz_axes_selection(x,y,z)
%
%   Input parameters:
%       x,y,z      - vectors/matrices containing the x-, y- and z-axis values / m
%
%   Output parameters:
%       dimensions - 1x3 vector containing 1 or 0 to indicate the activity
%                    of the single dimensions in the order [x y z]
%       x1         - vector/matrix containing the first axis / m
%       x2         - vector/matrix containing the second axis / m
%       x3         - vector/matrix containing the third axis / m
%
%   XYZ_AXES_SELECTION(x,y,z) returns an indication vector for the x-, y- and
%   z-axis if we have any activity on this axis or if it is a singleton axis.
%   In addition, the axes are reordered starting first with the non-singleton
%   axes.
%
%   See also: plot_sound_field, xyz_grid, is_dim_custom

%*****************************************************************************
% The MIT License (MIT)                                                      *
%                                                                            *
% Copyright (c) 2010-2019 SFS Toolbox Developers                             *
%                                                                            *
% Permission is hereby granted,  free of charge,  to any person  obtaining a *
% copy of this software and associated documentation files (the "Software"), *
% to deal in the Software without  restriction, including without limitation *
% the rights  to use, copy, modify, merge,  publish, distribute, sublicense, *
% and/or  sell copies of  the Software,  and to permit  persons to whom  the *
% Software is furnished to do so, subject to the following conditions:       *
%                                                                            *
% The above copyright notice and this permission notice shall be included in *
% all copies or substantial portions of the Software.                        *
%                                                                            *
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
% IMPLIED, INCLUDING BUT  NOT LIMITED TO THE  WARRANTIES OF MERCHANTABILITY, *
% FITNESS  FOR A PARTICULAR  PURPOSE AND  NONINFRINGEMENT. IN NO EVENT SHALL *
% THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
% LIABILITY, WHETHER  IN AN  ACTION OF CONTRACT, TORT  OR OTHERWISE, ARISING *
% FROM,  OUT OF  OR IN  CONNECTION  WITH THE  SOFTWARE OR  THE USE  OR OTHER *
% DEALINGS IN THE SOFTWARE.                                                  *
%                                                                            *
% The SFS Toolbox  allows to simulate and  investigate sound field synthesis *
% methods like wave field synthesis or higher order ambisonics.              *
%                                                                            *
% https://sfs.readthedocs.io                            sfstoolbox@gmail.com *
%*****************************************************************************

%% ===== Checking of input parameters ====================================
nargmin = 3;
nargmax = 3;
narginchk(nargmin,nargmax);
isargnumeric(x,y,z);

%% ===== Computation =====================================================
dims = {x,y,z};
dimensions = ~is_dim_singleton(dims{:});
Nd = sum(dimensions);

newdims = {x(1),y(1),z(1)};  % default case, if all dimensions are singleton
newdims(Nd+1:end) = newdims(~dimensions); % move singleton dimensions to the end
newdims(1:Nd) = dims(dimensions);  % move non-singleton dimensions to the front

[x1, x2, x3] = newdims{:};


function [x, infos] = spg_nmf(V, rank, in_options)
% Stochastic projected gradient for non-negative matrix factorization (SMU-NMF) algorithm.
%
% Inputs:
%       matrix      V
%       rank        rank
%       options     options
% Output:
%       w           solution of w
%       infos       information
%
%
% This file is part of NMFLibrary.
%
% Created by H.Kasai on Mar. 28, 2017
%
% Change log:
%
%       Mar. 14, 2018 (Hiroyuki Kasai): Fixed algorithm.
%
%       May. 20, 2019 (Hiroyuki Kasai): Added initialization module.
%
%       Jul. 12, 2022 (Hiroyuki Kasai): Modified code structures.
%

    % set dimensions and samples
    [m, n] = size(V);

    % set local options
    local_options = [];
    local_options.W_sub_mode          = 'STD';
    local_options.H_sub_mode          = 'LS';
    local_options.accel               = false;
    local_options.ls                  = false;
    local_options.precon              = false;
    local_options.h_repeat            = 1;
    local_options.rep_mode            = 'fix';
    local_options.robust              = false;

    % check input options
    if ~exist('in_options', 'var') || isempty(in_options)
        in_options = struct();
    end
    % merge options
    options = mergeOptions(get_nmf_default_options(), local_options);
    options = mergeOptions(options, in_options);

    % initialize factors
    init_options = options;
    [init_factors, ~] = generate_init_factors(V, rank, init_options);
    Wt = init_factors.W;
    H = init_factors.H;
    R = init_factors.R;

    % determine sub_mode
    if options.accel
        options.H_sub_mode = 'ACC';
    else
        if options.ls
            options.H_sub_mode = 'LS';
        else
            options.H_sub_mode = 'STD';
        end
        options.h_repeat = 1;
    end

    % permute samples
    if options.permute_on
        perm_idx = randperm(n);
    else
        perm_idx = 1:n;
    end
    V = V(:,perm_idx);
    H = H(:,perm_idx);

    % initialize
    method_name = sprintf('SPG-NMF (%s,%s)', options.W_sub_mode, options.H_sub_mode);
    epoch = 0;
    grad_calc_count = 0;
    nesterov_alpha = 1;

    if options.verbose > 0
        fprintf('# %s: started ...\n', method_name);
    end

    if strcmp(options.rep_mode, 'adaptive')
        rhoh = 1+(m+m*rank)/(1*(rank+1));
        alpha = 2;
        delta = 0.01;
    end

    % store initial info
    clear infos;
    [infos, f_val, optgap] = store_nmf_info(V, Wt, H, R, options, [], epoch, grad_calc_count, 0);

    if options.verbose > 1
        fprintf('SPG-NMF (%s,%s): Epoch = 0000, cost = %.16e, optgap = %.4e\n', options.W_sub_mode, options.H_sub_mode, f_val, optgap);
    end

    % set start time
    start_time = tic();

    % main outer loop
    while true

        % check stop condition
        [stop_flag, reason, max_reached_flag] = check_stop_condition(epoch, infos, options);
        if stop_flag
            display_stop_reason(epoch, infos, options, method_name, reason, max_reached_flag);
            break;
        end

        cnt = 0;

        % main inner loop
        for t = 1 : options.batch_size : n - 1
            cnt = cnt + 1;

            % retrieve vt and ht
            vt = V(:,t:t+options.batch_size-1);
            ht = H(:,t:t+options.batch_size-1);

            % uddate ht
            Wtv = Wt.' * vt;
            WtW = Wt.' * Wt;
            if strcmp(options.H_sub_mode, 'ACC')
                if strcmp(options.rep_mode, 'adaptive')
                    gamma = 1;
                    eps0 = 1;
                    j = 1;
                    rhoh_alpha = rhoh*alpha;
                    %while j <= floor(1+rhoh*alpha) &&  gamma >= delta*eps0
                    while j <= rhoh_alpha && gamma >= delta*eps0
                        ht0 = ht;
                        ht = ht .* (Wtv) ./ (WtW * ht);
                        ht = ht + (ht<eps) .* eps;
                        if j == 1
                            eps0 = norm(ht0-ht);
                        end
                        gamma = norm(ht0-ht);
                        j = j+1;
                    end
                else
                    for ii=1:options.h_repeat
                        ht = ht .* (Wtv) ./ (WtW * ht);
                        ht = ht + (ht<eps) .* eps;
                    end
                end
            elseif strcmp(options.H_sub_mode, 'LS')
                ht = calc_nls_nmf(vt, Wt, 1e-16);
                ht = ht + (ht<eps) * eps;
            else
                ht = ht .* (Wtv) ./ (WtW * ht);
                ht = ht + (ht<eps) .* eps;
            end

            % update W
            if strcmp(options.W_sub_mode, 'Precon')
                Wt = projection_precon_mnls(vt, ht', Wt);
            elseif strcmp(options.W_sub_mode, 'Nesterov')
                options.nesterov_maxit = 1;
                options.nesterov_func_type = 'smooth';
                %options.nesterov_func_type = 'stochastic';
                %options.nesterov_func_type = 'strong_alpha_beta';

                [Wt, apg_iter, nesterov_alpha] = nesterov_mnls(vt, ht', Wt, nesterov_alpha, options.nesterov_maxit, options.nesterov_func_type);
                %nesterov_alpha
            else
                Wt = projection_mnls(vt, ht', Wt);
            end
            Wt = Wt + (Wt<eps) .* eps;

            % store new h
            H(:,t:t+options.batch_size-1) = ht;

            grad_calc_count = grad_calc_count + m * options.batch_size;
        end

        % measure elapsed time
        elapsed_time = toc(start_time);

        % update epoch
        epoch = epoch + 1;

        % store info
        infos = store_nmf_info(V, Wt, H, R, options, infos, epoch, grad_calc_count, elapsed_time);

        % display info
        display_info(method_name, epoch, infos, options);

    end

    x.W = Wt;
    x.H(:,perm_idx) = H;

end


function spm_cost_SHC_path(qU,A)
% plots path for cost_SHC demo's
% FORMAT spm_cost_SHC_path(qU,A)
%
% qU  - DEM condotioal esimates of states
% A.x - locations of attrcuor
% A.d - radius
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_cost_SHC_path.m 3757 2010-03-08 11:41:53Z guillaume $

% plot
%==========================================================================
[X Y Z] = sphere;

% plot path
%--------------------------------------------------------------------------
x     = qU.v{1}(1:2,:);
plot(A.x(1,:),A.x(2,:),'c.','MarkerSize',64), hold on
plot(x(1,:),x(2,:));
plot(x(1,:),x(2,:),'.','MarkerSize',8);

% plot locations
%--------------------------------------------------------------------------
% for i = 1:size(A.x,2)
%     surf(X*A.d + A.x(1,i),Y*A.d + A.x(2,i),Z*A.d - 8)
% end
% shading interp

axis([-4 4 -4 4])
axis square
title('trajectory','FontSize',16),hold off

% occupancy
%--------------------------------------------------------------------------
for i = 1:size(A.x,2)
    for j = 1:2
        d(j,:) = x(j,:) - A.x(j,i);
    end
    b(i,:) = sqrt(sum(d.^2)) < 2*A.d;
end

% plot precent occupancy
%--------------------------------------------------------------------------
p     = 100*sum(b,2)/length(b);
for i = 1:length(p)
    text(A.x(1,i)*2,A.x(2,i)*1.4,sprintf('%2.0f%%',p(i)),'Fontsize',12)
end
hold off


function out = Dstar(R, n, e, P, fun)
%finds excess distortion for a given rate at a given blocklength for
%transmission of a GMS over an AWGN (noise variance 1)
%R - rate
%n - block length (scalar)
%e - excess probability
%P - channel SNR
%fun - which function to use for calculation

%
%   Created in 2012 by Victoria Kostina (vkostina@caltech.edu)
%

%starting points - 'persistent' to make optimization faster
persistent x0;
persistent y0;
persistent nprev;

if isempty(nprev)
    nprev = 0;
end

tol = 1e-12;

options = optimset('TolX',tol, 'MaxFunEvals', 500, 'Display', 'off');
dbar = 1/(1+P);
V1 = 2*dbar^2; %no coding (amplifiers)
V = 2*dbar^2*(2 - dbar^2); %optimal blocklength-n code

Vc = P*(P+2)/2/(P+1)^2;
Vs = .5;

deltan = n - nprev; %distance between consecutive n's
x0correction = sqrt(V1/n)*Qinv(e)/2/n^(3/2)*deltan;
y0slack = .1;

k = ceil(R*n);

if ~isempty(x0)
    x0 = x0 - x0correction;
    d0 = x0;
else
    d0 = 1.8*dbar;
end

switch lower(fun)
    case 'aseparate'
        out = ASeparate();
    case 'ajoint'
        %Achievability via JSCC coding
        options = optimset('TolX',tol, 'MaxFunEvals', 500, 'Algorithm', 'active-set', 'Display', 'off');
        out = AJoint();
    case 'cht'
        %Converse via hypothesis testing
        out = Cht();
    case 'nocoding'
        %no coding
        out = ANoCoding();
    case 'approxnocoding'
        out = ApproxNoCoding();
    case 'approx'
        %gaussian approximation
        out = Approx();
    otherwise
        disp('Error: unknown type.')
end

%--------------------------------------------------------------------------
    function out = ANoCoding()
        %No coding

        out = fzero(@(d)1 - e - chi2cdf(n*d/dbar, n), d0);
    end

%--------------------------------------------------------------------------
    function out = ApproxNoCoding()
        %No coding
        out = dbar + sqrt(V1/n)*Qinv(e);
    end
%--------------------------------------------------------------------------
    function out = Approx()
        %No coding
        %C = .5*log(1 + P);
       % Rd = .5*log(1/d);
        %out = fzero(@(d)C + .5*log(d) - sqrt(1/n*(Vc + Vs))*Qinv(e), d0);
        out = dbar + sqrt(V/n)*Qinv(e);
    end

%--------------------------------------------------------------------------
    function out = Cht()
        %Converse via meta-converse

        tau = thres(k);

        out = fzero(@(d)Qrob(d, tau) - 1, d0);
        updatex0(out);

        function out = Qrob(d, tau)

            b = sqrt( tau*n/k/d );
            out = quad(@density, 0, b);
            function out = density(r)
                % v - noncentral chi square (n) /n
                % v0 - central chi square (k) /n

                out = ncx2cdf(n*(tau - k/n*d*r.^2)/P, n, n*(1 + 1/P)).*r.^(k-1).*k;

            end
        end

        function out = thres(k)
            %find tau:
            tau0 = 1 + k/n + 2*sqrt( (n*Vc + k*Vs)/n^2 )*Qinv(e);
            %out = fzero(@(tau)Prob(tau) - 1 + e, tau0, options);
            out = fzero(@(tau)Prob(tau) - 1 + e, tau0, options);

            function out = Prob(tau)

                if n < 200
                    b = 10*k/n; %Ev0 = k/n
                else
                    b = 2*k/n;
                end

                out = quad(@density, 0, b);
                function out = density(v0)
                    % v - noncentral chi square (n) /n
                    % v0 - central chi square (k) /n

                    out = ncx2cdf(n*(tau - v0)*(1+P)/P, n, n/P).*chi2pdf(n*v0,k)*n;

                end
            end
        end
    end

%--------------------------------------------------------------------------
    function out = AJoint()
        %dP/dQ is bounded by a constant. Find that constant.
        [~, gamma2] = fminbnd(@(x)-ncx2pdf(n*x, n, n*P)./chi2pdf(n*x/(1+P), n)*(1+P), 0, 10);
        gamma2 = -gamma2;

        out = fzero(@(d)bound(d) - e, d0);
        updatex0(out);
%        fprintf('AJoint: n = %i, R = %f \n', n, out);

        function out = bound(d)
            %optimize with respect to r and g; y0 = [r gg] - TOO SLOW
            if d > 1
                out = NaN;
                return;
            end
            out = goal([d 3]);

            function out = goal(dg)
                d = dg(1);

                gamma1 = log(n)/2*dg(2);

                r0 = sqrt(1 - d);
                a = max(0, r0 - sqrt(d));
                b = r0 + sqrt(d);

                A = n/2*log(1+P) + n/2 - log(gamma1) - log(gamma2);

                Fouter = @(v0)chi2pdf(k*v0, k).*k.*Iinner(v0);
                Ftail = @(v0)(1 - ncx2cdf(thresv(v0)*n,n,n/P)).*chi2pdf(k*v0,k).*k;
                %   Ftail = @(v0) exp(n/P*t./(1-2*t))./(1-2*t).^(n/2).*exp(-t*thresv(v0)).*chi2pdf(k*v0,k).*k;
                out = quad(Fouter, a^2 + tol, b^2- tol)...                      %exponent < 1
                    + quad(Ftail, a^2 + tol, b^2 - tol)...                      %exponent = 1
                    + chi2cdf((a^2 + tol)*k, k) + 1 - chi2cdf((b^2 - tol)*k, k)...      %nontypical source realization
                    + exp(1-gamma1);

                %out = quad(Ftail, a^2 + tol, b^2 - tol); %most significant
                %term

                function out = Iinner(v0)
                    %exponent < 1
                    out = zeros(1, numel(v0));
                    for j0 = 1:numel(v0)
                        Finner = @(v) ncx2pdf(n*v, n, n/P).*n.*exp(P/2/(1+P)*n*(v - thresv(v0(j0))));
                        out(j0) = quad(Finner, 0, thresv(v0(j0)) );
                    end
                    if any(isnan(out))
                        return;
                    end
                end

                function out = thresv(x)
                    %threshold for v as a function of v0
                    out = max( 0, 2*(1+P)/P/n*(A + logPdball(x)) );
                end

                function out = logPdball(x)
                    %x - distance squared from the origin (x*k central chi square k)
                    %lower bound

                    cosa = (r0^2 + x - d)./(2*r0.*sqrt(x));
                    sina = (1 - cosa.^2).^(1/2);
                    out = loggamma(k/2+1, 'LB') - loggamma((k-1)/2 +1, 'UB')- .5*log(pi)-log(k) + (k-1)*log(sina);

                    %handle small distances from the origin
                    out(x <= sqrt(d) - r0) = -Inf;
                end
            end
        end
    end

%--------------------------------------------------------------------------
    function out = ASeparate()
        %achievability via separate coding
        %R = 1

        if isempty(y0)
            %first pass
            startL = 1;
            endL = n;
        else
            startL = max( ceil((y0 - 4*y0slack)*n), 1);
            endL = min( ceil((y0 + 4*y0slack)*n), n );
        end

        %precompute channel error probability
        PeCh = ones(1, endL);
        for L = startL:endL %exp(r) - the number of messages passed from the source enc to the channel enc
            PeCh(L) = PeubChannel(P, n, L);
        end

        Peubseparate(k, d0, PeCh, e);
        out = fzero(@(d)Peubseparate(k, d, PeCh, e) - e, d0);
        updatex0(out);
        [err, curL] = Peubseparate(k, out, PeCh, e);
        updatey0(curL/n);
    end

%--------------------------------------------------------------------------
%Utilities for faster computation
    function updatex0(x)
        if ~isnan(x) && n > 100
            x0 = x;
            nprev = n;
        end
    end

%--------------------------------------------------------------------------
%Utilities for faster computation
    function updatey0(y)
        if ~isnan(y) && n > 100
            y0 = y;
        end
    end
end


function X = mrdivide(A, B)
%/   Right matrix divide for an UNBNDFUN.
%   A/B divides the UNBNDFUN A by a scalar B. More generally, it gives the
%   least-squares solution (with respect to the continuous L^2 norm) to X*B = A
%   when either A or B is an UNBNDFUN. Note that in the latter case, formally it
%   is X.' that is returned, as UNBNDFUN objects are always columns.
%
% See also QR, RDIVIDE, MLDIVIDE.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Case B is a matrix: X*B = A  ==> X = A/B = (Q*R)/B = Q*(R/B)
%
% Case A is a matrix: X*B = X*(Q*R) = A ==> X = (A/R)*Q' ==> X' = Q*(A/R)'
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if ( (size(B, 2) ~= size(A, 2)) && ~(isa(B, 'double') && isscalar(B)) )
    error('CHEBFUN:UNBNDFUN:mrdivide:size', 'Matrix dimensions must agree.');

elseif ( isa(B, 'double') )  % UNBNDFUN / double

    if ( isscalar(B) )
        % Scalar case is easy:
        X = A;                              % Copy A to X
        X.onefun = X.onefun/B;              % mrdivide of the onefun
    else
        % Call MRDIVIDE at the ONEFUN level:
        X = A;
        X.onefun = (A.onefun/B);

%         % Alternatively, we could call QR() at the UNBNDFUN level
%         % For matrix case, we do least squares via QR:
%         [Q, R] = qr(A, 0);
%         X = Q*(R/B);
    end

elseif ( isa(A, 'double') )  % double / UNBNDFUN
    error('CHEBFUN:UNBNDFUN:mrdivide:doubleDivUnbndfun', ...
        ['/ does not support the division of a numerical matrix divided by ' ...
        'by an UNBNDFUN.']);

elseif ( isa(B, 'unbndfun') && isa(A, 'unbndfun') )
    error('CHEBFUN:UNBNDFUN:mrdivide:unbndfunDivUnbndfun', ...
        'Use ./ to divide UNBNDFUN by an UNBNDFUN.');

else
    error('CHEBFUN:UNBNDFUN:mrdivide:badArg', '%s/%s is not well-defined.', ...
        class(A), class(B));

end

end


function [ DKI, K_ADC, K_AKC, Kv1, Kv2, Kv3, I_DKI ] = fit_DKI_model( im, B_mat, thresh_val, method)
% FIT_DT    Fits the diffusion kurtosis model voxelwise to an image
%           S = S0 exp(-(bD + 1/6 b^2 * K))
%
%
% Inputs:
%
%   IM is the input image, of any dimensionality, with the diffusion
%   scans in the last dimension
%
%   B is the b-matrix, of size [3 3 N]. If the measured b matrix isn't
%   available, it can be approximated by:
%   b(:,:,n)=b_values(n)*unit_vectors(n,:)'*unit_vectors(n,:);
%
%   THRESH_VAL is a threshold for skipping the FA, ADC and vector field
%   computation in voxels x where im(x) < thresh_val. THRESH_VAL can
%   alternatively be a mask of voxels to process.
%
%   METHOD is a string, and can take the following values:
%       'linear' fits the tensor in the log domain (default)
%       'nonlinear' fits the tensor in the signal domain (recommended for
%       high b-values or when SNR is low)
%
% Outputs:
%
%   DKI is the array of coefficients

% Author: Darryl McClymont <darryl.mcclymont@gmail.com>
% Copyright © 2014-2015 University of Oxford
% Version: 0.1.1
%
% University of Oxford means the Chancellor, Masters and Scholars of
% the University of Oxford, having an administrative office at
% Wellington Square, Oxford OX1 2JD, UK.
%
% This file is part of Gerardus.
%
% This program 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 program 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. The offer of this
% program under the terms of the License is subject to the License
% being interpreted in accordance with English Law and subject to any
% action against the University of Oxford being under the jurisdiction
% of the English Courts.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

% check arguments
%narginchk(2, 5);
%nargoutchk(0, 5);

sz = size(im);

if (nargin < 3) || isempty(thresh_val)
    thresh_val = -inf;
end

if (nargin < 4) || isempty(method)% default method is linear least squares
    method = 'linear';
end

% check that a valid method has been given
if ~max([strcmp(method, 'linear'), strcmp(method, 'nonlinear')])
    disp('Unrecognised method, performing fast linear fitting')
end

% handle a vector with the wrong orientation
if (length(sz) == 2) && (sz(2) == 1)
    im = im';
    sz = size(im);
end

% reshape image
I_vector = reshape(im, [prod(sz(1:end-1)), sz(end)]);

% handle mask or threshold
if isscalar(thresh_val)
    thresh_val = I_vector(:,1) > thresh_val;
else
    thresh_val = logical(thresh_val);
end

% keep only the masked voxels
I_vector = double(I_vector(thresh_val(:), :));

[~, b0] = max(mean(I_vector, 1));

% normalise and take the log of the image to linearise the equation
imlog = log(abs(bsxfun(@rdivide, I_vector, I_vector(:,b0))));

% get the bvalue
bvalue = squeeze(B_mat(1,1,:) + B_mat(2,2,:) + B_mat(3,3,:))';

% normalise the B matrix
B_mat_norm = B_mat ./ repmat(permute(bvalue + eps, [3,1,2]), [3 3 1]);

% define A matrix

A_D = squeeze([B_mat_norm(1,1,:), B_mat_norm(2,2,:), B_mat_norm(3,3,:), 2*B_mat_norm(1,2,:), 2*B_mat_norm(1,3,:), 2*B_mat_norm(2,3,:)])';
% A_D in the paper is not scaled by the b value

A_K = squeeze([B_mat_norm(1,1,:).^2, B_mat_norm(2,2,:).^2, B_mat_norm(3,3,:).^2, ...
    4*B_mat_norm(1,1,:).*B_mat_norm(1,2,:), ...
    4*B_mat_norm(1,1,:).*B_mat_norm(1,3,:), ...
    4*B_mat_norm(1,2,:).*B_mat_norm(2,2,:), ...
    4*B_mat_norm(1,3,:).*B_mat_norm(3,3,:), ...
    4*B_mat_norm(2,2,:).*B_mat_norm(2,3,:), ...
    4*B_mat_norm(2,3,:).*B_mat_norm(3,3,:), ...
    2*B_mat_norm(1,1,:).*B_mat_norm(2,2,:) + 4*B_mat_norm(1,2,:).*B_mat_norm(1,2,:), ...
    2*B_mat_norm(1,1,:).*B_mat_norm(3,3,:) + 4*B_mat_norm(1,3,:).*B_mat_norm(1,3,:), ...
    2*B_mat_norm(2,2,:).*B_mat_norm(3,3,:) + 4*B_mat_norm(2,3,:).*B_mat_norm(2,3,:), ...
    8*B_mat_norm(1,2,:).*B_mat_norm(1,3,:) + 4*B_mat_norm(1,1,:).*B_mat_norm(2,3,:), ...
    8*B_mat_norm(1,2,:).*B_mat_norm(2,3,:) + 4*B_mat_norm(2,2,:).*B_mat_norm(1,3,:), ...
    8*B_mat_norm(1,3,:).*B_mat_norm(2,3,:) + 4*B_mat_norm(3,3,:).*B_mat_norm(1,2,:)])';
% this takes care of cross terms by saying that (for example) 6 x^2 y^2
% = 2 (x^2) (y^2) + 4 (xy) (xy)
% so it uses these elements of the B matrix.

A = [-bsxfun(@times, bvalue', A_D), 1/6 * bsxfun(@times, bvalue'.^2, A_K)];

% only keep b values less than 5000 in the linear fit
lin_bvals = bvalue < 5000;

% weighted fit
param.rician = 0;
param.unique_weights = 0;
param.verbose = 1;
M = weighted_linear_fit(imlog(:,lin_bvals), A(lin_bvals,:)', @(z)exp(z), param);
%M = weighted_linear_fit(imlog, A', @(z)exp(z), param);

%M = (pinv(A) * imlog')';
%M = (pinv(A' * (W.^2) * A) * A' * (W.^2) * (imlog'))';
% weighted version of M = (pinv(A) * imlog')';

% add the S0 column
M = [I_vector(:,1), M];

M_lin = M;

% perform nonlinear fitting (if required)
if strcmp(method, 'nonlinear')

    % get error
    I_recon = bsxfun(@times, M(:,1) , exp(M(:,2:end) * A'));
    rmse_lin = sqrt(mean((I_vector(:) - I_recon(:)).^2));
    %disp(['RMSE from quick fit = ' num2str(rmse_lin)])

    options = optimoptions('lsqcurvefit','Jacobian','on', 'DerivativeCheck', 'off', ...
        'display', 'off', 'MaxIter', 40);

    lb = [0 0 0 0 -1 -1 -1 ... % [S0 DTI
            0 0 0 -1 -1 -1 -1 -1 -1 0 0 0 -1 -1 -1] * 3E-3; % DKI ]

    ub = [inf, ones(1,21) * 3E-3]; % [S0 DTI DKI]

    % constrain initial guess between lower and upper bounds
    M = bsxfun(@max, M, lb);
    M = bsxfun(@min, M, ub);
    M(isnan(M(:))) = 0;
    M(isinf(M(:))) = 0;

    if (size(I_vector,1) > 1000) && (matlabpool('size') == 0)
        disp('Open matlabpool for parallel processing')
    end

    A_hat = -A';

    parfor i = 1:size(I_vector,1)

        M(i,:) = lsqcurvefit(@DKI_model, M(i,:), A_hat, I_vector(i,:), lb, ub, options);

    end

    I_recon = bsxfun(@times, M(:,1) , exp(M(:,2:end) * A'));
    rmse = sqrt(mean((I_vector(:) - I_recon(:)).^2));

    bad_fits = rmse > rmse_lin;
    %disp(['Number of bad fits: ' num2str(sum(bad_fits(:)))])
    M(bad_fits,:) = M_lin(bad_fits,:);

    %disp(['RMSE from slow fit = ' num2str(rmse)])

end

if nargout == 1
    % quick hack because reshape sz needs to be at least length 2
    if length(sz) == 2
        sz = [sz(1), 1, sz(2)];
    end

    % fill regions outside mask with zeros, and reshape
    DKI = zeros(prod(sz(1:end-1)), 22);
    DKI(thresh_val(:), :) = M;
    DKI = reshape(DKI, [sz(1:end-1), 22]);

    return
end

% compute the returned signal
I_DKI = bsxfun(@times, M(:,1), exp(M(:, 2:end) * A'));

K_v1 = zeros(1, size(M,1));
K_v2 = zeros(1, size(M,1));
K_v3 = zeros(1, size(M,1));
ADC = zeros(1, size(M,1));
AKC = zeros(1, size(M,1));

[x, y, z] = psphere(200);
x = x(1:end/2); y = y(1:end/2); z = z(1:end/2);

% Compute the kurtosis (mean and directional)
for v = 1:size(M,1)

    % reshape the coefficients

    % S0 = M(i,1);

    D_hat_i = M(v,2:7);

    % A_D = xx, yy, zz, xy, xz, yz

    % rearrange for convenience
    D_hat = D_hat_i([1 4 5 2 6 3]);

    D33 = [D_hat(1), D_hat(2), D_hat(3);
           D_hat(2), D_hat(4), D_hat(5);
           D_hat(3), D_hat(5), D_hat(6)];

    [vec, val] = eig(D33);
    % sort
    [~, ix] = sort(sum(val,1), 'descend');
    vec = vec(:, ix);
    val = sum(val(:, ix));

    % eigenvalues should be positive
    % taking the magnitude is a very bad way of handling them
    val = abs(val);

    ADC(v) = sum(val) / 3;

    D2i = M(v, 8:end);

    % A_K = xxxx, yyyy, zzzz, 1 2 3
    %       xxxy, xxxz, xyyy, 4 5 6
    %       xzzz, yyyz, yzzz, 7 8 9
    %       xxyy, xxzz, yyzz, 10 11 12
    %       xxyz, xyyz, xyzz ,13 14 15

    % re-arrange for convenience
    D2 = D2i([1, 4, 5, 10, 13, 11, 6, 14, 15, 7, 2, 8, 12, 9, 3]);

    % kurtosis tensor (includes ADC^2 internally)
    K = [ D2(01), D2(02), D2(03), D2(04), D2(05), D2(06); % xx
          D2(02), D2(04), D2(05), D2(07), D2(08), D2(09); % xy
          D2(03), D2(05), D2(06), D2(08), D2(09), D2(10); % xz
          D2(04), D2(07), D2(08), D2(11), D2(12), D2(13); % yy
          D2(05), D2(08), D2(09), D2(12), D2(13), D2(14); % yz
          D2(06), D2(09), D2(10), D2(13), D2(14), D2(15)];% zz
        %   xx      xy      xz      yy      yz      zz

    % Parallel kurtosis (kurtosis in the direction of highest diffusivity)
    v1 = vec(:, 1);

    % Diffusion in this direction
    D_v1 = v1' * D33 * v1; % = eigenvalue 1

    % kurtosis in this direction
    v1_sq = v1 * v1';
    v1_sq_vect = [v1_sq(1), 2*v1_sq(2), 2*v1_sq(3), v1_sq(5), 2*v1_sq(6), v1_sq(9)];
    K_v1(v) = (v1_sq_vect * K * v1_sq_vect') / (D_v1^2);

    % Parallel kurtosis (v2)
    v2 = vec(:, 2);

    % Diffusion in this direction
    D_v2 = v2' * D33 * v2; % = eigenvalue 2

    % kurtosis in this direction
    v2_sq = v2 * v2';
    v2_sq_vect = [v2_sq(1), 2*v2_sq(2), 2*v2_sq(3), v2_sq(5), 2*v2_sq(6), v2_sq(9)];
    K_v2(v) = (v2_sq_vect * K * v2_sq_vect') / (D_v2^2);

    % Parallel kurtosis (v3)
    v3 = vec(:, 3);

    % Diffusion in this direction
    D_v3 = v3' * D33 * v3; % = eigenvalue 3

    % kurtosis in this direction
    v3_sq = v3 * v3';
    v3_sq_vect = [v3_sq(1), 2*v3_sq(2), 2*v3_sq(3), v3_sq(5), 2*v3_sq(6), v3_sq(9)];
    K_v3(v) = (v3_sq_vect * K * v3_sq_vect') / (D_v3^2);

    % mean kurtosis
    % there are clever ways of getting this, but the easiest is to just
    % average the kurtosis over a bunch of uniformly spaced directions
    K_vi = zeros(1,length(x));
    for i = 1:length(x)
        vi = [x(i), y(i), z(i)]';
        D_vi = vi' * D33 * vi;
        vi_sq = vi * vi';
        vi_sq_vect = [vi_sq(1), 2*vi_sq(2), 2*vi_sq(3), vi_sq(5), 2*vi_sq(6), vi_sq(9)];
        K_vi(i) = (vi_sq_vect * K * vi_sq_vect') / (D_vi^2);
    end

    AKC(v) = mean(K_vi);

end

% quick hack because reshape sz needs to be at least length 2
if length(sz) == 2
    sz = [sz(1), 1, sz(2)];
end

% fill regions outside mask with zeros, and reshape
DKI = zeros(prod(sz(1:end-1)), 22);
DKI(thresh_val(:), :) = M;
DKI = reshape(DKI, [sz(1:end-1), 22]);

IDKI = zeros(prod(sz(1:end-1)), sz(end));
IDKI(thresh_val(:), :) = I_DKI;
I_DKI = reshape(IDKI, sz);

K_ADC = zeros(size(thresh_val));
K_ADC(thresh_val) = ADC;
K_ADC = reshape(K_ADC, sz(1:end-1));

K_AKC = zeros(size(thresh_val));
K_AKC(thresh_val) = AKC;
K_AKC = reshape(K_AKC, sz(1:end-1));

Kv1 = zeros(size(thresh_val));
Kv1(thresh_val) = K_v1;
Kv1 = reshape(Kv1, sz(1:end-1));

Kv2 = zeros(size(thresh_val));
Kv2(thresh_val) = K_v2;
Kv2 = reshape(Kv2, sz(1:end-1));

Kv3 = zeros(size(thresh_val));
Kv3(thresh_val) = K_v3;
Kv3 = reshape(Kv3, sz(1:end-1));

end

function [ F, J ] = DKI_model( x, Bv )

    F = x(1) * exp(-(x(2:end) * Bv));

    if nargout > 1 % Jacobian
        J = zeros(length(F), length(x));

        J(:,1) = exp(-(x(2:end) * Bv));

        J(:,2:end) = -bsxfun(@times, Bv, F)';

    end

end

%% This converts the 3x3 kurtosis tensor into a 3x3x3x3 kurtosis tensor
% It is not needed right now, but good to have for the future.

%     K_index =[01, 02, 03, 04, 05, 06; % xx
%               02, 04, 05, 07, 08, 09; % xy
%               03, 05, 06, 08, 09, 10; % xz
%               04, 07, 08, 11, 12, 13; % yy
%               05, 08, 09, 12, 13, 14; % yz
%               06, 09, 10, 13, 14, 15];% zz
%             % xx  xy  xz  yy  yz  zz
%
%     W = zeros([3 3 3 3]);
%
%     for i = 1:3
%         for j = 1:3
%              for k = 1:3
%                  for l = 1:3
%                     switch i
%                         case 1
%                             r1 = [1 2 3];
%                         case 2
%                             r1 = [2 4 5];
%                         case 3
%                             r1 = [3 5 6];
%                     end
%                     switch j
%                         case 1
%                             r2 = [1 2 3];
%                         case 2
%                             r2 = [2 4 5];
%                         case 3
%                             r2 = [3 5 6];
%                     end
%
%                     if i == j
%                         switch i
%                             case 1
%                                 r =1;
%                             case 2
%                                 r = 4;
%                             case 3
%                                 r = 6;
%                         end
%                     else
%                         r = intersect(r1, r2);
%                     end
%
%
%                     switch k
%                         case 1
%                             c1 = [1 2 3];
%                         case 2
%                             c1 = [2 4 5];
%                         case 3
%                             c1 = [3 5 6];
%                     end
%                     switch l
%                         case 1
%                             c2 = [1 2 3];
%                         case 2
%                             c2 = [2 4 5];
%                         case 3
%                             c2 = [3 5 6];
%                     end
%
%                     if k == l
%                         switch k
%                             case 1
%                                 c =1;
%                             case 2
%                                 c = 4;
%                             case 3
%                                 c = 6;
%                         end
%                     else
%                         c = intersect(c1, c2);
%                     end
%
%                     D2_index = K_index(r,c);
%
%                     W(i,j,k,l) = D2(D2_index);
%                  end
%              end
%         end
%     end
%     v1 = vec(:, 1);
%     % Diffusion in this direction
%     D_v1 = v1' * D33 * v1;
%     K_v1_vox = 0;
%     for i = 1:3
%         for j = 1:3
%             for k = 1:3
%                 for l = 1:3
%                     K_v1_vox = K_v1_vox + v1(i) * v1(j) * v1(k) * v1(l) * W(i,j,k,l);
%                 end
%             end
%         end
%     end
%     K_v1(v) = K_v1_vox / D_v1^2;


function [DF] = df_build_fast(V,C)
% DF_BUILD_FAST Build a volumetric distance field and spatial index that can be
% used to speed up approximate NN queries in 3D This code uses the kdtree
% toolbox
%
% [DF] = df_build_fast(V,C)
%
% Input:
%   V  Coordinates of the points
%   C  Column vector that defines the number of cells for every
%      dimension
% Output:
%   DF  structure to be passed to df_query to execute queries

if ~exist('C','var')
    C=[10;10;10];
end

MIN = min(V,[],1);
MAX = max(V,[],1);

S = (MAX-MIN)./C';

D = zeros(C'+1);
N = zeros(C'+1);

%% Build kd-tree
tree = kdtree_build( V );

%% Queries
for x=1:size(D,1)
    progressbar(x,size(D,1));
    for y=1:size(D,2)
        for z=1:size(D,3)
            i = [x,y,z];
            p = MIN + (i-1).*S;

            N(x,y,z) = kdtree_nearest_neighbor(tree,p);
            D(x,y,z) = normrow(p-V(N(x,y,z),:));
        end
    end
end

kdtree_delete(tree);

% Prepare DF struct
DF.MIN = MIN;
DF.MAX = MAX;
DF.D = D;
DF.N = N;
DF.C = C;
DF.S = S;
end


function [nbcd,bcdof] = BoundaryConditions(sdof,bc)

%--------------------------------------------------------------------------
% Purpose :
%         To get the arrested degree's of freedom for the beam depending on
% type of the boundary conditions
%
% Synopsis :
%          [nbcd,bcdof] = BoundaryConditions(sdof,bc)
%
% Variable Description:
% INPUT parameters:
%           sdof : system degrees of freedom
%           bc : boundary condition type
%
% OUTPUT PARAMETERS :
%           bcdof : boundary degrees of freedom
%           nbcd : number of boundary conditions
%
%--------------------------------------------------------------------------

 if bc == 'c-c'     % clamped-clamped beam
    bcdof = [1 2 sdof-1 sdof] ;
    nbcd = length(bcdof) ;

 elseif bc == 'c-f' % clamped-free beam
    bcdof = [1 2] ;
    nbcd = length(bcdof) ;

 elseif bc == 'c-s'     % clamped-supported beam
     bcdof = [1 2 sdof-1] ;
     nbcd = length(bcdof) ;

 elseif bc == 's-s'     % supported-supported beam
     bcdof = [ 1 sdof-1] ;
     nbcd = length(bcdof) ;

 end


function varargout = drawCurve3d(varargin)
%DRAWCURVE3D draw a 3D curve specified by a list of points.
%
%   drawCurve3d(COORD) packs coordinates in a single [N*3] array.
%
%   drawCurve3d(PX, PY, PZ) specify coordinates in separate arrays.
%
%   H = drawCurve3d(...) also return a handle to the list of line objects.
%
%   See Also :
%   drawPolygon

% ------
% Author: David Legland
% e-mail: david.legland@inrae.fr
% Created: 2005-02-18
% Copyright 2005 INRA - TPV URPOI - BIA IMASTE

% deprecation warning
warning('geom3d:deprecated', ...
    '''drawCurve3d'' is deprecated, use ''drawPolyline3d'' instead');

% default value for closed or open curve
closed = false;

% check case we want to draw several curves, stored in a cell array
var = varargin{1};
if iscell(var)
    hold on;
    h = [];
    for i = 1:length(var(:))
        h = [h; drawCurve3d(var{i}, varargin{2:end})]; %#ok<AGROW>
    end
    if nargout>0
        varargout{1}=h;
    end
    return;
end

% extract curve coordinate
if size(var, 2)==1
    % first argument contains x coord, second argument contains y coord
    % and third one the z coord
    px = var;
    if length(varargin)<3
        error('Wrong number of arguments in drawCurve3d');
    end
    py = varargin{2};
    pz = varargin{3};
    varargin = varargin(4:end);
else
    % first argument contains both coordinate
    px = var(:, 1);
    py = var(:, 2);
    pz = var(:, 3);
    varargin = varargin(2:end);
end

% check if curve is closed or open
if ~isempty(varargin)
    var = varargin{1};
    if strncmpi(var, 'close', 5)
        closed = true;
        varargin = varargin(2:end);
    elseif strncmpi(var, 'open', 4)
        closed = false;
        varargin = varargin(2:end);
    end
end

% for closed curve, add the first point at the end to close curve
if closed
    px = [px; px(1)];
    py = [py; py(1)];
    pz = [pz; pz(1)];
end

%% draw the curve ! !! ! !
h = plot3(px, py, pz, varargin{:});

if nargout>0
    varargout{1} = h;
end


% test example
% April 15th, 2010, By Reza Farrahi Moghaddam, Synchromedia Lab, ETS, Montreal, Canada

%
u0_filename = 'P03.tif';

% method name. Please try one of these methods: 'min_avg'; % 'avg'; 'normal';
% method = 'min_avg' is recommended for document image processing with less dependency on color.
method_name = 'min_avg'; % 'min_avg'; % 'avg'; 'normal';

u0_color = imread(u0_filename);

% ref image, using the standard method.
u0_gray_ref = double(rgb2gray(u0_color));

% main function
u0_gray = universal_color_to_gray_converter(u0_color , 'method', method_name);

% display
figure, imshow(u0_gray_ref / max(max(u0_gray_ref)))
figure, imshow(double(u0_gray) / max(max(double(u0_gray))))

% save the output
imwrite(u0_gray, [u0_filename,'-',method_name,'.tif']);