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

% 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('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('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;
  end

end

function [w, s] = 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 = {};

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

      % Hyperparameters of magnSigma2
      [wh sh] = gpcf.p.magnSigma2.fh.pak(gpcf.p.magnSigma2);
      w = [w wh];
      s = [s; sh];
    end

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

      % Hyperparameters of lengthScale
      [wh  sh] = gpcf.p.lengthScale.fh.pak(gpcf.p.lengthScale);
      w = [w wh];
      s = [s; sh];
    end

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

      % Hyperparameters of lengthScale_sexp
      [wh sh] = gpcf.p.lengthScale_sexp.fh.pak(gpcf.p.lengthScale_sexp);
      w = [w wh];
      s = [s; sh];
    end

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

      % Hyperparameters of period
      [wh sh] = gpcf.p.period.fh.pak(gpcf.p.period);
      w = [w wh];
      s = [s; sh];
    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., 2004, Bayesian data Analysis, second edition, p24.

    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 i=i1
        for j = 1:n
          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.e = @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


classdef Filter < dagnn.Layer
  properties
    pad = [0 0 0 0]
    stride = [1 1]
    dilate = [1 1]
  end
  methods
    function set.pad(obj, pad)
      if numel(pad) == 1
        obj.pad = [pad pad pad pad] ;
      elseif numel(pad) == 2
        obj.pad = pad([1 1 2 2]) ;
      else
        obj.pad = pad ;
      end
    end

    function set.stride(obj, stride)
      if numel(stride) == 1
        obj.stride = [stride stride] ;
      else
        obj.stride = stride ;
      end
    end

    function set.dilate(obj, dilate)
      if numel(dilate) == 1
        obj.dilate = [dilate dilate] ;
      else
        obj.dilate = dilate ;
      end
    end

    function kernelSize = getKernelSize(obj)
      kernelSize = [1 1] ;
    end

    function outputSizes = getOutputSizes(obj, inputSizes)
      ks = obj.getKernelSize() ;
      ke = (ks - 1) .* obj.dilate + 1 ;
      outputSizes{1} = [...
        fix((inputSizes{1}(1) + obj.pad(1) + obj.pad(2) - ke(1)) / obj.stride(1)) + 1, ...
        fix((inputSizes{1}(2) + obj.pad(3) + obj.pad(4) - ke(2)) / obj.stride(2)) + 1, ...
        1, ...
        inputSizes{1}(4)] ;
    end

    function rfs = getReceptiveFields(obj)
      ks = obj.getKernelSize() ;
      ke = (ks - 1) .* obj.dilate + 1 ;
      y1 = 1 - obj.pad(1) ;
      y2 = 1 - obj.pad(1) + ke(1) - 1 ;
      x1 = 1 - obj.pad(3) ;
      x2 = 1 - obj.pad(3) + ke(2) - 1 ;
      h = y2 - y1 + 1 ;
      w = x2 - x1 + 1 ;
      rfs.size = [h, w] ;
      rfs.stride = obj.stride ;
      rfs.offset = [y1+y2, x1+x2]/2 ;
    end
  end
end


function CPD = hhmmQ_CPD(bnet, self, Qnodes, d, D, varargin)
% HHMMQ_CPD Make the CPD for a Q node at depth D of a D-level hierarchical HMM
% CPD = hhmmQ_CPD(bnet, self, Qnodes, d, D, ...)
%
%  Fd(t-1) \   Q1:d-1(t)
%           \  |
%            \ v
%  Qd(t-1) -> Qd(t)
%            /
%           /
%  Fd+1(t-1)
%
% We assume parents are ordered (numbered) as follows:
% Qd(t-1), Fd+1(t-1), Fd(t-1), Q1(t), ..., Qd(t)
%
% The parents of Qd(t) can either be just Qd-1(t) or the whole stack Q1:d-1(t) (allQ)
% In either case, we will call them Qps.
% If d=1, Qps does not exist. Also, the F1(t-1) -> Q1(t) arc is optional.
% If the arc is missing, startprob does not need to be specified,
% since the toplevel is assumed to never reset (F1 does not exist).
% If d=D, Fd+1(t-1) does not exist (there is no signal from below).
%
% optional args [defaults]
%
% transprob - transprob(i,k,j) = prob transition from i to j given Qps = k ['leftright']
% selfprob  - prob of a transition from i to i given Qps=k [0.1]
% startprob - startprob(k,j) = prob start in j given Qps = k ['leftstart']
% startargs - other args to be passed to the sub tabular_CPD for learning startprob
% transargs - other args will be passed to the sub tabular_CPD for learning transprob
% allQ      - 1 means use all Q nodes above d as parents, 0 means just level d-1 [0]
% F1toQ1    - 1 means add F1(t-1) -> Q1(t) arc, 0 means level 1 never resets [0]
%
% For d=1, startprob(1,j) is only needed if F1toQ1=1
% Also, transprob(i,j) can be used instead of transprob(i,1,j).
%
% hhmmQ_CPD is a subclass of tabular_CPD so we inherit inference methods like CPD_to_pot, etc.
%
% We create isolated tabular_CPDs with no F parents to learn transprob/startprob
% so we can avail of e.g., entropic or Dirichlet priors.
% In the future, we will be able to represent the transprob using a tree_CPD.
%
% For details, see "Linear-time inference in hierarchical HMMs", Murphy and Paskin, NIPS'01.

ss = bnet.nnodes_per_slice;
%assert(self == Qnodes(d)+ss);
ns = bnet.node_sizes(:);
CPD.Qsizes = ns(Qnodes);
CPD.d = d;
CPD.D = D;
allQ = 0;

% find out which parents to use, to get right size
for i=1:2:length(varargin)
  switch varargin{i},
   case 'allQ', allQ = varargin{i+1};
  end
end

if d==1
  CPD.Qps = [];
else
  if allQ
    CPD.Qps = Qnodes(1:d-1);
  else
    CPD.Qps = Qnodes(d-1);
  end
end

Qsz = ns(self);
Qpsz = prod(ns(CPD.Qps));

% set default arguments
startprob = 'leftstart';
transprob = 'leftright';
startargs = {};
transargs = {};
CPD.F1toQ1 = 0;
selfprob = 0.1;

for i=1:2:length(varargin)
  switch varargin{i},
   case 'transprob', transprob = varargin{i+1};
   case 'selfprob',  selfprob = varargin{i+1};
   case 'startprob', startprob = varargin{i+1};
   case 'startargs', startargs = varargin{i+1};
   case 'transargs', transargs = varargin{i+1};
   case 'F1toQ1',    CPD.F1toQ1 = varargin{i+1};
  end
end

Qps = CPD.Qps + ss;
old_self = self-ss;

if strcmp(transprob, 'leftright')
  LR = mk_leftright_transmat(Qsz, selfprob);
  transprob = repmat(reshape(LR, [1 Qsz Qsz]), [Qpsz 1 1]); % transprob(k,i,j)
  transprob = permute(transprob, [2 1 3]); % now transprob(i,k,j)
end
transargs{end+1} = 'CPT';
transargs{end+1} = transprob;
CPD.sub_CPD_trans = mk_isolated_tabular_CPD([old_self Qps], ns([old_self Qps self]), transargs);
S = struct(CPD.sub_CPD_trans);
CPD.transprob = myreshape(S.CPT, [Qsz Qpsz Qsz]);

if strcmp(startprob, 'leftstart')
  startprob = zeros(Qpsz, Qsz);
  startprob(:,1) = 1;
end

if (d==1) & ~CPD.F1toQ1
  CPD.sub_CPD_start = [];
  CPD.startprob = [];
else
  startargs{end+1} = 'CPT';
  startargs{end+1} = startprob;
  CPD.sub_CPD_start = mk_isolated_tabular_CPD(Qps, ns([Qps self]), startargs);
  S = struct(CPD.sub_CPD_start);
  CPD.startprob = myreshape(S.CPT, [Qpsz Qsz]);
end

CPD = class(CPD, 'hhmmQ_CPD', tabular_CPD(bnet, self));

CPD = update_CPT(CPD);


% The COBRAToolbox: testMoomin.m
%
% Purpose:
%     - Tests the moomin function
%
% Author:
%     - Original file: Taneli Pusa 01/2020

global CBTDIR

% initialize the test
currentDir = pwd;
fileDir = fileparts(which('testMoomin.m'));
cd(fileDir);
testPath = pwd;

% glpk appears to be extremely slow with stoichiometric constraints
% gurobi needs further testing to ensure it works
solvers = prepareTest('needsMILP', true, 'requiredSolvers', {'ibm_cplex'}, 'excludeSolvers',...
	 {'glpk', 'gurobi'});

% load model and data for the test
inputModel = getDistributedModel('ecoli_core_model.mat');
load([testPath filesep 'testData_moomin.mat']);

nMets = size(inputModel.mets, 1);
nRxns = size(inputModel.rxns, 1);

% tolerance
tol = 1e-6;

% run tests with different solvers
for i=1:length(solvers.MILP)
	fprintf(' -- Running testMoomin using the solver interface: %s ... ', solvers.MILP{i});

	solverOK = changeCobraSolver(solvers.MILP{i}, 'MILP', 0);

	if solverOK
		% test stoichiometric version
		[outputModel, MILPsolutions, MILPproblem] = moomin(inputModel, expression, ...
			'enumerate', 5);

		% check that reading GPRs works (solver independent, only needs to be done once)
		if i==1
			assert(any(outputModel.inputColours));
		end

		% there should be only one solution
		assert(size(MILPsolutions, 1) == 2 && MILPsolutions{2, 1}.stat == 0);

		% check the MILP
		A = MILPproblem.A;
		x = MILPsolutions{1,1}.full;
		% stoichiometric balance
		assert(all(abs(A(1:nMets, 1:nRxns) * x(1:nRxns)) < tol));
		% binary variables worked as intended
		assert(all( abs((x(1:nRxns) > 0) - x(nRxns + 1:2 * nRxns)) < tol ));
		assert(all( abs((x(1:nRxns) < 0) - x(2 * nRxns + 1:end)) < tol ));

		% test the topological version
		[outputModel, MILPsolutions, MILPproblem] = moomin(inputModel, expression, ...
			'enumerate', 5, 'stoichiometry', 0);

		% there should be only one solution
		assert(size(MILPsolutions, 1) == 2 && MILPsolutions{2, 1}.stat == 0);

		% check the MILP
		A = MILPproblem.A;
		b = MILPproblem.b;
		x = MILPsolutions{1,1}.full;
		assert(all( A(1:nRxns + nMets, :) * x - b(1:nRxns + nMets) < tol ));
		assert(all( A(nRxns + nMets + 1:nRxns + 3 * nMets, :) * x -...
			b(nRxns + nMets + 1:nRxns + 3 * nMets) > -tol ));
	end
	fprintf('Done.\n');
end

% delete solver log files
delete('*.log');

cd(currentDir);


% MatrixUser, a multi-dimensional matrix analysis software package
% https://sourceforge.net/projects/matrixuser/
%
% The MatrixUser is a matrix analysis software package developed under Matlab
% Graphical User Interface Developing Environment (GUIDE). It features
% functions that are designed and optimized for working with multi-dimensional
% matrix under Matlab. These functions typically includes functions for
% multi-dimensional matrix display, matrix (image stack) analysis and matrix
% processing.
%
% Author:
%   Fang Liu <leoliuf@gmail.com>
%   University of Wisconsin-Madison
%   Aug-30-2014

function MU_funcMagnify(Temp,Event,handles)
handles = guidata(handles.MU_matrix_display);

MSize = inputdlg('Please specify the size of magnified region (pixels).','Specify Size',1,{'30'});
if isempty(MSize)
    warndlg('Magnifier was cancelled.');
    return;
end

try
    handles.MSize = str2double(MSize{1});
    A=handles.BMatrix(1:handles.MSize,1:handles.MSize); % quick test for size
    handles.MSize = max(2,handles.MSize - mod(handles.MSize,2)); % even value for size, minimum 2
    figure('Name', 'Instant Magnifier');
    imagesc(zeros(handles.MSize,handles.MSize));
catch me
    errordlg('Input size is invalid.');
    return;
end
handles.MHandle = get(gca,'Children');
colormap(handles.V.Color_map);
axis off;

guidata(handles.MU_matrix_display, handles);

end


%% THE OPTIMAL RECIPROCAL COLLISION AVOIDANCE (agent_2D_ORCA.m) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This agent class provides support for the RVO2 library for ORCA-based
% Reciprocal Velocity Obstacle collision avoidance.

% Author: James A. Douthwaite

classdef agent_2D_ORCA < agent_2D_RVO
%% INITIALISE THE AGENT SPECIFIC PARAMETERS
    properties
        % AGENT PARAMETERS
%         radius = 2;          % Declare (independant of the VO varients)
%         neighbourDist = 15;  %(s)
%         maxNeighbours = 10;
        timeHorizon     = 10;
        timeHorizonObst = 10;
        RVO_EPSILON     = 1E-5;

        % PARAMETERS UNIQUE TO RVO2
%         timeHorizon     = 10;
%         timeHorizonObst = 10;

        % ORCA-LINE CONTAINERS
        orcaLines = struct('point',[],'direction',[]);
    end
%%  CLASS METHODS
    methods
        % CONSTRUCTION METHOD
        function obj = agent_2D_ORCA(varargin)
            % This function is to construct the agent object using the
            % object defintions held in the 'objectDefinition' base class.
            % INPUTS:
            % namestr - Assigned name string
            % OUTPUTS:
            % obj     - The constructed object

            % CALL THE SUPERCLASS CONSTRUCTOR
            obj@agent_2D_RVO(varargin);

            % SENSORS
            % Sensor properties will be inherited from 'agent_2D_VO' and
            % 'agent_VO' unless explictly overridden here.

            % //////////////// Check for user overrides ///////////////////
            % - It is assumed that overrides to the properties are provided
            %   via the varargin structure.
            [obj] = obj.ApplyUserOverrides(varargin);
            % /////////////////////////////////////////////////////////////
        end
    end
    % //////////////////////// OPENMAS FUNCTIONS ///////////////////////////
    methods
        % MAPPING OF OPENMAS TO THE RVO LIBRARY
        function [headingVector,speed] = GetAvoidanceCorrection(obj,dt,desiredVelocity,visualiseProblem)
            % This function computes the mapping of the simulation inputs
            % for the RVO library functions and returns the calculated
            % optimal velocity.

            % AGENT KNOWLEDGE (2D)
            [p_i,v_i,r_i] = obj.GetAgentMeasurements();

            % PLOT INPUT VARIABLES
            inputDebugPlot = 0;
            if inputDebugPlot
                figureHandle = figure(1);
                hold on; grid on; box on;
                axis equal;
                [figureHandle] = obj.getObjectScene(knownObstacles,figureHandle);           % Plot the objects
            end

            % GET AGENT DATA
            agentIDs = [obj.MEMORY([obj.MEMORY.type] == OMAS_objectType.agent).objectID];

            % MOVE THROUGH THE PRIORITISED OBSTACLE SET
            agentNeighbours = cell(numel(agentIDs),1);
            agentNeighbourLength = 0;
            for item = 1:numel(agentIDs)
                % Get object data from memory structure
                p_j = obj.GetLastMeasurementByID(agentIDs(item),'position');
                v_j = obj.GetLastMeasurementByID(agentIDs(item),'velocity');
                r_j = obj.GetLastMeasurementByID(agentIDs(item),'radius');

                % NEIGHBOUR CONDITIONS
                neighbourConditionA = item < obj.maxNeighbours;            % Maximum number of neighbours
                neighbourConditionB = norm(p_j) < obj.neighbourDist;       % [CONFIRMED]
                neighbourConditionC = ~any(isnan(v_j));                    % Wait for a valid velocity reading
                if ~neighbourConditionB || ~neighbourConditionC
                    continue
                end

                p_j = p_j + p_i;
                v_j = v_j + v_i;

                % BUILD THE RVO AGENT DESCRIPTION
                agentRef = struct('position',p_j,...
                                  'velocity',v_j,...
                                  'unitDir',obj.nullVelocityCheck(v_j),...
                                  'radius',r_j,...
                                  'id',agentIDs(item));

                % BUILD LIST OF AGENTS
%                 agentNeighbours = horzcat(agentNeighbours,agentRef);
                agentNeighbours{item} = agentRef;
                agentNeighbourLength  = agentNeighbourLength + 1;
            end
            % REMOVE EMPTY CELLS
            agentNeighbours = agentNeighbours(~cellfun('isempty',agentNeighbours));

            % OBSTACLE DATA
            obstacleIDs  = [obj.MEMORY([obj.MEMORY.type] == OMAS_objectType.obstacle).objectID];

            % BUILD THE REQUIRED OBSTACLE STRUCTURES
            obstacleNeighbours = cell(numel(obstacleIDs),1);
            for item = 1:numel(obstacleIDs)
                % Get object data from memory structure
                p_j = obj.GetLastMeasurementByID(obstacleIDs(item),'position');
                v_j = obj.GetLastMeasurementByID(obstacleIDs(item),'velocity');

                % PROCESS THE OBSTACLE'S VERTICES
                obstacleRef.position = p_j + p_i;
                obstacleRef.velocity = v_j + v_i;
%               obstacleRef.geometry.vertices = obstacleRef.geometry.vertices + [agentPosition;0]';
                g_j = obj.GetLastMeasurementByID(obstacleIDs(item),'geometry');

                % [TO-DO] PASS THE OBSTACLE GEOMETRY
                obstacleNeighbours{item} = obj.createRVOobstacleVertexList(g_j);
            end
            % REMOVE EMPTY CELLS
            obstacleNeighbours = obstacleNeighbours(~cellfun('isempty',obstacleNeighbours));

            % COMPUTE THE AGENT'S NEW VELOCITY
            % This calls the libraries 'computeNewVelocity' function
            [obj,avoidanceVelocity] = obj.computeNewVelocity(dt,desiredVelocity,agentNeighbours,obstacleNeighbours);

            % SPECIAL CASE- VELOCITY MAGNITUDE IS ZERO
            speed = norm(avoidanceVelocity);
            headingVector = avoidanceVelocity/speed;
            if isnan(headingVector)
                headingVector = [1;0];  % Retain previous heading
            end

        end
    end
    %% FUNCTIONS IN THE "agent.cpp" FILE
    methods
        % COMPUTE THE AGENT VELOCITY
        function [obj,avoidanceVelocity] = computeNewVelocity(obj,dt,prefVelocity,agentNeighbours,obstacleNeighbours)
            % This function computes the new velocity of this agent from the
            % RVO2 library.

            % DEFINE THE AGENT PARAMETERS
            [p_a,v_a,r_a] = obj.GetAgentMeasurements();

            % MAP THE PREFERRED VELOCITY TO NEW FRAME
            prefVelocity = -prefVelocity;

            % RESET THE OCRA-LINES VECTOR FROM THE LAST TIMESTEP
            obj.orcaLines = [];

            % //////////// GET THE OBSTACLE ORCA LINES ////////////////////
            [obst_orcaLines] = obj.getObstacleORCAlines(p_a,v_a,r_a,obj.timeHorizonObst,obstacleNeighbours);
            obj.orcaLines = [obj.orcaLines,obst_orcaLines];
            numObstLines = numel(obj.orcaLines);

            % ////////////// GET THE AGENT ORCA LINES /////////////////////
            [agnt_orcaLines] = obj.getAgentORCAlines(dt,p_a,v_a,r_a,obj.timeHorizon,agentNeighbours);
            obj.orcaLines = [obj.orcaLines;agnt_orcaLines];

            % //////////////// DEBUG PLOTS ////////////////////////////////
%             if ~isempty(obj.orcaLines)
%                 % PLOT THE ORCA-LINES
%                 h = figure(1);
%                 ax = axes(h);
%                 hold on; grid on; box on;
%                 axis equal;
%
%                 g_a = obj.GEOMETRY.vertices + [p_a;0]';
%                 patch(ax,...
%                     'Faces',obj.GEOMETRY.faces,...
%                     'Vertices',g_a);
%
%                 % The objects vertices
%                 objectVertices = obstacleNeighbours{1};
%                 for i = 1:numel(objectVertices)
%                     scatter(ax,objectVertices(i).point(1),objectVertices(i).point(2),'r*');
%                 end
%
%                 for i = 1:numel(obj.orcaLines)
%                     lineVar = obj.orcaLines(i);
%                     len = 5;
%                     X = [lineVar.point(1);len*lineVar.direction(1)];
%                     Y = [lineVar.point(2);len*lineVar.direction(2)];
%                     plot(ax,X,Y,'b','marker','o','lineWidth',2);
%                 end
%                 close(h);
%             end

            % CALL THE SECOND LINEAR PROGRAM
            [obj,lineObjFail,optimalVelocity] = obj.linearProgram2(obj.orcaLines,obj.v_max,prefVelocity,logical(false));

            % IF SOME ORCA-LINES FAIL
            % This function will generate direction based ORCA lines where
            % the previous lines failed.
            if lineObjFail < numel(obj.orcaLines)
                [obj,optimalVelocity] = obj.linearProgram3(obj.orcaLines,numObstLines,lineObjFail,obj.v_max,optimalVelocity);
            end
            % REASSIGN OUTPUT VELOCITY
%             avoidanceVelocity = optimalVelocity;
            avoidanceVelocity = -optimalVelocity;
        end
        % PROCESS THE VERTEX SET
        function [vertexSet] = createRVOobstacleVertexList(obj,observedObject)
            % The obstacle consideration expects the obstacles to be of
            % polygonal form. Each obstacle is defined by a subset of
            % points that resemble its vertices.

            % We assume that each obstacle defines a set of obstacle
            % vertices. In the RVO2 library, the assumption is made
            % that the vertices are given in counter-clockwise order.

            %/*
            %* Add (polygonal) obstacles, specifying their vertices in counterclockwise
            %* order.
            %*/

            % THE OBSTACLES VERTICES
            observedVertices = observedObject.geometry.vertices;

            % GET THE VERTICES PROJECTED ONTO THE X/Y PLANE
            [planarVertices] = OMAS_graphics.geometryPlanarProjection([0;0;1],...
                                                                      observedObject.position,...
                                                                      observedVertices);
            % SORT THE VERTICES CCW ABOUT THE +ve Z-AXIS
            [verticesCCW] = OMAS_graphics.sortVerticesCCW([0;0;1],...
                                                          observedObject.position,...
                                                          planarVertices);
            % GETTING THE 2D VERTEX SET
            if size(verticesCCW,2) > 2
               verticesCCW = verticesCCW(:,1:2);
            end
            % BUILD THE RVO POLY-GONAL OSTACLE DESCRIPTION
            [vertexSet] = obj.defineRVOVertexObstacleSet(verticesCCW);
        end
        % GET THE OBSTACLE ORCA-LINE VECTOR % <<<<<<<<< THE FINAL PROBLEM
        function [obst_orcalines] = getObstacleORCAlines(obj,pa,va,ra,timeHorizonObst,obstacleNeighbours)
            % This function calculates the the orca-lines for static
            % obstacles using the prinicples of the VO.
            % INPUTS:
            % obj - Agent self-reference
            % dt  - The simulation timestep

            % The obstacle neighbour list is composed of the discrete
            % vertex sets. Each neighbour is a set of vertices with their
            % links to each other specified.

            invTimeHorizonObst = 1/timeHorizonObst;

            % ORCA-LINE CONTAINER
            obst_orcalines = [];

            if isempty(obstacleNeighbours)
                return
            end

            % FOR EACH NEIGHBOURING OBSTACLE
            for n = 1:numel(obstacleNeighbours)
                % THE SUBSET OF VERTICES
                obstacleVertexSet = obstacleNeighbours{n};
                % [JD] - Small notation change. The obstacleVertexSet is
                % now a list of structures that defines the relationship
                % between each vertex.

                % FOR EACH VERTEX IN THE NEIGHBOUR SET
                for i = 1:numel(obstacleVertexSet)
                    % GET THE OBSTACLE
                    obstacle1 = obstacleVertexSet(i);
                    obstacle2 = obstacleVertexSet(obstacle1.nextObstacle);
                    % RELATIVE POSITION OF THE POINTS
                    relativePosition1 = obstacle1.point - pa;
                    relativePosition2 = obstacle2.point - pa;

                    % Check if velocity obstacle of obstacle is already taken care of by
                    % previously constructed obstacle ORCA lines.
                    alreadyCovered = logical(false);

                    if ~isempty(obst_orcalines)
                        for j = 1:numel(obj.orcaLines)
                            conditionA = obj.det(invTimeHorizonObst*relativePosition1 - obst_orcalines(j).point, obst_orcalines(j).direction) - invTimeHorizonObst*ra >= -obj.RVO_EPSILON;
                            conditionB = obj.det(invTimeHorizonObst*relativePosition2 - obst_orcalines(j).point, obst_orcalines(j).direction) - invTimeHorizonObst*ra >= -obj.RVO_EPSILON;
                            if conditionA && conditionB
                                alreadyCovered = logical(true);
                                break;
                            end
                        end
                    end
                    % If covered, move to the next obstacle
                    if alreadyCovered
                        continue
                    end

                    % Obstacle is not yet covered, check for collisions
                    distSq1 = obj.absSq(relativePosition1);
                    distSq2 = obj.absSq(relativePosition2);

                    radiusSq = ra^2;

                    obstacleVector = obstacle2.point - obstacle1.point;
                    s = obj.dot(-relativePosition1,obstacleVector)/obj.absSq(obstacleVector);
                    distSqlineObj = obj.absSq(-relativePosition1 - s*obstacleVector);

                    % ////////////// CREATE ORCA (lineObj) ////////////////
                    lineObj = struct('point',[],'direction',[]);

                    if s < 0 && distSq1 <= radiusSq
                        % Collision with left vertex. Ignore if non-convex
                        if obstacle1.isConvex
                            lineObj.point = [0;0];
                            direction = [-relativePosition1(2);relativePosition1(1)];  % [y;x]
                            lineObj.direction = obj.normalise(direction);
                            obst_orcalines = [obst_orcalines;lineObj];
                        end
                        continue
                    elseif s > 1 && distSq2 <= radiusSq
                        % Collision with right vertex. Ignore if non-convex or
                        % if it will be taken care of by neighbouring obstacle.
                        if obstacle2.isConvex && obj.det(relativePosition2,obstacle2.unitDir) >= 0
                            lineObj.point = [0;0];
                            direction = [-relativePosition2(2);relativePosition2(1)];
                            lineObj.direction = obj.normalise(direction);
                            obst_orcalines = [obst_orcalines;lineObj];
                        end
                        continue
                    elseif s >= 0 && s < 1 && distSqlineObj <= radiusSq
                        % Collision with obstacle segment
                        lineObj.point = [0;0];
                        lineObj.direction = -obstacle1.unitDir;
                        obst_orcalines = [obst_orcalines;lineObj];
                        continue
                    end

%                     /*
%                      * No collision.
%                      * Compute legs. When obliquely viewed, both legs can come from a single
%                      * vertex. Legs extend cut-off line when nonconvex vertex.
%                      */

                    if s < 0 && distSqlineObj <= radiusSq
                        % Obstacle viewed obliquely so that left vertex defines
                        % velocity obstacle
                        if ~obstacle1.isConvex
                            continue;   % Ignore obstacle
                        end
                        obstacle2 = obstacle1;

                        leg1 = sqrt(distSq1 - radiusSq);
                        leftLegDirection  = [relativePosition1(1)*leg1 - relativePosition1(2)*ra;  relativePosition1(1)*ra + relativePosition1(2)*leg1] / distSq1;
                        rightLegDirection = [relativePosition1(1)*leg1 + relativePosition1(2)*ra; -relativePosition1(1)*ra + relativePosition1(2)*leg1] / distSq1; % Assuming [x;y]
                    elseif s > 1 && distSqlineObj <= radiusSq
                        % Obstacle viewed obliquely so that right vertex defines
                        % velocity obstacle.
                        if ~obstacle2.isConvex
                            % Ignore obstacle
                            continue
                        end
                        obstacle1 = obstacle2;

                        leg2 = sqrt(distSq2 - radiusSq);
                        leftLegDirection  = [relativePosition2(1)*leg2 - relativePosition2(2)*ra;  relativePosition2(1)*ra + relativePosition2(2)*leg2] / distSq2;
                        rightLegDirection = [relativePosition2(1)*leg2 + relativePosition2(2)*ra; -relativePosition2(1)*ra + relativePosition2(2)*leg2] / distSq2; % Assuming [x;y]
                    else
                        % Unusual situation
                        if obstacle1.isConvex
                            leg1 = sqrt(distSq1 - radiusSq);
                            leftLegDirection = [relativePosition1(1)*leg1 - relativePosition1(2)*ra; relativePosition1(1)*ra + relativePosition1(2)*leg1] / distSq1;
                        else
                            % Left vertex non-convex; left leg extends cut-off
                            % lineObj
                            leftLegDirection = -obstacle1.unitDir;
                        end

                        if obstacle2.isConvex
                            leg2 = sqrt(distSq2 - radiusSq);
                            rightLegDirection = [relativePosition2(1)*leg2 + relativePosition2(2)*ra; -relativePosition2(1)*ra + relativePosition2(2)*leg2] / distSq2;
                        else
                            % Right vertex non-convex; right leg extends cut-off
                            % lineObj
                            rightLegDirection = obstacle1.unitDir;
                        end
                    end

                    % Legs can never point into neighbouring edge when convex
                    % vertex, take cutoff-lineObj of neighbouring edge instead. If
                    % velocity projected on "foreign" leg, no constraint is
                    % added.

                    leftNeighbour = obstacleVertexSet(obstacle1.prevObstacle);
                    isLeftLegForeign  = logical(false);
                    isRightLegForeign = logical(false);

                    if obstacle1.isConvex && obj.det(leftLegDirection,-leftNeighbour.unitDir) >= 0
                        % Left leg points into obstacle.
                        leftLegDirection = -leftNeighbour.unitDir;
                        isLeftLegForeign = logical(true);
                    end
                    if obstacle2.isConvex && obj.det(rightLegDirection, obstacle2.unitDir) <= 0
                        % Right leg points into obstacle.
                        rightLegDirection = obstacle2.unitDir;
                        isRightLegForeign = logical(true);
                    end
                    % COMPUTE CUT-OFF CENTERS
                    leftCutoff  = invTimeHorizonObst*(obstacle1.point - pa);
                    rightCutoff = invTimeHorizonObst*(obstacle2.point - pa);
                    cutoffVec   = rightCutoff - leftCutoff;

                    % Project current velocity on velocity obstacle.

                    % Check if current velocity is projected on cutoff circles
                    if obstacle1.id == obstacle2.id
                        t = 0.5;
                    else
                        t = obj.dot((va - leftCutoff),cutoffVec) / obj.absSq(cutoffVec);
                    end

                    tLeft  = obj.dot((va - leftCutoff),leftLegDirection);
                    tRight = obj.dot((va - rightCutoff),rightLegDirection);

                    if ((t < 0 && tLeft < 0) || (obstacle1.id == obstacle2.id && tLeft < 0 && tRight < 0))
                        % Project on left cut-off circle.
                        unitW = obj.normalise(va - leftCutoff);

                        lineObj.direction = [unitW(2); -unitW(1)];
                        lineObj.point = leftCutoff + ra*invTimeHorizonObst*unitW;
                        obst_orcalines = [obst_orcalines;lineObj];
                        continue
                    elseif (t > 1 && tRight < 0)
                        % Project on right cut-off circle.
                        unitW = obj.normalise(va - rightCutoff);

                        lineObj.direction = [unitW(2); -unitW(1)];
                        lineObj.point = rightCutoff + ra*invTimeHorizonObst*unitW;
                        obst_orcalines = [obst_orcalines;lineObj];
                        continue;
                    end

                    % Project on left leg, right leg, or cut-off lineObj, whichever is closest
                    % to velocity.
                    % CALCULATE distSqCutoff
                    if t < 0 || t > 1 || obstacle1.id == obstacle2.id
                        distSqCutOff = inf;
                    else
                        distSqCutOff = obj.absSq(va - (leftCutoff + t*cutoffVec));
                    end

                    % CALCULATE distSqLeft
                    if (tLeft < 0)
                        distSqLeft = inf;
                    else
                        distSqLeft = obj.absSq(va - (leftCutoff + tLeft*leftLegDirection));
                    end
                    % CALCULATE distSqRight
                    if (tRight < 0)
                        distSqRight = inf;
                    else
                        distSqRight = obj.absSq(va - (rightCutoff + tRight*rightLegDirection));
                    end

                    if distSqCutOff <= distSqLeft && distSqCutOff <= distSqRight
                        % Project on cut-off lineObj.
                        lineObj.direction = -obstacle1.unitDir;
                        lineObj.point = leftCutoff + ra*invTimeHorizonObst*[-lineObj.direction(2); lineObj.direction(1)];
                        obst_orcalines = [obst_orcalines;lineObj];
                        continue;
                    elseif distSqLeft <= distSqRight
                        % Project on left leg.
                        if isLeftLegForeign
                            continue
                        end
                        lineObj.direction = leftLegDirection;
                        lineObj.point = leftCutoff + ra*invTimeHorizonObst*[-lineObj.direction(2); lineObj.direction(1)];
                        obst_orcalines = [obst_orcalines;lineObj];
                        continue;
                    else
                        % Project on right leg. */
                        if isRightLegForeign
                            continue;
                        end
                        lineObj.direction = -rightLegDirection;
                        lineObj.point = rightCutoff + ra*invTimeHorizonObst*[-lineObj.direction(2); lineObj.direction(1)];
                        obst_orcalines = [obst_orcalines;lineObj];
                        continue;
                    end

                end
            end
        end
        % GET THE AGENT ORCA-LINE VECTOR
        function [agnt_orcalines] = getAgentORCAlines(obj,dt,pa,va,ra,timehorizon,agentNeighbours)
            % This function computes the orca lines using the principles of
            % the RVO method.
            % INPUTS:
            % obj - Agent reference, for use of local functions.
            % dt  - The simulation timestep.
            % pa  - The agents global position
            % va  - The agents global velocity
            % ra  - The agents characteristic radius
            % timeHorizon - The time for which avoidance should be assured.
            % agentNeighbours - The list of considered agent-neighbours.

            % ORCA-LINE CONTAINER
            agnt_orcalines = [];

            if isempty(agentNeighbours)
                return
            end

            % BEGIN CREATING ORCA LINES FOR THE AGENTS
            invTimeHorizon  = 1/timehorizon;

            for i = 1:numel(agentNeighbours)
                % CALCULATE THE OTHER AGENT PROPERTIES
%                 other = agentNeighbours(i);
                %                     relativePosition = other.position - pa;
                %                     relativeVelocity = va - other.velocity;
%                 agentNeighbours{i}
                relativePosition = -(agentNeighbours{i}.position - pa);
                relativeVelocity = -(va - agentNeighbours{i}.velocity);

                distSq = obj.dot(relativePosition,relativePosition);
                combinedRadius = ra + agentNeighbours{i}.radius;
                combinedRadiusSq = combinedRadius^2;

                % CREATE ORCA lineObj
                lineObj = struct('point',[],'direction',[]);

                % NO COLLISION
                if distSq > combinedRadiusSq
                    w = relativeVelocity - invTimeHorizon*relativePosition; % Vector from cutoff center to relative velocity
                    wLengthSq   = obj.dot(w,w); % Square length
                    dotProduct1 = obj.dot(w,relativePosition);

                    if dotProduct1 < 0 && dotProduct1^2 > combinedRadiusSq*wLengthSq
                        % Project on cut-off circle
                        wLength = sqrt(wLengthSq);
                        unitW = w/wLength;

                        lineObj.direction = [unitW(2); -unitW(1)];
                        u = (combinedRadius*invTimeHorizon - wLength)*unitW;

                    else
                        % Project on legs
                        leg = sqrt(distSq - combinedRadiusSq);

                        if obj.det(relativePosition, w) > 0
                            % Project on left leg
                            lineObj.direction = ([relativePosition(1)*leg - relativePosition(2)*combinedRadius;
                                relativePosition(1)*combinedRadius + relativePosition(2)*leg]) / distSq;
                        else
                            % Project on right leg
                            lineObj.direction = -([relativePosition(1)*leg + relativePosition(2)*combinedRadius;
                                -relativePosition(1)*combinedRadius + relativePosition(2)*leg]) / distSq;
                        end

                        dotProduct2 = obj.dot(relativeVelocity,lineObj.direction);
                        u = dotProduct2*lineObj.direction - relativeVelocity;
                    end
                    % COLLISION
                else
                    % Collision. Project on cut-off circle of time timeStep.
                    invTimeStep = 1/dt;
                    % Vector from cutoff center to relative velocity. */
                    w = relativeVelocity - invTimeStep*relativePosition;
                    wLength = sqrt(obj.dot(w,w));
                    unitW = w/wLength;

                    lineObj.direction = [unitW(2);-unitW(1)];
                    u = (combinedRadius*invTimeStep - wLength)*unitW;
                end
                % lineObj.point = va + 0.5*u;  % Equation 5 (definition of ORCA_AB)
                lineObj.point  = -va + 0.5*u;  % Equation 5 (definition of ORCA_AB)
                agnt_orcalines = [agnt_orcalines;lineObj];
            end
        end

        % LINEAR PROGRAM ONE (CORRECT)
        function [obj,flag,result_LP1] = linearProgram1(obj,lineVector,lineNo,constraintRadius,optVelocity,directionOpt,currentResult)
            % Solves a one-dimensional lineObjar program on a specified lineObj
            % subject to linear constraints defined by lineObjs and a circular
            % constraint.
            % INPUTS:
            % obj          - The agent object
            % lineObjs        - A vector of lineObjs defining the lineObjar constraints.
            % lineObjNo       - The specified lineObj constraint
            % radius       - The radius of the circulat constraint.
            % optVelocity  - The optimization velocity.
            % directionOpt - True if the direction should be optimized.
            % result       - A reference to the result of the lineObjar program.
            % OUTPUT:
            % flag - True if successful.

            % THE PROBLEM IS THAT THE INDEX NUMBER 'lineNo' is different
            % between linearProgram1 and linearProgram2

            dotProduct   = dot(lineVector(lineNo).point,lineVector(lineNo).direction);
            discriminant = dotProduct^2 + constraintRadius^2 - dot(lineVector(lineNo).point,lineVector(lineNo).point);

            % Check if max speed circle fully invalidates lineObj constraint "lineObjNo"
            if discriminant < 0
                flag = logical(false);
                result_LP1 = currentResult;
                return
            end

            sqrtDiscriminant = sqrt(discriminant);
            tLeft  = -dotProduct - sqrtDiscriminant;
            tRight = -dotProduct + sqrtDiscriminant;

            % CHANGED TO PREVENT LOOP FROM ENTERING WHEN THE FAILURE OCCURS
            % ON THE FIRST ORCA-LINE
            i = 1;
            while (i < lineNo)
                denominator = obj.det(lineVector(lineNo).direction,lineVector(i).direction);
                numerator   = obj.det(lineVector(i).direction,(lineVector(lineNo).point - lineVector(i).point));

                if sqrt(dot(denominator,denominator)) <= obj.RVO_EPSILON    % The norm is less than the RVO constant
                    % lineObjs "lineObjNo" and "i" are (almost) parallel.
                    if numerator < 0
                        flag = logical(false);
                        result_LP1 = currentResult;
                        return
                    else
                        i = i + 1;                                          % Increment the counter before moving to next loop
                        continue
                    end
                end

                t = numerator/denominator;
                if denominator >= 0
                    tRight = min([tRight;t]);                               % lineObj "i" bounds lineObj "lineObjNo" on the right
                else
                    tLeft = max([tLeft,t]);                                 % lineObj "i" bounds lineObj "lineObjNo" on the left
                end

                if tLeft > tRight
                    flag = logical(false);
                    result_LP1 = currentResult;
                    return
                end
                i = i + 1;                                                  % Increment the counter before moving to next loop
            end

            % OPTIMISE DIRECTION & VELOCITY
            if directionOpt
                % Optimize direction
                if dot(optVelocity,lineVector(lineNo).direction) > 0
                    % Take right extreme
                    currentResult = lineVector(lineNo).point + tRight*lineVector(lineNo).direction;
                else
                    % Take left extreme
                    currentResult = lineVector(lineNo).point + tLeft*lineVector(lineNo).direction;
                end
            else
                % Optimise closest point
                t = dot(lineVector(lineNo).direction,(optVelocity - lineVector(lineNo).point));
                if t < tLeft
                    currentResult = lineVector(lineNo).point + tLeft*lineVector(lineNo).direction;
                elseif t > tRight
                    currentResult = lineVector(lineNo).point + tRight*lineVector(lineNo).direction;
                else
                    currentResult = lineVector(lineNo).point + t*lineVector(lineNo).direction;
                end
            end
            % DEFINE OUTPUT PARAMETERS
            result_LP1 = currentResult;
            flag = logical(true);

%             obj.LP1 = currentResult; % <<< REMOVE ME LATER
        end
        % LINEAR PROGRAM TWO
        function [obj,lineObjFlag,result_LP2]  = linearProgram2(obj,lineVector,constraintRadius,optVelocity,directionOpt)
            % Solves a two-dimensional lineObjar program subject to lineObjar
            % constraints defined by lineObjs and a circular constraint.
            % INPUTS:
            % obj          - The agent object
            % lineObjs        - lineObjs defining the lineObjar constraints.
            % radius       - The radius of the circular constraint.
            % optVelocity  - The optimization velocity.
            % directionOpt - True if the direction should be optimized.
            % result       - A reference to the result of the lineObjar program.
            % OUTPUT:
            % size_t       - The number of the lineObj it fails on, and the number of lineObjs if successful.

            if directionOpt
                % Optimise direction. Noe that the optimzation velocity is
                % of unit length in this case.
                currentResult = optVelocity*constraintRadius;
            elseif dot(optVelocity,optVelocity) > constraintRadius^2
                % Optimize closest point and ouside circle.
                currentResult = obj.normalise(optVelocity)*constraintRadius;
            else
                % Optimise closest point and inside circle
                currentResult = optVelocity;
            end

            % MOVE THROUGH THE lineObjS AND COMPARE THE lineObjAR CONSTRAINTS
            for i = 1:numel(lineVector)

                if obj.det(lineVector(i).direction,(lineVector(i).point - currentResult)) > 0
                    % "result" does not satisfy constraint "i". Compute new optimal "result".
                    tempResult = currentResult;

                    % EVALUATE lineObj USING
                    [obj,flag,currentResult] = obj.linearProgram1(lineVector,i,constraintRadius,optVelocity,directionOpt,currentResult);

                    if ~flag
                        % RETURN THE TEMP RESULT
                        result_LP2 = tempResult;
                        lineObjFlag = i;
                        return
                    end
                end

            end

            % RETURN THE FINAL RESULT OF THE LINEAR PROGRAM
            result_LP2 = currentResult;
            % RETURN THE TOTAL NUMBER OF LINES
            lineObjFlag = numel(lineVector); % The number of the line it fails on, and the number of lines if successful.
        end
        % LINEAR PROGRAM THREE
        function [obj,result_LP3] = linearProgram3(obj,lineVector,numObstLines,beginLine,constraintRadius,currentResult)
           % Solves a two-dimensional lineObjar program subject to lineObjar
           % constraints defined by lineObjs and a circular constraint.
           % INPUTS:
           % obj             - The agent object
           % lineObjs        - Linears defining the lineObjar constraints.
           % numObstlineObjs - Count of obstacle lineObjs.
           % beginlineObj    - The lineObj on which the 2-d linear program failed.
           % radius          - The radius of the circular constraint.
           % result          - A reference to the result of the linear program.

           % INITIAL DISTANCE TO "BEST CASE" POINT
           distance = 0;

           % IF THERE ARE NO OBSTACLES, ONLY AGENTS
           if numObstLines == 0
               numObstLines = 1; % Correct for c++ indexing
           end

           for i = beginLine:numel(lineVector)

               if obj.det(lineVector(i).direction,(lineVector(i).point - currentResult)) > distance
                   % Results does not satisfy constraint of lineObj 1
                   projlineObjs = [];
                   for j = numObstLines:i
                       % CREATE LINE OBJECT
                       newLine = struct('point',[],'direction',[]);

                       determinant = obj.det(lineVector(i).direction,lineVector(j).direction);
                       if sqrt(dot(determinant,determinant)) <= obj.RVO_EPSILON
                           % lineObj "i" and lineObj "j" are parallel.
                           if dot(lineVector(i).direction,lineVector(j).direction) > 0
                               % lineObj "i" and lineObj "j" point in the same
                               % direction
                               continue;
                           else
                               % lineObj "i" and lineObj "j" point in opposite
                               % directions
                               newLine.point = 0.5*(lineVector(i).point + lineVector(j).point);
                           end
                       else
                           newLine.point = lineVector(i).point + (obj.det(lineVector(j).direction,...
                                          (lineVector(i).point - lineVector(j).point)) / determinant) * lineVector(i).direction;
                       end

%                        newLine.direction = obj.normalise(lineVector(j).direction - lineVector(i).direction);
                       direction = lineVector(j).direction - lineVector(i).direction;
                       newLine.direction = direction/norm(direction);
                       projlineObjs = [projlineObjs;newLine];              % Append the new line to the projection lines
                   end
                   % HOLD ONTO THE CURRENT RESULT AS THE TEMP RESULT

                   tempResult = currentResult;

                   flowVector = [-lineVector(i).direction(2); lineVector(i).direction(1)]; % Vector in the best direction "going with the flow"

                   % RE-EVALUATE THE NEW LINE OBJECTS
                   [obj,lineObjFlag,currentResult] = obj.linearProgram2(projlineObjs,constraintRadius,flowVector,logical(true));
%                    obj.LP2 = currentResult;
                   if lineObjFlag < numel(projlineObjs)
                       % This should in principle not happen.  The result is by definition
                       % already in the feasible region of this linear program. If it fails,
                       % it is due to small floating point error, and the current result is
                       % kept.
                       currentResult = tempResult;
                   end
                   % The distance to the new optimal flow line
                   distance = obj.det(lineVector(i).direction,(lineVector(i).point - currentResult));
               end
           end
           % REDEFINE THE RESULT
           result_LP3 = currentResult;
%            obj.LP3 = result_LP3;
       end
    end
    %/////////////////////// "agent.cpp" FUNCTIONS ////////////////////////
    methods (Static)
        % AGENT UPDATE FUNCTION (CORRECT)
        function [position,velocity] = update(position,newVelocity)
            % INPUT HANDLING
            assert(numel(position) == numel(newVelocity),'Position and velocity updates must be of the same dimensions.');
            % UPDATE THE AGENT'S VELOCITY
            velocity = newVelocity;
            position = position + dt*velocity; % Update in accordance to the RVO2 library
        end
    end

    % ////////////////////// "vector_2.h" FUNCTIONS ///////////////////////
    methods (Static)
        % COMPUTES THE LENGTH OF A 2D VECTOR
        function [absLength] = abs(v)
           absLength = sqrt(dot(v,v));
        end
        % RETURN THE SQUARE LENGTH OF A 2D VECTOR
        function [absSq] = absSq(v)
           absSq = dot(v,v);
        end
        % COMPUTE THE DETERMININANT OF A 2D SQUARE MATRIX
        function [det_uv] = det(u,v)
            det_uv = u(1)*v(2) - u(2)*v(1);
        end
        % THE DOT PRODUCT
        function [dot_uv] = dot(u,v)
           dot_uv = u(1)*v(1) + u(2)*v(2);
        end
        % NORMALIZE A VECTOR
        function [unitVector] = normalise(v)
           unitVector = v/sqrt(dot(v,v));
        end
    end

    %/////////////////////// HELPER FUNCTIONS [JD] ////////////////////////
    methods (Static)
        % BUILD THE VERTEX-OBSTACLE LIST
        function [vertexSet] = defineRVOVertexObstacleSet(vertexData)
            % This function takes a list of points and contstucts a linked
            % obstacle list description. The vertices are assumed ordered
            % in terms of their connectivity (i.e. clockwise).

            % VERTEX STRUCTURE
            obstacleTemp = struct(...
                'point',zeros(2,1),...
                'nextObstacle',0,...
                'prevObstacle',0,...
                'unitDir',zeros(2,1),...
                'isConvex',boolean(true),...
                'id',0);

            numberOfVertices = size(vertexData,1);

            % LIST OF OBSTACLES
            vertexSet = repmat(obstacleTemp,numberOfVertices, 1 );
            % MOVE THROUGH THE VERTEX SET
            for i = 1:numberOfVertices
                % INDEX OF i+1
                ind_i_plus = mod(i,numberOfVertices) + 1;
                ind_i_plus2 = mod(i+1,numberOfVertices) + 1;
                % ALLOCATE POINT
                vertexSet(i).point = vertexData(i,:)';                     % Assign obstacle
                % SET NEXT AND PREVIOUS OBSTACLES/VERTEX POINTS
                vertexSet(i).nextObstacle = ind_i_plus;
                vertexSet(ind_i_plus).prevObstacle = i;
                % UNIT DIRECTION
                lineDirection = vertexData(ind_i_plus,:)' - vertexData(i,:)';
                vertexSet(i).unitDir = OMAS_geometry.unit(lineDirection);
                % IS CONVEX
                if numberOfVertices == 2
                    vertexSet(ind_i_plus).isConvex = 1;
                else
                    % CHECK IF NEXT POINT IS ON LEFT (IS CONVEX)
                    isLeft = OMAS_geometry.leftOf(vertexData(i,:),vertexData(ind_i_plus,:),vertexData(ind_i_plus2,:));
                    % IS POINT LEFT OF
                    vertexSet(ind_i_plus).isConvex = isLeft <= 0;          % If list is clockwise "<=0" , else list is not ">=0"
                end
                % SET THE INDEX OF POINT
                vertexSet(i).id = i;
            end
        end
    end
end


function [ clmParams, pdm_mouth] = Load_CLM_params_inner()
%LOAD_CLM_PARAMS_WILD Summary of this function goes here
%   Detailed explanation goes here
    clmParams.window_size = [19,19;];
    clmParams.numPatchIters = size(clmParams.window_size,1);

    % the PDM created from in the wild data
    pdmLoc = ['../models/hierarch_pdm/pdm_51_inner.mat'];

    load(pdmLoc);

    pdm_mouth = struct;
    pdm_mouth.M = double(M);
    pdm_mouth.E = double(E);
    pdm_mouth.V = double(V);

    % the default model parameters to use
    clmParams.regFactor = 2.5;
    clmParams.sigmaMeanShift = 1.75;
    clmParams.tikhonov_factor = 2.5;

    clmParams.startScale = 1;
    clmParams.num_RLMS_iter = 5;
    clmParams.fTol = 0.01;
    clmParams.useMultiScale = true;
    clmParams.use_multi_modal = 1;

end


function F = diff(F, dim, n)
%DIFF   Componentwise derivative of a DISKFUNV.
%   DIFF(F) is the derivative of each component of F in
%   x-direction.
%
%   DIFF(F, DIM) is the first derivative of F along the
%   dimension DIM.
%     DIM = 1 (default) is the derivative in the x-direction.
%     DIM = 2 is the derivative in the y-direction.
%
%   DIFF(F, DIM, N) is the Nth derivative each component of F
%   along the dimension specified.
%
% See also CURL and DIV

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

% Empty check:
if ( isempty( F ) )
    return
end

% Default to x-derivative:
if ( nargin == 1 || isempty(dim) )
    dim = 1;
end

% Default to the first derivative:
if ( nargin < 3 )
    n = 1;
end

% Differentiate each component:
F.components{1} = diff(F.components{1}, dim, n);
F.components{2} = diff(F.components{2}, dim, n);

end


function [Dtf, Dtph] = spm_eeg_tf(S)
% Compute instantaneous power and phase in peri-stimulus time and frequency
% FORMAT [Dtf, Dtph] = spm_eeg_tf(S)
%
% S                     - input structure
%
% fields of S:
%   S.D                 - MEEG object or filename of M/EEG mat-file with
%
%   S.channels          - cell array of channel names. Can include generic
%                         wildcards: 'All', 'EEG', 'MEG' etc.
%
%   S.frequencies      - vector of frequencies of interest
%
%   S.timewin          - time window of interest in PST in ms.
%
%   S.method           - name for the spectral estimation to use. This
%                        corresponds to the name of a plug-in function that comes
%                        after 'spm_eeg_specest_' prefix.
%   S.settings         - plug-in specific settings
%
%   S.phase            - also save phase dataset (1) or not (0)
%                        phase dataset cannot be computed for some
%                        spectral estimation methods
%   S.prefix           - prefix added before the standard prefix (tf_ or tph_)
%
% Output:
% Dtf                   - M/EEG object with power (also written on disk)
% Dtph                  - M/EEG object with phase (also written on disk)
%__________________________________________________________________________
% This is a modular function for which plugins can be developed implementing
% specific spectral estimation methods. There are 3 basic plugins presently
% implemented and they can be used as templates for new plugins.
% The name of a plugin function should start with 'spm_eeg_specest_'
%
% morlet (spm_eeg_specest_morlet) - Morlet wavelet transform
%
% hilbert (spm_eeg_specest_hilbert) - filtering + Hilbert transform
%
% ft_mtmconvol (spm_eeg_specest_mtmconvol) - Fieldtrip implementation
%                                           of multi-taper spectral
%                                           analysis
%__________________________________________________________________________
% Copyright (C) 2010 Wellcome Trust Centre for Neuroimaging

% Vladimir Litvak
% $Id: spm_eeg_tf.m 7449 2018-10-16 13:52:04Z vladimir $

SVNrev = '$Rev: 7449 $';

%-Startup
%--------------------------------------------------------------------------
spm('FnBanner', mfilename, SVNrev);
spm('FigName','M/EEG Time-Frequency'); spm('Pointer','Watch');

%-Configure the analysis
%--------------------------------------------------------------------------
if ~isfield(S, 'channels'),   S.channels = 'all';                 end
if ~isfield(S, 'timewin'),    S.timewin  = [-Inf Inf];            end
if ~isfield(S, 'phase'),      S.phase    = 0;                     end
if ~isfield(S, 'prefix'),     S.prefix   = '';                    end
if ~isfield(S, 'frequencies') || isempty(S.frequencies)
    S.frequencies = 1:48;
end
if ~isfield(S, 'method')
    S.method = 'morlet';
    S.settings = [];
end

D = spm_eeg_load(S.D);

chanind = D.selectchannels(S.channels);

if isempty(chanind)
    error('No channels selected.');
end

if isfield(S, 'settings')
    S1 = S.settings;
else
    S1 = [];
end

timeind = D.indsample(1e-3*min(S.timewin)):D.indsample(1e-3*max(S.timewin));

% remove uninetended non-uniformities in the frequency axis
frequencies = S.frequencies;

if length(frequencies)>1
    df = unique(diff(frequencies));
    if length(df) == 1 || (max(diff(df))/mean(df))<0.1
        df = mean(diff(frequencies));
        frequencies = (0:(length(frequencies)-1))*df + frequencies(1);
    end
end

S1.frequencies = frequencies;

if ~isequal(D.type, 'continuous')

    %-Run the analysis on all trials
    %--------------------------------------------------------------------------
    spm_progress_bar('Init', D.ntrials, 'trials done');
    if D.ntrials > 100, Ibar = floor(linspace(1, D.ntrials, 100));
    else Ibar = 1:D.ntrials; end

    for k = 1:D.ntrials
        spm_progress_bar('Set','ylabel','reading and computing...');
        trial = feval(['spm_eeg_specest_' S.method], S1, D(chanind, timeind, k), D.time(timeind));

        if k == 1
            spm_progress_bar('Set','ylabel','initialising output...');
            if isfield(trial, 'fourier')
                outdata = trial.fourier;
            else
                outdata = trial.pow;
            end

            Nchannels = size(outdata, 1);
            Nfrequencies = size(outdata, 2);
            Nsamples = size(outdata, 3);

            %-Generate output datasets
            %--------------------------------------------------------------------------
            Dtf = clone(D, [S.prefix 'tf_' D.fname], [Nchannels Nfrequencies Nsamples D.ntrials], 0, 1);
            Dtf = Dtf.frequencies(':', trial.freq);
            Dtf = timeonset(Dtf, trial.time(1));
            Dtf = fsample(Dtf, 1/diff(trial.time(1:2)));
            Dtf = transformtype(Dtf, 'TF');

            Dtf = chanlabels(Dtf, 1:Nchannels, D.chanlabels(chanind));
            Dtf = badchannels(Dtf, 1:Nchannels, D.badchannels(chanind));
            Dtf = chantype(Dtf, 1:Nchannels, D.chantype(chanind));
            Dtf = coor2D(Dtf, 1:Nchannels, coor2D(D,chanind));

            if S.phase && isfield(trial, 'fourier')
                Dtph = clone(Dtf, [S.prefix 'tph_' D.fname]);
                Dtph = transformtype(Dtph, 'TFphase');
            else
                if ~isfield(trial, 'fourier')
                    warning('Phase cannot be estimated with the requested method. Estimating power only.');
                end

                Dtph = [];
            end

        end

        spm_progress_bar('Set','ylabel','writing...');
        if isfield(trial, 'fourier')
            Dtf(:, :, :, k) = trial.fourier.*conj(trial.fourier);

            if S.phase
                Dtph(:, :, :, k) = angle(trial.fourier);
            end
        elseif isfield(trial, 'pow')
            Dtf(:, :, :, k) = trial.pow;
        else
            error('The plug-in returned unexpected output');
        end

        if ismember(k, Ibar), spm_progress_bar('Set', k); end
    end

    spm_progress_bar('Clear');

else % by channel for continuous data
    Nchannels = length(chanind);

    spm_progress_bar('Init', Nchannels , 'channels done');
    if Nchannels > 100, Ibar = floor(linspace(1, Nchannels, 100));
    else Ibar = 1:Nchannels; end

    for k = 1:Nchannels
        spm_progress_bar('Set','ylabel','reading and computing...');
        trial = feval(['spm_eeg_specest_' S.method], S1, D(chanind(k), timeind), D.time(timeind));

        if k == 1
            spm_progress_bar('Set','ylabel','initialising output...');
            if isfield(trial, 'fourier')
                outdata = trial.fourier;
            else
                outdata = trial.pow;
            end

            Nfrequencies = size(outdata, 2);
            Nsamples = size(outdata, 3);

            %-Generate output datasets
            %--------------------------------------------------------------------------
            Dtf = clone(D, [S.prefix 'tf_' D.fname], [Nchannels Nfrequencies Nsamples 1]);
            Dtf = Dtf.frequencies(':', trial.freq);
            Dtf = timeonset(Dtf, trial.time(1));
            Dtf = fsample(Dtf, 1/diff(trial.time(1:2)));
            Dtf = transformtype(Dtf, 'TF');

            Dtf = chanlabels(Dtf, 1:Nchannels, D.chanlabels(chanind));
            Dtf = badchannels(Dtf, 1:Nchannels, D.badchannels(chanind));
            Dtf = chantype(Dtf, 1:Nchannels, D.chantype(chanind));
            Dtf = coor2D(Dtf, 1:Nchannels, coor2D(D,chanind));

            ev  = Dtf.events;

            if ~isempty(ev)
                ev  = ev([ev.time]>=Dtf.time(1) & [ev.time]<=Dtf.time(end));
                Dtf = events(Dtf, 1, ev);
            end

            if S.phase && isfield(trial, 'fourier')
                Dtph = clone(Dtf, [S.prefix 'tph_' D.fname]);
                Dtph = transformtype(Dtph, 'TFphase');
            else
                if ~isfield(trial, 'fourier')
                    warning('Phase cannot be estimated with the requested method. Estimating power only.');
                end

                Dtph = [];
            end
        end

        spm_progress_bar('Set','ylabel','writing...');
        if isfield(trial, 'fourier')
            Dtf(k, :, :, 1) = trial.fourier.*conj(trial.fourier);

            if S.phase
                Dtph(k, :, :, 1) = angle(trial.fourier);
            end
        elseif isfield(trial, 'pow')
            Dtf(k, :, :, 1) = trial.pow;
        else
            error('The plug-in returned unexpected output');
        end

        if ismember(k, Ibar), spm_progress_bar('Set', k); end
    end

    spm_progress_bar('Clear');

end
%-Save new M/EEG dataset(s)
%--------------------------------------------------------------------------
Dtf = Dtf.history('spm_eeg_tf', S);
save(Dtf);
if ~isempty(Dtph)
    Dtph = Dtph.history('spm_eeg_tf', S);
    save(Dtph);
end

%-Cleanup
%--------------------------------------------------------------------------
fprintf('%-40s: %30s\n','Completed',spm('time'));                       %-#
spm('FigName','M/EEG Time Frequency: done'); spm('Pointer','Arrow');


function score = scoreModel_helper(metric, files, imnum, map, FixationDBCleaned, FIXATIONMAPS, STIMULI, HASH)

score = nan;

switch metric
        case 'ROC'
            % find the ROC performance
            [~, ~, fixations] = showAllEyeDataForImage2(files(imnum).name, FixationDBCleaned, STIMULI, HASH);
            score = AUC_Judd(map, fixations);

        case 'S'
            % find the similarity
            fixMap = im2double(imread(fullfile(FIXATIONMAPS, files(imnum).name)));
            score= similarity(map, fixMap);

        case 'EMD'
            % find the EMD performance
            fixMap = im2double(imread(fullfile(FIXATIONMAPS, files(imnum).name)));
            score = EMD(map, fixMap);

       case 'ROC_borji'
            % find Borji's ROC performance
            [~, ~, fixations] = showAllEyeDataForImage2(files(imnum).name, FixationDBCleaned, STIMULI, HASH);
            score = AUC_Borji(map, fixations);

        case 'sROC_borji'
            % find the shuffled ROC performance
            [~, ~, fixations] = showAllEyeDataForImage2(files(imnum).name, FixationDBCleaned, STIMULI, HASH);

            otherMap = zeros(size(fixations));

            % choose nOther other images to sample fixations from
            nOther = 10; % number of images to get fixations from
            otherImInd = [1:(imnum-1),(imnum+1):length(files)]; % indices of images other than the current one
            whichOther = otherImInd(randperm(length(otherImInd))); % randomize choice of images

            % accumulate fixations from nOther other images into one
            % fixation map: otherMap
            for ii = 1:nOther
                [~, ~, fixations2] = showAllEyeDataForImage2(files(whichOther(ii)).name, FixationDBCleaned, STIMULI, HASH);
                otherMap = otherMap + imresize(fixations2,size(otherMap));
            end

            % find the shuffled AUC score
            score = AUC_shuffled(map, fixations, otherMap);

    case 'CC'
            % find the cross correlation performance
            fixMap = im2double(imread(fullfile(FIXATIONMAPS, files(imnum).name)));
            score = CC(map,fixMap);

    case 'NSS'
            % find the normalized scanpath saliency
            [~, ~, fixations] = showAllEyeDataForImage2(files(imnum).name, FixationDBCleaned, STIMULI, HASH);
            score = NSS(map,fixations);
 end


%%% AttitudeControl_HIL
clear
path('./icon/',path);

%Constant value
RAD2DEG = 57.2957795;
DEG2RAD = 0.0174533;
%throttle when UAV is hovering
THR_HOVER = 0.609;

%% control parameter
%Attitude PID parameters
Kp_YAW_AngleRate = 0.5;
Ki_YAW_AngleRate = 0.01;
Kd_YAW_AngleRate = 0.00;
%integral saturation
Saturation_I_RP_Max = 0.3;
Saturation_I_RP_Min = -0.3;
Saturation_I_Y_Max = 0.2;
Saturation_I_Y_Min = -0.2;
%max control angle,default 35deg
MAX_CONTROL_ANGLE_ROLL = 35;
MAX_CONTROL_ANGLE_PITCH  = 35;
%max control angle rate,rad/s
MAX_CONTROL_ANGLE_RATE_PITCH = 220;
MAX_CONTROL_ANGLE_RATE_ROLL = 220;
MAX_CONTROL_ANGLE_RATE_Y = 200;
%Throttle amplitude
MAX_MAN_THR=0.9;
MIN_MAN_THR=0.05;
%% run simulink model
AttitudeControl_HIL