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_*
%
%   Reference
%     Boukouvalas et al. (2012). Direct Gaussian Process Quantile
%     Regression Using Expectation Propagation. In Proceedings of the
%     29th International Conference on Machine Learning, Edinburgh,
%     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 h] = 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 = {}; h=[];
  if ~isempty(lik.p.sigma2)
    w = [w log(lik.sigma2)];
    s = [s; 'log(qgp.sigma2)'];
    h = [h 0];
    % Hyperparameters of sigma2
    [wh, sh, hh] = lik.p.sigma2.fh.pak(lik.p.sigma2);
    w = [w wh];
    s = [s; sh];
    h = [h hh];
  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, sigma2_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)
    if isscalar(sigma2_i)
      sigma2_ii = sigma2_i;
    else
      sigma2_ii = sigma2_i(i);
    end

    % 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),sigma2_ii,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);
    if isnan(m_0)
      logM_0=NaN;
      return
    end
    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) >= sigma2_ii
      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) >= sigma2_ii
        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);
  if (size(Ef,2) > 1) && (size(Ef,2) > 1) && size(yt,2) == 1
    % Approximate integral with sum of grid points when using corrected
    % marginal posterior
    for i1=1:length(yt)
      py = arrayfun(@(f) exp(lik.fh.ll(lik, yt(i1), f, [])), Ef(i1,:));
      pf = Varf(i1,:)./sum(Varf(i1,:));
      lpy(i1) = log(sum(py.*pf));
    end
  else
    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
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);
  iter=0;
  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.sigma2)
      reclik.p.sigma2 = lik.p.sigma2.fh.recappend(reclik.p.sigma2, ri, lik.p.sigma2);
    end
    reclik.quantile = lik.quantile;
  end
end


function [err_p,elerr_p] = diffpost_bc(aez,fez,elerror,xy,ev,ebound);
%diffpost_bc postprocesses local Poisson error estimator
%   [err_p,elerr_p] = diffpost_bc(aez,fez,elerror,xy,ev,ebound);
%   input
%          aez       elementwise Poisson problem matrices
%          fez       elementwise rhs vectors
%          elerror   elementwise error estimate (without BC imposition)
%          xy        vertex coordinate vector
%          ev        element mapping matrix
%          ebound    element edge boundary matrix
%   output
%          err_p     global error estimate
%          elerr_p   elementwise error estimate
%   IFISS function: DJS; 4 March 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage
      x=xy(:,1); y=xy(:,2);
      nel=length(ev(:,1));
      lev=[ev,ev(:,1)]; elerr_p=elerror;
%
% recompute contributions from elements with Dirichlet boundaries
      nbde=length(ebound(:,1));
      ebdy = zeros(nel,1);
      edge = zeros(nel,1);
% isolate boundary elements
      for el = 1:nbde
      ee = ebound(el,1);
      ebdy(ee) = ebdy(ee)+1; edge(ee)=ebound(el,2);
      end
%
% two edge elements
      k2=find(ebdy==2);
      nel2b=length(k2);
% loop over two edge elements
      for el = 1:nel2b
      el2e=k2(el);
      kk=find(ebound(:,1) == el2e);
      edges=ebound(kk,2);
% set up original matrix and RHS vector
	  ae=squeeze(aez(el2e,1:5,1:5));
      fe=fez(el2e,:)';
% set up local coordinates and impose interpolated error as Dirichlet bc
      xl=x(lev(el2e,:)); yl=y(lev(el2e,:));
      [bae,fe] = localbc_p(ae,fe,edges,xl,yl);
% solve local problem
      err=bae\fe;
      elerr_p(el2e,1) = err'*fe;
      end
% end of element loop
%
% one edge elements
      k1=find(ebdy==1);
      nel1b=length(k1);
% loop over one edge elements
      for el = 1:nel1b
      el1e=k1(el);
      kk=find(ebound(:,1) == el1e);
      edges=ebound(kk,2);
% set up original matrix and RHS vector
      fe=fez(el1e,:)';
	  ae=squeeze(aez(el1e,1:5,1:5));
% set up local coordinates and impose interpolated error as Dirichlet bc
      xl=x(lev(el1e,:)); yl=y(lev(el1e,:));
      [bae,fe] = localbc_p(ae,fe,edges,xl,yl);
% solve local problem
      err=bae\fe;
      elerr_p(el1e,1) = err'*fe;
      end
% end of element loop
%
      err_p = sqrt(sum(elerr_p));
      elerr_p = sqrt(elerr_p);
      fprintf('done\n')
      fprintf('estimated global error (in energy):  %10.6e\n',err_p)
 return


function V = lumScotopic(imgXYZ)
%
%       V = lumScotopic(imgXYZ)
%
%       This function calculates the scotopic luminance
%
%       input:
%           img: an image in the XYZ color space
%
%       output:
%           V: scotopic luminance approximation by Larson, Rushmeier and Piatko 1997
%
%     Copyright (C) 2011-14  Francesco Banterle
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
%

check3Color(imgXYZ);

eps = 1e-6;
t = (imgXYZ(:,:,2) + imgXYZ(:,:,3)) ./ (imgXYZ(:,:,1) + eps);
V = imgXYZ(:,:,2) .* (1.33 * (1.0 + t) - 1.68);

end


function result = cross_entropy(head_embedding, tail_embedding, head, tail, weights, a, b, same_embedding)
%CROSS_ENTROPY Given a distance for each 1-simplex in low-dimensional space
% and the original weights of the 1-simplices in high-dimensional space,
% compute the approximation to the cross-entropy between the two simplicial
% complexes. This calculation uses the modified smooth formula Phi for
% low-dimensional weight that is used in the stochastic gradient descent.
%
% result = cross_entropy(head_embedding, tail_embedding, head, tail, weights, a, b, same_embedding)
%
% Parameters
% ----------
% dists: array of size (n_1_simplices, 1)
%     The current distance between the two endpoints of the 1-simplex in
%     low-dimensional Euclidean space.
%
% weights: array of size (n_1_simplices, 1)
%     The original weights assigned to the 1-simplices in high-dimensional
%     space.
%
% a: double
%     Parameter of differentiable approximation of right adjoint functor.
%
% b: double
%     Parameter of differentiable approximation of right adjoint functor.
%
% Returns
% -------
% result: double
%     The total approximated cross entropy between the two simplicial complexes.
%
% See also: NEG_SAMPLING_OBJECTIVE
%
%   AUTHORSHIP
%   Math Lead & Primary Developer:  Connor Meehan <connor.gw.meehan@gmail.com>
%   Secondary Developer: Stephen Meehan <swmeehan@stanford.edu>
%   Bioinformatics Lead:  Wayne Moore <wmoore@stanford.edu>
%   Provided by the Herzenberg Lab at Stanford University
%   License: BSD 3 clause
%
    n1 = size(head_embedding, 1);
    n2 = size(tail_embedding, 1);

    if n1*n2 > 1e8
        error('HALTED: MATLAB usually freezes for embeddings this large.');
    end

    full_dists = pdist2(head_embedding, tail_embedding);
    full_weights = full(sparse(head, tail, weights, n1, n2));
    if same_embedding
        full_weights = full_weights + eye(n1);
    end
    Phi = ones(size(full_weights))./(1 + a*(full_dists.^(2*b)));
    fw0 = full_weights == 0;
    fw1 = full_weights == 1;
    other = ~fw0 & ~fw1;
    Phi_summands = zeros(size(full_weights));
    Phi_summands(fw0) = log(1-Phi(fw0));
    Phi_summands(fw1) = log(Phi(fw1));
    Phi_summands(other) = full_weights(other).*log(Phi(other)) + (1-full_weights(other)).*log(1-Phi(other));

    result = -sum(sum(Phi_summands));
end


% input: rvqs0: r s in s0, v s in s0, q s0 2 s, note s0 is a fixed local
% world frame, also called w-frame
% rqs02e: rs0 in e, q s0 2 e, a, acceleration by IMU, w, angular rate by
% gyro, dt, sample interval from the last measurement to the current
% measurement.
% gwomegaw, $g^w$, gravity in local world frame, 3-vector, and $w_{iw}^w$, 3-vector,
% if all of them set 0, this amount to integration in inertial frame

function rvqs0_new=strapdown_local_quat_bias(rvqs0, rqs02e, a, w, dt, gwomegaw)
rvqs0_new=zeros(size(rvqs0));
gs0=gwomegaw (1:3); % gravity in s0 frame
wie2s0=gwomegaw (4:6);
%Update attitude
%% method (1) second order integration
qe=rvec2quat_v000(wie2s0*dt);
vr_a=quatmult_v000(rvqs0(7:10),qe);
qb=rvec2quat_v000(-w*dt);
rvqs0_new(7:10)=quatmult_v000(qb,vr_a);
%% method (2) Runge-Kutta 4th order integration, empirically, this sometimes
% gives worse result than second order integration
% wie2s=quatrot_v000(rvqs0(7:10),wie2s0,0);
% omega=zeros(4,2);
% omega(1,2)=dt;
% omega(2:4,1)=w-wie2s;
% omega(2:4,2)=lastw-wie2s;
% qs2s0=rotationRK4( omega, [rvqs0(7); -rvqs0(8:10)]);
% rvqs0_new(7:10)=[qs2s0(1); -qs2s0(2:4)];

%% better velocity and position integration than first order rectanglar rule
%Update Velocity
vel_inc1=(quatrot_v000(rvqs0(7:10),a*dt,1)+quatrot_v000(rvqs0_new(7:10),a*dt,1))/2;
vel_inc2=(gs0+2*cross(rvqs0(4:6),wie2s0))*dt;
rvqs0_new(4:6)=rvqs0(4:6)+vel_inc1+vel_inc2;
%Update_pos
rvqs0_new(1:3)=rvqs0(1:3)+(rvqs0(4:6)+rvqs0_new(4:6))*dt/2;


function [x, resvec, lsvec] = dense_overdetermined_lsqr(A, b, R, tol, maxit)
% x = dense_overdetermined_lsqr(A, b, R, tol, maxit)
% [x, resvec, lsvec] = dense_overdetermined_lsqr(A, b, R, tol, maxit)
%
% LSQR on a dense overdetermined matrix with a dense preconditioner R.
% Solves x = arg min (A*x - b)
% Inputs:
%   A, b
%   R - Upper triangular preconditioner. kappa(A * inv(R)) governs
%       the convergence.
%   tol - tolerance. Stop when
%      norm(inv(R') * A' * r) / (norm(A * inv(R), 'fro') * norm(r)) < tol
%   maxit - Maximum number of iterations
%
% Outputs:
%   x
%   optional: resvec, lsvec - values of norm(A' * r) and norm(r).
%
% 6-December 2009, Version 1.3
% Copyright (C) 2009, Haim Avron and Sivan Toledo.


function q=qrdivide(q1,q2)
%QRDIVIDE divdes two real quaternions q=[q1,q2]
%
% Inputs:
%
%     q1(4,1), q2(4,1)  Two real quaternions in the form [r, i, j, k]' where i^2=j^2=k^2=ijk=-1
%
% Outputs:
%
%     q(4,1)   Quotient of q1/q2 such that q1=q*q2.
%              Note that q*q2 ~= q2*q since quaternion multiplication does not commute.

%      Copyright (C) Mike Brookes 2000-2008
%      Version: $Id: qrdivide.m,v 1.1 2008/12/03 10:07:53 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.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
persistent a b c d
if isempty(a)
    a=[5 8 9 10 15 13];
    b=[6 7 11 12 14 16];
    c=[1 2 3 4 6 7 11 12 16 14];
    d=[1 2 3 4 5 8 9 10 13 15];
end
if nargin<2
    %    just take the inverse of the only input argument
    q=q1/(q1'*q1);
    q(2:4)=-q(2:4);
else
    %    invert q2 and do a multiply
    q=q2/(q2'*q2);
    q(2:4)=-q(2:4);
    t=q1*q.';
    s=zeros(4,4);
    s(a)=-t(b);
    s(c)=t(d);
    q=sum(s,2);
end


function diff_vol = dtiSmoothAnisoPM(vol, num_iter, delta_t, kappa, option, voxel_spacing)
%ANISODIFF2D Conventional anisotropic diffusion
%   DIFF_VOL = ANISODIFF3D(VOL, NUM_ITER, DELTA_T, KAPPA, OPTION, VOXEL_SPACING) perfoms
%   conventional anisotropic diffusion (Perona & Malik) upon a stack of gray scale images.
%   A 3D network structure of 26 neighboring nodes is considered for diffusion conduction.
%
%       ARGUMENT DESCRIPTION:
%               VOL      - gray scale volume data (MxNxP).
%               NUM_ITER - number of iterations.
%               DELTA_T  - integration constant (0 <= delta_t <= 3/44).
%                          Usually, due to numerical stability this
%                          parameter is set to its maximum value.
%               KAPPA    - gradient modulus threshold that controls the conduction.
%               OPTION   - conduction coefficient functions proposed by Perona & Malik:
%                          1 - c(x,y,z,t) = exp(-(nablaI/kappa).^2),
%                              privileges high-contrast edges over low-contrast ones.
%                          2 - c(x,y,z,t) = 1./(1 + (nablaI/kappa).^2),
%                              privileges wide regions over smaller ones.
%          VOXEL_SPACING - 3x1 vector column with the x, y and z dimensions of
%                          the voxel (milimeters). In particular, only cubic and
%                          anisotropic voxels in the z-direction are considered.
%                          When dealing with DICOM images, the voxel spacing
%                          dimensions can be extracted using MATLAB's dicominfo(.).
%
%       OUTPUT DESCRIPTION:
%               DIFF_VOL - (diffused) volume with the largest scale-space parameter.
%
%   Example
%   -------------
%   vol = randn(100,100,100);
%   num_iter = 4;
%   delta_t = 3/44;
%   kappa = 70;
%   option = 2;
%   voxel_spacing = ones(3,1);
%   diff_vol = anisodiff3D(vol, num_iter, delta_t, kappa, option, voxel_spacing);
%   figure, subplot 121, imshow(vol(:,:,50),[]), subplot 122, imshow(diff_vol(:,:,50),[])
%
% See also anisodiff1D, anisodiff2D.

% References:
%   P. Perona and J. Malik.
%   Scale-Space and Edge Detection Using Anisotropic Diffusion.
%   IEEE Transactions on Pattern Analysis and Machine Intelligence,
%   12(7):629-639, July 1990.
%
%   G. Grieg, O. Kubler, R. Kikinis, and F. A. Jolesz.
%   Nonlinear Anisotropic Filtering of MRI Data.
%   IEEE Transactions on Medical Imaging,
%   11(2):221-232, June 1992.
%
%   MATLAB implementation based on Peter Kovesi's anisodiff(.):
%   P. D. Kovesi. MATLAB and Octave Functions for Computer Vision and Image Processing.
%   School of Computer Science & Software Engineering,
%   The University of Western Australia. Available from:
%   <http://www.csse.uwa.edu.au/~pk/research/matlabfns/>.
%
% Credits:
% Daniel Simoes Lopes
% ICIST
% Instituto Superior Tecnico - Universidade Tecnica de Lisboa
% danlopes (at) civil ist utl pt
% http://www.civil.ist.utl.pt/~danlopes
%
% May 2007 original version.

% Convert input volume to double.
vol = double(vol);

% Useful variables.
[rows cols pags] = size(vol);

% PDE (partial differential equation) initial condition.
diff_vol = vol;
clear vol

% Center voxel distances.
x = voxel_spacing(1);
y = voxel_spacing(2);
z = voxel_spacing(3);
dx = 1;
dy = 1;
dz = z/x;
dd = sqrt(dx^2+dy^2);
dh = sqrt(dx^2+dz^2);
dc = sqrt(dd^2+dz^2);

% 3D convolution masks - finite differences.
h1 = zeros(3,3,3); h1(2,2,2) = -1; h1(2,2,1) = 1;
h2 = zeros(3,3,3); h2(2,2,2) = -1; h2(2,2,3) = 1;
h3 = zeros(3,3,3); h3(2,2,2) = -1; h3(2,1,2) = 1;
h4 = zeros(3,3,3); h4(2,2,2) = -1; h4(2,3,2) = 1;
h5 = zeros(3,3,3); h5(2,2,2) = -1; h5(3,2,2) = 1;
h6 = zeros(3,3,3); h6(2,2,2) = -1; h6(1,2,2) = 1;

h7 = zeros(3,3,3); h7(2,2,2) = -1; h7(3,1,1) = 1;
h8 = zeros(3,3,3); h8(2,2,2) = -1; h8(2,1,1) = 1;
h9 = zeros(3,3,3); h9(2,2,2) = -1; h9(1,1,1) = 1;
h10 = zeros(3,3,3); h10(2,2,2) = -1; h10(3,2,1) = 1;
h11 = zeros(3,3,3); h11(2,2,2) = -1; h11(1,2,1) = 1;
h12 = zeros(3,3,3); h12(2,2,2) = -1; h12(3,3,1) = 1;
h13 = zeros(3,3,3); h13(2,2,2) = -1; h13(2,3,1) = 1;
h14 = zeros(3,3,3); h14(2,2,2) = -1; h14(1,3,1) = 1;

h15 = zeros(3,3,3); h15(2,2,2) = -1; h15(3,1,2) = 1;
h16 = zeros(3,3,3); h16(2,2,2) = -1; h16(1,1,2) = 1;
h17 = zeros(3,3,3); h17(2,2,2) = -1; h17(3,3,2) = 1;
h18 = zeros(3,3,3); h18(2,2,2) = -1; h18(1,3,2) = 1;

h19 = zeros(3,3,3); h19(2,2,2) = -1; h19(3,1,3) = 1;
h20 = zeros(3,3,3); h20(2,2,2) = -1; h20(2,1,3) = 1;
h21 = zeros(3,3,3); h21(2,2,2) = -1; h21(1,1,3) = 1;
h22 = zeros(3,3,3); h22(2,2,2) = -1; h22(3,2,3) = 1;
h23 = zeros(3,3,3); h23(2,2,2) = -1; h23(1,2,3) = 1;
h24 = zeros(3,3,3); h24(2,2,2) = -1; h24(3,3,3) = 1;
h25 = zeros(3,3,3); h25(2,2,2) = -1; h25(2,3,3) = 1;
h26 = zeros(3,3,3); h26(2,2,2) = -1; h26(1,3,3) = 1;

% Anisotropic diffusion.
for t = 1:num_iter

    % Finite differences. [imfilter(.,.,'conv') can be replaced by convn(.,.,'same')]
    % Due to possible memory limitations, the diffusion
    % will be calculated at each page/slice of the volume.
    for p = 1:pags-2
        diff3pp = diff_vol(:,:,p:p+2);
        aux = imfilter(diff3pp,h1,'conv'); nabla1 = aux(:,:,2);
        aux = imfilter(diff3pp,h2,'conv'); nabla2 = aux(:,:,2);
        aux = imfilter(diff3pp,h3,'conv'); nabla3 = aux(:,:,2);
        aux = imfilter(diff3pp,h4,'conv'); nabla4 = aux(:,:,2);
        aux = imfilter(diff3pp,h5,'conv'); nabla5 = aux(:,:,2);
        aux = imfilter(diff3pp,h6,'conv'); nabla6 = aux(:,:,2);
        aux = imfilter(diff3pp,h7,'conv'); nabla7 = aux(:,:,2);
        aux = imfilter(diff3pp,h8,'conv'); nabla8 = aux(:,:,2);
        aux = imfilter(diff3pp,h9,'conv'); nabla9 = aux(:,:,2);
        aux = imfilter(diff3pp,h10,'conv'); nabla10 = aux(:,:,2);
        aux = imfilter(diff3pp,h11,'conv'); nabla11 = aux(:,:,2);
        aux = imfilter(diff3pp,h12,'conv'); nabla12 = aux(:,:,2);
        aux = imfilter(diff3pp,h13,'conv'); nabla13 = aux(:,:,2);
        aux = imfilter(diff3pp,h14,'conv'); nabla14 = aux(:,:,2);
        aux = imfilter(diff3pp,h15,'conv'); nabla15 = aux(:,:,2);
        aux = imfilter(diff3pp,h16,'conv'); nabla16 = aux(:,:,2);
        aux = imfilter(diff3pp,h17,'conv'); nabla17 = aux(:,:,2);
        aux = imfilter(diff3pp,h18,'conv'); nabla18 = aux(:,:,2);
        aux = imfilter(diff3pp,h19,'conv'); nabla19 = aux(:,:,2);
        aux = imfilter(diff3pp,h20,'conv'); nabla20 = aux(:,:,2);
        aux = imfilter(diff3pp,h21,'conv'); nabla21 = aux(:,:,2);
        aux = imfilter(diff3pp,h22,'conv'); nabla22 = aux(:,:,2);
        aux = imfilter(diff3pp,h23,'conv'); nabla23 = aux(:,:,2);
        aux = imfilter(diff3pp,h24,'conv'); nabla24 = aux(:,:,2);
        aux = imfilter(diff3pp,h25,'conv'); nabla25 = aux(:,:,2);
        aux = imfilter(diff3pp,h26,'conv'); nabla26 = aux(:,:,2);

        % Diffusion function.
        if option == 1
            c1 = exp(-(nabla1/kappa).^2);
            c2 = exp(-(nabla2/kappa).^2);
            c3 = exp(-(nabla3/kappa).^2);
            c4 = exp(-(nabla4/kappa).^2);
            c5 = exp(-(nabla5/kappa).^2);
            c6 = exp(-(nabla6/kappa).^2);
            c7 = exp(-(nabla7/kappa).^2);
            c8 = exp(-(nabla8/kappa).^2);
            c9 = exp(-(nabla9/kappa).^2);
            c10 = exp(-(nabla10/kappa).^2);
            c11 = exp(-(nabla11/kappa).^2);
            c12 = exp(-(nabla12/kappa).^2);
            c13 = exp(-(nabla13/kappa).^2);
            c14 = exp(-(nabla14/kappa).^2);
            c15 = exp(-(nabla15/kappa).^2);
            c16 = exp(-(nabla16/kappa).^2);
            c17 = exp(-(nabla17/kappa).^2);
            c18 = exp(-(nabla18/kappa).^2);
            c19 = exp(-(nabla19/kappa).^2);
            c20 = exp(-(nabla20/kappa).^2);
            c21 = exp(-(nabla21/kappa).^2);
            c22 = exp(-(nabla22/kappa).^2);
            c23 = exp(-(nabla23/kappa).^2);
            c24 = exp(-(nabla24/kappa).^2);
            c25 = exp(-(nabla25/kappa).^2);
            c26 = exp(-(nabla26/kappa).^2);
        elseif option == 2
            c1 = 1./(1 + (nabla1/kappa).^2);
            c2 = 1./(1 + (nabla2/kappa).^2);
            c3 = 1./(1 + (nabla3/kappa).^2);
            c4 = 1./(1 + (nabla4/kappa).^2);
            c5 = 1./(1 + (nabla5/kappa).^2);
            c6 = 1./(1 + (nabla6/kappa).^2);
            c7 = 1./(1 + (nabla7/kappa).^2);
            c8 = 1./(1 + (nabla8/kappa).^2);
            c9 = 1./(1 + (nabla9/kappa).^2);
            c10 = 1./(1 + (nabla10/kappa).^2);
            c11 = 1./(1 + (nabla11/kappa).^2);
            c12 = 1./(1 + (nabla12/kappa).^2);
            c13 = 1./(1 + (nabla13/kappa).^2);
            c14 = 1./(1 + (nabla14/kappa).^2);
            c15 = 1./(1 + (nabla15/kappa).^2);
            c16 = 1./(1 + (nabla16/kappa).^2);
            c17 = 1./(1 + (nabla17/kappa).^2);
            c18 = 1./(1 + (nabla18/kappa).^2);
            c19 = 1./(1 + (nabla19/kappa).^2);
            c20 = 1./(1 + (nabla20/kappa).^2);
            c21 = 1./(1 + (nabla21/kappa).^2);
            c22 = 1./(1 + (nabla22/kappa).^2);
            c23 = 1./(1 + (nabla23/kappa).^2);
            c24 = 1./(1 + (nabla24/kappa).^2);
            c25 = 1./(1 + (nabla25/kappa).^2);
            c26 = 1./(1 + (nabla26/kappa).^2);
        end

    % Discrete PDE solution.
    diff_vol(:,:,p+1) = diff_vol(:,:,p+1) + ...
                        delta_t*(...
                        (1/(dz^2))*c1.*nabla1 + (1/(dz^2))*c2.*nabla2 + ...
                        (1/(dx^2))*c3.*nabla3 + (1/(dx^2))*c4.*nabla4 + ...
                        (1/(dy^2))*c5.*nabla5 + (1/(dy^2))*c6.*nabla6 + ...
                        ...
                        (1/(dc^2))*c7.*nabla7 + (1/(dh^2))*c8.*nabla8 + ...
                        (1/(dc^2))*c9.*nabla9 + (1/(dh^2))*c10.*nabla10 + ...
                        (1/(dh^2))*c11.*nabla11 + (1/(dc^2))*c12.*nabla12 + ...
                        (1/(dh^2))*c13.*nabla13 + (1/(dc^2))*c14.*nabla14 + ...
                        ...
                        (1/(dd^2))*c15.*nabla15 + (1/(dd^2))*c16.*nabla16 + ...
                        (1/(dd^2))*c17.*nabla17 + (1/(dd^2))*c18.*nabla18 + ...
                        ...
                        (1/(dc^2))*c19.*nabla19 + (1/(dh^2))*c20.*nabla20 + ...
                        (1/(dc^2))*c21.*nabla21 + (1/(dh^2))*c22.*nabla22 + ...
                        (1/(dh^2))*c23.*nabla23 + (1/(dc^2))*c24.*nabla24 + ...
                        (1/(dh^2))*c25.*nabla25 + (1/(dc^2))*c26.*nabla26);
    end
end

return;


function [u, s, U_p, U_k, U_d] = pinHole(p,k,d)

% PINHOLE Pin-hole camera model, with optional radial distortion.
%   U = PINHOLE(P) gives the projected pixel U of a point P in a canonical
%   pin-hole camera, that is, with calibration parameters
%     u0 = 0
%     v0 = 0
%     au = 1
%     av = 1
%   It uses reference frames {RDF,RD} (right-down-front for the 3D world
%   points and right-down for the pixel), according to this scheme:
%
%         / z (forward)
%        /
%       +------- x                 +------- u
%       |                          |
%       |      3D : P=[x;y;z]      |     image : U=[u;v]
%       | y                        | v
%
%   U = PINHOLE(P,K) allows the introduction of the camera's calibration
%   parameters:
%     K = [u0 v0 au av]'.
%
%   U = PINHOLE(P,K,D) allows the introduction of the camera's radial
%   distortion parameters:
%     D = [K2 K4 K6 ...]'
%   so that the new pixel is distorted following the distortion equation:
%     U_D = U * (1 + K2*R^2 + K4*R^4 + ...)
%   with R^2 = sum(U.^2), being U the projected point in the image plane
%   for a camera with unit focal length.
%
%   [U,S] = PINHOLE(...) returns the depth S from the camera center.
%
%   If P is a points matrix, PINHOLE(P,...) returns a pixel matrix U and a
%   depths row-vector S. P, U and S are defined as
%     P = [P1 ... Pn];   Pi = [xi;yi;zi]
%     U = [U1 ... Un];   Ui = [ui;vi]
%     S = [S1 ... Sn]
%
%   [U,S,U_p,U_k,U_d] returns the Jacobians of U wrt P, K and D. It only
%   works for single points P=[x;y;z].
%
%   See also PERSP_PROJECT, DISTORT, PIXELLISE, INVPINHOLE, PINHOLEIDP, INVDISTORTION.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

% Point's depth
s = p(3,:);

if nargout <= 2 % only pixel

    switch nargin
        case 1
            u = persp_project(p);
        case 2
            u = pixellise(persp_project(p),k);
        case 3
            if ~isempty(d)
                u = pixellise(distort(persp_project(p),d),k);
            else
                u = pixellise(persp_project(p),k);
            end
    end

else % Jacobians

    if size(p,2) > 1
        error('Jacobians not available for multiple points')
    else

        switch nargin
            case 1
                [u, U_p] = persp_project(p);

            case 2
                [u1, U1_p]     = persp_project(p);
                [u, U_u1, U_k] = pixellise(u1,k);
                U_p            = U_u1*U1_p;

            case 3
                if ~isempty(d)
                    [u1, U1_p]        = persp_project(p);
                    [u2, U2_u1, U2_d] = distort(u1,d);
                    [u, U_u2, U_k]    = pixellise(u2,k);
                    U_d               = U_u2*U2_d;
                    U_p               = U_u2*U2_u1*U1_p;
                else
                    [u1, U1_p]     = persp_project(p);
                    [u, U_u1, U_k] = pixellise(u1,k);
                    U_p            = U_u1*U1_p;
                    U_d            = zeros(2,0);
                end

        end

    end

end

return

%% jacobians
syms x y z u0 v0 au av d2 d4 d6 real
p = [x;y;z];
k = [u0;v0;au;av];
d = [d2;d4;d6];

[u, s, U_p, U_k, U_d] = pinHole(p,k,d);

simplify(U_p - jacobian(u,p))
simplify(U_k - jacobian(u,k))
simplify(U_d - jacobian(u,d))

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

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

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


classdef Anodal2gausComputer < handle

    properties (Access = public)
        A_nodal_2_gauss
    end

    properties (Access = private)
        nnode
        nelem
        npnod
        ngaus
        connec
        shape
    end

    methods (Access = public)
        function obj = Anodal2gausComputer(cParams)
            obj.init(cParams);
        end

        function compute(obj)
            obj.computeA();
        end

        function intX = integrateP1FunctionWithShapeFunction(obj,cParams)
            ndof = size(obj.A_nodal_2_gauss{1},2);
            intX = zeros(ndof,1);
            for igaus = 1:obj.ngaus
                dVG  = cParams.dV(:,igaus);
                xG   = cParams.x(:,igaus);
                A    = obj.A_nodal_2_gauss{igaus};
                intX = intX + A'*(xG.*dVG);
            end
        end
    end

    methods (Access = private)
        function init(obj,cParams)
            obj.nnode  = cParams.nnode;
            obj.nelem  = cParams.nelem;
            obj.npnod  = cParams.npnod;
            obj.ngaus  = cParams.ngaus;
            obj.connec = cParams.connec;
            obj.shape  = cParams.shape;
        end

        function computeA(obj)
            A0    = sparse(obj.nelem,obj.npnod);
            A2g   = cell(obj.ngaus,1);
            nodes = obj.connec;
            for igaus = 1:obj.ngaus
                A2g{igaus} = A0;
                for inode = 1:obj.nnode
                    node   = nodes(:,inode);
                    shapeN = obj.shape(inode,igaus);
                    Ni = ones(obj.nelem,1)*shapeN;
                    A  = sparse(1:obj.nelem,node,Ni,obj.nelem,obj.npnod);
                    A2g{igaus} = A2g{igaus} + A;
                end
            end
            obj.A_nodal_2_gauss = A2g;
        end
    end

end


function [model] = BCPF_MP(Y, varargin)
% Bayesian CP Factorization using Gaussian Mixture Priors for Image Completion
% Author : Qibin Zhao  2014
%
% -----------------------------------------------------------------------
%  [model] = BCPF_MP(Y, 'PARAM1', val1, 'PARAM2', val2, ...)
%
%  INPUTS
%     Y              - Input tensor
%     'obs'          - Binary (0-1) tensor indicating missing entries
%                      (0: missing; 1: observed)
%     'init'         - Initialization method
%                     - 'ml'  : SVD initilization (default)
%                     - 'rand': Random matrices
%     'maxRank'      - The initialization of rank (larger than true rank)
%     'dimRed'       - 1: Remove unnecessary components automaticly (default)
%                    - 0: Not remove
%     'maxiters'     - max number of iterations (default: 100)
%     'tol'          - lower band change tolerance for convergence dection
%                      (default: 1e-5)
%     'noise'        - whether noise is updated
%                        - 'on': update noise parameter (default)
%                        - 'off': fixed noise parameter (1e-5)
%     'predVar'      - Predictive distribution
%                         - 1:  compute and output
%                         - 0:  doesnot compute  (default)
%     'verbose'      - visualization of results
%                       - 0: no
%                       - 1: text (default)
%                       - 2: online display image
%                       - 3: show factors by image
%                       - 4: show factors by hinton plot (very slow)
%   OUTPUTS
%      model         - Model parameters and hyperparameters
% -----------------------------------------------------------------------
%
%   Example:
%
%     [model] = BCPF_MP(Y, 'obs', O, 'init', 'rand', 'maxRank', 10, 'dimRed', 1, 'maxiters', 100, ...
%                                'tol', 1e-6, 'verbose', 3);
%
% < Bayesian CP Factorization of Incomplete Image using Gaussian Mixture Priors >
% Copyright (C) 2014  Qibin Zhao
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%
warning off; %#ok<WNOFF>
randn('state',1); rand('state',1); %#ok<RAND>
dimY = size(Y);
N = ndims(Y);

%% Set parameters from input or by using defaults
ip = inputParser;
ip.addParamValue('obs', ones(dimY), @(x) (isnumeric(x) || islogical(x)) );
ip.addParamValue('init', 'rand', @(x) (ismember(x,{'ml','rand'})));
ip.addParamValue('maxRank', max(dimY), @isscalar);
ip.addParamValue('maxiters', 100, @isscalar);
ip.addParamValue('tol', 1e-5, @isscalar);
ip.addParamValue('verbose', 1, @isscalar);
ip.addParamValue('noise', 'on', @(x)ismember(x,{'on','off'}));
ip.addParamValue('dimRed', 1, @isscalar);
ip.addParamValue('predVar', 0, @isscalar);
ip.addParamValue('nd', 1, @isscalar);
ip.parse(varargin{:});

O     = ip.Results.obs;
init  = ip.Results.init;
R   = ip.Results.maxRank;
maxiters  = ip.Results.maxiters;
tol   = ip.Results.tol;
verbose  = ip.Results.verbose;
DIMRED   = ip.Results.dimRed;
noise = ip.Results.noise;
predVar = ip.Results.predVar;
nd = ip.Results.nd;

%% Initialization
Y = tensor(Y.*O);
O = tensor(O);
nObs = sum(O(:));

a_gamma0     = 1e-6;
b_gamma0     = 1e-6;
if  strcmp(noise,'on')
    a_beta0      = 1e-6;
    b_beta0      = 1e-6;
else
    a_beta0      = 1e-1;
    b_beta0      = 1e-6;
end
gammas = ones(R,1);
beta = 1e4;
dscale =1;

W = cell(N,1);
W1 = cell(N,1);
oR = nObs/prod(dimY);
for n=1:N
    W{n} = zeros(dimY(n),dimY(n));
    for i=1:dimY(n)
        for j=1:dimY(n)
            W{n}(i,j) = exp(-2*oR*abs(i-j)^2);
        end
    end
    W1{n} = W{n} - diag(diag(W{n}));
    W1{n} = nd*5*bsxfun(@times, W1{n}, 1./sum(W1{n},2));
end

switch init,
    case 'ml'    % Maximum likelihood
        Z = cell(N,1);
        ZSigma = cell(N,1);
        if ~isempty(find(O==0))
            %   Y(find(O==0)) = sum(Y(:))/nObs;
            Y1 = Y;
            for n=1:N
                Y1 = ttm(Y1, W{n}, n);
            end
            Y(find(O==0)) = Y1(find(O==0));
        end
        for n = 1:N
            ZSigma{n} = (repmat(eye(R), [1 1 dimY(n)]));
            [U, S, V] = svd(double(tenmat(Y,n)), 'econ');
            if R <= size(U,2)
                Z{n} = U(:,1:R)*(S(1:R,1:R)).^(0.5);
            else
                Z{n} = [U*(S.^(0.5)) randn(dimY(n), R-size(U,2))];
            end
        end
        Y = Y.*O;
    case 'rand'   % Random initialization
        Z = cell(N,1);
        ZSigma = cell(N,1);
        for n = 1:N
            Z{n} = rand(dimY(n),R);
            ZSigma{n} = (repmat(eye(R), [1 1 dimY(n)]));
            if n<3
                %   Z{n} = cholcov(W{n})'*Z{n};
                Z{n} = Z{n} +  W1{n}*Z{n};
            end
        end
end

% --------- E(aa') = cov(a,a) + E(a)E(a')----------------
EZZT = cell(N,1);
for n=1:N
    EZZT{n} = (reshape(ZSigma{n}, [R*R, dimY(n)]))';
end

Fit =0;
LB = 0;
X = double(ktensor(Z));

%% Create figures
if verbose >2,
    scrsz = get(0,'ScreenSize');
    h1 = figure('Position',[scrsz(3)*0.2 scrsz(4)*0.3 scrsz(3)*0.6 scrsz(4)*0.4]);
    figure(h1);
    switch verbose,
        case 4,
            subplot(2,3,1); hintonDiagram(Z{1}); title('Mode-1'); ylabel('Length of #-mode');
            subplot(2,3,2); hintonDiagram(Z{2}); title('Mode-2'); xlabel('Latent dimensions');
            if N>=3, subplot(2,3,3); hintonDiagram(Z{3}); title('Mode-3'); end
        case 3,
            subplot(2,3,1); imagesc(Z{1}); title('Mode-1'); ylabel('Length of #-mode');
            subplot(2,3,2); imagesc(Z{2}); title('Mode-2'); xlabel('Latent dimensions');
            if N>=3, subplot(2,3,3); imagesc(Z{3}); title('Mode-3');end
    end
    subplot(2,3,4); bar(gammas); title('Posterior mean of \lambda'); xlabel('Latent components'); ylabel(''); axis tight;
    subplot(2,3,5); plot(LB, '-r.','LineWidth',1.5,'MarkerSize',10 ); title('Lower bound'); xlabel('Iteration');  grid on;
    subplot(2,3,6); plotGamma(a_beta0, a_beta0); title('Posterior pdf'); xlabel('Noise precision \tau');grid on;
    set(findall(h1,'type','text'),'fontSize',12);
    drawnow;
end
if verbose ==2;
    h3 = figure;
    temp = 255.*(X-min(X(:)))/(max(X(:))-min(X(:)));
    imshow(uint8(temp));
    title(['Iter.= '  num2str(0),',  Rank = ' num2str(R)],'FontSize', 13, 'color','b');
    tic;
    xlabel(['(BCPF-MP)  Time: ' num2str(round(toc)), ' seconds'],'FontSize', 13, 'color','b');
    drawnow;
end

%% Model learning
for it=1:maxiters,
    %% Update factor matrices
    Aw = diag(gammas);
    for n=1:N
        % compute E(Z_{\n}^{T} Z_{\n})
        ENZZT = reshape(khatrirao_fast(EZZT{[1:n-1, n+1:N]},'r')' * double(tenmat(O,n)'), [R,R,dimY(n)]);
        % compute E(Z_{\n})
        FslashY = khatrirao_fast(Z{[1:n-1, n+1:N]},'r')' * tenmat(Y.*O, n)';
        for i=1:dimY(n)
            ZSigma{n}(:,:,i) = (beta * ENZZT(:,:,i) + Aw )^(-1);
            Z{n}(i,:) = (beta * ZSigma{n}(:,:,i) * FslashY(:,i))';
        end
        if n<3
            Z{n} = Z{n} + W1{n}*Z{n};
        end
        EZZT{n} = (reshape(ZSigma{n}, [R*R, dimY(n)]) + khatrirao_fast(Z{n}',Z{n}'))';
    end

    %% Update latent tensor X
    X = double(ktensor(Z));

    %% Update hyperparameters gamma
    a_gammaN = (0.5*sum(dimY) + a_gamma0)*ones(R,1);
    b_gammaN = 0;
    for n=1:N
        b_gammaN = b_gammaN + diag(Z{n}'*Z{n}) + diag(sum(ZSigma{n},3));
    end
    b_gammaN = b_gamma0 + 0.5.* b_gammaN;
    gammas = a_gammaN./b_gammaN;

    %% update noise beta
    %  The most time and space consuming part
    if 0 % save time but large space needed
        EX2 =  O(:)' * khatrirao_fast(EZZT,'r') * ones(R*R,1);
    else  % save space but slow
        temp1 = cell(N,1);
        EX2 =0;
        for i =1:R
            for n=1:N
                temp1{n} = EZZT{n}(:,(i-1)*R+1: i*R);
            end
            EX2 = EX2 + O(:)' * khatrirao_fast(temp1,'r')* ones(R,1);
        end
    end
    err = Y(:)'*Y(:) - 2*Y(:)'*X(:) + EX2;
    if  strcmp(noise,'on')
        a_betaN = a_beta0 + 0.5*nObs;
        b_betaN = b_beta0 + 0.5*err;
    else
        a_betaN = a_beta0;
        b_betaN = b_beta0;
    end
    beta = a_betaN/b_betaN;
    Fit = 1 - sqrt(sum(err(:)))/norm(Y(:));

    %% Lower bound
    temp1 = -0.5*nObs*safelog(2*pi) + 0.5*nObs*(psi(a_betaN)-safelog(b_betaN)) - 0.5*(a_betaN/b_betaN)*err;
    temp22 =0;
    for n=1:N
        temp22= temp22 + Z{n}'*Z{n} + sum(ZSigma{n},3);
    end
    temp2 = -0.5*R*sum(dimY)*safelog(2*pi) + 0.5*sum(dimY)*sum(psi(a_gammaN)-safelog(b_gammaN)) -0.5*trace(diag(gammas)* temp22);
    temp3 = sum(-safelog(gamma(a_gamma0)) + a_gamma0*safelog(b_gamma0) -  b_gamma0.*(a_gammaN./b_gammaN) + (a_gamma0-1).*(psi(a_gammaN)-safelog(b_gammaN)));
    temp4 = -safelog(gamma(a_beta0)) + a_beta0*safelog(b_beta0) + (a_beta0-1)*(psi(a_betaN)-safelog(b_betaN)) - b_beta0*(a_betaN/b_betaN);
    temp5=0;
    for n=1:N
        for i=1:size(ZSigma{n},3)
            temp5 = temp5 + 0.5*safelog(det(ZSigma{n}(:,:,i))) + 0.5*R*(1+safelog(2*pi));
        end
    end
    temp6 = sum(safelog(gamma(a_gammaN)) - (a_gammaN-1).*psi(a_gammaN) -safelog(b_gammaN) + a_gammaN);
    temp7 = safelog(gamma(a_betaN)) - (a_betaN-1)*psi(a_betaN) -safelog(b_betaN) + a_betaN;
    LB(it) = temp1 + temp2 + temp3 + temp4 + temp5 + temp6 + temp7;

    %% Prune irrelevant dimensions?
    Zall = cell2mat(Z);
    comPower = diag(Zall' * Zall);
    comTol = sum(dimY)*eps(norm(Zall,'fro'));
    rankest = sum(comPower> comTol );
    if max(rankest)==0
        disp('Rank becomes 0 !!!');
        break;
    end
    if DIMRED==1  && it >=2,
        if R~= max(rankest)
            indices = comPower > comTol;
            gammas = gammas(indices);
            temp = ones(R,R);
            temp(indices,indices) = 0;
            temp = temp(:);
            for n=1:N
                Z{n} = Z{n}(:,indices);
                ZSigma{n} = ZSigma{n}(indices,indices,:);
                EZZT{n} = EZZT{n}(:, temp == 0);
            end
            R = max(rankest);
        end
    end

    %% Display progress
    if it>2
        LBRelChan = abs(LB(it) - 2*LB(it-1) + LB(it-2))/-LB(2);
    else
        LBRelChan = NaN;
    end
    if verbose,
        fprintf('Iter. %d: RelChan = %g, Fit = %g, R = %d \n', it, LBRelChan, Fit, rankest);
    end

    %% visualize online results
    if verbose >2 ,
        switch verbose,
            case 4,
                set(0,'CurrentFigure',h1);
                subplot(2,3,1); hintonDiagram(Z{1}); title('Mode-1'); ylabel('Length of #-mode');
                subplot(2,3,2); hintonDiagram(Z{2}); title('Mode-2'); xlabel('Latent dimensions');
                if N>=3, subplot(2,3,3); hintonDiagram(Z{3}); title('Mode-3'); end
            case 3,
                set(0,'CurrentFigure',h1);
                subplot(2,3,1); imagesc(Z{1}); title('Mode-1'); ylabel('Length of #-mode');
                subplot(2,3,2); imagesc(Z{2}); title('Mode-2'); xlabel('Latent dimensions');
                if N>=3, subplot(2,3,3); imagesc(Z{3}); title('Mode-3'); end
        end
        subplot(2,3,4); bar(gammas); title('Posterior mean of \lambda'); xlabel('Latent components'); ylabel(''); axis tight;
        subplot(2,3,5); plot(LB, '-r.','LineWidth',1.5,'MarkerSize',10 ); title('Lower bound'); xlabel('Iteration');  grid on;
        subplot(2,3,6); plotGamma(a_betaN, b_betaN); title('Posterior pdf'); xlabel('Noise precision \tau');grid on;
        set(findall(h1,'type','text'),'fontSize',12);
        drawnow;
    end
    if verbose==2
        set(0,'CurrentFigure',h3);
        figure(h3);
        %        temp = (X-min(X(:)))/(max(X(:))-min(X(:)));
        %        image(temp);
        %        axis off;
        imshow(uint8(X));
        title(['Iter.= '  num2str(it),',  Rank = ' num2str(max(rankest))],'FontSize', 13, 'color','b');
        xlabel(['(BCPF-MP)  Time: ' num2str(round(toc)), ' seconds'],'FontSize', 13, 'color','b');
        drawnow;
    end

    %% Convergence check
    if it>5 && abs(LBRelChan) < tol
        disp('\\\======= Converged===========\\\');
        break;
    end
end

%% Predictive distribution
if predVar==1
    Xvar =  tenzeros(size(Y));
    for n=1:N
        Xvar = tenmat(Xvar,n);
        Fslash = khatrirao_fast(Z{[1:n-1, n+1:N]},'r');
        if 1
            temp1 = double(tenmat(tensor(ZSigma{n}),3));
            temp2 = khatrirao_fast(Fslash', Fslash');
            Xvar(:,:) = Xvar(:,:) + temp1*temp2;
        else
            % ---  slow computation ------
            for i=1:size(Xvar,1)     %#ok
                Xvar(i,:) = Xvar(i,:) + diag(Fslash * ZSigma{n}(:,:,i) *Fslash')';
            end
            % ---  slow computation ------
        end
        Xvar = tensor(Xvar);
    end
    Xvar = Xvar + beta^(-1);
    Xvar = Xvar.*(2*a_betaN)/(2*a_betaN-2);
    Xvar = Xvar.*(dscale^2);
else
    Xvar =[];
end

%% Prepare the results
SNR = 10*log10(var(X(:))*beta);
X = ktensor(Z)*dscale;
X = arrange(X);

%% Output
model.X = X;
model.ZSigma = ZSigma;
model.gammas = gammas;
model.Fit = Fit;
model.SNR = SNR;
model.Xvar = double(Xvar);
model.TrueRank = rankest;
model.LowBound = max(LB);

function y = safelog(x)
x(x<1e-300)=1e-200;
x(x>1e300)=1e300;
y=log(x);


function eclipVec=ICRS2Eliptic(vec,TT1,TT2,method)
%%ICRS2ECLIPTIC Convert a location vector from the International
%               Celestial Reference System (ICRS) to eliptic coordinates
%               either using the IAU 2006 precession model or the Vondrak
%               400 millennia precession model.
%
%INPUTS: x The NXnumVec collection of vectors in the ICRS to convert. N
%          can be 2, or 3. If the vectors are 2D, then they are assumed to
%          be azimuth and elevation in radians. 3D vectors are assumed to
%          be Cartesian position.
% Jul1, Jul2 Two parts of a Julian date given in terrestrial time (TT).
%          The units of the date are days. The full date is the sum of
%          both terms. The date is broken into two parts to provide more
%          bits of precision. It does not matter how the date is split.
%   method An optional parameter specifying which algorithm is to be used.
%          Possible values are
%          0 (The default if omitted or an empty matrix is passed) Use the
%            IAU 2006 precession model.
%          1 Use the long-term (Vondrak) precession model.
%
%OUTPUTS: xG The vectors rotated into the ecliptic coordinate system. If
%            the input was 2D azimuth and elevation, the output will be
%            the same. If the input was Cartesian, then the output will be
%            Cartesian.
%
%This function is a Matlab interface for the relevant functions in the
%International Astronomical Union's (IAU) Standard's of Fundamental
%Astronomy library.
%
%The ecliptic is defined in the IERS Conventions [1] to be the "the
%plane perpendicular to the mean heliocentric orbital angular momentum
%vector of the Earth-Moon barycentre in the BCRS".
%
%The algorithm can be compiled for use in Matlab  using the
%CompileCLibraries function.
%
%The algorithm is run in Matlab using the command format
%eclipVec=ICRS2Eliptic(vec,TT1,TT2,method);
%
%REFERENCES:
%[1] G. Petit and B. Luzum, IERS Conventions (2010), International Earth
%    Rotation and Reference Systems Service Std. 36, 2010.
%
%July 2016 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

error('This function is only implemented as a mexed C or C++ function. Please run CompileCLibraries.m to compile the function for use.')

end

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


function [I1,wl1,rgb1,g,H1,extra,s,G,S] = estimate_PSF_SRF(I,wl,bbl,rgb,s,extra,options)
%ESTIMATE_PSF_SRF Estimate point spread function (PSF) and spectral
%response function (SRF)
disp('Start estimating PSF and SRF');
show_fig = parse_param(options,'show_fig',0);

I1 = I(:,:,bbl);
wl1 = wl(bbl);
rgb1 = rgb;

Y = reshape_hsi(I1);
X = reshape(rgb1, [size(rgb1,1)*size(rgb1,2), size(rgb1,3)]);
[N,B] = size(Y);
b = size(X,2);
R2 = s + extra*2;
R = prod(R2);

convergence_t = 1e-3;

SRF_range = parse_param(options,'SRF_range','color');
% construct S
[I_sel,bands_sel] = select_relevant_bands(I1,wl1,SRF_range);
wl_sel = wl1(bands_sel)';
S = zeros(B,length(wl_sel));
for i = 1:size(S,2)
    S(bands_sel(i),i) = 1;
end

% construct C
Cs = construct_C(I1,rgb1,s,extra);
C = cat(2,Cs{:});

Y1 = [ones(N,1),Y*S];

% construct D
D = zeros(N*b,R);
for i = 1:N
    D((i-1)*b+1:i*b,:) = X'*Cs{i}';
end

% contruct options
options = [];
options.D = D;
options.Y1 = Y1;
options.D1D = D'*D;
options.D1YI = D'*kron(Y1,eye(b));

g = (1/R) * ones(R,1);
G = g2G(C,g,N);

delta_t0 = 1e-4;
delta_t_g = delta_t0;

errors = [];
for iter = 1:200
    % Update H1
    H1 = solve_for_H1(G*X, Y1, struct('lambda',1e-4));
    options.H1 = H1;

    % Update G
    der_g = calc_der_g(g, options);
    options.der_g = der_g;
    delta_t_g = calc_time_step_adaptive(@eval_obj_fun_g, @update_g, ...
        g, options, delta_t_g, delta_t0);
    g = update_g(g, options, delta_t_g);
    options.g = g;
    G = g2G(C,g,N);

    % Test convergence
    errors(end+1) = eval_obj_fun(options);

    if test_convergence(errors, convergence_t)
        break;
    end

    if mod(iter,100) == 0
        disp(['Process iteration ',num2str(iter)]);
    end
end

if show_fig
    figure('name','PSF'), mesh(reshape(g,R2));
    figure('name','SRF'), plot(H1);
end

function der_g = calc_der_g(g, options)
vecH1 = (options.H1)';
vecH1 = vecH1(:);
der_g = options.D1D * g - options.D1YI * vecH1;

function val = eval_obj_fun_g(g, options)
vecYH = options.Y1 * options.H1;
vecYH = vecYH';
vecYH = vecYH(:);
val = sum((options.D * g - vecYH).^2);

function g_new = update_g(g, options, delta_t)
der_g = options.der_g;

g_new = g - delta_t * der_g;
g_new = project_to_simplex(g_new');
g_new = g_new';

function val = eval_obj_fun(options)
g = options.g;
val = eval_obj_fun_g(g,options);


%GM_PHD_Construct_Update_Components
%Last modified 12th September 2013
%Matlab code by Bryan Clarke b.clarke@acfr.usyd.edu.au

%This file creates the components needed for performing a Kalman filter update on the
%targets using the measurement.
s = sprintf('Step 3: Constructing update components for all targets, new and existing.');
disp(s);

%We need to clear the data structures each iteration
eta = [];
S = [];
K = [];
P_k_k = [];

for j = 1:numTargets_Jk_k_minus_1
    m_j = mk_k_minus_1(:,j);
    eta_j = H2 * m_j;%Observation model. Assume we see position AND velocity of the target.

    P_range = calculateDataRange4(j); %4x4 array

    PHt = Pk_k_minus_1(:,P_range) * H2'; %Taken from Tim Bailey's EKF code. 4x4 array

    %Calculate K via Tim Bailey's method.
    S_j = R2 + H2 * PHt;
    %At this point, Tim Bailey's code makes S_j symmetric. In this case, it leads to the matrix being non-positive definite a lot of the time and chol() crashes.
    %So we won't do that.
    SChol= chol(S_j);

    SCholInv= SChol \ eye(size(SChol)); % triangular matrix, invert via left division
    W1 = PHt * SCholInv;

    K_j = W1 * SCholInv';

    P_j = Pk_k_minus_1(:,P_range) - W1*W1';%4x4 array
    %End Tim Bailey's code.

    eta = [eta, eta_j];
    S = [S, S_j];
    K = [K, K_j];
    P_k_k = [P_k_k, P_j];
end


function [kernel, S] = dat2Kernel(data, kSize)
% kernel = dat2Kernel(data, kSize,thresh)
%
% Function to perform k-space calibration step for ESPIRiT and create
% k-space kernels. Only works for 2D multi-coil images for now.
%
% Inputs:
%       data - calibration data [kx,ky,coils]
%       kSize - size of kernel (for example kSize=[6,6])
%
% Outputs:
%       kernel - k-space kernels matrix (not cropped), which correspond to
%                the basis vectors of overlapping blocks in k-space
%       S      - (Optional parameter) The singular vectors of the
%                 calibration matrix
%
%
% See also:
%           kernelEig
%
% (c) Michael Lustig 2013

[sx,sy,nc] = size(data);
imSize = [sx,sy] ;

tmp = im2row(data,kSize); [tsx,tsy,tsz] = size(tmp);
A = reshape(tmp,tsx,tsy*tsz);

[U,S,V] = svd(A,'econ');

kernel = reshape(V,kSize(1),kSize(2),nc,size(V,2));
S = diag(S);S = S(:);


function r = exp(a)
%EXP          Taylor exponential  exp(a)
%

% written  05/21/09     S.M. Rump
% modified 08/26/12     S.M. Rump  global variables removed
%

  e = 1e-30;
  if 1+e==1-e                   % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  K1 = getappdata(0,'INTLAB_TAYLOR_ORDER') + 1;

  r = a;
  N = size(a.t,2);
  r.t(1,:) = exp(a.t(1,:));
  for j=2:K1
    r.t(j,:) = sum( repmat((1:j-1)',1,N).*r.t(j-1:-1:1,:).*a.t(2:j,:) , 1 ) ./ (j-1);
  end

  if rndold
    setround(rndold)
  end


% [func, fx] = mcmc_init_hamiltonian(x_init, get_logpdf, get_dlogpdf, ...
% epsilon, L, func_x)
%
% epsilon is the parameter to the exponential distribution for sampling
% the momentum
%
% L is the number of simulation steps
function [func, fx] = mcmc_init_hamiltonian(x_init, get_logpdf, get_dlogpdf, ...
                                            epsilon, L, func_x)

if nargin < 6
  func_x = @func_x_default;
end

x_current = x_init;
[fx_current, dfx_current] = func_x(x_current);
logpdf_current = get_logpdf(fx_current);
dlogpdf_current = get_dlogpdf(dfx_current);

func = @hamiltonian;
fx = fx_current;

  function [x, fx] = hamiltonian(varargin)

  x = x_current;

  p = normrnd(0,1,size(x_current));
  p_current = p;

  lp_current = logpdf_current(varargin{:});
  dlp_current = dlogpdf_current(varargin{:});

% $$$   mycheckgrad(@chkgrad, x, 1e-6)
% $$$
% $$$     function [y,dy] = chkgrad(x)
% $$$     [fx, dfx] = func_x(x);
% $$$     f = get_logpdf(fx);
% $$$     df = get_dlogpdf(dfx);
% $$$     y = f(varargin{:});
% $$$     dy = df(varargin{:});
% $$$     end

  % Random step size
  e = exprnd(epsilon);

  % Make a half step for momentum at the beginning
  p = p + 0.5*e*dlp_current;

  for l=1:L
    x = x + e * p;

    if any(isnan(x))
      break;
    end

    [fx, dfx] = func_x(x);
    dlogpdf = get_dlogpdf(dfx);
    dlp = dlogpdf(varargin{:});
    if l < L
      p = p + e * dlp;
    else
      logpdf = get_logpdf(fx);
      lp = logpdf(varargin{:});
    end
  end

  if all(~isnan(x))

    % Make a half step for momentum at the end
    p = p + 0.5*e*dlp;

    % Negate momentum to make the proposal symmetric
    p = -p;

    if log(rand()) < ( (lp - 0.5*(p'*p)) - ...
                       (lp_current - 0.5*(p_current'*p_current)) )
      % Accept
      disp('Accept hamiltonian')
      x_current = x;
      fx_current = fx;
      dfx_current = dfx;
      logpdf_current = logpdf;
      dlogpdf_current = dlogpdf;
    else
      disp('Reject hamiltonian')
    end

  else
      disp('Reject hamiltonian')
  end

  x = x_current;
  fx = fx_current;
  end

end

function [fx, dfx] = func_x_default(x)
fx = x;
dfx = x;
end


%Normalizes homogenous coordinates such that the last coordinate is 1
%You can use any dimension of the vectors
%
%Author:    Christian Wengert,
%           Institute of Computer Vision
%           Swiss Federale Institute of Technology, Zurich (ETHZ)
%           wengert@vision.ee.ethz.ch
%           www.vision.ee.ethz.ch/~cwengert/
%
%Input:     x       unnormalized homogenous coordinates
%
%Output     y       normalized homogenous coordinates
%
%Syntax:    y = normalizeHomogenousCoordinates(x)

function y = normalizeHomogenousCoordinates(x)

    %get dimension of array
    ni = size(x,1);
    nj = size(x,2);
    %go through
    for j=1:1:nj
        y(:,j) = x(:,j)./x(ni,j);
    end
    y(ni,:) = ones(1,nj);


function [L,E,EMAP] = crouzeix_raviart_cotmatrix(V,F)
  % CROUZEIX_RAVIART_COTMATRIX Compute the Crouzeix-Raviart cotangent
  % Laplacian matrix where we use test functions define at edge midpoints.
  %
  % See for example "Discrete Quadratic Curvature Energies" [Wardetzky, Bergou,
  % Harmon, Zorin, Grinspun 2007]
  %
  % Inputs:
  %   V  #V by dim list of vertex positions
  %   F  #F by element-size list of triangle indices
  % Outputs:
  %   L  #E by #E edge-based sparse cotangent matrix
  %   E  #E by 2 list of edges
  %
  % Examples:
  %   % mesh in (V,F)
  %   [Lcr,E,EMAP] = crouzeix_raviart_cotmatrix(V,F);
  %   [Mcr,E,EMAP] = crouzeix_raviart_massmatrix(V,F);
  %   [Ucr,~] = eigs(Lcr,Mcr,5,'sm');
  %   % Convert between edge values and vertex values
  %   E2V = sparse(E(:),repmat(1:size(E,1),1,2)',1,size(V,1),size(E,1));
  %   E2V = bsxfun(@rdivide,E2V,sum(E2V,2));
  %   V2E = sparse(E(:),repmat(1:size(E,1),1,2)',1,size(V,1),size(E,1))';
  %   V2E = bsxfun(@rdivide,V2E,sum(V2E,2));
  %   tsurf(F,[V(:,1:2) E2V*Ucr(:,end-1)])
  %
  %   % Display discontinous solution
  %   FF = reshape(1:numel(F),size(F));
  %   VV = V(F,:);
  %   EMAP = reshape(EMAP,size(F));
  %   A = sparse( ...
  %     [FF FF FF], ...
  %     EMAP(:,[1 2 3 2 3 1 3 1 2]), ...
  %     repmat(-[1 1 1 -1 -1 -1 -1 -1 -1],size(F,1),1), ...
  %     size(VV,1), ...
  %     size(E,1));
  %   tsurf(FF,[VV(:,1:2) A*Ucr(:,end-1)])
  %
  %
  %
  % See also: edge_laplacian, is_boundary_edge, crouzeix_raviart_massmatrix,
  %   cotmatrix
  %

  %% check for non-manifold edges
  %S = statistics(V,F,'Fast',true);
  %if S.num_nonmanifold_edges > 0
  %  error(sprintf('There are %d non-manifold edges',num_nonmanifold_edges));
  %end

  % number of vertices
  n = size(V,1);
  % number of elements
  m = size(F,1);
  % simplex size
  ss = size(F,2);
  switch ss
  case 3
    % Compute cotangents: seems to be 0.5*C
    C = 2*cotangent(V,F);

    allE = [F(:,[2 3]);F(:,[3 1]);F(:,[1 2])];
    % Map each face-edge to a unique edge
    F2E = reshape(1:3*m,m,3);
    % Lij = -2 cot aij
    %
    % o
    % |\
    % | \
    % |  \
    % |   \
    % i    \
    % |     \
    % |      \
    % |αij    \
    % o----j---o
    %
    %
    LI =  F2E(:, [1 2 3 2 3 1        1 2 3 2 3 1]);
    LJ =  F2E(:, [2 3 1 1 2 3        1 2 3 2 3 1]);
    LV = 2*[-C(:,[3 1 2 3 1 2]) C(:,[3 1 2 3 1 2])];

    % Map duplicate edges to first instance
    [E,~,EMAP] = unique(sort(allE,2),'rows');

    assert(all(size(LI)==size(LJ)));
    assert(all(size(LI)==size(LV)));
    L = sparse(EMAP(LI),EMAP(LJ),LV,size(E,1),size(E,1));
  case 4
    % tets
    T = F;
    C = -2*cotangent(V,T);
    allF = [ ...
      T(:,2) T(:,4) T(:,3); ...
      T(:,1) T(:,3) T(:,4); ...
      T(:,1) T(:,4) T(:,2); ...
      T(:,1) T(:,2) T(:,3); ...
      ];
    % Map each element-face to a unique face
    T2F = reshape(1:4*m,m,4);
    % Lij = -2 lij cot αij
    LI = T2F(:,[1 4 4 4 2 3 2 1 2 3 3 1        1 4 4 4 2 3 2 1 2 3 3 1]);
    LJ = T2F(:,[2 1 2 3 3 1 1 4 4 4 2 3        1 4 4 4 2 3 2 1 2 3 3 1]);
    LV = [-C(:,[3 4 5 6 1 2 3 4 5 6 1 2]) C(:,[3 4 5 6 1 2 3 4 5 6 1 2])];
    % Map duplicate facets to first instance
    [F,~,FMAP] = unique(sort(allF,2),'rows');

    assert(all(size(LI)==size(LJ)));
    assert(all(size(LI)==size(LV)));
    L = sparse(FMAP(LI),FMAP(LJ),LV,size(F,1),size(F,1));
    E = F;
    EMAP = FMAP;

  otherwise
    error(['Simplex size ' num2str(ss) ' unsupported']);
  end

end


function fe2dx_r_fast_test ( )

%*****************************************************************************80
%
%% FE2DX_R_FAST_TEST tests the FE2DX_R_FAST code.
%
%  Discussion:
%
%    This function sets all parameter values and initial condition information
%    necessary to execute the "fast" version of the fe2dx_r algorithm.
%
%  Licensing:
%
%    Copyright (C) 2014 Marcus R. Garvie.
%    See 'mycopyright.txt' for details.
%
%  Modified:
%
%    28 April 2014
%
%  Author:
%
%    Marcus R. Garvie.
%
%  Reference:
%
%    Marcus R Garvie, John Burkardt, Jeff Morgan,
%    Simple Finite Element Methods for Approximating Predator-Prey Dynamics
%    in Two Dimensions using MATLAB,
%    Submitted to Bulletin of Mathematical Biology, 2014.
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'FE2DX_R_FAST_TEST:\n' );
  fprintf ( 1, '  Test the FE2DX_R_FAST function\n' );
  fprintf ( 1, '  which applies Robin boundary conditions as it\n' );
  fprintf ( 1, '  approximates a solution to a predator-prey system.\n' );
%
%  Set the parameters.
%
  alpha = 0.4;
  beta = 2.0;
  gamma = 0.6;
  delta = 1.0;
%
%  Use T=150.0 for normal run.
%  Use T=0.50 for a "quick" run that might take 15 minutes of computing.
%
% T = 150.0;
  T = 0.50;
  delt = 1.0 / 384.0;
  k1 = 0.01;
  k2 = 0.01;

  t = tic;
  fe2dx_r_fast ( alpha, beta, gamma, delta, T, delt, @u0f, @v0f, k1, k2 );
  t = toc ( t );

  fprintf ( 1, '  Execution took %10.2g minutes \n', t / 60.0 );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'FE2DX_R_FAST_TEST:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end

function value = u0f ( x, y )

%*****************************************************************************80
%
%% U0F evaluates the initial condition for U.
%
%  Licensing:
%
%    Copyright (C) 2014 Marcus R. Garvie.
%    See 'mycopyright.txt' for details.
%
%  Modified:
%
%    26 April 2014
%
%  Author:
%
%    Marcus R. Garvie.
%
%  Parameters:
%
%    Input, real X, Y, a location in the region.
%
%    Output, real VALUE, the initial condition for U at (X,Y).
%
  value = 6.0 / 35.0 - 2.0E-07 * ( x - 0.1 * y - 225.0 ) * ( x - 0.1 * y - 675.0 );

  return
end

function value = v0f ( x, y )

%*****************************************************************************80
%
%% V0F evaluates the initial condition for V.
%
%  Licensing:
%
%    Copyright (C) 2014 Marcus R. Garvie.
%    See 'mycopyright.txt' for details.
%
%  Modified:
%
%    26 April 2014
%
%  Author:
%
%    Marcus R. Garvie.
%
%  Parameters:
%
%    Input, real X, Y, a location in the region.
%
%    Output, real VALUE, the initial condition for V at (X,Y).
%
  value = 116.0 / 245.0 - 3.0E-05 * ( x - 450.0 ) - 1.2E-04 * ( y - 150.0 );

  return
end


%% Define Feedforward Convolutional Code
SNR_dB = 3
LM = 1600         % Mesg Length excluding pre-determined bits for starting & ending trellis at state 0
TREL_TYPE = 'Feedback'  % {'Feedback', 'Feedforward'}

%% Matlab Generator Polynomial convention
% Build a binary number representation by placing a 1 in each spot where a connection line from the shift register feeds into the adder,
% and a 0 elsewhere. The leftmost spot in the binary number represents the current input, while the rightmost spot represents the
% oldest input that still remains in the shift register.

if strcmp(TREL_TYPE, 'Feedback')
%     CL = 2		% Rate 1/2 Feedback encoder with 2 states
%     GenPoly0_1by2 = 3 % in octal
%     GenPoly1_1by2 = 2
%     FeedBackCoef = [1,1];  % For computing i/p bits needed to terminate trellis at state =0.
%     TREL = poly2trellis(CL, [GenPoly0_1by2, GenPoly1_1by2], GenPoly0_1by2)

%% Rate 1/2 Feedback encoder with 8-states used in 3GPP cellular 3G/4G standard
    CL = 4
    GenPoly0_1by2 = 13 % in octal
    GenPoly1_1by2 = 15
    FeedBackCoef = [1,0,1,1];% For computing i/p bits needed to terminate trellis at state =0.
    TREL = poly2trellis(CL, [GenPoly0_1by2, GenPoly1_1by2], GenPoly0_1by2)

else
    %% Rate 1/3 Feedforward encoder with 4 states
%     CL = 3; % constraint length
%     GenPoly0_third = 4; % in octal
%     GenPoly1_third = 5;
%     GenPoly2_third = 7;
%     TREL = poly2trellis( CL, [GenPoly0_third, GenPoly1_third, GenPoly2_third])

%     CL = 4 % constraint length
%     GenPoly0_1by4 = 13 % in octal
%     GenPoly1_1by4 = 15
%     GenPoly2_1by4 = 15
%     GenPoly3_1by4 = 17
%     TREL = poly2trellis( CL, [GenPoly0_1by4, GenPoly1_1by4, GenPoly2_1by4, GenPoly3_1by4])

end
LM = LM + 2*(CL-1); % space for start & tail bits

%% Verify trellis-structure is OK
[isok, status] = istrellis(TREL)

randn('state', sum(100*clock)); % initialize to random state

%% The encoder is assumed to have both started and ended at the all-zeros state
%% Ensure msg is s.t. trellis starts at "0" state and ends at "0" state.
%%  Generate Random binary stream & encode
if strcmp(TREL_TYPE, 'Feedback')
    msg1(1 : CL-1) = zeros(1, CL-1); % 1st (CL-1) bits must be 0
    msg1(CL : LM -CL+1) = randint(LM -2*CL +2, 1, 2)' ; % Random data
% 	msg1(CL : LM -CL+1) = [0 1];

    % Encode first part of msg, recording final state for later use.
    [cenc_o1, final_state1] = convenc(msg1, TREL);

    % Rest of msg depends on final_state1; it makes trellis terminate at final_state=0.
    bvec_fs = bitget(final_state1, (CL-1) : -1 : 1); % All possible binary-vectors of length EncoderN
    for idx = 1 : (CL-1)
        msg2(idx) = rem( [0, bvec_fs]*FeedBackCoef', 2);
        bvec_fs = [0, bvec_fs(1: (end-1))];
    end
%     msg2(1 : CL -1) = bvec_fs; % Last (CL-1) bits depend on state at time=(LM-CL+1)
    [cenc_o2, final_state] = convenc(msg2, TREL, final_state1);

    msg = [msg1, msg2]; clear msg1 msg2
    [cenc_o] = [cenc_o1, cenc_o2]; clear cenc_o1 cenc_o2
    final_state
else % Feedforward
    msg(1 : CL-1) = zeros(1,CL-1); % 1st (CL-1) bits must be 0
    msg(CL : LM -CL+1) = randint(LM -2*CL +2, 1, 2)' ; % Random data
    msg(LM -CL + 2 : LM) = zeros(CL-1, 1) % Last (CL-1) bits must be 0
    [cenc_o, final_state] = convenc(msg, TREL);
end

if final_state ~= 0
    disp('trellis not terminated properly: check last (CL-1) bits of mesg')
    return
end

%%	Map to BPSK constellation: bit0 -> 1, bit1 -> -1
signal_power = 1;
chan_in = (1 - 2*cenc_o)*sqrt(signal_power);

%%	Generate and add Gaussian-noise mean=0, variance= noise_power
noise_power = signal_power / 10^(SNR_dB/10);
noise = randn(size(chan_in))*sqrt(noise_power);
chan_o = chan_in + noise;
% chan_o = chan_in;

Es = log2(TREL.numOutputSymbols)*signal_power;
No = noise_power;
Lc = 4*Es/No

%%	Soft-Decision decoding, map to decision values
soft_in = chan_o;
% decodeds = vitdec_htm(TREL, soft_in');

[LLR, Alpha, Beta] = LogMAPdecode_htm(TREL, chan_o, Lc);

decd_msg = (1 - sign(LLR))/2;
err_vec = abs(decd_msg - msg);

sprintf('# of Bit-Errors = %d out of %d info-bits ', sum(abs(err_vec)), LM -CL -2)


function B=unimodalcrossproducts(XtX,XtY,Bold)

%UNIMODALCROSSPRODUCTS
% Solves the problem min|Y-XB'| subject to the columns of
% B are unimodal and nonnegative. The algorithm is iterative and
% only one iteration is given, hence the solution is only improving
% the current estimate
%
% I/O B=unimodalcrossproducts(XtX,XtY,Bold)
% Modified from unimodal.m to handle crossproducts in input 1999
% Reference
% Bro and Sidiropoulos, "Journal of Chemometrics", 1998, 12, 223-247.

% Copyright (C) 1995-2006  Rasmus Bro & Claus Andersson
% Copenhagen University, DK-1958 Frederiksberg, Denmark, rb@life.ku.dk
%
% 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 should have received a copy of the GNU General Public License along with
% this program; if not, write to the Free Software Foundation, Inc., 51 Franklin
% Street, Fifth Floor, Boston, MA  02110-1301, USA.

B=Bold;
F=size(B,2);
for f=1:F
   xty = XtY(f,:)-XtX(f,[1:f-1 f+1:F])*B(:,[1:f-1 f+1:F])';
   beta=pinv(XtX(f,f))*xty;
   B(:,f)=ulsr(beta',1);
end

function [b,All,MaxML]=ulsr(x,NonNeg);

% ------INPUT------
%
% x          is the vector to be approximated
% NonNeg     If NonNeg is one, nonnegativity is imposed
%
%
%
% ------OUTPUT-----
%
% b 	     is the best ULSR vector
% All 	     is containing in its i'th column the ULSRFIX solution for mode
% 	     location at the i'th element. The ULSR solution given in All
%            is found disregarding the i'th element and hence NOT optimal
% MaxML      is the optimal (leftmost) mode location (i.e. position of maximum)
%
% ___________________________________________________________
%
%
%               Copyright 1997
%
% Nikos Sidiroupolos
% University of Maryland
% Maryland, US
%
%       &
%
% Rasmus Bro
% Royal Veterinary & Agricultural University
% Denmark
%
%
% ___________________________________________________________

% This file uses MONREG.M

x=x(:);
I=length(x);
xmin=min(x);
if xmin<0
  x=x-xmin;
end

% THE SUBSEQUENT
% CALCULATES BEST BY TWO MONOTONIC REGRESSIONS

% B1(1:i,i) contains the monontonic increasing regr. on x(1:i)
[b1,out,B1]=monreg(x);

% BI is the opposite of B1. Hence BI(i:I,i) holds the monotonic
% decreasing regression on x(i:I)
[bI,out,BI]=monreg(flipud(x));
BI=flipud(fliplr(BI));

% Together B1 and BI can be concatenated to give the solution to
% problem ULSR for any modloc position AS long as we do not pay
% attention to the element of x at this position

All=zeros(I,I+2);
All(1:I,3:I+2)=B1;
All(1:I,1:I)=All(1:I,1:I)+BI;
All=All(:,2:I+1);
Allmin=All;
Allmax=All;
% All(:,i) holds the ULSR solution for modloc = i, disregarding x(i),

iii=find(x>=max(All)');
b=All(:,iii(1));
b(iii(1))=x(iii(1));
Bestfit=sum((b-x).^2);
MaxML=iii(1);
for ii=2:length(iii)
  this=All(:,iii(ii));
  this(iii(ii))=x(iii(ii));
  thisfit=sum((this-x).^2);
  if thisfit<Bestfit
    b=this;
    Bestfit=thisfit;
    MaxML=iii(ii);
  end
end

if xmin<0
  b=b+xmin;
end

% Impose nonnegativity
if NonNeg==1
  if any(b<0)
    id=find(b<0);
    % Note that changing the negative values to zero does not affect the
    % solution with respect to nonnegative parameters and position of the
    % maximum.
    b(id)=zeros(size(id))+0;
  end
end

function [b,B,AllBs]=monreg(x);

% Monotonic regression according
% to J. B. Kruskal 64
%
% b     = min|x-b| subject to monotonic increase
% B     = b, but condensed
% AllBs = All monotonic regressions, i.e. AllBs(1:i,i) is the
%         monotonic regression of x(1:i)
%
%
% Copyright 1997
%
% Rasmus Bro
% Royal Veterinary & Agricultural University
% Denmark
% rb@kvl.dk
%

I=length(x);
if size(x,2)==2
   B=x;
else
   B=[x(:) ones(I,1)];
end

   AllBs=zeros(I,I);
   AllBs(1,1)=x(1);
   i=1;
   while i<size(B,1)
      if B(i,1)>B(min(I,i+1),1)
          summ=B(i,2)+B(i+1,2);
          B=[B(1:i-1,:);[(B(i,1)*B(i,2)+B(i+1,1)*B(i+1,2))/(summ) summ];B(i+2:size(B,1),:)];
          OK=1;
          while OK
             if B(i,1)<B(max(1,i-1),1)
                summ=B(i,2)+B(i-1,2);
                B=[B(1:i-2,:);[(B(i,1)*B(i,2)+B(i-1,1)*B(i-1,2))/(summ) summ];B(i+1:size(B,1),:)];
                i=max(1,i-1);
             else
                OK=0;
             end
          end
          bInterim=[];
          for i2=1:i
             bInterim=[bInterim;zeros(B(i2,2),1)+B(i2,1)];
          end
          No=sum(B(1:i,2));
          AllBs(1:No,No)=bInterim;
      else
          i=i+1;
          bInterim=[];
          for i2=1:i
             bInterim=[bInterim;zeros(B(i2,2),1)+B(i2,1)];
          end
          No=sum(B(1:i,2));
          AllBs(1:No,No)=bInterim;
      end
  end

  b=[];
  for i=1:size(B,1)
    b=[b;zeros(B(i,2),1)+B(i,1)];
 end


function [Zin,ip_it] = plain_foopsi(H,D,I_est,eps)

% solves argmin ||X-H||^2 subject to D*X>=0 with an interior point method
% using I_est as the initial value and eps as the initial barrier weight

ln = length(H);
step_back_frac = 0.5;
iter = 0;
if nargin == 2
    I_est = 1e-3*ones(ln,1);
    eps = 1;
end
Zin = I_est(:);

if nargin == 3
    eps = 1;
end
while eps>1e-5
    n = D*Zin;
    nnd = 10;
    E = norm(Zin-H)^2 - eps*sum(log(D*Zin));
    grad = 2*(Zin-H) - eps*D'*(n.^(-1));
    Hs = 2*speye(ln) + eps*D'*spdiags(n.^(-2),0,ln,ln)*D;
    while nnd/2>1
        iter = iter + 1;
        Z_dir = -Hs\grad;
        hit = -n./(D*Z_dir);
        if all(hit<0)
            s = 1;
        else
            s = min(1,.9*min(hit(hit>=0)));
        end
        E_new = E; s = s/step_back_frac;
        x_dg = grad'*Z_dir;
        while E_new > E + 0.25*s*x_dg
            s=s*step_back_frac;
            Z_new = Zin + s*Z_dir;
            n = D*Zin;
            E_new = norm(Z_new-H)^2 - eps*sum(log(D*Z_new));
        end
        %E = E_new;
        Zin = Zin + s*Z_dir;
        nnd = -x_dg;
        E = norm(Zin-H)^2 - eps*sum(log(D*Zin));
        n = D*Zin;
        grad = 2*(Zin-H) - eps*D'*(n.^(-1));
        Hs = 2*speye(ln) + eps*D'*spdiags(n.^(-2),0,ln,ln)*D;
        %disp(nnd)
    end
    eps = eps/10;
end
%fprintf('Interior point method converged after %i iterations \n',iter);
ip_it = iter;


function node_boundary = triangulation_order3_boundary_node ( node_num, ...
  triangle_num, triangle_node )

%*****************************************************************************80
%
%% TRIANGULATION_ORDER3_BOUNDARY_NODE indicates which nodes are on the boundary.
%
%  Discussion:
%
%    This routine is given a triangulation, an abstract list of triples
%    of nodes.  It is assumed that the nodes in each triangle are listed
%    in a counterclockwise order, although the routine should work
%    if the nodes are consistently listed in a clockwise order as well.
%
%    It is assumed that each edge of the triangulation is either
%    * an INTERIOR edge, which is listed twice, once with positive
%      orientation and once with negative orientation, or;
%    * a BOUNDARY edge, which will occur only once.
%
%    This routine should work even if the region has holes - as long
%    as the boundary of the hole comprises more than 3 edges!
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 June 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NODE_NUM, the number of nodes.
%
%    Input, integer TRIANGLE_NUM, the number of triangles.
%
%    Input, integer TRIANGLE_NODE(3,TRIANGLE_NUM), the nodes that make up the
%    triangles.  These should be listed in counterclockwise order.
%
%    Output, logical NODE_BOUNDARY(NODE_NUM), is TRUE if the node
%    is on a boundary edge.
%
  m = 2;
  n = 3 * triangle_num;
%
%  Set up the edge array.
%
  edge(1:2,               1:  triangle_num) = triangle_node(1:2,1:triangle_num);
  edge(1:2,  triangle_num+1:2*triangle_num) = triangle_node(2:3,1:triangle_num);
  edge(1,  2*triangle_num+1:3*triangle_num) = triangle_node(3,  1:triangle_num);
  edge(2,  2*triangle_num+1:3*triangle_num) = triangle_node(1,  1:triangle_num);
%
%  In each column, force the smaller entry to appear first.
%
  e1(1:n) = min ( edge(1:2,1:n) );
  e2(1:n) = max ( edge(1:2,1:n) );

  edge(1,1:n) = e1(1:n);
  edge(2,1:n) = e2(1:n);
%
%  Ascending sort the column array.
%
  edge = ( sortrows ( edge' ) )';
%
%  Records which appear twice are internal edges and can be ignored.
%
  node_boundary(1:node_num) = 0;

  j = 0;

  while ( j < 3 * triangle_num )

    j = j + 1;

    if ( j == 3 * triangle_num )
      node_boundary(edge(1:m,j)) = 1;
    elseif ( all ( edge(1:m,j) == edge(1:m,j+1) ) )
      j = j + 1;
    else
      node_boundary(edge(1:m,j)) = 1;
    end

  end

  return
end