function lik = lik_qgp(varargin)
%LIK_QGP  Create a Quantile Gaussian Process likelihood (utility) structure
%
%  Description
%    LIK = LIK_QGP('PARAM1',VALUE1,'PARAM2,VALUE2,...)
%    creates a quantile gp likelihood structure in which the named
%    parameters have the specified values. Any unspecified
%    parameters are set to default values.
%
%    LIK = LIK_QGP(LIK,'PARAM1',VALUE1,'PARAM2,VALUE2,...)
%    modify a likelihood function structure with the named
%    parameters altered with the specified values.
%
%    Parameters for QGP likelihood function [default]
%      sigma2       - variance [0.1]
%      sigma2_prior - prior for sigma2 [prior_logunif]
%      quantile     - Quantile of interest [0.5]
%
%    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.
%
%    The likelihood is defined as follows:
%                            __ n
%      p(y|f, sigma2, tau) = || i=1 tau*(1-tau)/sigma*exp(-(y-f)/sigma*
%                                 (tau - I(t <= f)))
%
%    where tau is the quantile of interest, sigma is the standard deviation
%    of the distribution and I(t <= f) = 1 if t <= f, 0 otherwise.
%
%    Note that because the form of the likelihood, second order derivatives
%    with respect to latent values are 0. Because this, EP should be used
%    instead of Laplace approximation.
%
%  See also
%    GP_SET, PRIOR_*, LIK_*
%
%   References
%     Boukouvalas et al. (2012). Direct Gaussian Process Quantile Regression
%     Using Expectation Propagation. Appearing in Proceedings of the 29th
%     International Conference on Machine Learning, Edinburg, Scotland, UK,
%     2012.
%

% Copyright (c) 2012 Ville Tolvanen

% 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 = 'LIK_QGP';
  ip.addOptional('lik', [], @isstruct);
  ip.addParamValue('sigma2',0.1, @(x) isscalar(x) && x>0);
  ip.addParamValue('sigma2_prior',prior_logunif(), @(x) isstruct(x) || isempty(x));
  ip.addParamValue('quantile',0.5, @(x) isscalar(x) && x>0 && x<1);
  ip.parse(varargin{:});
  lik=ip.Results.lik;

  if isempty(lik)
    init=true;
    lik.type = 'QGP';
  else
    if ~isfield(lik,'type') || ~isequal(lik.type,'QGP')
      error('First argument does not seem to be a valid likelihood function structure')
    end
    init=false;
  end

  % Initialize parameters
  if init || ~ismember('sigma2',ip.UsingDefaults)
    lik.sigma2 = ip.Results.sigma2;
  end
  if init || ~ismember('quantile',ip.UsingDefaults)
    lik.quantile = ip.Results.quantile;
  end
  % Initialize prior structure
  if init
    lik.p=[];
  end
  if init || ~ismember('sigma2_prior',ip.UsingDefaults)
    lik.p.sigma2=ip.Results.sigma2_prior;
  end
  if init
    % Set the function handles to the subfunctions
    lik.fh.pak = @lik_qgp_pak;
    lik.fh.unpak = @lik_qgp_unpak;
    lik.fh.lp = @lik_qgp_lp;
    lik.fh.lpg = @lik_qgp_lpg;
    lik.fh.ll = @lik_qgp_ll;
    lik.fh.llg = @lik_qgp_llg;
    lik.fh.llg2 = @lik_qgp_llg2;
    lik.fh.llg3 = @lik_qgp_llg3;
    lik.fh.tiltedMoments = @lik_qgp_tiltedMoments;
    lik.fh.siteDeriv = @lik_qgp_siteDeriv;
    lik.fh.predy = @lik_qgp_predy;
    lik.fh.invlink = @lik_qgp_invlink;
    lik.fh.recappend = @lik_qgp_recappend;
  end

end

function [w s] = lik_qgp_pak(lik)
%LIK_QGP_PAK  Combine likelihood parameters into one vector.
%
%  Description
%    W = LIK_QGP_PAK(LIK) takes a likelihood structure LIK
%    and combines the parameters into a single row vector W.
%    This is a mandatory subfunction used for example in
%    energy and gradient computations.
%
%       w = [ log(lik.sigma2)
%             (hyperparameters of lik.magnSigma2)]'
%
%  See also
%    LIK_QGP_UNPAK

  w = []; s = {};
  if ~isempty(lik.p.sigma2)
    w = [w log(lik.sigma2)];
    s = [s; 'log(qgp.sigma2)'];
    % Hyperparameters of sigma2
    [wh sh] = lik.p.sigma2.fh.pak(lik.p.sigma2);
    w = [w wh];
    s = [s; sh];
  end

end

function [lik, w] = lik_qgp_unpak(lik, w)
%LIK_QGP_UNPAK  Extract likelihood parameters from the vector.
%
%  Description
%    W = LIK_QGP_UNPAK(W, LIK) takes a likelihood structure
%    LIK and extracts the parameters from the vector W to the LIK
%    structure. This is a mandatory subfunction used for example
%    in energy and gradient computations.
%
%    Assignment is inverse of
%       w = [ log(lik.sigma2)
%             (hyperparameters of lik.magnSigma2)]'
%
%  See also
%    LIK_QGP_PAK

  if ~isempty(lik.p.sigma2)
    lik.sigma2 = exp(w(1));
    w = w(2:end);

    % Hyperparameters of sigma2
    [p, w] = lik.p.sigma2.fh.unpak(lik.p.sigma2, w);
    lik.p.sigma2 = p;
  end
end

function lp = lik_qgp_lp(lik)
%LIK_QGP_LP  Evaluate the log prior of likelihood parameters
%
%  Description
%    LP = LIK_QGP_LP(LIK) takes a likelihood structure LIK and
%    returns log(p(th)), where th collects the parameters. This
%    subfunction is needed when there are likelihood parameters.
%
%  See also
%    LIK_QGP_PAK, LIK_QGP_UNPAK, LIK_QGP_G, GP_E

  lp = 0;

  if ~isempty(lik.p.sigma2)
    likp=lik.p;
    lp = likp.sigma2.fh.lp(lik.sigma2, likp.sigma2) + log(lik.sigma2);
  end
end

function lpg = lik_qgp_lpg(lik)
%LIK_QGP_LPG  Evaluate gradient of the log prior with respect
%                  to the parameters.
%
%  Description
%    LPG = LIK_QGP_LPG(LIK) takes a QGP likelihood
%    function structure LIK and returns LPG = d log (p(th))/dth,
%    where th is the vector of parameters. This subfunction is
%    needed when there are likelihood parameters.
%
%  See also
%    LIK_QGP_PAK, LIK_QGP_UNPAK, LIK_QGP_E, GP_G

  lpg = [];

  if ~isempty(lik.p.sigma2)
    likp=lik.p;

    lpgs = likp.sigma2.fh.lpg(lik.sigma2, likp.sigma2);
    lpg = lpgs(1).*lik.sigma2 + 1;
    if length(lpgs) > 1
      lpg = [lpg lpgs(2:end)];
    end
  end
end

function ll = lik_qgp_ll(lik, y, f, z)
%LIK_QGP_LL  Log likelihood
%
%  Description
%    LL = LIK_QGP_LL(LIK, Y, F, Z) takes a likelihood
%    structure LIK, observations Y and latent values F.
%    Returns the log likelihood, log p(y|f,z). This subfunction
%    is needed when using Laplace approximation or MCMC for
%    inference with non-Gaussian likelihoods. This subfunction
%    is also used in information criteria (DIC, WAIC) computations.
%
%  See also
%    LIK_QGP_LLG, LIK_QGP_LLG3, LIK_QGP_LLG2, GPLA_E

  tau=lik.quantile;
  sigma=sqrt(lik.sigma2);
  ll = sum(log(tau*(1-tau)/sigma) - (y-f)./sigma.*(tau-(y<=f)));
end

function llg = lik_qgp_llg(lik, y, f, param, z)
%LIK_QGP_LLG  Gradient of the log likelihood
%
%  Description
%    LLG = LIK_QGP_LLG(LIK, Y, F, PARAM) takes a likelihood
%    structure LIK, observations Y and latent values F. Returns
%    the gradient of the log likelihood with respect to PARAM.
%    At the moment PARAM can be 'param' or 'latent'. This subfunction
%    is needed when using Laplace approximation or MCMC for inference
%    with non-Gaussian likelihoods.
%
%  See also
%    LIK_QGP_LL, LIK_QGP_LLG2, LIK_QGP_LLG3, GPLA_E

  tau=lik.quantile;
  sigma2=sqrt(lik.sigma2);
  switch param
    case 'param'
      llg = sum(-1/(2.*sigma2) + (y-f)./(2.*sigma2^(3/2)).*(tau-(y<=f)));

      % correction for the log transformation
      llg = llg.*lik.sigma2;
    case 'latent'
      llg = (tau-(y<=f))/sqrt(sigma2);
  end
end

function llg2 = lik_qgp_llg2(lik, y, f, param, z)
%LIK_QGP_LLG2  Second gradients of the log likelihood
%
%  Description
%    LLG2 = LIK_QGP_LLG2(LIK, Y, F, PARAM) takes a likelihood
%    structure LIK, observations Y and latent values F. Returns
%    the Hessian of the log likelihood with respect to PARAM.
%    At the moment PARAM can be 'param' or 'latent'. LLG2 is
%    a vector with diagonal elements of the Hessian matrix
%    (off diagonals are zero). This subfunction is needed
%    when using Laplace approximation or EP for inference
%    with non-Gaussian likelihoods.
%
%  See also
%    LIK_QGP_LL, LIK_QGP_LLG, LIK_QGP_LLG3, GPLA_E

  tau=lik.quantile;
  sigma2=lik.sigma2;
  switch param
    case 'param'
      llg2 = sum(1/(2*sigma2^2) - 3.*(tau-(y<=f)).*(y-f)./(4.*sigma2^(5/2)));

      % correction due to the log transformation
      llg2 = llg2.*lik.sigma2;
    case 'latent'
      llg2 = zeros(size(f));
    case 'latent+param'
      llg2 = -(tau-(y<=f))./(2*sigma2^(3/2));

      % correction due to the log transformation
      llg2 = llg2.*lik.disper;
  end
end

function llg3 = lik_qgp_llg3(lik, y, f, param, z)
%LIK_QGP_LLG3  Third gradients of the log likelihood
%
%  Description
%    LLG3 = LIK_QGP_LLG3(LIK, Y, F, PARAM) takes a likelihood
%    structure LIK, observations Y and latent values F and
%    returns the third gradients of the log likelihood with
%    respect to PARAM. At the moment PARAM can be 'param' or
%    'latent'. LLG3 is a vector with third gradients. This
%    subfunction is needed when using Laplace approximation for
%    inference with non-Gaussian likelihoods.
%
%  See also
%    LIK_QGP_LL, LIK_QGP_LLG, LIK_QGP_LLG2, GPLA_E, GPLA_G

  tau=lik.quantile;
  sigma2=lik.sigma2;
  switch param
    case 'param'
      llg3 = sum(-1/sigma2^3 + 15.*(tau-(y<=f)).*(y-f)./(8.*sigma2^(7/2)));
    case 'latent'
      llg3 = 0;
    case 'latent2+param'
      llg3 = 0;

      % correction due to the log transformation
      llg3 = llg3.*lik.sigma2;
  end
end

function [logM_0, m_1, sigm2hati1] = lik_qgp_tiltedMoments(lik, y, i1, sigm2_i, myy_i, z)
%LIK_QGP_TILTEDMOMENTS  Returns the marginal moments for EP algorithm
%
%  Description
%    [M_0, M_1, M2] = LIK_QGP_TILTEDMOMENTS(LIK, Y, I, S2,
%    MYY, Z) takes a likelihood structure LIK, observations
%    Y, index I and cavity variance S2 and mean MYY. Returns
%    the zeroth moment M_0, mean M_1 and variance M_2 of the
%    posterior marginal (see Rasmussen and Williams (2006):
%    Gaussian processes for Machine Learning, page 55). This
%    subfunction is needed when using EP for inference with
%    non-Gaussian likelihoods.
%
%  See also
%    GPEP_E

  yy = y(i1);
  sigma2 = lik.sigma2;
  tau=lik.quantile;
  logM_0=zeros(size(yy));
  m_1=zeros(size(yy));
  sigm2hati1=zeros(size(yy));

  for i=1:length(i1)
    % get a function handle of an unnormalized tilted distribution
    % (likelihood * cavity = Quantile-GP * Gaussian)
    % and useful integration limits
    [tf,minf,maxf]=init_qgp_norm(yy(i),myy_i(i),sigm2_i(i),sigma2,tau);

    % Integrate with quadrature
    RTOL = 1.e-6;
    ATOL = 1.e-10;
    [m_0, m_1(i), m_2] = quad_moments(tf, minf, maxf, RTOL, ATOL);
    sigm2hati1(i) = m_2 - m_1(i).^2;

    % If the second central moment is less than cavity variance
    % integrate more precisely. Theoretically for log-concave
    % likelihood should be sigm2hati1 < sigm2_i.
    if sigm2hati1(i) >= sigm2_i(i)
      ATOL = ATOL.^2;
      RTOL = RTOL.^2;
      [m_0, m_1(i), m_2] = quad_moments(tf, minf, maxf, RTOL, ATOL);
      sigm2hati1(i) = m_2 - m_1(i).^2;
      if sigm2hati1(i) >= sigm2_i(i)
        error('lik_qgp_tilted_moments: sigm2hati1 >= sigm2_i');
      end
    end
    logM_0(i) = log(m_0);
  end
end

function [g_i] = lik_qgp_siteDeriv(lik, y, i1, sigm2_i, myy_i, z)
%LIK_QGP_SITEDERIV  Evaluate the expectation of the gradient
%                      of the log likelihood term with respect
%                      to the likelihood parameters for EP
%
%  Description [M_0, M_1, M2] =
%    LIK_QGP_SITEDERIV(LIK, Y, I, S2, MYY, Z) takes a
%    likelihood structure LIK, observations Y, index I
%    and cavity variance S2 and mean MYY. Returns E_f
%    [d log p(y_i|f_i) /d a], where a is the likelihood
%    parameter and the expectation is over the marginal posterior.
%    This term is needed when evaluating the gradients of
%    the marginal likelihood estimate Z_EP with respect to
%    the likelihood parameters (see Seeger (2008):
%    Expectation propagation for exponential families).This
%    subfunction is needed when using EP for inference with
%    non-Gaussian likelihoods and there are likelihood parameters.
%
%  See also
%    GPEP_G

  yy = y(i1);
  sigma2=lik.sigma2;
  tau=lik.quantile;

  % get a function handle of an unnormalized tilted distribution
  % (likelihood * cavity = Quantile-GP * Gaussian)
  % and useful integration limits
  [tf,minf,maxf]=init_qgp_norm(yy,myy_i,sigm2_i,sigma2,tau);
  % additionally get function handle for the derivative
  td = @deriv;

  % Integrate with quadgk
  [m_0, fhncnt] = quadgk(tf, minf, maxf);
  [g_i, fhncnt] = quadgk(@(f) td(f).*tf(f)./m_0, minf, maxf);
  g_i = g_i.*sigma2;

  function g = deriv(f)

    g = -1/(2.*sigma2) + (yy-f)./(2.*sigma2^(3/2)).*(tau-(yy<=f));

  end
end

function [lpy, Ey, Vary] = lik_qgp_predy(lik, Ef, Varf, yt, zt)
%LIK_QGP_PREDY  Returns the predictive mean, variance and density of y
%
%  Description
%    LPY = LIK_QGP_PREDY(LIK, EF, VARF YT, ZT)
%    Returns logarithm of the predictive density PY of YT, that is
%        p(yt | zt) = \int p(yt | f, zt) p(f|y) df.
%    This subfunction is needed when computing posterior predictive
%    distributions for future observations.
%
%    [LPY, EY, VARY] = LIK_QGP_PREDY(LIK, EF, VARF) takes a
%    likelihood structure LIK, posterior mean EF and posterior
%    Variance VARF of the latent variable and returns the
%    posterior predictive mean EY and variance VARY of the
%    observations related to the latent variables. This
%    subfunction is needed when computing posterior predictive
%    distributions for future observations.
%

%
%  See also
%    GPLA_PRED, GPEP_PRED, GPMC_PRED

  sigma2=lik.sigma2;
  tau=lik.quantile;

  Ey=[];
  Vary=[];

  % Evaluate the posterior predictive densities of the given observations
  lpy = zeros(length(yt),1);
  for i1=1:length(yt)
    % get a function handle of the likelihood times posterior
    % (likelihood * posterior = Quantile-GP * Gaussian)
    % and useful integration limits
    [pdf,minf,maxf]=init_qgp_norm(...
      yt(i1),Ef(i1),Varf(i1),sigma2, tau);
    % integrate over the f to get posterior predictive distribution
    lpy(i1) = log(quadgk(pdf, minf, maxf));
  end
end

function [df,minf,maxf] = init_qgp_norm(yy,myy_i,sigm2_i,sigma2,tau)
%INIT_QGP_NORM
%
%  Description
%    Return function handle to a function evaluating
%    Quantile-GP * Gaussian which is used for evaluating
%    (likelihood * cavity) or (likelihood * posterior) Return
%    also useful limits for integration. This is private function
%    for lik_qgp. This subfunction is needed by subfunctions
%    tiltedMoments, siteDeriv and predy.
%
%  See also
%    LIK_QGP_TILTEDMOMENTS, LIK_QGP_SITEDERIV,
%    LIK_QGP_PREDY

  sigma=sqrt(sigma2);
% avoid repetitive evaluation of constant part
  ldconst = log(tau*(1-tau)/sigma) ...
            - log(sigm2_i)/2 - log(2*pi)/2;
  % Create function handle for the function to be integrated
  df = @qgp_norm;
  % use log to avoid underflow, and derivates for faster search
  ld = @log_qgp_norm;
  ldg = @log_qgp_norm_g;
%   ldg2 = @log_qgp_norm_g2;

  % Set the limits for integration
  % Quantile-GP likelihood is log-concave so the qgp_norm
  % function is unimodal, which makes things easier
  if yy==0
    % with yy==0, the mode of the likelihood is not defined
    % use the mode of the Gaussian (cavity or posterior) as a first guess
    modef = myy_i;
  else
    % use precision weighted mean of the Gaussian approximation
    % of the Quantile-GP likelihood and Gaussian
    modef = (myy_i/sigm2_i + yy/sigma2)/(1/sigm2_i + 1/sigma2);
  end
  % find the mode of the integrand using Newton iterations
  % few iterations is enough, since the first guess in the right direction
  niter=8;       % number of Newton iterations

  minf=modef-6*sigm2_i;
  while ldg(minf) < 0
    minf=minf-2*sigm2_i;
  end
  maxf=modef+6*sigm2_i;
  while ldg(maxf) > 0
    maxf=maxf+2*sigm2_i;
  end
  for ni=1:niter
%     h=ldg2(modef);
    modef=0.5*(minf+maxf);
    if ldg(modef) < 0
      maxf=modef;
    else
      minf=modef;
    end
  end
  % integrand limits based on Gaussian approximation at mode
  minf=modef-6*sqrt(sigm2_i);
  maxf=modef+6*sqrt(sigm2_i);
  modeld=ld(modef);
  iter=0;
  % check that density at end points is low enough
  lddiff=20; % min difference in log-density between mode and end-points
  minld=ld(minf);
  step=1;
  while minld>(modeld-lddiff)
    minf=minf-step*sqrt(sigm2_i);
    minld=ld(minf);
    iter=iter+1;
    step=step*2;
    if iter>100
      error(['lik_qgp -> init_qgp_norm: ' ...
             'integration interval minimun not found ' ...
             'even after looking hard!'])
    end
  end
  maxld=ld(maxf);
  step=1;
  while maxld>(modeld-lddiff)
    maxf=maxf+step*sqrt(sigm2_i);
    maxld=ld(maxf);
    iter=iter+1;
    step=step*2;
    if iter>100
      error(['lik_qgp -> init_qgp_norm: ' ...
             'integration interval maximun not found ' ...
             'even after looking hard!'])
    end
  end

  function integrand = qgp_norm(f)
  % Quantile-GP * Gaussian
    integrand = exp(ldconst ...
                    -(yy-f)./sqrt(sigma2).*(tau-(yy<=f)) ...
                    -0.5*(f-myy_i).^2./sigm2_i);
  end

  function log_int = log_qgp_norm(f)
  % log(Quantile-GP * Gaussian)
  % log_qgp_norm is used to avoid underflow when searching
  % integration interval
    log_int = ldconst...
              -(yy-f)./sqrt(sigma2).*(tau-(yy<=f)) ...
              -0.5*(f-myy_i).^2./sigm2_i;
  end

  function g = log_qgp_norm_g(f)
  % d/df log(Quantile-GP * Gaussian)
  % derivative of log_qgp_norm
    g = (tau-(yy<=f))/sqrt(sigma2) ...
        + (myy_i - f)./sigm2_i;
  end

end

function mu = lik_qgp_invlink(lik, f, z)
%LIK_QGP_INVLINK  Returns values of inverse link function
%
%  Description
%    MU = LIK_QGP_INVLINK(LIK, F) takes a likelihood structure LIK and
%    latent values F and returns the values MU of inverse link function.
%    This subfunction is needed when using function gp_predprctmu.
%
%     See also
%     LIK_QGP_LL, LIK_QGP_PREDY

  mu = f;
end

function reclik = lik_qgp_recappend(reclik, ri, lik)
%RECAPPEND  Append the parameters to the record
%
%  Description
%    RECLIK = LIK_QGP_RECAPPEND(RECLIK, RI, LIK) takes a
%    likelihood record structure RECLIK, record index RI and
%    likelihood structure LIK with the current MCMC samples of
%    the parameters. Returns RECLIK which contains all the old
%    samples and the current samples from LIK.  This subfunction
%    is needed when using MCMC sampling (gp_mc).
%
%  See also
%    GP_MC

  if nargin == 2
    % Initialize the record
    reclik.type = 'Quantile-GP';

    % Initialize parameter
    reclik.sigma2 = [];

    % Set the function handles
    reclik.fh.pak = @lik_qgp_pak;
    reclik.fh.unpak = @lik_qgp_unpak;
    reclik.fh.lp = @lik_qgp_lp;
    reclik.fh.lpg = @lik_qgp_lpg;
    reclik.fh.ll = @lik_qgp_ll;
    reclik.fh.llg = @lik_qgp_llg;
    reclik.fh.llg2 = @lik_qgp_llg2;
    reclik.fh.llg3 = @lik_qgp_llg3;
    reclik.fh.tiltedMoments = @lik_qgp_tiltedMoments;
    reclik.fh.predy = @lik_qgp_predy;
    reclik.fh.invlink = @lik_qgp_invlink;
    reclik.fh.recappend = @lik_qgp_recappend;
    reclik.p=[];
    reclik.p.sigma2=[];
    if ~isempty(ri.p.sigma2)
      reclik.p.sigma2 = ri.p.sigma2;
    end
  else

    % Append to the record
    reclik.sigma2(ri,:)=lik.sigma2;
    if ~isempty(lik.p)
      reclik.p.sigma2 = lik.p.sigma2.fh.recappend(reclik.p.sigma2, ri, lik.p.sigma2);
    end
  end
end


function [C_pos,C_neg,Ctot_pos,Ctot_neg] = clustering_coef_wu_sign(W,coef_type)
%CLUSTERING_COEF_WU_SIGN     Multiple generalizations of the clustering coefficient
%
%   [C_pos,C_neg,Ctot_pos,Ctot_neg] = clustering_coef_wu_sign(W,coef_type);
%
%   The weighted clustering coefficient is the average weight or intensity
%   of all triangles associated with each node.
%
%   Inputs:
%       W,
%           Weighted undirected connection matrix
%
%       corr_type,
%           Desired type of clustering coefficient.
%           Options:
%           1,  (default) Onnela et al. formula, used in original
%               clustering_coef_wu.m. Computed separately for positive &
%               negative weights.
%           2,  Zhang & Horvath formula, similar to Onnela formula except
%               denominator of Onnela formula relies on binarizing the
%               network whereas this denominator is based on weight value,
%               which reduces the sensitivity of this measure to the
%               weights directly connected to the node of interest.
%               Computed separately for positive & negative weights.
%           3,  Constantini & Perugini's generalization of the Zhang &
%               Horvath formula. This formula takes both positive &
%               negative weights into account simultaneously, & is
%               particularly sensitive to non-redundancy in path
%               information based on sign (i.e., when two weights are
%               positive & one negative, or all three are negative, both of
%               which indicate that the weight of the third path is not
%               redundant information). Produces only one value.
%
%
%   Outputs:
%       C_pos/C_neg,
%           Clustering coefficient vector for positive/negative weights.
%           For the third option, only one vector is outputted (as C_pos).
%       Ctot_pos/Ctot_neg,
%           Mean clustering coefficient for positive and negative weights.
%
%   References:
%       Onnela et al. (2005) Phys Rev E 71:065103
%       Zhang & Horvath (2005) Stat Appl Genet Mol Biol 41:1544-6115
%       Costantini & Perugini (2014) PLOS ONE 9:e88669
%
%
%   Contributor: Jeff Spielberg, Boston University, 2014-2015
%                (script based on clustering_coef_wu.m)

%
%   Modification History:
%   May 2014: Added computation of pos & neg weights separately &
%             computation of mean coefficient (Jeff Spielberg)
%   May 2015: Added computation of Zhang & Horvath and Constantini &
%             Perugini formulas (Jeff Spielberg)
%   May 2016: Bugfix in computation of the denominator of the Costantini &
%             Perugini (flag 3) version (Chiara Pintossi)

if ~exist('coef_type','var')
    coef_type = 1;
end

n            = length(W);                   %number of nodes
W(1:n+1:end) = 0;

switch coef_type
    case 1
        W_pos                = W.*(W>0);
        K_pos                = sum(W_pos~=0,2);
        cyc3_pos             = diag((W_pos.^(1/3))^3);
        K_pos(cyc3_pos == 0) = inf;             %if no 3-cycles exist, make C=0 (via K=inf)
        C_pos                = cyc3_pos./(K_pos.*(K_pos-1));         %clustering coefficient
        Ctot_pos             = mean(C_pos);

        W_neg                = -W.*(W<0);
        K_neg                = sum(W_neg~=0,2);
        cyc3_neg             = diag((W_neg.^(1/3))^3);
        K_neg(cyc3_neg == 0) = inf;             %if no 3-cycles exist, make C=0 (via K=inf)
        C_neg                = cyc3_neg./(K_neg.*(K_neg-1));         %clustering coefficient
        Ctot_neg             = mean(C_neg);
    case 2
        W_pos    = W.*(W>0);
        cyc3_pos = zeros(n,1);
        cyc2_pos = zeros(n,1);
        for i = 1:n
            for j = 1:n
                for q = 1:n
                    cyc3_pos(i) = cyc3_pos(i)+(W_pos(j,i)*W_pos(i,q)*W_pos(j,q));
                    if j~=q
                        cyc2_pos(i) = cyc2_pos(i)+(W_pos(j,i)*W_pos(i,q));
                    end
                end
            end
        end
        cyc2_pos(cyc3_pos == 0) = inf;             %if no 3-cycles exist, make C=0 (via K=inf)
        C_pos                   = cyc3_pos./cyc2_pos;         %clustering coefficient
        Ctot_pos                = mean(C_pos);

        W_neg    = -W.*(W<0);
        cyc3_neg = zeros(n,1);
        cyc2_neg = zeros(n,1);
        for i = 1:n
            for j = 1:n
                for q = 1:n
                    cyc3_neg(i) = cyc3_neg(i)+(W_neg(j,i)*W_neg(i,q)*W_neg(j,q));
                    if j~=q
                        cyc2_neg(i) = cyc2_neg(i)+(W_neg(j,i)*W_neg(i,q));
                    end
                end
            end
        end
        cyc2_neg(cyc3_neg == 0) = inf;             %if no 3-cycles exist, make C=0 (via K=inf)
        C_neg                   = cyc3_neg./cyc2_neg;         %clustering coefficient
        Ctot_neg                = mean(C_neg);
    case 3
        cyc3         = zeros(n,1);
        cyc2         = zeros(n,1);

        for i = 1:n
            for j = 1:n
                for q = 1:n
                    cyc3(i) = cyc3(i)+(W(j,i)*W(i,q)*W(j,q));
                    if j~=q
                        cyc2(i) = cyc2(i)+abs(W(j,i)*W(i,q));
                    end
                end
            end
        end

        cyc2(cyc3 == 0) = inf;             %if no 3-cycles exist, make C=0 (via K=inf)
        C_pos           = cyc3./cyc2;         %clustering coefficient
        Ctot_pos        = mean(C_pos);
        C_neg           = nan(size(C_pos));
        Ctot_neg        = nan(size(Ctot_pos));
end


function newFaces = minConvexHull(points, varargin)
%MINCONVEXHULL Return the unique minimal convex hull of a set of 3D points.
%
%   FACES = minConvexHull(PTS)
%   NODES is a set of 3D points  (as a Nx3 array). The function computes
%   the convex hull, and merge contiguous coplanar faces. The result is a
%   set of polygonal faces, such that there are no coplanar faces.
%   FACES is a cell array, each cell containing the vector of indices of
%   nodes given in NODES for the corresponding face.
%
%   FACES = minConvexHull(PTS, PRECISION)
%   Adjust the threshold for deciding if two faces are coplanar or
%   parallel. Default value is 1e-14.
%
%   Example
%     % extract square faces from a cube
%     [n, e, f] = createCube;
%     f2 = minConvexHull(n);
%     drawMesh(n, f2);
%
%     % Subdivides and smooths a mesh rpresenting a cube
%     [n, e, f] = createCube;
%     [n2, f2] = subdivideMesh(n, triangulateFaces(f), 4);
%     [n3, f3] = smoothMesh(n2, f2);
%     figure; drawMesh(n3, f3);
%     axis equal; view(3);
%     % merge coplanar faces, making apparent the faces of the original cube
%     f4 = minConvexHull(n3);
%     figure; drawMesh(n3, f4);
%     axis equal; view(3);
%
%
%   See also
%   meshes3d, mergeCoplanarFaces, drawMesh, convhull, convhulln
%

% ------
% Author: David Legland
% E-mail: david.legland@inra.fr
% Created: 2006-07-05
% Copyright 2006-2022 INRA - CEPIA Nantes - MIAJ (Jouy-en-Josas)

% set up precision
acc = 1e-14;
if ~isempty(varargin)
    acc = varargin{1};
end

% triangulated convex hull. It is not uniquely defined.
faces = convhulln(points);

% compute centroid of the nodes
pointsCentroid = centroid(points);

% number of base triangular faces
N = size(faces, 1);

% compute normals of given faces
normals = planeNormal(createPlane(...
    points(faces(:,1),:), points(faces(:,2),:), points(faces(:,3),:)));

% initialize empty faces
newFaces = {};

% Processing flag for each triangle
% 1 : triangle to process, 0 : already processed
% in the beginning, every triangle face need to be processed
flag = ones(N, 1);

% iterate on each triangular face of the convex hull
for iFace = 1:N

    % check if face was already performed
    if ~flag(iFace)
        continue;
    end

    % indices of faces with same normal
    ind = find(abs(vectorNorm3d(cross(repmat(normals(iFace, :), [N 1]), normals)))<acc);
    ind = ind(ind~=iFace);

    % keep only coplanar faces (test coplanarity of points in both face)
    ind2 = iFace;
    for j = 1:length(ind)
        if isCoplanar(points([faces(iFace,:) faces(ind(j),:)], :), acc)
            ind2 = [ind2 ind(j)]; %#ok<AGROW>
        end
    end

    % compute order of the vertices in current face
    faceVertices = unique(faces(ind2, :));
    [tmp, I]  = angleSort3d(points(faceVertices, :)); %#ok<ASGLU>

    % create the new face, ensuring it is a row vector
    face = faceVertices(I);
    face = face(:)';

    % ensure face has normal pointing outwards
    outerNormal = meshFaceCentroids(points, face) - pointsCentroid;
    if dot(meshFaceNormals(points, face), outerNormal, 2) < 0
        face = face([1 end:-1:2]);
    end

    % add a new face to the list
    newFaces = [newFaces {face}]; %#ok<AGROW>

    % mark processed faces
    flag(ind2) = 0;
end


function [ o, x, w ] = en_r2_05_6 ( n )

%*****************************************************************************80
%
%% EN_R2_05_6 implements the Stroud rule 5.6 for region EN_R2.
%
%  Discussion:
%
%    The rule has order O = ( N + 1 ) * 2^N.
%
%    The rule has precision P = 5.
%
%    EN_R2 is the entire N-dimensional space with weight function
%
%      w(x) = exp ( - x1^2 - x2^2 ... - xn^2 )
%
%    The rule requires 5 <= N.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 January 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Arthur Stroud,
%    Approximate Calculation of Multiple Integrals,
%    Prentice Hall, 1971,
%    ISBN: 0130438936,
%    LC: QA311.S85.
%
%  Parameters:
%
%    Input, integer N, the spatial dimension.
%    5 <= N.
%
%    Output, integer O, the order.
%
%    Output, real X(N,O), the abscissas.
%
%    Output, real W(O), the weights.
%
  if ( n < 5 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'EN_R2_05_6 - Fatal error!\n' );
    fprintf ( 1, '  5 <= N is required.\n' );
    error ( 'EN_R2_05_6 - Fatal error!' );
  end

  o = 2^n * ( n + 1 );
  volume = sqrt ( pi^n );

  a = volume / 2^n / ( n + 1 );

  r = sqrt ( ( n - sqrt ( 2 ) + ( n - 1 ) * sqrt ( 2 * ( n + 1 ) ) ) / 2 / n );
  s = sqrt ( ( n - sqrt ( 2 ) -             sqrt ( 2 * ( n + 1 ) ) ) / 2 / n );
  t = sqrt ( ( 1 + sqrt ( 2 ) ) / 2 );

  x = zeros ( n, o );
  w = zeros ( o, 1 );

  k = 0;
%
%  N * 2^N points.
%
  for i = 1 : n

    k = k + 1;
    x(1:n,k) = - s;
    x(i,k)   = - r;
    w(k) = a;

    more = 1;

    while ( more )
      more = 0;
      for j = n : -1 : 1
        if ( x(j,k) < 0.0 )
          k = k + 1;
          x(1:n,k) = x(1:n,k-1);
          x(j,k)     =   abs ( x(j,k) );
          x(j+1:n,k) = - abs ( x(j+1:n,k) );
          w(k) = a;
          more = 1;
          break;
        end
      end
    end

  end
%
%  2^N points.
%
  k = k + 1;
  x(1:n,k) = - t;
  w(k) = a;
  more = 1;
  while ( more )
    more = 0;
    for j = n : -1 : 1
      if ( x(j,k) < 0.0 )
        k = k + 1;
        x(1:n,k) = x(1:n,k-1);
        x(j,k)     =   abs ( x(j,k) );
        x(j+1:n,k) = - abs ( x(j+1:n,k) );
        w(k) = a;
        more = 1;
        break;
      end
    end
  end

  return
end


%TROTX Rotation about X axis
%
% T = TROTX(THETA) is a homogeneous transformation (4x4) representing a rotation
% of THETA radians about the x-axis.
%
% T = TROTX(THETA, 'deg') as above but THETA is in degrees.
%
% Notes::
% - Translational component is zero.
%
% See also ROTX, TROTY, TROTZ, TROT2.

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

function T = trotx(t, varargin)
	T = [rotx(t, varargin{:}) [0 0 0]'; 0 0 0 1];


function idxRRtoBeRemoved = FindSpikesInRR(RR, th)
%
% idxRRtoBeRemoved = FindSpikesInRR(RR, th)
%
% OVERVIEW : Code used to clean RR intervals that change more than a given
% threshold (eg., th = 0.2 = 20%) with respect to the median value of the
% previous 5 and next 5 RR intervals (using a forward-backward approach).
%
% INPUTS:
%       RR : a single row of rr interval data in seconds
%       th : threshold percent limit of change from one interval to the next
% OUTPUTS:
%       idxRRtoBeRemoved : a single vector of indexes related to RR
%                          intervals corresponding to a change > th
%
%   DEPENDENCIES & LIBRARIES:
%       PhysioNet Cardiovascular Signal Toolbox
%       https://github.com/cliffordlab/PhysioNet-Cardiovascular-Signal-Toolbox
%
%   REFERENCE:
%       Vest et al. "An Open Source Benchmarked HRV Toolbox for Cardiovascular
%       Waveform and Interval Analysis" Physiological Measurement (In Press), 2018.
%
%	REPO:
%       https://github.com/cliffordlab/PhysioNet-Cardiovascular-Signal-Toolbox
%
%   Written by Giulia Da Poian (giulia.dap@gmail.com), 09-13-2017
%
%   LICENSE:
%       This software is offered freely and without warranty under
%       the GNU (v3 or later) public license. See license file for
%       more information
%

if size(RR,1)>size(RR,2)
    RR = RR';
end

% Forward search
FiveRR_MedianVal = medfilt1(RR,5); % compute as median RR(-i-2: i+2)
% shift of three position to aligne with to corresponding RR
FiveRR_MedianVal = [RR(1:5) FiveRR_MedianVal(3:end-3)];
rr_above_th = find((abs(RR-FiveRR_MedianVal)./FiveRR_MedianVal)>=th);

RR_forward = RR;
RR_forward(rr_above_th) = NaN;

% Backward search
RRfilpped = fliplr(RR);

FiveRR_MedianVal = medfilt1(RRfilpped,5); % compute as median RR(-i-2: i+2)
% shift of three position to aligne with to corresponding RR
FiveRR_MedianVal = [RRfilpped(1:5) FiveRR_MedianVal(3:end-3)];
rr_above_th = find(abs(RRfilpped-FiveRR_MedianVal)./FiveRR_MedianVal>=th);
rr_above_th = sort(length(RR)-rr_above_th+1);

RR_backward = RRfilpped;
RR_backward(rr_above_th) = NaN;

% Combine

idxRRtoBeRemoved = (isnan(RR_forward) & isnan(RR_backward));

end


function x = VBA_spm_invFcdf(F,v,w)
% Inverse Cumulative Distribution (CDF) of F (Fisher-Snedecor) distribution
% FORMAT x = spm_invFpdf(F,df)
% FORMAT x = spm_invFpdf(F,v,w)
%
% F  - CDF (lower tail p-value)
% df - Degrees of freedom, concatenated along last dimension
%      Eg. Scalar (or column vector) v & w. Then df=[v,w];
% v  - Shape parameter 1 /   numerator degrees of freedom (v>0)
% w  - Shape parameter 2 / denominator degrees of freedom (w>0)
% x  - F-variate   (F has range [0,Inf) )
%__________________________________________________________________________
%
% spm_Fcdf implements the inverse Cumulative Distribution Function
% for the F-distribution.
%
% Definition:
%--------------------------------------------------------------------------
% The CDF F(x) of the F distribution with degrees of freedom v & w,
% defined for positive integer degrees of freedom v & w, is the
% probability that a realisation of an F random variable X has value
% less than x F(x)=Pr{X<x} for X~F(v,w). The F-distribution is defined
% for v>0 & w>0, and for x in [0,Inf) (See Evans et al., Ch16).
%
% Variate relationships: (Evans et al., Ch16 & 37)
%--------------------------------------------------------------------------
% The square of a Student's t variate with w degrees of freedom is
% distributed as an F-distribution with [1,w] degrees of freedom.
%
% For X an F-variate with v,w degrees of freedom, w/(w+v*X^2) has
% distribution related to a Beta random variable with shape parameters
% w/2 & v/2, as described below.
%
% Algorithm:
%--------------------------------------------------------------------------
% Using the routine spm_invBcdf for the Beta distribution, with
% appropriate parameters:  The CDF of the F-distribution with v,w
% degrees of freedom is related to the incomplete beta function by:
%       Pr(X<x) = 1 - betainc(w/(w+v*x^2),w/2,v/2)
% See Abramowitz & Stegun, 26.6.2; Press et al., Sec6.4 for
% definitions of the incomplete beta function. The relationship is
% easily verified by substituting for w/(w+v*x^2) in the integral of the
% incomplete beta function.
%
%
% References:
%--------------------------------------------------------------------------
% Evans M, Hastings N, Peacock B (1993)
%       "Statistical Distributions"
%        2nd Ed. Wiley, New York
%
% Abramowitz M, Stegun IA, (1964)
%       "Handbook of Mathematical Functions"
%        US Government Printing Office
%
% Press WH, Teukolsky SA, Vetterling AT, Flannery BP (1992)
%       "Numerical Recipes in C"
%        Cambridge
%
%__________________________________________________________________________
% Copyright (C) 1993-2011 Wellcome Trust Centre for Neuroimaging

% Andrew Holmes
% $Id: spm_invFcdf.m 4182 2011-02-01 12:29:09Z guillaume $

%-Format arguments, note & check sizes
%--------------------------------------------------------------------------
if nargin<2, error('Insufficient arguments'), end

%-Unpack degrees of freedom v & w from single df parameter (v)
if nargin<3
    vs = size(v);
    if prod(vs)==2
        %-DF is a 2-vector
        w = v(2); v = v(1);
    elseif vs(end)==2
        %-DF has last dimension 2 - unpack v & w
        nv = prod(vs);
        w  = reshape(v(nv/2+1:nv),vs(1:end-1));
        v  = reshape(v(1:nv/2)   ,vs(1:end-1));
    else
        error('Can''t unpack both df components from single argument')
    end
end

%-Check argument sizes
ad = [ndims(F);ndims(v);ndims(w)];
rd = max(ad);
as = [[size(F),ones(1,rd-ad(1))];...
      [size(v),ones(1,rd-ad(2))];...
      [size(w),ones(1,rd-ad(3))]];
rs = max(as);
xa = prod(as,2)>1;
if sum(xa)>1 && any(any(diff(as(xa,:)),1))
    error('non-scalar args must match in size');
end

%-Computation
%--------------------------------------------------------------------------
%-Initialise result to zeros
x = zeros(rs);

%-Only defined for F in [0,1] & strictly positive v & w.
% Return NaN if undefined.
md = ( F>=0  &  F<=1  &  v>0  &  w>0 );
if any(~md(:))
    x(~md) = NaN;
    warning('Returning NaN for out of range arguments');
end

%-Special cases: x=0 when F=0, x=Inf when F=1
x(md & F==1) = Inf;

%-Compute where defined & not special case
Q  = find( md  &  F>0  &  F<1 );
if isempty(Q), return, end
if xa(1), QF=Q; else QF=1; end
if xa(2), Qv=Q; else Qv=1; end
if xa(3), Qw=Q; else Qw=1; end

%-Compute
bQ   = VBA_spm_invBcdf(1-F(QF),w(Qw)/2,v(Qv)/2);
x(Q) = (w(Qw)./bQ -w(Qw))./v(Qv);


function [ signal ] = msk_mod( bit_stream,frequency,Tb,Eb )
% %UNTITLED Summary of this function goes here
% %   Detailed explanation goes here

% clc;
% clear all;
% close all;
%
% % bitstream to be tx
% bit_stream=[1 0 0 1 0 0 1 1];
%
f=frequency;
%
% % carrier frequency
% f=30;
%
% % bit energy
%
% Eb=1;
% %
% % bit period
% Tb=0.001;

%time
t=0.001 : .001 : 1;

% time for p(t)

% t2=0.01:.01:(2*Tb);

nn=length(bit_stream);

amp=sqrt(2*Eb);

% make the data stream length even
if (mod(nn,2)~=0)
    bit_stream(1,nn+1)=0;
    nn=nn+1;
end

% length of bit stream
N=length(bit_stream);

m_i=zeros(1,N/2);% Even bit stream
m_q=zeros(1,N/2);% Odd bit stream

a=1;
% Generate the odd and even stream from the input bit stream
% in bipolar nrz form
for i=1:1:N/2
    if (bit_stream(1,a)==0)
        m_i(1,i)=-1;
    else
        m_i(1,i)=1;
    end
    a=a+1;
      if (bit_stream(1,a)==0)
        m_q(1,i)=-1;
    else
        m_q(1,i)=1;
    end
    a=a+1;
end

Smsk=zeros(N/2,1000);

% for loop=1:N/2
%     if (m_i(1,loop)==1)
%         fi_k=0;
%     else
%         fi_k=pi;
%     end
%
%     Smsk(loop,:)=amp*cos(2*pi*f*t-(m_i(1,loop)*m_q(1,loop)*pi*(t/(2*Tb)))+fi_k);
% end
%
% signal=column_to_row(Smsk);

%
for loop =1:N/2
    Smsk(loop,:)=(amp*(m_i(1,loop)*sin(2*pi*t/(4*Tb))).*cos(2*pi*f*t)) + (amp*(m_q(1,loop)*cos(2*pi*(t/(4*Tb)))).*sin(2*pi*f*t));
end
signal=column_to_row(Smsk);

end


function [in3] = cc2in3(cc)
% Convert volume from cubic centimeters to cubic inches.
% Chad Greene 2012
in3 = cc*0.061023744095;


classdef SimpAllInterpolationImplicit < MaterialInterpolation

    properties (Access = protected)
       dmu0
       dmu1
       dk0
       dk1
    end

    methods (Access = protected)

        function [mS,dmS] = computeMuSymbolicFunctionAndDerivative(obj)
            s.f0     = obj.matProp.mu0;
            s.f1     = obj.matProp.mu1;
            s.df0    = obj.dmu0;
            s.df1    = obj.dmu1;
            [mS,dmS] = obj.computeParameterInterpolationAndDerivative(s);
        end

        function [kS,dkS] = computeKappaSymbolicFunctionAndDerivative(obj)
            s.f0     = obj.matProp.kappa0;
            s.f1     = obj.matProp.kappa1;
            s.df0    = obj.dk0;
            s.df1    = obj.dk1;
            [kS,dkS] = obj.computeParameterInterpolationAndDerivative(s);
        end

        function [f,df] = computeParameterInterpolationAndDerivative(obj,s)
            c     = obj.computeCoefficients(s);
            rho   = sym('rho','positive');
            fSym  = obj.rationalFunction(c,rho);
            dfSym = obj.rationalFunctionDerivative(c,rho);
            f     = simplify(fSym);
            df    = simplify(dfSym);
        end

        function c = computeCoefficients(obj,s)
            f1    = s.f1;
            f0    = s.f0;
            df1   = s.df1;
            df0   = s.df0;
            c1    = sym('c1','real');
            c2    = sym('c2','real');
            c3    = sym('c3','real');
            c4    = sym('c4','real');
            coef  = [c1 c2 c3 c4];
            r1    = obj.matProp.rho1;
            r0    = obj.matProp.rho0;
            eq(1) = obj.rationalFunction(coef,r1) - f1;
            eq(2) = obj.rationalFunction(coef,r0) - f0;
            eq(3) = obj.rationalFunctionDerivative(coef,r1) - df1;
            eq(4) = obj.rationalFunctionDerivative(coef,r0) - df0;
            c     = solve(eq,[c1,c2,c3,c4]);
            c     = struct2cell(c);
            c     = [c{:}];
        end

    end

    methods (Access = protected, Static)

        function r = rationalFunction(coef,rho)
            c1  = coef(1);
            c2  = coef(2);
            c3  = coef(3);
            c4  = coef(4);
            num = (c1*rho^2 + c2*rho + 1);
            den = (c4 + rho*c3);
            r   = num/den;
        end

        function dr = rationalFunctionDerivative(coef,rho)
            c1  = coef(1);
            c2  = coef(2);
            c3  = coef(3);
            c4  = coef(4);
            n1  = c2 + 2*rho*c1;
            d1  = c4 + rho*c3;
            dr1 = n1/d1;
            n2  = -c3*(c1*rho^2 + c2*rho + 1);
            d2  = (c4 + rho*c3)^2;
            dr2 = n2/d2;
            dr  = dr1 + dr2;
        end

    end

end


function [tt] = tt_qtoepl(x, d)

% returns the multilevel Toeplitz matrix tt generated by the multi-dimensional input vector x
% in the QTT format
%
% The size of the input vector is 2^(d(1) + 1) x ... x 2^(d(D) + 1),
% The output matrix is square of order 2^d(1) x ... x 2^d(D).
% The (i_1,...i_D)-th component of x is not used if at least one of i_1,...,i_D is equal to 1
% (e. g., in the one-dimensional case the first component of x is not used)
%
% April 20, 2011
% Vladimir Kazeev
% vladimir.kazeev@gmail.com
% INM RAS
% Moscow, Russia
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% For details please see the preprint
% http://www.mis.mpg.de/publications/preprints/2011/prepr2011-36.html
% Vladimir A. Kazeev, Boris N. Khoromskij and Eugene E. Tyrtyshnikov
% Multilevel Toeplitz matrices generated by QTT tensor-structured vectors and convolution with logarithmic complexity
% January 12, 2012
% Vladimir Kazeev,
% Seminar for Applied Mathematics, ETH Zurich
% vladimir.kazeev@sam.math.ethz.ch
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

D = numel(d);
tt = tt_qshiftstack(d);

sz=[];
for K=1:D
	sz = [sz; 4*ones(d(K),1), 2*ones(d(K),1); 1, 2];
end

tt = tt_qreshape(tt, 3, sz);
tt = tt_matrix(tt) * x;

tt1 = cell(sum(d),1);
ind = 0;
ind1 = 0;
for K = 1:D
	for k = 1:d(K)-1
		tt1{ind1 + k} = tt{ind + k};
	end

	p = size(tt{ind + d(K)}, 1);
	q = size(tt{ind + d(K)}, 3);
	r = size(tt{ind + d(K) + 1}, 3);
	tt1{ind1 + d(K)} = reshape(reshape(permute(tt{ind+d(K)},[2, 1, 3]),[4*p,q])*...
        reshape(permute(tt{ind+d(K)+1},[2, 1, 3]),[q,r]),[4,p,r]);
    tt1{ind1 + d(K)} = permute(tt1{ind1 + d(K)}, [2, 1, 3]);
	ind=ind+d(K)+1;
	ind1=ind1+d(K);
end
tt = tt1;
tt1 = tt_tensor;
tt = cell2core(tt1, tt);
tt = tt_matrix(tt);
return
end


function [y,t]=schmitt(x,thresh,minwid)
% Pass input signal X through a schmitt trigger
% SCHMITT(X,[LOW HIGH]) gives low and high thresholds. LOW and HIGH can be
% scalars or can be vectors specifiying different thresholds for each X element.
%
% SCHMITT(X,HYSTERESIS) specifies the thresholds as MAX-DELTA and MIN+DELTA where
% DELTA=(MAX-MIN)*(1-HYSTERESIS)/2 and MAX and MIN are the max and min of X.
% HYSTERESIS must be in the range 0 to 1 and represents the fraction of MAX-MIN between
% the two threshold values; default is 0.5.
%
% MINWID specifies the minimum width of a pulse (in samples). Pulses thinner than this will
% be ignored.
%
% Output Y takes values -1, +1 according to whether X<LOW or X>HIGH most recently.
% Y may be 0 for an initial segment if neither condition is initially true.
% For [Y,T]=SCHMITT(...) Y contains alternate +1 and -1 values and T contains
% the sample numbers at which X crossed the thresholds.

%      Copyright (C) Mike Brookes 1998
%      Version: $Id: schmitt.m,v 1.4 2007/05/04 07:01:39 dmb Exp $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if nargin<3
    minwid=0;
    if nargin<2
        thresh=0.5;
    end
end
if length(thresh)<2
    xmax=max(x);
    xmin=min(x);
    low=(xmax-xmin)*(1-thresh)/2;
    high=xmax-low;
    low=xmin+low;
else
    low=thresh(1);
    high=thresh(2);
end
c=(x>high)-(x<low);
c(2:end)=c(2:end).*(c(2:end)~=c(1:end-1));
t=find(c);
t(1+find(c(t(2:end))==c(t(1:end-1))))=[]; % remove duplicates
if minwid>=1
    t(t(2:end)-t(1:end-1)<minwid)=[];
    t(1+find(c(t(2:end))==c(t(1:end-1))))=[]; % remove duplicates
end
if nargout>1 y=c(t);
else
    y=zeros(size(c));
    if ~isempty(t)
        y(t)=2*c(t);
        y(t(1))=c(t(1));
        y=cumsum(y);
    end
end
if ~nargout
        xmax=max(x);
    xmin=min(x);
    if high-low<0.1*(xmax-xmin)
        high=xmax;
        low=xmin
    end
    plot([x(:) low+(y(:)+1)*(high-low)/2]);
end


function score = DM(Population,optimum)
% <max> <multi/many> <real/integer/label/binary/permutation> <large/none> <constrained/none> <expensive/none> <multimodal/none> <sparse/none> <dynamic/none>
% Metric for diversity

%------------------------------- Reference --------------------------------
% K. Deb and S. Jain, Running performance metrics for evolutionary
% multi-objective optimization, KanGAL Report 2002004, 2002.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    PopObj = Population.best.objs;
    if size(PopObj,2) ~= size(optimum,2)
        score = nan;
    else
        fmax  = max(optimum,[],1);
        fmin  = min(optimum,[],1);
        H     = calGrid(optimum(:,1:end-1),fmax(1:end-1),fmin(1:end-1),size(PopObj,1));
        h     = H & calGrid(PopObj(:,1:end-1),fmax(1:end-1),fmin(1:end-1),size(PopObj,1));
        score = calM(h,H)./calM(H,H);
    end
end

function h = calGrid(P,fmax,fmin,div)
% Determine whether each grid has at least one point

    [N,M] = size(P);
    d     = (fmax-fmin)./div;
    GLoc  = ceil((P-repmat(fmin,N,1))./repmat(d,N,1));
    GLoc  = max(1,GLoc);
    h     = zeros(M,div);
    for i = 1 : M
        h(i,:) = ismember(1:div,GLoc(:,i));
    end
end

function m = calM(h,H)
% Calculate the value function m()

    M = size(h,1);
    h = [ones(M,1),h,ones(M,1)];
    H = [ones(M,1),H,ones(M,1)];
    m = 0;
    for i = 1 : M
        for j = 2 : size(h,2)-1
            if H(i,j)
                if h(i,j)
                    if h(i,j-1)
                        if h(i,j+1)
                            m = m + 1;
                        else
                            m = m + 0.67;
                        end
                    else
                        if h(i,j+1)
                            m = m + 0.67;
                        else
                            m = m + 0.75;
                        end
                    end
                else
                    if h(i,j-1)
                        if h(i,j+1)
                            m = m + 0.75;
                        else
                            m = m + 0.5;
                        end
                    else
                        if h(i,j+1)
                            m = m + 0.5;
                        else
                            m = m + 0;
                        end
                    end
                end
            end
        end
    end
end


function [varargout] = likGumbel(sign, hyp, y, mu, s2, inf, i)

% likGumbel - Gumbel likelihood function for extremal value regression.
% The expression for the likelihood is
%   likGumbel(t) = exp(-z-exp(-z))/be, z = ga+s*(y-t)/be, be = sn*sqrt(6)/pi
% where s={+1,-1} is a sign switching between left and right skewed, ga is the
% Euler-Mascheroni constant, y is the mean, sn^2 is the variance.
% The skewness and kurtosis of likGumbel are 1.14*s and 2.4, respectively.
%
% The hyperparameters are:
%
% hyp = [ log(sn)  ]
%
% Several modes are provided, for computing likelihoods, derivatives and moments
% respectively, see likFunctions.m for the details. In general, care is taken
% to avoid numerical issues when the arguments are extreme.
%
% Copyright (c) by Hannes Nickisch, 2013-11-01.
%
% See also LIKFUNCTIONS.M.

if nargin<4, varargout = {'1'}; return; end   % report number of hyperparameters
if sign=='-', s = -1; else s = 1; end                 % extract sign of skewness

sn2 = exp(2*hyp);                                      % extract hyperparameters
ga = 0.5772156649;                                   % Euler-Mascheroni constant
be = sqrt(6*sn2)/pi;
lZ = -log(be);

if nargin<6                              % prediction mode if inf is not present
  if numel(y)==0,  y = zeros(size(mu)); end
  s2zero = 1; if nargin>4, if norm(s2)>0, s2zero = 0; end, end         % s2==0 ?
  if s2zero                                         % log probability evaluation
    lp = likGumbel(sign, hyp, y, mu, [], 'infLaplace'); s2 = 0;
  else                                                              % prediction
    lp = likGumbel(sign, hyp, y, mu, s2, 'infEP');
  end
  ymu = {}; ys2 = {};
  if nargout>1
    ymu = mu;                                                   % first y moment
    if nargout>2
      ys2 = s2 + sn2;                                          % second y moment
    end
  end
  varargout = {lp,ymu,ys2};
else
  switch inf
  case 'infLaplace'
    z = ga+s*(y-mu)/be; emz = exp(-z);
    if nargin<7                                             % no derivative mode
      dlp = {}; d2lp = {}; d3lp = {};
      lp = lZ -z -emz;
      if nargout>1
        dz = -s/be;                                                     % dz/dmu
        dlp = dz*(emz-1);                    % dlp, derivative of log likelihood
        if nargout>2                    % d2lp, 2nd derivative of log likelihood
          d2lp = -dz^2*emz;
          if nargout>3                  % d3lp, 3rd derivative of log likelihood
            d3lp = dz^3*emz;
          end
        end
      end
      varargout = {lp,dlp,d2lp,d3lp};
    else                                             % derivative w.r.t. log(sn)
      dz = -s/be;                                                       % dz/dmu
      dzs = -s*(y-mu)/be;                                          % dz/dlog(sn)
      lp_dhyp   =  dzs.*(emz-1) -1;
      dlp_dhyp  = dz*(1-emz.*(1+dzs));
      d2lp_dhyp = dz^2*emz.*(2+dzs);
      varargout = {lp_dhyp,dlp_dhyp,d2lp_dhyp};
    end

  case 'infEP'
    if nargout>1
      error('infEP not supported since likT is not log-concave')
    end
    n = max([length(y),length(mu),length(s2)]); on = ones(n,1);
    y = y(:).*on; mu = mu(:).*on; sig = sqrt(s2(:)).*on;          % vectors only
    % since we are not aware of an analytical expression of the integral,
    % we use Gaussian-Hermite quadrature
    N = 20; [t,w] = gauher(N); oN = ones(1,N);
    lZ = likGumbel(sign, hyp, y*oN, sig*t'+mu*oN, []);
    lZ = log_expA_x(lZ,w); % log( exp(lZ)*w )
    varargout = {lZ};

  case 'infVB'
    error('infVB not supported')
  end
end

%  computes y = log( exp(A)*x ) in a numerically safe way by subtracting the
%  maximal value in each row to avoid cancelation after taking the exp
function y = log_expA_x(A,x)
  N = size(A,2);  maxA = max(A,[],2);      % number of columns, max over columns
  y = log(exp(A-maxA*ones(1,N))*x) + maxA;  % exp(A) = exp(A-max(A))*exp(max(A))


function dz = dynamics_6_link(~,z,P)
%DZ = DYNAMICS_6_LINK(T,Z,P)
%
%FUNCTION:  This function computes the dynamics of a 6
%    link pendulum, and is designed to be called from ode45.
%    The model allows for arbitrary mass and inertia for each
%    link, but no friction or actuation
%
%INPUTS:
%    t = time. Dummy input for ode45. Not used.
%    z = [12 X nTime]  matrix of states
%    P = struct of parameters
%OUTPUTS:
%    dz = [12 X nTime]  matrix of state derivatives
%
%NOTES:
%    This file was automatically generated by writeDynamics.m

g  = P.g ; %gravity
m1 = P.m(1); % Link 1 mass
m2 = P.m(2); % Link 2 mass
m3 = P.m(3); % Link 3 mass
m4 = P.m(4); % Link 4 mass
m5 = P.m(5); % Link 5 mass
m6 = P.m(6); % Link 6 mass
l1 = P.l(1); % Link 1 length
l2 = P.l(2); % Link 2 length
l3 = P.l(3); % Link 3 length
l4 = P.l(4); % Link 4 length
l5 = P.l(5); % Link 5 length
l6 = P.l(6); % Link 6 length
I1 = P.I(1); % Link 1 moment of inertia about its center of mass
I2 = P.I(2); % Link 2 moment of inertia about its center of mass
I3 = P.I(3); % Link 3 moment of inertia about its center of mass
I4 = P.I(4); % Link 4 moment of inertia about its center of mass
I5 = P.I(5); % Link 5 moment of inertia about its center of mass
I6 = P.I(6); % Link 6 moment of inertia about its center of mass
d1 = P.d(1); % Link 1 distance between center of mass and parent joint
d2 = P.d(2); % Link 2 distance between center of mass and parent joint
d3 = P.d(3); % Link 3 distance between center of mass and parent joint
d4 = P.d(4); % Link 4 distance between center of mass and parent joint
d5 = P.d(5); % Link 5 distance between center of mass and parent joint
d6 = P.d(6); % Link 6 distance between center of mass and parent joint

nTime = size(z,2);
dz = zeros(size(z));
M = zeros(6,6,nTime);
f = zeros(6,nTime);

th1 = z(1,:);
th2 = z(2,:);
th3 = z(3,:);
th4 = z(4,:);
th5 = z(5,:);
th6 = z(6,:);

dth1 = z(7,:);
dth2 = z(8,:);
dth3 = z(9,:);
dth4 = z(10,:);
dth5 = z(11,:);
dth6 = z(12,:);

dz(1,:) = dth1;
dz(2,:) = dth2;
dz(3,:) = dth3;
dz(4,:) = dth4;
dz(5,:) = dth5;
dz(6,:) = dth6;

M(1,1,:) = - I1 - d1.^2.*m1 - l1.^2.*m2 - l1.^2.*m3 - l1.^2.*m4 - l1.^2.*m5 - l1.^2.*m6;
M(1,2,:) = -l1.*cos(th1 - th2).*(d2.*m2 + l2.*m3 + l2.*m4 + l2.*m5 + l2.*m6);
M(1,3,:) = -l1.*cos(th1 - th3).*(d3.*m3 + l3.*m4 + l3.*m5 + l3.*m6);
M(1,4,:) = -l1.*cos(th1 - th4).*(d4.*m4 + l4.*m5 + l4.*m6);
M(1,5,:) = -l1.*cos(th1 - th5).*(d5.*m5 + l5.*m6);
M(1,6,:) = -d6.*l1.*m6.*cos(th1 - th6);
M(2,1,:) = -l1.*cos(th1 - th2).*(d2.*m2 + l2.*m3 + l2.*m4 + l2.*m5 + l2.*m6);
M(2,2,:) = - I2 - d2.^2.*m2 - l2.^2.*m3 - l2.^2.*m4 - l2.^2.*m5 - l2.^2.*m6;
M(2,3,:) = -l2.*cos(th2 - th3).*(d3.*m3 + l3.*m4 + l3.*m5 + l3.*m6);
M(2,4,:) = -l2.*cos(th2 - th4).*(d4.*m4 + l4.*m5 + l4.*m6);
M(2,5,:) = -l2.*cos(th2 - th5).*(d5.*m5 + l5.*m6);
M(2,6,:) = -d6.*l2.*m6.*cos(th2 - th6);
M(3,1,:) = -l1.*cos(th1 - th3).*(d3.*m3 + l3.*m4 + l3.*m5 + l3.*m6);
M(3,2,:) = -l2.*cos(th2 - th3).*(d3.*m3 + l3.*m4 + l3.*m5 + l3.*m6);
M(3,3,:) = - I3 - d3.^2.*m3 - l3.^2.*m4 - l3.^2.*m5 - l3.^2.*m6;
M(3,4,:) = -l3.*cos(th3 - th4).*(d4.*m4 + l4.*m5 + l4.*m6);
M(3,5,:) = -l3.*cos(th3 - th5).*(d5.*m5 + l5.*m6);
M(3,6,:) = -d6.*l3.*m6.*cos(th3 - th6);
M(4,1,:) = -l1.*cos(th1 - th4).*(d4.*m4 + l4.*m5 + l4.*m6);
M(4,2,:) = -l2.*cos(th2 - th4).*(d4.*m4 + l4.*m5 + l4.*m6);
M(4,3,:) = -l3.*cos(th3 - th4).*(d4.*m4 + l4.*m5 + l4.*m6);
M(4,4,:) = - I4 - d4.^2.*m4 - l4.^2.*m5 - l4.^2.*m6;
M(4,5,:) = -l4.*cos(th4 - th5).*(d5.*m5 + l5.*m6);
M(4,6,:) = -d6.*l4.*m6.*cos(th4 - th6);
M(5,1,:) = -l1.*cos(th1 - th5).*(d5.*m5 + l5.*m6);
M(5,2,:) = -l2.*cos(th2 - th5).*(d5.*m5 + l5.*m6);
M(5,3,:) = -l3.*cos(th3 - th5).*(d5.*m5 + l5.*m6);
M(5,4,:) = -l4.*cos(th4 - th5).*(d5.*m5 + l5.*m6);
M(5,5,:) = - I5 - d5.^2.*m5 - l5.^2.*m6;
M(5,6,:) = -d6.*l5.*m6.*cos(th5 - th6);
M(6,1,:) = -d6.*l1.*m6.*cos(th1 - th6);
M(6,2,:) = -d6.*l2.*m6.*cos(th2 - th6);
M(6,3,:) = -d6.*l3.*m6.*cos(th3 - th6);
M(6,4,:) = -d6.*l4.*m6.*cos(th4 - th6);
M(6,5,:) = -d6.*l5.*m6.*cos(th5 - th6);
M(6,6,:) = - I6 - d6.^2.*m6;

f(1,:) = - d1.*g.*m1.*cos(th1) - g.*l1.*m2.*cos(th1) - g.*l1.*m3.*cos(th1) - g.*l1.*m4.*cos(th1) - g.*l1.*m5.*cos(th1) - g.*l1.*m6.*cos(th1) - d2.*dth2.^2.*l1.*m2.*sin(th1 - th2) - d3.*dth3.^2.*l1.*m3.*sin(th1 - th3) - d4.*dth4.^2.*l1.*m4.*sin(th1 - th4) - d5.*dth5.^2.*l1.*m5.*sin(th1 - th5) - d6.*dth6.^2.*l1.*m6.*sin(th1 - th6) - dth2.^2.*l1.*l2.*m3.*sin(th1 - th2) - dth2.^2.*l1.*l2.*m4.*sin(th1 - th2) - dth2.^2.*l1.*l2.*m5.*sin(th1 - th2) - dth2.^2.*l1.*l2.*m6.*sin(th1 - th2) - dth3.^2.*l1.*l3.*m4.*sin(th1 - th3) - dth3.^2.*l1.*l3.*m5.*sin(th1 - th3) - dth3.^2.*l1.*l3.*m6.*sin(th1 - th3) - dth4.^2.*l1.*l4.*m5.*sin(th1 - th4) - dth4.^2.*l1.*l4.*m6.*sin(th1 - th4) - dth5.^2.*l1.*l5.*m6.*sin(th1 - th5);
f(2,:) = d2.*dth1.^2.*l1.*m2.*sin(th1 - th2) - g.*l2.*m3.*cos(th2) - g.*l2.*m4.*cos(th2) - g.*l2.*m5.*cos(th2) - g.*l2.*m6.*cos(th2) - d2.*g.*m2.*cos(th2) - d3.*dth3.^2.*l2.*m3.*sin(th2 - th3) - d4.*dth4.^2.*l2.*m4.*sin(th2 - th4) - d5.*dth5.^2.*l2.*m5.*sin(th2 - th5) - d6.*dth6.^2.*l2.*m6.*sin(th2 - th6) + dth1.^2.*l1.*l2.*m3.*sin(th1 - th2) + dth1.^2.*l1.*l2.*m4.*sin(th1 - th2) + dth1.^2.*l1.*l2.*m5.*sin(th1 - th2) + dth1.^2.*l1.*l2.*m6.*sin(th1 - th2) - dth3.^2.*l2.*l3.*m4.*sin(th2 - th3) - dth3.^2.*l2.*l3.*m5.*sin(th2 - th3) - dth3.^2.*l2.*l3.*m6.*sin(th2 - th3) - dth4.^2.*l2.*l4.*m5.*sin(th2 - th4) - dth4.^2.*l2.*l4.*m6.*sin(th2 - th4) - dth5.^2.*l2.*l5.*m6.*sin(th2 - th5);
f(3,:) = d3.*dth1.^2.*l1.*m3.*sin(th1 - th3) - g.*l3.*m4.*cos(th3) - g.*l3.*m5.*cos(th3) - g.*l3.*m6.*cos(th3) - d3.*g.*m3.*cos(th3) + d3.*dth2.^2.*l2.*m3.*sin(th2 - th3) - d4.*dth4.^2.*l3.*m4.*sin(th3 - th4) - d5.*dth5.^2.*l3.*m5.*sin(th3 - th5) - d6.*dth6.^2.*l3.*m6.*sin(th3 - th6) + dth1.^2.*l1.*l3.*m4.*sin(th1 - th3) + dth1.^2.*l1.*l3.*m5.*sin(th1 - th3) + dth1.^2.*l1.*l3.*m6.*sin(th1 - th3) + dth2.^2.*l2.*l3.*m4.*sin(th2 - th3) + dth2.^2.*l2.*l3.*m5.*sin(th2 - th3) + dth2.^2.*l2.*l3.*m6.*sin(th2 - th3) - dth4.^2.*l3.*l4.*m5.*sin(th3 - th4) - dth4.^2.*l3.*l4.*m6.*sin(th3 - th4) - dth5.^2.*l3.*l5.*m6.*sin(th3 - th5);
f(4,:) = d4.*dth1.^2.*l1.*m4.*sin(th1 - th4) - g.*l4.*m5.*cos(th4) - g.*l4.*m6.*cos(th4) - d4.*g.*m4.*cos(th4) + d4.*dth2.^2.*l2.*m4.*sin(th2 - th4) + d4.*dth3.^2.*l3.*m4.*sin(th3 - th4) - d5.*dth5.^2.*l4.*m5.*sin(th4 - th5) - d6.*dth6.^2.*l4.*m6.*sin(th4 - th6) + dth1.^2.*l1.*l4.*m5.*sin(th1 - th4) + dth1.^2.*l1.*l4.*m6.*sin(th1 - th4) + dth2.^2.*l2.*l4.*m5.*sin(th2 - th4) + dth2.^2.*l2.*l4.*m6.*sin(th2 - th4) + dth3.^2.*l3.*l4.*m5.*sin(th3 - th4) + dth3.^2.*l3.*l4.*m6.*sin(th3 - th4) - dth5.^2.*l4.*l5.*m6.*sin(th4 - th5);
f(5,:) = d5.*dth1.^2.*l1.*m5.*sin(th1 - th5) - g.*l5.*m6.*cos(th5) - d5.*g.*m5.*cos(th5) + d5.*dth2.^2.*l2.*m5.*sin(th2 - th5) + d5.*dth3.^2.*l3.*m5.*sin(th3 - th5) + d5.*dth4.^2.*l4.*m5.*sin(th4 - th5) - d6.*dth6.^2.*l5.*m6.*sin(th5 - th6) + dth1.^2.*l1.*l5.*m6.*sin(th1 - th5) + dth2.^2.*l2.*l5.*m6.*sin(th2 - th5) + dth3.^2.*l3.*l5.*m6.*sin(th3 - th5) + dth4.^2.*l4.*l5.*m6.*sin(th4 - th5);
f(6,:) = d6.*m6.*(dth1.^2.*l1.*sin(th1 - th6) - g.*cos(th6) + dth2.^2.*l2.*sin(th2 - th6) + dth3.^2.*l3.*sin(th3 - th6) + dth4.^2.*l4.*sin(th4 - th6) + dth5.^2.*l5.*sin(th5 - th6));

for i=1:nTime
    MM = M(:,:,i);  ff = f(:,i);
    dz(7:12,i) = -MM \ ff;
end

end


function [X, Sigma2] = Truncate(Z, tau)
% [Y, n, Sigma2] = Pro2TraceNorm(Z, tau);
% X = Z-Y;

%% new
[m, n] = size(Z);
if 2*m < n
    AAT = Z*Z';
    [S, Sigma2, D] = svd(AAT);
    Sigma2 = diag(Sigma2);
    V = sqrt(Sigma2);
    tol = max(size(Z)) * eps(max(V));
    n = sum(V > max(tol, tau));
    mid = max(V(1:n)-tau, 0) ./ V(1:n) ;
    X = (eye(m)-S(:, 1:n) * diag(mid) * S(:, 1:n)') * Z;
    return;
end
if m > 2*n
    [X, Sigma2] = Truncate(Z', tau);
    X = X';
    return;
end
[S,V,D] = svd(Z, 0);
Sigma2 = diag(V).^2;
n = sum(diag(V) > tau);
X = Z - S(:, 1:n) * diag(diag(V(1:n,1:n))-tau) * D(:, 1:n)';


function [ mat ] = tsgReadMatrix( filename )
%
% [ mat ] = tsgReadMatrix( filename )
%
% reads a matrix from a file format
%
% 3 4
% 1 2 3 4
% 5 6 7 8
% 9 10 11 12
%
% results in the matrix [ 1 2 3 4; 5 6 7 8; 9 10 11 12; ]
%

fid = fopen ( filename, 'rt' );

[ s ] = fscanf( fid, ' %d ', [1, 2 ] ); % load the number of points

Ni = s(1);
Nj = s(2);

if ( (Ni>0) && (Nj>0) )
    mat = zeros( Ni, Nj );
else
    mat = [];
end

for i = 1:Ni

    [ s ] = fscanf( fid, ' %f ', [1, Nj ] );

    mat(i,:) = s;

end;

fclose( fid );


function digit = r8_digit ( x, idigit )

%*****************************************************************************80
%
%% R8_DIGIT returns a particular decimal digit of an R8.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 April 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the number whose NDIG-th decimal digit
%    is desired.  If X is zero, all digits will be returned as 0.
%
%    Input, integer IDIGIT, the position of the desired decimal digit.
%    A value of 1 means the leading digit, a value of 2 the second digit
%    and so on.
%
%    Output, integer DIGIT, the value of the IDIGIT-th decimal digit of X.
%
  if ( x == 0.0 )
    digit = 0;
    return
  end

  if ( idigit <= 0 )
    digit = 0;
    return
  end
%
%  Force X to lie between 1 and 10.
%
  x = abs ( x );

  while ( x < 1.0 )
    x = x * 10.0;
  end

  while ( 10.0 <= x )
    x = x / 10.0;
  end

  for i = 1 : idigit
    ival = floor ( x );
    x = ( x - ival ) * 10.0;
  end

  digit = ival;

  return
end


function test09 ( dim_num, n, z )

%*****************************************************************************80
%
%% TEST09 tests SPHERE_MEASURE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 November 2008
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST09\n' );
  fprintf ( 1, '  SPHERE_MEASURE computes the SPHERE measure of quality.\n' );
  fprintf ( 1, '  Nonintersecting sphere volume    S = %14f\n', ...
    sphere_measure ( dim_num, n, z ) );

  return
end


% Fig. 6.49   Feedback Control of Dynamic Systems, 6e
%             Franklin, Powell, Emami
%

clf
clear all;
%close all;
clf

num=0.01*[20 1];
den=[1 0 0]+[0 num];
w=logspace(-3,1,100);
[m,p]=bode(num,den,w);
loglog(w,m);
axis([.01 1 .1 10])
text(.12,1.2,'T(j\omega)')
text(.031,.15,'S(j\omega)')
bodegrid;
hold on
% add line at mag = 0.707
w2=[.01 1];
mcl=[.707 .707];
loglog(w2,mcl,'r')

% now error FR
numE=[1 0 0];
denE=den;
[me,pe]=bode(numE,denE,w);
loglog(w,me)

xlabel('\omega (rad/sec)');
ylabel('Magnitude');
title('Fig. 6.49 Closed-loop frequency response.');
bodegrid;
hold off


function C=corrdelays(A, tdelay)
% C=corrdelays(A, tdelays)
% Computes correlation matrices of MxN matrix A (rows are time series) for
% time delayes specified in the 1xT vector TDELAY

[m,n]=size(A);
t=length(tdelay);
C=zeros(m,m,t);
for indext = 1:t
    Ashift = (circshift(A',tdelay(indext)))';
    C(:,:,indext)=corr(A',Ashift');
end


function [ A, D ] = SIC( Fopt, H, N , SNR)
[Nt,Ns] = size(Fopt);
M = Nt/N;
T = eye(N);
A = [];
D = [];
P = [];
R = [eye(M),zeros(M,M*(N-1))];
G = R*H'*H*R';
for n = 1:N
    [U,S,V] = svd(G);
    v1 = V(:,1);

    A = blkdiag( A, exp(1i*angle(v1))/sqrt(M) );
    D = blkdiag( D, norm(v1,1)/sqrt(M) );

%     P = [P, [zeros(M*(n-1),1);norm(v1,1)/M*exp(1i*angle(v1));zeros(M*(N-n),1)] ];
%     T = eye(size(H,1)) + SNR/Ns * H*P*P'*H';
%     R = [zeros(M,M*(n-1)),eye(M),zeros(M,M*(N-n))];
%     G = R*H'*inv(T)*H*R';

    G = G - SNR/Ns * S(1)^2 * v1*v1' / (1+SNR/Ns*S(1));
end


function srad = solarradiation(dem,lat,cs,r)
% PUPROSE: Calculate solar radiation for a digital elevation model (DEM)
%          over one year for clear sky conditions in W/m2
% -------------------------------------------------------------------
% USAGE: srad = solarradiation(dem,lat,cs)
% where: dem is the DEM to calculate hillshade for
%        lat is the latitude vector for the DEM - same size as size(dem,2)
%        cs is the cellsize in meters
%        r is the ground reflectance (global value or map, default is 0.2)
%
%       srad is the solar radiation in W/m2 over one year per grid cell
%
% EXAMPLE:
%       srad = solarradiation(peaks(50)*100,54.9:-0.1:50,1000,0.2);
%       - calculates the solar radiation for an example 50x50 peak surface.
%
% See also: GRADIENT, CART2POL
%
% Note: Follows the approach of Kumar et al 1997. Calculates clear sky
%       radiation corrected for the incident angle (selfshading) plus
%       diffuse and reflected radiation. Insolation is depending on time of year (and day),
%       latitude, elevation, slope and aspect.
%       Relief shading is not considered.
%       Script uses simple unweighed gradient of 4 nearest neighbours for slope
%       calculation.
%
% Reference: Kumar, L, Skidmore AK and Knowles E 1997: Modelling topographic variation in solar radiation in
%            a GIS environment. Int.J.Geogr.Info.Sys. 11(5), 475-497
%
%
% Felix Hebeler, Dept. of Geography, University Zurich, May 2008.

%% parameters
%It ;               % total hours of daily sunshine (calculated inline)
%M ;                % air mass ratio parameter (calculated inline)
%r = 0.20;          % ground reflectance coefficient (more sensible to give as input)
%L=lat;             %latitude
n = 1;              % timestep of calculation over sunshine hours: 1=hourly, 0.5=30min, 2=2hours etc
tau_a    = 365;     %length of the year in days
S0 = 1367;          % solar constant W m^-2   default 1367

dr= 0.0174532925;   % degree to radians conversion factor

%%  convert factors
[slop,asp]=get_ders(dem,cs);   % calculate slope and aspect in radians using given cellsize cs
[dummy,L]=meshgrid(1:size(dem,2),lat);   % grid latitude
clear dummy;
L=L*dr;                     % convert to radians
fcirc = 360*dr; % 360 degrees in radians

%% some setup calculations
srad=0;
sinL=sin(L);
cosL=cos(L);
tanL=tan(L);
sinSlop=sin(slop);
cosSlop=cos(slop);
cosSlop2=cosSlop.*cosSlop;
sinSlop2=sinSlop.*sinSlop;
sinAsp=sin(asp);
cosAsp=cos(asp);
term1 = ( sinL.*cosSlop - cosL.*sinSlop.*cosAsp);
term2 = ( cosL.*cosSlop + sinL.*sinSlop.*cosAsp);
term3 = sinSlop.*sinAsp;
%% loop over year
for d = 1:tau_a;
    %display(['Calculating melt for day ',num2str(d)])
    % clear sky solar radiation
    I0 = S0 * (1 + 0.0344*cos(fcirc*d/tau_a)); % extraterr rad per day
    % sun declination dS
    dS = 23.45 * dr* sin(fcirc * ( (284+d)/tau_a ) ); %in radians, correct/verified
    % angle at sunrise/sunset
    % t = 1:It; % sun hour
    hsr = real(acos(-tanL*tan(dS)));  % angle at sunrise
    % this only works for latitudes up to 66.5 deg N! Workaround:
    % hsr(hsr<-1)=acos(-1);
    % hsr(hsr>1)=acos(1);
    It=round(12*(1+mean(hsr(:))/pi)-12*(1-mean(hsr(:))/pi)); % calc daylength
%%  daily loop
    I=0;
    for t=1:n:It % loop over sunshine hours
        % if accounting for shading should be included, calc hillshade here
        % hourangle of sun hs
        hs=hsr-(pi*t/It);               % hs(t)
        %solar angle and azimuth
        %alpha = asin(sinL*sin(dS)+cosL*cos(dS)*cos(hs));% solar altitude angle
        sinAlpha = sinL.*sin(dS)+cosL.*cos(dS).*cos(hs);
        %alpha_s = asin(cos(dS)*sin(hs)/cos(alpha)); % solar azimuth angle
        % correction  using atmospheric transmissivity taub_b
        M=sqrt(1229+((614.*sinAlpha)).^2)-614.*sinAlpha; % Air mass ratio
        tau_b = 0.56 * (exp(-0.65*M) + exp(-0.095*M));
        tau_d = 0.271-0.294*tau_b; % radiation diffusion coefficient for diffuse insolation
        tau_r = 0.271+0.706*tau_b; % reflectance transmitivity
        % correct for local incident angle
        cos_i = (sin(dS).*term1) + (cos(dS).*cos(hs).*term2) + (cos(dS).*term3.*sin(hs));
        Is = I0 * tau_b; % potential incoming shortwave radiation at surface normal (equator)
        % R = potential clear sky solar radiation W m2
        R = Is .* cos_i;
        R(R<0)=0;  % kick out negative values
        Id = I0 .* tau_d .* cosSlop2./ 2.*sinAlpha; %diffuse radiation;
        Ir = I0 .* r .* tau_r .* sinSlop2./ 2.* sinAlpha; % reflectance
        R= R + Id + Ir;
        R(R<0)=0;
        I=I+R;% solar radiation per day (sunshine hours)
     end % end of sun hours in day loop
%%  add up radiation part melt for every day
    srad = srad + I;
end   % end of days in year loop

%%
function [grad,asp] = get_ders(dem,cs)
% calculate slope and aspect (deg) using GRADIENT function
[fx,fy] = gradient(dem,cs,cs); % uses simple, unweighted gradient of immediate neighbours
[asp,grad]=cart2pol(fy,fx); % convert to carthesian coordinates
grad=atan(grad); %steepest slope
asp=asp.*-1+pi; % convert asp 0 facing south


function [coef]=comp_dwiltii(coef2,a)
%COMP_DWILT  Compute Discrete Wilson transform.
%

M=size(coef2,1)/2;
N=size(coef2,2);
W=size(coef2,3);
L=N*a;

coef=zeros(2*M,N/2,W,assert_classname(coef2));

% ---- m is zero ---------
coef(1,:,:)=coef2(1,1:2:N,:);

% --- m is odd ----------
coef(2:2:M,:,:)    = i/sqrt(2)*(coef2(2:2:M,1:2:N,:)+coef2(2*M:-2:M+2,1:2:N,:));
coef(M+2:2:2*M,:,:)= 1/sqrt(2)*(coef2(2:2:M,2:2:N,:)-coef2(2*M:-2:M+2,2:2:N,:));

% --- m is even ---------
coef(3:2:M,:,:)=     1/sqrt(2)*(coef2(3:2:M,1:2:N,:)-coef2(2*M-1:-2:M+2,1:2:N,:));
coef(M+3:2:2*M,:,:)= i/sqrt(2)*(coef2(3:2:M,2:2:N,:)+coef2(2*M-1:-2:M+2,2:2:N,:));

% --- m is nyquest ------
if mod(M,2)==0
  coef(M+1,:,:) = i*coef2(M+1,2:2:N,:);
else
  coef(M+1,:,:) = i*coef2(M+1,1:2:N,:);
end;

coef=reshape(coef,M*N,W);


function J = evalObjectiveFCN(u,x,xref,Q,R,Ru)
% [J,Js,Ju]
%% Cost function of nonlinear MPC for Lotka-Volterra system
%
% Inputs:
%   u:      optimization variable, from time k to time k+N-1
%   x:      current state at time k
%   Ts:     controller sample time
%   N:      prediction horizon
%   xref:   state references, constant from time k+1 to k+N
%   u0:     previous controller output at time k-1
%
% Output:
%   J:      objective function cost
%

%% Nonlinear MPC design parameters

%% Cost Calculation
% Set initial cost and input
N = size(x,2);
J = zeros(N,1);
u0 = 0;

% Loop through each time step
for ct=1:N

    % Accumulate state tracking cost from x(k+1) to x(k+N)
    J(ct) = (x(:,ct)-xref(:,ct))'*Q*(x(:,ct)-xref(:,ct));

    % Accumulate MV rate of change cost from u(k) to u(k+N-1)
    if ct==1
        J(ct) = J(ct) + (u(:,ct)-u0)'*R*(u(:,ct)-u0) + u(:,ct)'*Ru*u(:,ct);
    else
        J(ct) = J(ct) + (u(:,ct)-u(:,ct-1))'*R*(u(:,ct)-u(:,ct-1)) + u(:,ct)'*Ru*u(:,ct);
    end
end


function desired_state = desiredState(traj_obj, t)
%desiredState computes desired position (x,y,z), yaw and its derivatives
% INPUT
%

tau_vec = traj_obj.tau_vec';
P = traj_obj.P;
path = traj_obj.path;

% Check dimensions
if size(path,2) ~= size(P,2)
    path = path';
end

cumsum_tau_vec = cumsum(tau_vec);
ts = [0; cumsum_tau_vec(:)];
D = size(P,2);

% Declare variables
pos = zeros(1,D);
vel = zeros(1,D);
acc = zeros(1,D);
jerk = zeros(1,D);
snap = zeros(1,D);

% If time is out of range, return the end point of the path
if t < 0
    error('time has to be greater than zero.')
elseif t >= sum(tau_vec)
    pos = path(end,:);
else
    k = find(ts<=t);
    k = k(end);
    for m = 1:D
        tau = t-ts(k);
        pos(m) = [tau^9,tau^8,tau^7,tau^6,tau^5,tau^4,tau^3,tau^2,tau,1] * P(10*k:-1:10*(k-1)+1,m);
        vel(m) = [9*tau^8,8*tau^7,7*tau^6,6*tau^5,5*tau^4,4*tau^3,3*tau^2,2*tau,1,0] * P(10*k:-1:10*(k-1)+1,m);
        acc(m) = [72*tau^7,56*tau^6,42*tau^5,30*tau^4,20*tau^3,12*tau^2,6*tau,2,0,0] * P(10*k:-1:10*(k-1)+1,m);
        jerk(m) = [504*tau^6,336*tau^5,210*tau^4,120*tau^3,60*tau^2,24*tau,6,0,0,0] * P(10*k:-1:10*(k-1)+1,m);
        snap(m) = [3024*tau^5,1680*tau^4,840*tau^3,360*tau^2,120*tau,24,0,0,0,0] * P(10*k:-1:10*(k-1)+1,m);
%         pos(m) = polyval(P(10*k:-1:10*(k-1)+1,m),t-ts(k));
%         vel(m) = polyval(polyder(P(10*k:-1:10*(k-1)+1,m)),t-ts(k));
%         acc(m) = polyval(polyder(polyder(P(10*k:-1:10*(k-1)+1,m))),t-ts(k));
%         jerk(m) = polyval(polyder(polyder(polyder(P(10*k:-1:10*(k-1)+1,m)))),t-ts(k));
%         snap(m) = polyval(polyder(polyder(polyder(polyder(P(10*k:-1:10*(k-1)+1,m))))),t-ts(k));
    end
end

% Desired yaw and its derivatives are all zero.
yaw = 0;
yawdot = 0;
yawddot = 0;

desired_state.pos = pos(:);
desired_state.vel = vel(:);
desired_state.acc = acc(:);
desired_state.jerk = jerk(:);
desired_state.snap = snap(:);
desired_state.yaw = yaw;
desired_state.yawdot = yawdot;
desired_state.yawddot = yawddot;


function p = eval_pdf_clg(X,Y,mu,Sigma,W)
% function p = eval_pdf_clg(X,Y,mu,Sigma,W)
%
% p(c,t) = N(Y(:,t); mu(:,c) + W(:,:,c)*X(:,t), Sigma(:,:,c))

[d T] = size(Y);
[d nc] = size(mu);
p = zeros(nc,T);
for c=1:nc
  denom = (2*pi)^(d/2)*sqrt(abs(det(Sigma(:,:,c))));
  M = repmat(mu(:,c), 1, T) + W(:,:,c)*X;
  mahal = sum(((Y-M)'*inv(Sigma(:,:,c))).*(Y-M)',2);
  p(c,:) = (exp(-0.5*mahal) / denom)';
end


function [ t, w ] = u_quadrature_rule ( n )

%*****************************************************************************80
%
%% U_QUADRATURE_RULE: quadrature rule for U(n,x).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 April 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the rule.
%
%    Output, real T(N,1), W(N,1), the points and weights of the rule.
%
  aj = zeros ( n, 1 );

  bj = 0.5 * ones ( n, 1 );

  w = zeros ( n, 1 );
  w(1,1) = sqrt ( pi / 2.0 );

  [ t, w ] = imtqlx ( n, aj, bj, w );

  w(1:n,1) = w(1:n,1).^2;

  return
end


function value = box_contains_segment_nd ( dim_num, p1, p2, pa, pb  )

%*****************************************************************************80
%
%% BOX_CONTAINS_SEGMENT_ND reports if a box contains a line segment in ND.
%
%  Discussion:
%
%    A box is assumed to be a rectangle with sides aligned on coordinate
%    axes.  It can be described by its low and high corner, P1 and P2:
%
%      points P so that P1(1:DIM_NUM) <= P(1:DIM_NUM) <= P2(1:DIM_NUM).
%
%    A line segment is the finite portion of a line that lies between
%    two points.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 March 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, real P1(DIM_NUM), P2(DIM_NUM), the low and high corners of the box.
%
%    Input, real PA(DIM_NUM), PB(DIM_NUM), the endpoints of the line segment.
%
%    Output, logical VALUE, is TRUE if the box contains
%    the line segment.
%
  value = 0;

  if ( ~box_contains_point_nd ( dim_num, p1, p2, pa ) )
    return
  end

  if ( ~box_contains_point_nd ( dim_num, p1, p2, pb ) )
    return
  end

  value = 1;

  return
end


function seed = get_seed ( )

%*****************************************************************************80
%
%% GET_SEED returns a seed for the random number generator.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 November 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, integer SEED, a random seed value.
%
  I_MAX = 2147483647;

  time_array = clock;

  hour = time_array(4);
  minute = time_array(5);
  second = time_array(6);

  temp = ( second + 60 * ( minute + 60 * hour ) ) / ( 60.0 * 60.0 * 24.0 );

  if ( temp <= 0.0 )
    temp = temp + 1.0;
  end

  if ( 1.0 < temp )
    temp = temp - 1.0;
  end

  seed = 1 + floor ( I_MAX * temp );

  return
end


function spherical_harmonic_test01 ( )

%*****************************************************************************80
%
%% SPHERICAL_HARMONIC_TEST01 tests LEGENDRE_ASSOCIATED_NORMALIZED,  LEGENDRE_ASSOCIATED_NORMALIZED_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 September 2010
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SPHERICAL_HARMONIC_TEST01:\n' );
  fprintf ( 1, '  LEGENDRE_ASSOCIATED_NORMALIZED evaluates the\n' );
  fprintf ( 1, '  associated Legrendre functions, using a\n' );
  fprintf ( 1, '  normalization appropriate for spherical harmonics.\n' );
  fprintf ( 1, '  LEGENDRE_ASSOCIATED_NORMALIZED_VALUES returns some exact values.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      N       M    X     Exact F     PNM(X)\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, n, m, x, fx ] = legendre_associated_normalized_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fx2 = legendre_associated_normalized ( n, m, x );

    fprintf ( 1, '  %6d  %6d  %6f  %12f  %12f\n', n, m, x, fx, fx2(n+1) );

  end

  return
end


function daub6_scale_plot ( n )

%*****************************************************************************80
%
%% DAUB6_SCALE_PLOT plots the DAUB6 scaling function.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 August 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the recursion level.
%
  x = linspace ( 0.0, 6.0, 801 );

  y = daub6_scale ( n, x );

  plot ( x, y, 'LineWidth', 2 );

  grid on
  xlabel ( '<---X--->' );
  ylabel ( '<---Y--->' );
  title ( sprintf ( 'DAUB6 Scale Function, Recursion level n = %d', n ) );

  return
end


%% Canny Edge Detection
% This sample demonstrates Canny edge detection.
%
% In this demo, we show how to use the OpenCV function |cv.Canny| to implement
% the Canny Edge Detector.
%
% Sources:
%
% * <https://github.com/opencv/opencv/blob/3.2.0/samples/cpp/edge.cpp>
% * <https://github.com/opencv/opencv/blob/3.2.0/samples/python/edge.py>
% * <https://docs.opencv.org/3.2.0/da/d5c/tutorial_canny_detector.html>
% * <https://docs.opencv.org/3.2.0/da/d22/tutorial_py_canny.html>
% * <https://github.com/opencv/opencv/blob/3.2.0/samples/cpp/tutorial_code/ImgTrans/CannyDetector_Demo.cpp>
%

%% Theory
%
% The _Canny Edge detector_ was developed by John F. Canny in 1986. Also known
% to many as the _optimal detector_, the Canny algorithm aims to satisfy three
% main criteria:
%
% * *Low error rate:* Meaning a good detection of only existent edges.
% * *Good localization:* The distance between edge pixels detected and real
%   edge pixels have to be minimized.
% * *Minimal response:* Only one detector response per edge.
%
%% Steps
%
% 1) Filter out any noise. The Gaussian filter is used for this purpose. An
%    example of a Gaussian kernel of $size = 5$ that might be used is shown
%    below:
%
% $$K = \frac{1}{159}
%       \left[{\matrix{
%           2 &  4 &  5 &  4 & 2 \cr
%           4 &  9 & 12 &  9 & 4 \cr
%           5 & 12 & 15 & 12 & 5 \cr
%           4 &  9 & 12 &  9 & 4 \cr
%           2 &  4 &  5 &  4 & 2
%       }}\right]$$
%
% 2) Find the intensity gradient of the image. For this, we follow a procedure
%    analogous to Sobel:
%
% * Apply a pair of convolution masks (in $x$ and $y$ directions):
%
% $$G_{x} = \left[{\matrix{
%               -1 & 0 & +1 \cr
%               -2 & 0 & +2 \cr
%               -1 & 0 & +1
%           }}\right]$$
%
% $$G_{y} = \left[{\matrix{
%               -1 & -2 & -1 \cr
%                0 &  0 &  0 \cr
%               +1 & +2 & +1
%           }}\right]$$
%
% * Find the gradient strength and direction. The direction is rounded to
%   one of four possible angles (namely 0, 45, 90 or 135)
%
% $$G = \sqrt{ G_{x}^{2} + G_{y}^{2} }$$
%
% $$\theta = \arctan(\frac{ G_{y} }{ G_{x} })$$
%
% 3) _Non-maximum suppression_ is applied. This removes pixels that are not
%    considered to be part of an edge. Hence, only thin lines (candidate edges)
%    will remain.
%
% 4) _Hysteresis_: The final step. Canny does use two thresholds
%    (_upper_ and _lower_). Canny recommended a |upper:lower| ratio between
%    2:1 and 3:1.
%
% * If a pixel gradient is higher than the _upper_ threshold, the pixel is
%   accepted as an edge
% * If a pixel gradient value is below the _lower_ threshold, then it is
%   rejected.
% * If the pixel gradient is between the two thresholds, then it will be
%   accepted only if it is connected to a pixel that is above the _upper_
%   threshold.
%
% For more details, you can always consult your favorite Computer Vision book.
%

%% Code
%
% This program:
%
% * Asks the user to enter a numerical value to set the lower threshold for
%   our _Canny Edge Detector_ (by means of a slider)
% * Applies the _Canny Detector_ and generates a *mask* (bright lines
%   representing the edges on a black background)
% * Applies the mask obtained on the original image and display it in a window
%

function varargout = edge_demo_gui(im)
    % load source image
    if nargin < 1
        im = fullfile(mexopencv.root(),'test','fruits.jpg');
        src = cv.imread(im, 'Color',true);
    elseif ischar(im)
        src = cv.imread(im, 'Color',true);
    else
        src = im;
    end

    % create the UI
    h = buildGUI(src);
    if nargout > 0, varargout{1} = h; end
end

function onChange(~,~,h)
    %ONCHANGE  Event handler for UI controls

    % retrieve current values from UI controls
    apertures = [3, 5, 7];
    aIdx = get(h.pop, 'Value');
    thresh = round(get(h.slid, 'Value'));
    set(h.txt, 'String',sprintf('Threshold: %3d',thresh));

    % convert image to grayscale, and blur to reduce the noise
    gray = cv.cvtColor(h.src, 'RGB2GRAY');
    gray = cv.blur(gray, 'KSize',[3 3]);

    % detect edges, with 3:1 as threshold ratio
    if true
        % default canny (Sobel gradient)
        edges = cv.Canny(gray, thresh*[1 3], 'ApertureSize',apertures(aIdx));
    else
        % canny with custom gradient (Scharr)
        dx = cv.Scharr(gray, 'DDepth','int16', 'XOrder',1, 'YOrder',0);
        dy = cv.Scharr(gray, 'DDepth','int16', 'XOrder',0, 'YOrder',1);
        edges = cv.Canny2(dx, dy, thresh*[1 3]);
    end

    % apply edges mask on original image
    if true
        out = cv.copyTo(h.src, 'Mask',edges);
    else
        out = bsxfun(@times, h.src, uint8(edges~=0));
    end

    % show result
    set(h.img, 'CData',out);
    drawnow;
end

function h = buildGUI(img)
    %BUILDGUI  Creates the UI

    % parameters
    thresh = 10;
    max_thresh = 150;
    sz = size(img);
    sz(2) = max(sz(2), 300);  % minimum figure width

    % build the user interface (no resizing to keep it simple)
    h = struct();
    h.src = img;
    h.fig = figure('Name','Edge map', ...
        'NumberTitle','off', 'Menubar','none', 'Resize','off', ...
        'Position',[200 200 sz(2) sz(1)+29]);
    if ~mexopencv.isOctave()
        %HACK: not implemented in Octave
        movegui(h.fig, 'center');
    end
    h.ax = axes('Parent',h.fig, ...
        'Units','pixels', 'Position',[1 30 sz(2) sz(1)]);
    if ~mexopencv.isOctave()
        h.img = imshow(img, 'Parent',h.ax);
    else
        %HACK: https://savannah.gnu.org/bugs/index.php?45473
        axes(h.ax);
        h.img = imshow(img);
    end
    uicontrol('Parent',h.fig, 'Style','text', 'FontSize',11, ...
        'Position',[5 5 65 20], 'String','Aperture');
    h.pop = uicontrol('Parent',h.fig, 'Style','popupmenu', 'Value',1, ...
       'Position',[70 5 40 20], 'String',{'3','5','7'});
    h.txt = uicontrol('Parent',h.fig, 'Style','text', 'FontSize',11, ...
        'Position',[110 5 120 20], 'String',sprintf('Threshold: %3d',thresh));
    h.slid = uicontrol('Parent',h.fig, 'Style','slider', 'Value',thresh, ...
        'Min',0, 'Max',max_thresh, 'SliderStep',[1 10]./(max_thresh-0), ...
        'Position',[230 5 sz(2)-230-5 20]);

    % hook event handlers, and trigger default start
    set([h.pop, h.slid], 'Callback',{@onChange,h}, ...
        'Interruptible','off', 'BusyAction','cancel');
    onChange([],[],h);
end


function acosh_test ( )

%*****************************************************************************80
%
%% ACOSH_TEST tests R4_ACOSH and R8_ACOSH.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 September 2011
%
%  Author:
%
%    John Burkardt
%
  addpath ( '../test_values' );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'ACOSH_TEST:\n' );
  fprintf ( 1, '  Test ARCCOSH_VALUES, R4_ACOSH, R8_ACOSH\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '             X      ARCCOSH(X)\n' );
  fprintf ( 1, '                   R4_ACOSH(X)        Diff\n' );
  fprintf ( 1, '                   R8_ACOSH(X)        Diff\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, x, fx1 ] = arccosh_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fx2 = r4_acosh ( single ( x ) );
    fx3 = r8_acosh ( x );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  %14.4f  %14.6g\n', x, fx1 );
    fprintf ( 1, '                  %14.6g  %14.6g\n', fx2, abs ( fx1 - fx2 ) );
    fprintf ( 1, '                  %14.6g  %14.6g\n', fx3, abs ( fx1 - fx3 ) );

  end

  rmpath ( '../test_values' );

  return
end


function dx = repmat_der(~, varargin)
%REPMAT_DER
%   REPMAT_DER(X, N, DZDY)
%   REPMAT_DER(X, D1, D2, ..., DZDY)
%   REPMAT_DER(X, D, DZDY)
%   Derivative of REPMAT function, w.r.t. first input. Same syntax as
%   native REPMAT, plus derivative.

% Copyright (C) 2016 Joao F. Henriques.
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).

  dzdy = varargin{end} ;

  assert(numel(varargin) >= 2, 'Invalid syntax.') ;

  % collect repetitions for each dimension in a single vector
  if numel(varargin) == 2
    reps = varargin{1} ;
    if isscalar(reps)
      reps = [reps, reps] ;
    end
  else
    reps = [varargin{1:end-1}] ;
  end

  % iterate dimensions, summing derivative across all repetitions in that
  % dimension
  dx = dzdy ;
  for dim = 1:numel(reps)
    if reps(dim) > 1
      % split dimension in 2: one for the original, one for the repetitions
      sz = size(dx) ;
      dx = reshape(dx, [sz(1:dim-1), sz(dim) / reps(dim), reps(dim), sz(dim+1:end)]) ;

      % sum across the repetitions dimension
      dx = sum(dx, dim + 1) ;

      % merge the two dimensions again
      dx = reshape(dx, [sz(1:dim-1), sz(dim) / reps(dim), sz(dim+1:end)]) ;
    end
  end
end


function lambda = ortega_eigenvalues ( n, u, v, d )

%*****************************************************************************80
%
%% ORTEGA_EIGENVALUES returns the eigenvalues of the ORTEGA matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 September 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    James Ortega,
%    Generation of Test Matrices by Similarity Transformations,
%    Communications of the ACM,
%    Volume 7, 1964, pages 377-378.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%    2 <= N.
%
%    Input, real U(N,1), V(N,1), vectors which define the matrix.
%    U'V must not equal -1.0.  If, in fact, U'V = 0, and U, V and D are
%    integers, then the matrix, inverse, eigenvalues, and eigenvectors
%    will be integers.
%
%    Input, real D(N,1), the desired eigenvalues.
%
%    Output, real LAMBDA(N,1), the determinant.
%
  lambda = zeros ( n, 1 );

  lambda(1:n,1) = d(:);

  return
end


classdef TP5 < PROBLEM
% <multi> <real> <large/none> <robust>
% Test problem for robust multi-objective optimization
% delta --- 0.05 --- Maximum disturbance degree
% H     ---   50 --- Number of disturbances

%------------------------------- Reference --------------------------------
% A. Gaspar-Cunha, J. Ferreira, and G. Recio, Evolutionary robustness
% analysis for multi-objective optimization: benchmark problems, Structural
% and Multidisciplinary Optimization, 2014, 49: 771-793.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    properties
        delta;      % Maximum disturbance degree
        H;          % Number of disturbances
    end
    methods
        %% Default settings of the problem
        function Setting(obj)
            [obj.delta,obj.H] = obj.ParameterSet(0.05,50);
            obj.M = 2;
            if isempty(obj.D); obj.D = 10; end
            obj.lower    = zeros(1,obj.D);
            obj.upper    = ones(1,obj.D);
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,PopDec)
            PopObj(:,1) = PopDec(:,1);
            g = 1 + 10*mean(PopDec(:,2:end),2);
            h = sin(4*pi*PopDec(:,1))/15 - PopDec(:,1) + 1;
            PopObj(:,2) = h.*g;
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(~,N)
            R(:,1) = linspace(0,1,N)';
            R(:,2) = sin(4*pi*R(:,1))/15 - R(:,1) + 1;
        end
        %% Generate the image of Pareto front
        function R = GetPF(obj)
            R = obj.GetOptimum(100);
        end
        %% Calculate the metric value
        function score = CalMetric(obj,metName,Population)
            switch metName
                case {'Mean_IGD','Mean_HV','Worst_IGD','Worst_HV'}
                    score = feval(metName,Population,obj);
                otherwise
                    score = feval(metName,Population,obj.optimum);
            end
        end
        %% Perturb solutions multiple times
        function PopX = Perturb(obj,PopDec,N)
            if nargin < 3; N = obj.H; end
            Delta = repmat(obj.delta.*(obj.upper-obj.lower),N*size(PopDec,1),1);
            w     = UniformPoint(N,obj.D,'Latin');
            Dec   = 2*Delta.*w(reshape(repmat(1:end,size(PopDec,1),1),1,[]),:) + repmat(PopDec,N,1) - Delta;
            Dec   = obj.CalDec(Dec);
            PopX  = SOLUTION(Dec,obj.CalObj(Dec),obj.CalCon(Dec));
        end
    end
end


% Fig. 5.38  Feedback Control of Dynamic Systems, 5e
%             Franklin, Powell, Emami
clf
numG = 160*conv ([1 2.5],[1 0.7]);
denG = conv([1 5 40],[1  .03  .06]);
sysG = tf(numG,denG);
sysD=tf([1   3],[1   20]);
sysDG=sysD*sysG;
K = 0.3;
sysH=tf(1,1);
sysT = feedback (K*sysG,sysH);
sysTD=feedback(sysDG,sysH);
step(sysT)
hold on
step(sysTD)
grid on
title('Step responses of auto-pilot with P and lead control')
hold off


clear; clc; close all;
dataset = 'grid';
img = imread(strcat('../noncvx-lowrank(ICDM 2015)/data/images/', dataset, '.jpg'));

if(size(img, 2) > size(img, 1))
    img = permute(img, [2,1,3]);
end

imgSize = size(img);

img = double(img)/255;
imgMean = mean(img(:));
img = img - imgMean;
img = img/std(img(:));

img = reshape(img, imgSize(1), prod(imgSize)/imgSize(1));

missRatio = 0.95;
noisRatio = 0.05;

mask = (rand(size(img)) > missRatio);
G = randn(size(img))*noisRatio;
traD = img + G;
traD = traD.*mask;
traD = sparse(traD);

clear maxk missRatio noisRatio mask G;

para.maxIter = 5000;
para.tol = 1e-6;
para.maxR = 50 ;

%% ---------------------------------------------------------------
mask = (rand(size(img)) > 0.9);
tstD = img.*mask;
tstD = sparse(tstD);

[tstRow, tstCol, tstVal] = find(tstD);

para.test.row  = tstRow;
para.test.col  = tstCol;
para.test.data = tstVal;
para.test.m = size(tstD, 1);
para.test.n = size(tstD, 2);

clear tstRow tstCol tstVal tstD;

%% ---------------------------------------------------------------
lambdaMax = 4;
gridLambda = lambdaMax*(0.8).^(0:9);

gridNMSE = zeros(2, size(gridLambda,2 ));
gridRank = zeros(1, size(gridLambda,2 ));
for g = 1:length(gridLambda)
    lambda = gridLambda(g);

    % [U, S, V] = SoftImpute(traD, lambda, para );
    [U, S, V] = AISImpute(traD, lambda, para );
    gridRank(g) = nnz(S);

    X = U*S*V';
    gridNMSE(1, g) = sqrt(norm(X - img, 'fro')^2/numel(img));

    [ U, S, V ] = PostProcess(traD, U, V, S);
    X = U*S*V';

    gridNMSE(2, g) = sqrt(norm(X - img, 'fro')^2/numel(img));

    if(g > 1 && gridNMSE(2, g) > gridNMSE(2, g - 1))
        break;
    end
end

gridNMSE = gridNMSE(2,1:g);
[~, lambda] = min(gridNMSE);
gndRank = gridRank(lambda);
para.maxR = ceil(gndRank*1.2);

lambda = gridLambda(lambda);

clear gridNMSE gridRank g X U S V gridLambda;

%% active --------------------------------------------------------
method = 1;
t = tic;
[U, S, V, out{method}] = ActiveSubspace(traD, lambda, para );
% [U, S, V, out{1}]=mc_alt(traData', lambda, para);
Time(method) = toc(t);

[ U, S, V ] = PostProcess(traD, U, V, S);
X = U*S*V';
RMSE(method) = sqrt(norm(X - img, 'fro')^2/numel(img));
clear U S V t;

figure(1);
plot(out{method}.Time, out{method}.RMSE);
hold on;
figure(2);
semilogy(out{method}.Time, out{method}.obj - min(out{method}.obj));
hold on;
figure;
X = X + imgMean;
X = reshape(X, imgSize);
imshow(X, []);
title('active');

%% boost ---------------------------------------------------------
method = 2;
t = tic;
[U, S, V, out{method}] = Boost( traD, lambda, para);
Time(method) = toc(t);

[ U, S, V ] = PostProcess(traD, U, V, S);
X = U*S*V';
RMSE(method) = sqrt(norm(X - img, 'fro')^2/numel(img));
clear U S V t;

figure(1);
hold on;
plot(out{method}.Time, out{method}.RMSE);
figure(2);
hold on;
semilogy(out{method}.Time, out{method}.obj - min(out{method}.obj));
figure;
X = X + imgMean;
X = reshape(X, imgSize);
imshow(X, []);
title('boost');

%% TR ------------------------------------------------------------
method = 3;
t = tic;
[U, S, V, out{method}] = MMBS( traD, lambda, para );
Time(method) = toc(t);

[ U, S, V ] = PostProcess(traD, U, V, S);
X = U*S*V';
RMSE(method) = sqrt(norm(X - img, 'fro')^2/numel(img));
clear U S V t;

figure(1);
plot(out{method}.Time, out{method}.RMSE);
figure(2);
semilogy(out{method}.Time, out{method}.obj - min(out{method}.obj));
figure;
X = X + imgMean;
X = reshape(X, imgSize);
imshow(X, []);
title('TR');

%% ALT-Impute ----------------------------------------------------
method = 4;
t = tic;
[U, S, V, out{method}] = SoftImputeALS( traD, lambda, para.maxR, para );
Time(method) = toc(t);

[ U, S, V ] = PostProcess(traD, U, V, S);
X = U*S*V';
RMSE(method) = sqrt(norm(X - img, 'fro')^2/numel(img));
clear U S V t;

figure(1);
plot(out{method}.Time, out{method}.RMSE);
figure(2);
semilogy(out{method}.Time, out{method}.obj - min(out{method}.obj));
figure;
X = X + imgMean;
X = reshape(X, imgSize);
imshow(X, []);
title('ALT-Impute');

%% SSGD ----------------------------------------------------------
method = 5;
t = tic;
[U, S, V, out{method}] = SSGD( traD, lambda, gndRank, para );
Time(method) = toc(t);

[ U, S, V ] = PostProcess(traD, U, V, S);
X = U*S*V';
RMSE(method) = sqrt(norm(X - img, 'fro')^2/numel(img));
clear U S V t;

figure(1);
plot(out{method}.Time, out{method}.RMSE);
figure(2);
semilogy(out{method}.Time, out{method}.obj - min(out{method}.obj));
figure;
X = X + imgMean;
X = reshape(X, imgSize);
imshow(X, []);
title('SSGD');

%% LMaFit --------------------------------------------------------

gridNMSE = zeros(1, 10);
for g = 1:10
    [U, S, V] = FixedRank( traD, g, para );

    X = U*S*V';
    gridNMSE(g) = sqrt(norm(X - img, 'fro')^2/numel(img));

    if(g > 1 && gridNMSE(g) > gridNMSE(g - 1))
        break;
    end
end

gridNMSE = gridNMSE(1:g);
[~, rnk] = min(gridNMSE);

clear gridNMSE X g U S V;

method = 6;
t = tic;
[U, S, V, out{method}] = FixedRank( traD, rnk, para );
Time(method) = toc(t);

X = U*S*V';
RMSE(method) = out{method}.RMSE(end);
clear U S V t;

figure(1);
plot(out{method}.Time, out{method}.RMSE);
figure(2);
semilogy(out{method}.Time, out{method}.obj - min(out{method}.obj));
figure;
X = X + imgMean;
X = reshape(X, imgSize);
imshow(X, []);
title('LMaFit');

%% APG -----------------------------------------------------------
method = 7;
t = tic;
[U, S, V, out{method}] = APGMatComp( traD, lambda, para );
Time(method) = toc(t);

[ U, S, V ] = PostProcess(traD, U, V, S);
X = U*S*V';
RMSE(method) = sqrt(norm(X - img, 'fro')^2/numel(img));
clear U S V t;

figure(1);
plot(out{method}.Time, out{method}.RMSE);
figure(2);
semilogy(out{method}.Time, out{method}.obj - min(out{method}.obj));
figure;
X = X + imgMean;
X = reshape(X, imgSize);
imshow(X, []);
title('APG');

%% Soft-Impute ---------------------------------------------------
method = 8;
t = tic;
[U, S, V, out{method}] = SoftImpute( traD, lambda, para );
Time(method) = toc(t);

[ U, S, V ] = PostProcess(traD, U, V, S);
X = U*S*V';
RMSE(method) = sqrt(norm(X - img, 'fro')^2/numel(img));
clear U S V t;

figure(1);
plot(out{method}.Time, out{method}.RMSE);
figure(2);
semilogy(out{method}.Time, out{method}.obj - min(out{method}.obj));
figure;
X = X + imgMean;
X = reshape(X, imgSize);
imshow(X, []);
title('Soft-Impute');

%% AIS-Impute ----------------------------------------------------
method = 9;
t = tic;
[U, S, V, out{method}] = AISImpute( traD, lambda, para );
Time(method) = toc(t);

[ U, S, V ] = PostProcess(traD, U, V, S);
X = U*S*V';
RMSE(method) = sqrt(norm(X - img, 'fro')^2/numel(img));
clear U S V t;

figure(1);
plot(out{method}.Time, out{method}.RMSE);
figure(2);
semilogy(out{method}.Time, out{method}.obj - min(out{method}.obj));
figure;
X = X + imgMean;
X = reshape(X, imgSize);
imshow(X, []);
title('AIS-Impute');

% clear gndRank img imgMean lambda lambdaMax method rnk traD X mask;