//#########################################################//
//#                                                       #//
//# gaussian_mixture_models  gaussian.cpp                 #//
//# Roberto Capobianco  <capobianco@dis.uniroma1.it>      #//
//#                                                       #//
//#########################################################//

#include <Eigen/Eigenvalues>
#include <cmath>

#include <particle_filter/gaussian.h>

namespace gmms {
double Gaussian::evaluate_point(const Eigen::VectorXd& pt) const {
    if (pt.size() != dimensionality_) {
        throw std::runtime_error(dimensionality_mismatch());
    }

    Eigen::VectorXd dist = pt - mean_;

    double factor = std::sqrt(
            std::pow(2 * M_PI, dimensionality_) * cov_abs_determinant_);
    double exp = std::exp(-0.5 * dist.transpose() * inv_covariance_ * dist);

    double result = exp / factor;

    if (result == 0) {
        srand(time(NULL));
        double random = ((double) rand() / (RAND_MAX));
        result = random * 1e-15;
    }

    return result;
}

void Gaussian::setCovariance(const Eigen::MatrixXd& covariance) {
    if (covariance.rows() != covariance.cols() ||
            covariance.rows() != dimensionality_) {
        throw std::runtime_error(dimensionality_mismatch());
    }

    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> solver(covariance.cols());
    solver.compute(covariance);

    Eigen::VectorXd eigenvalues = solver.eigenvalues();
    Eigen::MatrixXd eigenvectors = solver.eigenvectors();

    for (int i = 0; i < eigenvalues.rows(); ++i) {
        if (eigenvalues(i) < M_EPS) {
            eigenvalues(i) = M_EPS;
        }
    }

    covariance_ =
            eigenvectors * eigenvalues.asDiagonal() * eigenvectors.inverse();
    double abs_determinant = std::fabs(covariance_.determinant());

    inv_covariance_ = covariance_.inverse();
    cov_abs_determinant_ = abs_determinant;
}

void Gaussian::setMeanCovariance(const Eigen::VectorXd& mean,
        const Eigen::MatrixXd& covariance) {
    int dimensionality = mean.size();
    dimensionality_ = dimensionality;
    setMean(mean);
    setCovariance(covariance);
}

std::string Gaussian::toString() const {
    Eigen::IOFormat mean_format(
            5, 0, ", ", "\n", "[", "]", "Mean:       ", "\n");
    Eigen::IOFormat cov_format(
            5, 0, ", ", "\n", "[", "]", "Covariance: ", "\n");
    std::stringstream gaussian_string;
    gaussian_string << "Gaussian: \n";
    gaussian_string << mean_.format(mean_format);
    gaussian_string << covariance_.format(cov_format);
    return gaussian_string.str();
}

void Gaussian::addToEigenMatrix(Eigen::MatrixXd& matrix,
        float x0,
        float y0,
        float x1,
        float y1,
        int stepcount) const {
    assert(x0 < x1);
    assert(y0 < y1);
    float x_delta = std::abs(x1 - x0);
    float y_delta = std::abs(y1 - y0);
    float x, y;
    float x_mean = mean_(0, 0);
    float y_mean = mean_(1, 0);
    // TODO: save stepsize

    for (int y_step = 0; y_step < stepcount; y_step++) {
        y = y0 + (y_delta / stepcount * y_step);
        for (int x_step = 0; x_step < stepcount; x_step++) {
            x = x0 + (x_delta / stepcount * x_step);
            // Function taken from
            // https://en.wikipedia.org/wiki/Gaussian_function#Two-dimensional_Gaussian_function
            // matrix(y_step, x_step) += 1 * std::exp(-(std::pow(x - x_mean, 2)
            // / covariance_(0, 0)  + 2 * (x - x_mean) * (y - y_mean) /
            // covariance_(1, 0) + std::pow(y - y_mean, 2) / covariance_(1,
            // 1)));
            matrix(y_step, x_step) +=
                    1 * std::exp(-((std::pow(x - x_mean, 2) /
                                           (2 * covariance_(0, 0))) +
                                   (std::pow(y - y_mean, 2) /
                                           (2 * covariance_(1, 1)))));
        }
    }
}

double Gaussian::calcDistance(const Eigen::VectorXd& mean) const {
    if (mean.size() != dimensionality_) {
        throw std::runtime_error(dimensionality_mismatch());
    }
    Eigen::VectorXd diff_vec = mean_ - mean;
    return diff_vec.cwiseAbs().sum() / static_cast<double>(dimensionality_);
}

}  // namespace gmms


#ifndef MCMC_UTILS_CPP_UPDATES_HPP
#define MCMC_UTILS_CPP_UPDATES_HPP

#include <algorithm>
#include <cmath>
#include <numeric>
#include <tuple>
#include <vector>

#include <Eigen/Dense>

namespace updates {
/*
 * Computes the posterior parameters for a model
 *     x_1, ... x_n ~ N(mu, 1 / tau)
 *     \mu | tau ~ N(mu0, 1 / (lambda * tau))
 *     \tau ~ Gamma(a, b)
 *
 * returns an std::vector{mu0_post, a_post, b_post, lambda_post}
 */
std::vector<double> normalGammaUpdate(
    std::vector<double> data, double priorMean, double priorA,
    double priorB, double priorLambda);

/*
 * Computes the posterior parameters of the parameter \beta
 * for a linear regression model (univariate)
 *
 *          y_i = x_i^T \beta + \epsilon_i
 *          epsilon_i ~ N(0, \sigma^2)
 *          \beta ~ N(\mu0, \Lambda)
 *
 * where \Lambda is the precision matrix
 *
 * Then \beta | \sigma^2, y ~ N(mu_post, Lambda_post)
 * where
 *          Lambda_post = \Lambda + X^t X
 *          mu_post = Lambda_post^{-1} (X^t y + \Lambda \mu0)
 */
std::tuple<Eigen::VectorXd, Eigen::MatrixXd> simpleLinearRegressionUpdate(
    Eigen::VectorXd y, Eigen::MatrixXd X, Eigen::VectorXd betaMean,
    Eigen::MatrixXd betaPrec);

/*
 * Computes the posterior parameters of the parameter \beta
 * for a linear regression model (univariate)
 *
 *          y_i = x_i^T \beta + \epsilon_i
 *          epsilon_i ~ N(\mu_i, \sigma_i^2)
 *          \beta ~ N(\mu0, \Lambda)
 *
 * where \Lambda is the precision matrix
 *
 * Then \beta | \sigma^2, y ~ N(mu_post, Lambda_post)
 * where, being V = diag(sigma^2_i)^{-1}
 *
 *          Lambda_post = \Lambda + X^t V X
 *          mu_post = Lambda_post^{-1} (X^t y + \Lambda \mu0)
 */
std::tuple<Eigen::VectorXd, Eigen::MatrixXd> heteroSchedLinearRegressionUpdate(
    Eigen::VectorXd y, Eigen::MatrixXd X, Eigen::VectorXd betaMean,
    Eigen::MatrixXd betaPrec, Eigen::VectorXd mu,
    Eigen::VectorXd V);

}

#endif


/*******************************************************************************
 *         Copyright 2003-2012 LASMEA UMR 6602 CNRS/U.B.P
 *         Copyright 2011-2012 LRI    UMR 8623 CNRS/Univ Paris Sud XI
 *
 *          Distributed under the Boost Software License, Version 1.0.
 *                 See accompanying file LICENSE.txt or copy at
 *                     http://www.boost.org/LICENSE_1_0.txt
 ******************************************************************************/
#ifndef NT2_TOOLBOX_LINALG_FUNCTIONS_DETAILS_LU_HPP_INCLUDED
#define NT2_TOOLBOX_LINALG_FUNCTIONS_DETAILS_LU_HPP_INCLUDED

#include <nt2/sdk/error/warning.hpp>
#include <nt2/include/functions/triu.hpp>
#include <nt2/include/functions/tri1l.hpp>
#include <nt2/include/functions/height.hpp>
#include <nt2/include/functions/width.hpp>
#include <nt2/include/functions/numel.hpp>
#include <nt2/include/functions/diag_of.hpp>
#include <nt2/include/functions/eye.hpp>
#include <nt2/include/functions/expand.hpp>
#include <nt2/include/functions/rif.hpp>
#include <nt2/include/functions/max.hpp>
#include <nt2/include/functions/rec.hpp>
#include <nt2/include/functions/norm.hpp>
#include <nt2/include/functions/prod.hpp>
#include <nt2/include/functions/eps.hpp>
#include <nt2/include/functions/frexp.hpp>
#include <nt2/include/functions/abs.hpp>
#include <nt2/include/functions/sb2b.hpp>
#include <nt2/include/functions/is_eqz.hpp>
#include <nt2/include/functions/isempty.hpp>
#include <nt2/include/functions/issquare.hpp>
#include <nt2/include/functions/if_one_else_zero.hpp>
#include <nt2/include/constants/eps.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/toolbox/linalg/details/utility/options.hpp>
#include <nt2/toolbox/linalg/details/utility/workspace.hpp>
#include <nt2/toolbox/linalg/details/lapack/getrf.hpp>
#include <nt2/toolbox/linalg/details/lapack/gecon.hpp>
#include <nt2/toolbox/linalg/details/lapack/getri.hpp>
#include <nt2/toolbox/linalg/details/lapack/gesvx.hpp>
#include <nt2/toolbox/linalg/details/lapack/lange.hpp>
#include <boost/dispatch/details/ignore_unused.hpp>
#include <nt2/core/container/table/table.hpp>

// TODO:
// these are the kind of syntaxes to be enforced by nt2::chol
//  lu     lu factorization.
//     [l,u] = lu(a) stores an upper triangular matrix in u and a
//     "psychologically lower triangular matrix" (i.e. a product of lower
//     triangular and permutation matrices) in l, so that a = l*u. a can be
//     rectangular.
// -> {pl, u]

//     [l,u,p] = lu(a) returns unit lower triangular matrix l, upper
//     triangular matrix u, and permutation matrix p so that p*a = l*u.
// -> {l, u, p]

//     [l,u,p] = lu(a,'vector') returns the permutation information as a
//     vector instead of a matrix.  that is, p is a row vector such that
//     a(p,:) = l*u.  similarly, [l,u,p] = lu(a,'matrix') returns a
//     permutation matrix p.  this is the default behavior.
// -> {l, u, ip]

////////////////////////////////////////////////////////////////////////////////////////
// The class provides:
// constructor from an expression or matrix a
//
// accesors to l, u, p, pl and ip
//
// l is lower unittriangular part of lu_
// u is upper triangular part of lu_
// p is a permutation matrix
// pl is l permuted by p
// ip is the permutation index vector defining p
// we have l*u =  p*a
//         pl*u =  a
/////////////////////////////////////////////////// TODO  perhaps l and u could return an expresiion rather than a matrix
//
// the class allow to compute
//
// status the plu status from lapack
//
// rank the matrix rank
//
// When a is square
// det and absdet determinant and absolute value of the determinant
// They have two syntaxes
//  1 - d =  f.det()   or d =  f.absdet()
//  2 - m =  f.det(e)  or m =  f.absdet(e)
// In the second case m is a mantissa and e and exponent
// If there is no overflow or undeflow in e the determinant is then equal to ldexp(m, e)
//
// rcond the reciprocal condition number of a
//
// Two solvers
// solve and inplace_solve
//
// a matrix inversion
// inv
// inv() can emit a warning if rcond is bad
// inv(false) never warns
//     please avoid using inv if you need not the inverse coefficients.

namespace nt2 { namespace details
{
  template<class T> struct lu_result
  {
    typedef typename meta::strip<T>::type                   source_t;
    typedef typename source_t::value_type                     type_t;
    typedef typename meta::as_integer<type_t, signed>::type  itype_t;
    typedef typename source_t::index_type                    index_t;
    typedef typename meta::as_real<type_t>::type              base_t;
    typedef T                                                 data_t;
    typedef nt2::table<type_t,nt2::matlab_index_>              tab_t;
    typedef nt2::table<base_t,nt2::matlab_index_>             btab_t;
    typedef nt2::table<itype_t,nt2::matlab_index_>            itab_t;
    typedef nt2::details::workspace<type_t>              workspace_t;
    typedef nt2::table<nt2_la_int,nt2::matlab_index_>         ibuf_t;
    typedef nt2::table<type_t,index_t>                   result_type;

    template<class Input>
    lu_result ( Input& xpr
                , typename boost::
                disable_if_c<boost::is_same<lu_result,Input>::value>::type* = 0
      )
      : a_(xpr)
      , lu_(xpr)
      , m_( nt2::height(xpr) )
      , n_( nt2::width(xpr)  )
      , ldlu_( lu_.leading_size() )
      , ipiv_(nt2::of_size(nt2::min(n_, m_), 1))
      , info_(0)
      , p_(of_size(0, 1))
      , ip_(of_size(0, 1))
      , pl_(of_size(0, 1))
      , invt_(of_size(0, 1))
    {
      nt2::details::getrf(&m_, &n_, lu_.raw(), &ldlu_, ipiv_.raw(), &info_, w_);
    }

    lu_result(lu_result const& src)
      : a_(src.a_) , lu_(src.lu_), m_( src.m_ ), n_( src.n_ )
      , ldlu_( src.ldlu_ ) , ipiv_(src.ipiv_)
      , info_(src.info_) , w_(src.w_)
      , p_(src.p_), ip_(src.ip_), pl_(src.pl_), invt_(src.invt_)
    {}

    lu_result& operator=(lu_result const& src)
    {
      a_      = src.a_;
      lu_     = src.lu_;
      m_      = src.m_;
      n_      = src.n_;
      ldlu_   = src.ldlu_;
      ipiv_   = src.ipiv_;
      info_   = src.info_;
      w_      = src.w_;
      p_      = src.p_;
      ip_     = src.ip_;
      pl_     = src.pl_;
      invt_   = src.invt_;
      return *this;
    }

    //==========================================================================
    // Return raw values
    //==========================================================================
    const tab_t& values() const { return lu_; }

    //==========================================================================
    // Return raw values
    //==========================================================================
    const tab_t& original() const { return a_; }

    //==========================================================================
    // Return u part of the decomposition
    //==========================================================================
    typedef typename meta::call < tag::colon_(ptrdiff_t, ptrdiff_t)>::type                          u_T2;
    typedef typename meta::call < tag::function_(tab_t const&, u_T2, nt2::container::colon_)>::type u_T0;
    typedef typename meta::call < tag::triu_(u_T0)>::type                                           u_result;
    u_result u() const
    {
      ptrdiff_t mm =  std::min(n_, m_);
      return nt2::triu(lu_(_(ptrdiff_t(1), mm),_));
    }
    //==========================================================================
    // Return l part of the decomposition
    //==========================================================================
    typedef typename meta::call < tag::colon_(ptrdiff_t, ptrdiff_t)>::type                          l_T2;
    typedef typename meta::call < tag::function_(tab_t const&, nt2::container::colon_, l_T2)>::type l_T0;
    typedef typename meta::call < tag::tri1l_(l_T0)>::type                                          l_result;
    l_result l() const
    {
      ptrdiff_t mm =  std::min(n_, m_);
      return nt2::tri1l(lu_(_,_(ptrdiff_t(1), mm)));
    }

    //==========================================================================
    // Return p part of the decomposition as a matrix such that p*a = l*u
    //==========================================================================
    const tab_t& p()
    {
      if (isempty(p_))
      {
        std::size_t mm = nt2::numel(ipiv_);
        p_ = nt2::eye(mm, mm, meta::as_<type_t>());
        for(size_t i=1; i <= mm; ++i)
          // p_({i, ipiv_(i)}, _) =  p_({ipiv_(i),i}, _)
        {
          tab_t c = p_(i, _);
          p_(i,_) = p_(ipiv_(i),_);
          p_(ipiv_(i),_) = c;
        }
      }
      return p_;
    }
    //==========================================================================
    // Return p part of the decomposition as a vector
    //==========================================================================
    const itab_t& ip()
    {
      if (isempty(ip_))
      {
        //      itab_t ip = itab_t(ipiv_.raw(), ipiv_.raw()+numel(ipiv_));
        ip_.resize(of_size(1, numel(ipiv_)));
        for(size_t i=1; i <= numel(ipiv_); ++i) ip_(i) = ipiv_(i);
      }
      return ip_;
    }

    //==========================================================================
    // Return tpl part of the decomposition  a =  tpl*u (pl = tp *l)
    //==========================================================================
    const tab_t& pl()
    {
      if (isempty(pl_))
      {
        //    return trans(p())*l;
        std::size_t mm = nt2::numel(ipiv_); //incorrect
        pl_ = l();
        for(size_t i=1; i <= mm; ++i)
          // pp({i, ipiv_(i)}, _) =  pp({ipiv_(i),i}, _)
        {
          tab_t c = pl_(ipiv_(i), _);
          pl_(ipiv_(i),_) = pl_(i,_);
          pl_(i,_) = c;
        }
      }
      return pl_;
    }

    //==========================================================================
    // Return post-computation status
    //==========================================================================
    nt2_la_int  status() const { return info_; }

    //==========================================================================
    // Reverse conditioning evaluation
    //==========================================================================
    base_t rcond(char c = '1')
    {
      /* this method which is presumably faster provides results that depend on
      // the lapack used version
      // it seems that gecon is buggy (J.T.L. 28/2/2013)
      //
      //   base_t rc = 0;
      //   tab_t aa = a_;
      //   char norm = (c == 1) ? '1' : ((c == 0) ? 'o' : c);
      //   base_t anorm = nt2::details::lange(&norm,  &n_,  &n_, aa.raw(), &ldlu_);
      //   nt2::details::gecon(&norm, &n_,  lu_.raw(), &ldlu_, &anorm, &rc, &info_);
      //   return rc;
      //
        So we switch to a direct computation
      */
      return nt2::rec(nt2::norm(a_, c)*nt2::norm(this->inv(false), c));
    }

    //==========================================================================
    // system rank
    //==========================================================================
    size_t rank(base_t epsi = nt2::Eps<base_t>()) //provisouare
    {
      //int32_t r = 0;
      base_t thresh = nt2::max(n_, m_)*epsi*nt2::max(nt2::abs(nt2::diag_of(lu_)(_)));
      return  size_t(sum(if_one_else_zero(gt(nt2::diag_of(lu_), thresh))(_)));

      //       for(int i=1; i <= nt2::min(n_, m_); ++i)
      //         {
      //           if(nt2::abs(lu_(i, i)) > thresh) ++r;
      //         }
      //       return r;
      //      nt2::inbtrue(nt2::abs(diag_of(lu_)) > nt2::max(n_, m_)*epsi*nt2::max(abs(diag_of(lu_()))) );
    }

    base_t absdet()
    {
      BOOST_ASSERT_MSG(m_ == n_, "non square matrix in determinant computation");
      return nt2::prod(nt2::abs(nt2::diag_of(lu_)(_)));
    }

    type_t signdet(bool check = true){
      BOOST_ASSERT_MSG(m_ == n_, "non square matrix in determinant computation");
      boost::dispatch::ignore_unused(check);
      //if (check)     BOOST_ASSERT_MSG(is_real<type_t>::value, "determinant sign is not avalaible for complex matrices");
      //count modulo 2 the number of ipiv_ elements such that ipiv_(i) !=  i
      //return nt2::sum(nt2::sb2b(ipiv_ != cif(numel(ipiv_), 1, meta::as_<itype_t>())))&1 ? Mone<type_t>() : One<type_t>();
      type_t s = One<type_t>();
      const nt2_la_int num = numel(ipiv_);
      for(nt2_la_int i=1; i < num ; ++i)
      {
        if (ipiv_(i) !=  i) s = -s;
      }
      return s;
    }

    type_t det(){
      BOOST_ASSERT_MSG(m_ == n_, "non square matrix in determinant computation");
      //     BOOST_ASSERT_MSG(is_real<type_t>::value, "determinant sign is not avalaible for complex matrices");
      //count modulo 2 the number of ipiv_ elements such that ipiv_(i) !=  i
      return  nt2::prod(nt2::diag_of(lu_)(_))*signdet(false);
    }

    type_t absdet(itype_t & exponent)
    {
      BOOST_ASSERT_MSG(m_ ==  n_, "non square matrix in determinant computation");
      // compute e and return m for matrix determinant such that |det| = ldexp(m, e)
      // if no overflow or underflow can occur,  with 0.5 < abs(m) < 1
      // the exponent result is enough to know the order of magnitude of the determinant
      // (between 0.5*2^e and 2^e if the mantissa is non zero)
      // This routine is inspired from linpack http://www.netlib.org/linpack/dgedi.f
      // that use ten power factor instead
      type_t   m1 = One<type_t>();
      exponent = Zero<itype_t>();
      for(size_t i = 1;  i <= size_t(n_); ++i)
      {
        itype_t e;
        m1 *=  nt2::abs(nt2::frexp(nt2::abs(lu_(i, i)), e));
        exponent+= e;
      }
      if (is_eqz(m1)){
        exponent = Zero<itype_t>();
      }
      return m1;
    }

    type_t det(itype_t & exponent)
    {
      BOOST_ASSERT_MSG(m_ ==  n_, "non square matrix in determinant computation");
      // compute e and m for matrix determinant such that det = ldexp(m, e)
      // if no overflow or underflow can occur,  with 0.5 < abs(m) < 1
      // the exponent result is enough to know the order of magnitude of the determinant
      // (between 0.5*2^e and 2^e if the mantissa is non zero)
      // This routine is inspired from linpack http://www.netlib.org/linpack/dgedi.f
      // that use ten power factor instead
      type_t   m1 = One<type_t>();
      exponent = Zero<itype_t>();
      for(size_t i = 1;  i <= size_t(n_); ++i)
      {
        itype_t e;
        m1 *=  nt2::frexp(nt2::abs(lu_(i, i)), e);
        exponent+= e;
      }
      if (is_eqz(m1)){
        exponent = Zero<itype_t>();
      }
      return m1*signdet(false);
    }

    //==========================================================================
    // Solver interface
    //==========================================================================
    template<class XPR> result_type solve(const XPR& b )
    {
      result_type bb = b;
      inplace_solve(bb);
      return bb;
    }

    //==========================================================================
    // inverse matrix: DO NOT USE THAT TO SOLVE A SYSTEM
    //==========================================================================
    const tab_t& inv(bool warn = true)
    {
      if(isempty(invt_))
      {
        if (warn)
        {
          base_t rc = rcond();
          NT2_WARNING ( (rc >= nt2::Eps<base_t>())
                        , "Matrix is close to singular or badly scaled."
                        " Results may be inaccurate."
            );
          return invt_;  /* it has been calculated by rcond */
        }
        invt_ = lu_;
        nt2::details::getri(&n_, invt_.raw(), &ldlu_, ipiv_.raw(), &info_, w_);
      }
      return invt_;
    }

    template<class Xpr> void inplace_solve(Xpr& b )
    {
      BOOST_ASSERT_MSG(issquare(a_), "matrix must be square to use the lu solver");
      nt2_la_int nrhs = nt2::size(b, 2);
      nt2_la_int ldb  = b.leading_size();
      tab_t x(b);
      nt2_la_int ldx  = x.leading_size();
      btab_t ferr(of_size(nrhs, 1)), berr(of_size(nrhs, 1));
      btab_t r_(of_size(n_, 1)), c_(of_size(n_, 1));
      char equed = 'N';
      base_t rc = rcond();
      nt2::details::gesvx(nt2::details::lapack_option('F'),
                          nt2::details::lapack_option('N'),
                          &n_, &nrhs,
                          a_.raw(), &ldlu_,
                          lu_.raw(), &ldlu_,
                          ipiv_.raw(),
                          &equed,
                          r_.raw(),
                          c_.raw(),
                          b.raw(), &ldb,
                          x.raw(), &ldx,
                          &rc,
                          ferr.raw(),
                          berr.raw(),
                          &info_,
                          w_);
      b = x;
    }

  private:
    data_t                            a_;
    tab_t                            lu_;
    nt2_la_int                     m_,n_;
    nt2_la_int                     ldlu_;
    ibuf_t                         ipiv_;
    nt2_la_int                     info_;
    workspace_t                       w_;
    tab_t                             p_;
    itab_t                           ip_;
    tab_t                            pl_;
    tab_t                          invt_;
  };
} }

#endif


#include "energies.h"

#include "bond.h"

#include <Eigen/Dense>
#include <memory>
#include <vector>
#include <algorithm>
#include <numeric>

double calculate_kinetic_energy(const Eigen::MatrixXd& velocities, const Eigen::MatrixXd& masses) {
    return (masses.transpose()  * velocities.rowwise().squaredNorm()).sum() / 2.0;
}

double calculate_potential_energy(const std::vector<std::unique_ptr<Bond>>& bonds, const Eigen::MatrixXd& positions) {
    std::vector<double> potential_energies;
    potential_energies.reserve(bonds.size());
    std::transform(bonds.begin(), bonds.end(), std::back_inserter(potential_energies),
    [positions](const auto& bond){ return bond->energy(positions);});

    return std::accumulate(potential_energies.begin(),
                                                  potential_energies.end(),
                                                    0.0
                                                   );
}


#include "setup_skinning_handles.h"
#include "kmeans_clustering.h"
#include <Eigen/Sparse>
#include <Eigen/Dense>
#include <iostream>
#include <igl/boundary_conditions.h>
#include <igl/lbs_matrix.h>
#include <igl/bbw.h>
#include <unsupported/Eigen/KroneckerProduct>
#include <igl/slice_into.h>
#include <igl/remove_unreferenced.h>
#include <igl/normalize_row_sums.h>
#include <igl/writeOBJ.h>

using namespace Eigen;
using namespace std;
MatrixXd bbw_strain_skinning_matrix(VectorXi& handles, const MatrixXd& mV, MatrixXi& mT){
    std::set<int> unique_vertex_handles;
    std::set<int>::iterator it;
    //from the tet handle indexes, get the unique verts that can act as handles
    for(int i=0; i<handles.size(); i++){
        unique_vertex_handles.insert(mT(handles[i], 0));
        unique_vertex_handles.insert(mT(handles[i], 1));
        unique_vertex_handles.insert(mT(handles[i], 2));
        unique_vertex_handles.insert(mT(handles[i], 3));
    }

    int i=0;
    it = unique_vertex_handles.end();
    VectorXi map_verts_to_unique_verts = VectorXi::Zero(*(--it)+1).array() -1;
    for (it=unique_vertex_handles.begin(); it!=unique_vertex_handles.end(); ++it){
        map_verts_to_unique_verts[*it] = i;
        i++;
    }

    MatrixXi vert_to_tet = MatrixXi::Zero(handles.size(), 4);
    i=0;
    for(i=0; i<handles.size(); i++){
        vert_to_tet.row(i)[0] = map_verts_to_unique_verts[mT.row(handles[i])[0]];
        vert_to_tet.row(i)[1] = map_verts_to_unique_verts[mT.row(handles[i])[1]];
        vert_to_tet.row(i)[2] = map_verts_to_unique_verts[mT.row(handles[i])[2]];
        vert_to_tet.row(i)[3] = map_verts_to_unique_verts[mT.row(handles[i])[3]];
    }

    MatrixXd C = MatrixXd::Zero(unique_vertex_handles.size(), 3);
    VectorXi P = VectorXi::Zero(unique_vertex_handles.size());
    i=0;
    for (it=unique_vertex_handles.begin(); it!=unique_vertex_handles.end(); ++it){
        C.row(i) = mV.row(*it);
        P(i) = i;
        i++;
    }

    // List of boundary indices (aka fixed value indices into VV)
    VectorXi b;
    // List of boundary conditions of each weight function
    MatrixXd bc;
    cout<<"---------0--------"<<endl;
    igl::boundary_conditions(mV, mT, C, P, MatrixXi(), MatrixXi(), b, bc);
    // compute BBW weights matrix
    igl::BBWData bbw_data;
    // only a few iterations for sake of demo
    bbw_data.active_set_params.max_iter = 100;
    bbw_data.verbosity = 2;

    MatrixXd W, M;
    cout<<"---------1--------"<<endl;
    if(!igl::bbw(mV, mT, b, bc, bbw_data, W))
    {
        std::cout<<"EXIT: Error here"<<std::endl;
        exit(0);
        return MatrixXd();
    }
    cout<<"---------2--------"<<endl;

    // Normalize weights to sum to one
    igl::normalize_row_sums(W,W);
    cout<<"---------3--------"<<endl;

    // precompute linear blend skinning matrix
    igl::lbs_matrix(mV,W,M);
    cout<<"---------4--------"<<endl;

    MatrixXd tW = MatrixXd::Zero(mT.rows(), handles.size());
    for(int t =0; t<mT.rows(); t++){
        VectorXi e = mT.row(t);
        for(int h=0; h<handles.size(); h++){
            if(t==handles[h]){
                tW.row(t) *= 0;
                tW(t,h) = 1;
                break;
            }
            double p0 = 0;
            double p1 = 0;
            double p2 = 0;
            double p3 = 0;
            for(int j=0; j<vert_to_tet.cols(); ++j){
                p0 += W(e[0], vert_to_tet(h, j));
                p1 += W(e[1], vert_to_tet(h, j));
                p2 += W(e[2], vert_to_tet(h, j));
                p3 += W(e[3], vert_to_tet(h, j));
            }
            tW(t, h) = (p0+p1+p2+p3)/4;
        }
    }
    cout<<"---------5--------"<<endl;
    igl::normalize_row_sums(tW, tW);

    cout<<"---------6--------"<<endl;
    return tW;
}

Vector3d tet_center(MatrixXi& T, MatrixXd& V, int ind){
    Vector3d v1 = V.row(T.row(ind)[0]);
    Vector3d v2 = V.row(T.row(ind)[1]);
    Vector3d v3 = V.row(T.row(ind)[2]);
    Vector3d v4 = V.row(T.row(ind)[3]);

    return (v1 + v2 + v3 + v4)/4;
}

MatrixXd setup_skinning_helper(int indx,
    int nsh_on_component,
    MatrixXi& mT,
    MatrixXd& mV,
    SparseMatrix<double>& mC,
    SparseMatrix<double>& mA,
    VectorXd& mx0,
    std::map<int, std::vector<int>>& ms_handle_elem_map){

    VectorXi handles_ind = VectorXi::Zero(nsh_on_component);
    VectorXd CAx0 = mC*mA*mx0;

    for(int k=0; k<nsh_on_component; k++){
        std::vector<int> els = ms_handle_elem_map[indx-k];
        double centx = 0;//= VectorXd::Zero(els.size());
        double centy = 0;//= VectorXd::Zero(els.size());
        double centz = 0;//= VectorXd::Zero(els.size());
        Vector3d avg_cent;
        for(int i=0; i<els.size(); i++){
            centx += CAx0[12*els[i]];
            centy += CAx0[12*els[i]+1];
            centz += CAx0[12*els[i]+2];
        }

        avg_cent<<centx/els.size(), centy/els.size(), centz/els.size();
        int minind = 0;
        double mindist = (avg_cent - tet_center(mT, mV, 0)).norm();
        for(int i=1; i<mT.rows(); i++){
            double dist = (avg_cent - tet_center(mT, mV, i)).norm();
            if(dist<mindist){
                mindist = dist;
                minind = i;
            }
        }

        handles_ind[k] = minind;
    }

    for(int i =0; i<handles_ind.size(); i++){
        std::cout<<tet_center(mT, mV, handles_ind[i])<<std::endl;
    }

    return bbw_strain_skinning_matrix(handles_ind, mV, mT);

}

void setup_skinning_handles(int nsh, bool reduced, const MatrixXi& mT, const MatrixXd& mV, std::vector<VectorXi>& ibones, std::vector<VectorXi>& imuscle,
	SparseMatrix<double>& mC, SparseMatrix<double>& mA, MatrixXd& mG, VectorXd& mx0, VectorXd& mred_s, MatrixXd& msW, std::map<int, std::vector<int>>& ms_handle_elem_map){

    std::cout<<"+ Skinning Handles"<<std::endl;
    VectorXi skinning_elem_cluster_map;
    std::map<int, std::vector<int>> skinning_cluster_elem_map;

    if(nsh<(ibones.size()+imuscle.size())){
        std::cout<<"Too few skinning handles, too many components"<<std::endl;
        exit(0);
    }

    if(nsh==0){
        nsh = mT.rows();
    }

    //-----------------------------------------------------
    if(nsh==mT.rows() && reduced==false){
    //unreduced
        for(int i=0; i<mT.rows(); i++){
            skinning_elem_cluster_map[i] = i;
        }
    }else{
        kmeans_clustering(skinning_elem_cluster_map, nsh, ibones, imuscle, mG, mC, mA, mx0);
    }

    for(int i=0; i<mT.rows(); i++){
        ms_handle_elem_map[skinning_elem_cluster_map[i]].push_back(i);
    }
    //------------------------------------------------------

    if(reduced==false){
        mred_s.resize(6*mT.rows());
        for(int i=0; i<mT.rows(); i++){
            mred_s[6*i+0] = 1;
            mred_s[6*i+1] = 1;
            mred_s[6*i+2] = 1;
            mred_s[6*i+3] = 0;
            mred_s[6*i+4] = 0;
            mred_s[6*i+5] = 0;
        }
        return;

    }
    if(nsh==mT.rows()){
        std::cout<<"Too many skinning handles. "<<std::endl;
        exit(0);
    }

    mred_s.resize(6*nsh);
    for(int i=0; i<nsh; i++){
        mred_s[6*i+0] = 1;
        mred_s[6*i+1] = 1;
        mred_s[6*i+2] = 1;
        mred_s[6*i+3] = 0;
        mred_s[6*i+4] = 0;
        mred_s[6*i+5] = 0;
    }

    //blocked construction of full skinning weights matrix
    MatrixXd sW = MatrixXd::Zero(mT.rows(), nsh);
    sW.setZero();
    int maxnsh = nsh;
    cout<<"----------Bone HANDLES------------"<<maxnsh<<"--"<<ms_handle_elem_map.size()<<endl;
    int insert_index = 0;
    for(int b=0; b<ibones.size(); b++){
        MatrixXi subT(ms_handle_elem_map[maxnsh - 1 - insert_index].size(), 4);
        MatrixXi componentT;
        MatrixXd componentV;
        VectorXi J;

        for(int i=0; i<ms_handle_elem_map[maxnsh -1 - insert_index].size() ; i++){
            subT.row(i) = mT.row(ms_handle_elem_map[maxnsh -1 - insert_index][i]);
        }

        igl::remove_unreferenced(mV, subT, componentV, componentT, J);

        std::string nam = "bone"+to_string(b)+".obj";

        igl::writeOBJ(nam, componentV, componentT);

        MatrixXd sWi = setup_skinning_helper(maxnsh-1- insert_index , 1, componentT, componentV, mC, mA, mx0, ms_handle_elem_map);

        MatrixXd sWslice = MatrixXd::Zero(mT.rows(), sWi.cols());
        igl::slice_into(sWi , ibones[b], 1, sWslice);
        sW.block(0,insert_index, mT.rows(), sWi.cols()) = sWslice;
        nsh = nsh - 1; //bone skinning handle has been made, so decrease nsh by 1
        insert_index += 1;
    }

    cout<<"----------MUSCLE HANDLES----"<<nsh<<"---"<<insert_index<<"-----"<<endl;
    int number_handles_per_muscle = nsh/imuscle.size();
    for(int m=0; m<imuscle.size(); m++){ //through muscle vector
        MatrixXi componentT;
        MatrixXd componentV;
        MatrixXi subT(imuscle[m].size(), 4);
        VectorXi J;
        for(int i=0; i<imuscle[m].size() ; i++){
            subT.row(i) = mT.row(imuscle[m][i]);
        }
        igl::remove_unreferenced(mV, subT, componentV, componentT, J);
        std::string nam = "muscle"+to_string(m)+".obj";
        igl::writeOBJ(nam, componentV, componentT);
        MatrixXd sWi;
        if(m==imuscle.size()-1){
            //Deal with remainder handles
            sWi = setup_skinning_helper(maxnsh - 1 - insert_index, nsh, componentT, componentV, mC, mA, mx0, ms_handle_elem_map);
        }else{
            sWi = setup_skinning_helper(maxnsh - 1 - insert_index, number_handles_per_muscle, componentT, componentV, mC, mA, mx0, ms_handle_elem_map );
        }

        MatrixXd sWslice = MatrixXd::Zero(mT.rows(), sWi.cols());

        igl::slice_into(sWi , imuscle[m], 1, sWslice);
        sW.block(0,insert_index, mT.rows(), sWi.cols()) = sWslice;
        insert_index += number_handles_per_muscle;
        nsh =  nsh - number_handles_per_muscle;
    }
    assert(nsh==0);

    MatrixXd Id6 = MatrixXd::Identity(6, 6);
    msW =  Eigen::kroneckerProduct(sW, Id6);

    std::cout<<"- Skinning Handles"<<std::endl;
}


// -------------------------------------------------------------------
// $ProjectName     : DiagramDistanceRunner_D64
// $FileName        : DiagramDistanceRunner_D64.cpp$
// $Programmer      : Tran Quoc Hoan$
// -------------------------------------------------------------------

#include "stdafx.h"
#include "fstream"
#include <ostream>
#include <filesystem>

#include "DiagramDistanceRunner_D64.h"
#include "../KernelStatsUtils_D64/KernelFisherDA.h"
#include "../KernelStatsUtils_D64/MultiScaleKernel.h"
#include "../KernelStatsUtils_D64/RiemannianManifoldKernel.h"
#include "../PersistenceUtils_D64/PersistenceBarcodes.h"
#include "../PersistenceUtils_D64/PersistenceDeclarations.h"
#include "../PersistenceUtils_D64/PersistenceUtils_D64.h"

#include "../TopoUtils_D64/FileUtils.h"
#include "../TopoUtils_D64/StringUtils.h"

#include <boost/program_options.hpp>
#include <boost/iostreams/device/file.hpp>
#include <boost/iostreams/stream.hpp>
#include <Eigen/Dense>

using namespace boost::program_options;
using namespace NPersistenceUtils;
using namespace NKernelStatsUtils;
using namespace Eigen;
namespace fs = std::experimental::filesystem;

namespace NDiagramDistanceRunner {
    using namespace NMultiScaleKernelUtils;
    typedef Eigen::Matrix<FType, Dynamic, Dynamic> MaxtrixFType;

    enum KernelType {
        L2_INNER_MULTI_SCALE_SSE,
        L2_SQUARE_DIST,
        SLICE_WASS,
        L2_INNER_MULTI_SCALE_NOSSE,
        RIEMANNIAN_METRIC,
    };

    template <class Dtype>
    struct KernelPrm {
        KernelType kertype = KernelType::L2_INNER_MULTI_SCALE_SSE;
        Dtype T1 = (Dtype)1.0;
        Dtype T2 = (Dtype)1.0; // T_2 = time_hole * time_rate
        size_t theta_ndirs = 1;
        size_t phi_ndirs = 1;
        Dtype gamma = (Dtype)1.0;
        std::wstring postfix = L"";
    };

    bool KernelProduct(FType &result, CPersistenceBarcodesPtr<FType> &bar1, CPersistenceBarcodesPtr<FType> &bar2, KernelPrm<FType>& prm) {
        auto T1 = prm.T1;
        auto T2 = prm.T2;

        switch (prm.kertype)
        {
        case KernelType::L2_INNER_MULTI_SCALE_SSE:
            L2DiagramMskInnerProductSSE(result, bar1->barcodes(), bar2->barcodes(), T1, T2);
            break;
        case KernelType::L2_SQUARE_DIST:
            L2DiagramMskDistanceSquare(result, bar1->barcodes(), bar2->barcodes(), T1);
            break;
        case KernelType::L2_INNER_MULTI_SCALE_NOSSE:
            L2DiagramMskInnerProduct(result, bar1->barcodes(), bar2->barcodes(), T1, T2);
            break;
        case KernelType::RIEMANNIAN_METRIC:
            NRiemannianManifoldKernelUtils::RiemannGeodesicMetric(result, bar1->barcodes(), bar2->barcodes(), T1, T2);
            break;
        default:
            break;
        }
        return true;
    }

    MaxtrixFType KernelVecsProduct(TypeBarcodesPtrVec<FType> diagram_vecs, KernelPrm<FType>& prm) {
        std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now();

        size_t num_diagrams = diagram_vecs.size();
        MaxtrixFType gram_mat(num_diagrams, num_diagrams);

        for (size_t i = 0; i < num_diagrams; ++i) {
            auto total = std::chrono::duration_cast<std::chrono::seconds>(std::chrono::steady_clock::now() - begin).count();
            auto hours = total / 3600;
            auto mins = (total - hours * 3600) / 60;
            auto secs = total - hours * 3600 - mins * 60;
            std::cout << "Ellapsed time (h:m:s)= " << hours << ":" << mins << ":" << secs <<
                ", Index of diagram: " << i << " per total of " << num_diagrams << std::endl;

            // multi-threads
            concurrency::parallel_for(i, num_diagrams, [&](size_t j) {
                FType result(0);
                KernelProduct(result, diagram_vecs[i], diagram_vecs[j], prm);
                gram_mat(i, j) = result;
                //std::cout << i << " " << j << " " << diagram_vecs[i]->numbars()
                //    << " " << diagram_vecs[j]->numbars() << std::endl;
                return true;
            });
        }
        // gram matrix should be symmetric
        for (int i = 0; i < num_diagrams; ++i) {
            for (int j = 0; j < i; ++j) {
                gram_mat(i, j) = gram_mat(j, i);
            }
        }
        return gram_mat;
    }

    MaxtrixFType KernelVecsProduct(TypeBarcodesPtrVec<FType> dvecs1, TypeBarcodesPtrVec<FType> dvecs2, KernelPrm<FType>& prm) {
        std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now();
        size_t nums1 = dvecs1.size();
        size_t nums2 = dvecs2.size();
        if (nums1 == 0) return KernelVecsProduct(dvecs2, prm);
        if (nums2 == 0) return KernelVecsProduct(dvecs1, prm);

        MaxtrixFType gram_mat(nums1, nums2);

        for (size_t i = 0; i < nums1; ++i) {
            auto total = std::chrono::duration_cast<std::chrono::seconds>(std::chrono::steady_clock::now() - begin).count();
            auto hours = total / 3600;
            auto mins = (total - hours * 3600) / 60;
            auto secs = total - hours * 3600 - mins * 60;
            std::cout << "Ellapsed time (h:m:s)= " << hours << ":" << mins << ":" << secs <<
                ", Index of diagram: " << i << " per total of " << nums1 << std::endl;

            // multi-threads
            concurrency::parallel_for((size_t)0, nums2, [&](size_t j) {
                FType result(0);
                KernelProduct(result, dvecs1[i], dvecs2[j], prm);
                gram_mat(i, j) = result;
                return true;
            });
        }
        return gram_mat;
    }

    // Get the median of an unordered set of numbers of arbitrary
    // type without modifying the underlying dataset.
    template <typename It>
    auto GetMedian(It begin, It end)
    {
        using T = typename std::iterator_traits<It>::value_type;
        std::vector<T> data(begin, end);
        std::nth_element(data.begin(), data.begin() + data.size() / 2, data.end());
        return data[data.size() / 2];
    }

    bool FindOptimalTimeHole(TypeBarcodesPtrVec<FType> dvecs, FType &tHole) {
        if(dvecs.empty())
            return false;
        auto N = dvecs.size();
        std::vector<FType> time_hole(N, (FType)0.0);
        concurrency::parallel_for((size_t)0, N, [&](size_t z) {
            time_hole[z] = dvecs[z]->GetOptimalTimeHole();
        });

        tHole = GetMedian(time_hole.begin(), time_hole.end());
        return true;
    }
    bool FindOptimalTimeHole(TypeBarcodesPtrVec<FType> dvecs1, TypeBarcodesPtrVec<FType> dvecs2, FType &tHole) {
        TypeBarcodesPtrVec<FType> dvecs = dvecs1;
        dvecs.insert(dvecs.end(), dvecs2.begin(), dvecs2.end());
        FindOptimalTimeHole(dvecs, tHole);
        return true;
    }

    bool MakeGramMatAndSaveResultToFile(TypeBarcodesPtrVec<FType> dvecs1, TypeBarcodesPtrVec<FType> dvecs2,
        std::wstring& output_path, KernelPrm<FType>& prm) {
        std::cout << "Computing gram matrix and save to file: " << NStringUtil::_w2s(output_path) << std::endl;
        fs::path opath(output_path);
        if (prm.kertype == KernelType::L2_INNER_MULTI_SCALE_NOSSE ||
            prm.kertype == KernelType::L2_INNER_MULTI_SCALE_SSE ||
            prm.kertype == KernelType::L2_SQUARE_DIST ||
            prm.kertype == KernelType::RIEMANNIAN_METRIC) {
            if (prm.T1 == 0.0) FindOptimalTimeHole(dvecs1, dvecs2, prm.T1);
            std::cout << "Optimal T1 = " << prm.T1 << ", T2 = " << prm.T2 << std::endl;
        }

        MaxtrixFType gram_mat = KernelVecsProduct(dvecs1, dvecs2, prm);

        const size_t rows = gram_mat.rows();
        const size_t cols = gram_mat.cols();

        std::wstring outfile = output_path;
        if (fs::is_directory(opath)) {
            fs::path file(prm.postfix + L".txt");
            outfile = (opath / file).c_str();
        }

        // create folder if not exist
        fs::path p(outfile);
        fs::path dir = p.parent_path();
        auto dst_folder = dir.wstring();
        if (!fs::exists(dst_folder))
            fs::create_directory(dst_folder);

        // write result
        std::ofstream outf(outfile);
        Eigen::IOFormat fmt(FullPrecision, DontAlignCols, "\t");
        if (outf.is_open()) {
            outf << gram_mat.format(fmt) << std::endl;
        }
        outf.close();
        return true;
    }

    bool SaveKernelResultFromBarcodeListFile(std::wstring& barcodes_path, std::wstring& output_path, KernelPrm<FType>& prm,
        const SHoleParam<FType> rprms, const SKerParam<FType> kpm) {
        TypeBarcodesPtrVec<FType> dvecs1, dvecs2;
        MakeDiagramVec(dvecs1, barcodes_path, rprms, kpm);
        if(dvecs1.empty())
            return false;
        MakeGramMatAndSaveResultToFile(dvecs1, dvecs2, output_path, prm);
        return true;
    }

    bool SaveKernelResultFromBarcodeListFile(std::wstring& barcodes_lpath, std::wstring& barcodes_rpath,
        std::wstring& output_path, KernelPrm<FType>& prm,
        const SHoleParam<FType> rprms, const SKerParam<FType> kpm) {
        TypeBarcodesPtrVec<FType> dvecs1, dvecs2;
        std::cout << "Making barcodes for left path" << std::endl;
        MakeDiagramVec(dvecs1, barcodes_lpath, rprms, kpm);
        std::cout << "Making barcodes for right path" << std::endl;
        MakeDiagramVec(dvecs2, barcodes_rpath, rprms, kpm);
        if(dvecs1.empty() == true && dvecs2.empty() == true)
            return false;
        MakeGramMatAndSaveResultToFile(dvecs1, dvecs2, output_path, prm);
        return true;
    }

    bool SaveKFDRFromGrammatToFile(const std::wstring& outfile, const MaxtrixFType gram_mat, std::vector<double> gammas) {
        // create folder if not exist
        fs::path p(outfile);
        fs::path dir = p.parent_path();
        auto dst_folder = dir.wstring();
        if (!fs::exists(dst_folder))
            fs::create_directory(dst_folder);

        namespace io = boost::iostreams;
        io::stream_buffer<io::file_sink> buf(NStringUtil::_w2s(outfile));
        std::ostream out(&buf);
        for (auto &gamma : gammas) {
            auto kfdrs = ComputeKFDRs((int)gram_mat.rows(), gram_mat, gamma);
            out << gamma << ' ';
            for (auto val : kfdrs) {
                out << val << ' ';
            }
            out << std::endl;
        }
        return true;
    }

    bool SaveKFDRFromBarcodeListFile(std::wstring& barcodes_path, std::wstring& kernel_outfile, KernelPrm<FType>& prm,
        const SHoleParam<FType>& rprms, const SKerParam<FType>& kpm, std::vector<double> gammas) {
        TypeBarcodesPtrVec<FType> dvecs1, dvecs2;
        MakeDiagramVec(dvecs1, barcodes_path, rprms, kpm);
        if(dvecs1.empty())
            return false;

        if (prm.kertype == KernelType::L2_INNER_MULTI_SCALE_NOSSE ||
            prm.kertype == KernelType::L2_INNER_MULTI_SCALE_SSE ||
            prm.kertype == KernelType::L2_SQUARE_DIST ||
            prm.kertype == KernelType::RIEMANNIAN_METRIC) {
            if (prm.T1 == 0.0) FindOptimalTimeHole(dvecs1, dvecs2, prm.T1);
        }

        MaxtrixFType gram_mat = KernelVecsProduct(dvecs1, dvecs2, prm);
        // save gram_mat file
        // write result
        std::ofstream outf(kernel_outfile);
        Eigen::IOFormat fmt(FullPrecision, DontAlignCols, "\t");
        if (outf.is_open()) {
            outf << gram_mat.format(fmt) << std::endl;
        }
        outf.close();
        fs::path p(kernel_outfile);
        std::wstring kfdr_outfile = p.parent_path().c_str();
        kfdr_outfile = kfdr_outfile + L"/kfdr_" + p.filename().c_str(); // des.wstring();

        return SaveKFDRFromGrammatToFile(kfdr_outfile, gram_mat, gammas);
    }

    bool ReadKernelFileFromFileList(std::vector<std::wstring>&kernel_list, std::wstring& kernel_list_file)
    {
        std::wifstream read_op;
        read_op.open(kernel_list_file);
        if (!read_op.good()) {
            return false;
        }
        else {
            while (read_op.good()) {
                std::wstring line;
                std::getline(read_op, line);

                if (!line.empty()) {
                    kernel_list.push_back(line);
                }
            }
        }
        return true;
    }
    bool SaveKFDRFromKernelList(std::wstring& kernel_list_file, std::wstring& dst_folder, size_t nbegin, size_t nend, std::vector<double> gammas) {
        if (!fs::exists(dst_folder))
            fs::create_directory(dst_folder);

        std::vector<std::wstring> kernel_list;
        ReadKernelFileFromFileList(kernel_list, kernel_list_file);
        for (auto kf : kernel_list) {
            std::vector<FType> xs;
            std::wifstream in(kf);
            std::wstring line;
            if (in.is_open()) {
                while (std::getline(in, line)) {
                    std::wstringstream wstream(line);
                    while (!wstream.eof()) {
                        FType tmp = (FType)0.0;
                        wstream >> tmp;
                        xs.push_back(tmp);
                    }
                }
            }
            in.close();
            size_t rows = (size_t)(std::sqrt(xs.size()));
            size_t cols = (size_t)(std::sqrt(xs.size()));
            if (rows*cols != xs.size())
                continue;
            size_t rs = rows;
            size_t cs = cols;
            if (nbegin < nend) {
                rs = std::min(rows, nend - nbegin + 1);
                cs = rs;
            }
            MaxtrixFType grammat = MaxtrixFType(rs, cs);
            for (size_t i = 0; i < rows; ++i) {
                if (i < nbegin) continue;
                if (i >= nbegin + rs) continue;
                for (size_t j = 0; j < cols; ++j) {
                    if (j < nbegin) continue;
                    if (j >= nbegin + rs) continue;
                    grammat(i - nbegin, j - nbegin) = xs[i*cols + j];
                }
            }
            fs::path p(kf);
            std::wstring outfile = dst_folder + L"/kfdr_" + p.filename().c_str();
            if (nbegin < nend)
                outfile = dst_folder + L"/kfdr_bg_" + std::to_wstring(nbegin) + L"_ed_"+ std::to_wstring(nend) + L"_" + p.filename().c_str(); // des.wstring();
            return SaveKFDRFromGrammatToFile(outfile, grammat, gammas);
        }
        return true;
    }
}

int main(int argc, char** argv)
{
    using namespace NDiagramDistanceRunner;
    int nRetCode = 0;

    options_description description("DiagramDistance");
    description.add_options()
        ("T1", value<double>()->default_value(0.0), "Parameter T1 in the kernel (=0 for optimal value)")
        ("T2", value<double>()->default_value(1.0),  "Parameter T2 in the kernel")
        ("left,l", value<std::string>()->default_value(""), "(left) Input as List of barcodes")
        ("right,r", value<std::string>()->default_value(""), "(right) Input as List of barcodes")
        ("dim,d", value<unsigned>()->default_value(0), "Dimension of holes to compute kernel")
        ("infval", value<double>()->default_value(0.0),
            "If infval<=0.0 skip holes with infinity death-scale, otherwise replace these death-scales with a default value")
        ("thres", value<double>()->default_value(0.0), "Threshold to skip holes with death-birth < thres (default=0 to use all holes)")
        ("output,o", value<std::string>()->default_value("grammat.txt"), "Output file of gram matrix")
        ("posfix", value<std::string>()->default_value(""), "postfix for output file")
        ("method", value<int>()->default_value(0),
            "0: L2_inner_multi_sse, 1: L2_squared_distance, 2: Slice Wasserstein, 3:L2_inner_multi_nosse, 4:riemmannian_metric")
        ("single_tau", value<double>()->default_value(-1.0), "Specify for single tau, < 0: take all tau")
        ("max_tau", value<double>()->default_value(0.0),
            "Maximum of scale when calculating kernel, max_tau = 0 means that taking all possible tau")
		("interval", value<double>()->default_value(1.0),
				"Interval to skip tau when calculating kernel with all taus")
        ("kfdr", value<bool>()->default_value(false), "option to calculate kfdr, 1: kfdr, 0:kernel")
        ("kfdrout", value<std::string>()->default_value("kfdr"), "Output folder for kfdr")
        ("nbegin", value<unsigned>()->default_value(0))
        ("nend", value<unsigned>()->default_value(0))
        ("kerls", value<std::string>()->default_value(""), "Input as list of kernel for kfdr")
        ("help,H", "Help: Diagram Distance to compute kernel of diagrams")
        ("version,v", "v1.0")
        ;
    variables_map vm;
    store(parse_command_line(argc, argv, description), vm);
    notify(vm);
    if (vm.count("help")) {
        std::cout << description << std::endl;
        return nRetCode;
    }

    auto T1 = static_cast<FType>(vm["T1"].as<double>());
    auto T2 = static_cast<FType>(vm["T2"].as<double>());

    auto nbegin = static_cast<size_t>(vm["nbegin"].as<unsigned>());
    auto nend = static_cast<size_t>(vm["nend"].as<unsigned>());
    auto dim = vm["dim"].as<unsigned>();
    auto single_tau = vm["single_tau"].as<double>();
    auto max_tau = static_cast<FType>(vm["max_tau"].as<double>());
	auto interval = static_cast<FType>(vm["interval"].as<double>());
	if (interval <= 0.0) interval = 1.0;

    bool skip = true;
    auto infval = static_cast<FType>(vm["infval"].as<double>());
    if (infval > 0.0) skip = false;

    auto threshold = static_cast<FType>(vm["thres"].as<double>());

    auto kernel_list_path = NStringUtil::_s2w(vm["kerls"].as<std::string>());
    auto left_path = NStringUtil::_s2w(vm["left"].as<std::string>());
    auto right_path = NStringUtil::_s2w(vm["right"].as<std::string>());

    auto output_kfdr = NStringUtil::_s2w(vm["kfdrout"].as<std::string>());
    auto output_path = NStringUtil::_s2w(vm["output"].as<std::string>());
    auto posfix = NStringUtil::_s2w(vm["posfix"].as<std::string>());

    auto method = vm["method"].as<int>();

    KernelType kertype;
    switch (method) {
    case 0:
        kertype = KernelType::L2_INNER_MULTI_SCALE_SSE;
        break;
    case 1:
        kertype = KernelType::L2_SQUARE_DIST;
        break;
    case 2:
        kertype = KernelType::SLICE_WASS;
        break;
    case 3:
        kertype = KernelType::L2_INNER_MULTI_SCALE_NOSSE;
        break;
    case 4:
        kertype = KernelType::RIEMANNIAN_METRIC;
        break;
    default:
        std::cout << "Kernel method is not defined!" << std::endl;
        std::cout << description;
        return nRetCode;
    }

    KernelPrm<FType> prm;
    prm.kertype = kertype;
    prm.T1 = T1;
    prm.T2 = T2 * interval * interval;
    prm.postfix = posfix + L"_T1_" + std::to_wstring(T1) +
        L"_T2_" + std::to_wstring(T2);

    const SHoleParam<FType> rprms(dim, infval, skip, threshold);
    const SKerParam<FType> kpm(single_tau, max_tau, interval);

    auto kfdr = vm["kfdr"].as<bool>();
    if (kfdr == TRUE) {
        std::vector<double> gammas = {
            1e-6, 2e-6, 3e-6, 4e-6, 5e-6, 6e-6, 7e-6, 8e-6, 9e-6,
            1e-5, 2e-5, 3e-5, 4e-5, 5e-5, 6e-5, 7e-5, 8e-5, 9e-5,
            1e-4, 2e-4, 3e-4, 4e-4, 5e-4, 6e-4, 7e-4, 8e-4, 9e-4,
            1e-3, 2e-3, 3e-3, 4e-3, 5e-3, 6e-3, 7e-3, 8e-3, 9e-3,
            1e-2, 2e-2, 3e-2, 4e-2, 5e-2, 6e-2, 7e-2, 8e-2, 9e-2,
            1e-1, 2e-1, 3e-1, 4e-1, 5e-1, 6e-1, 7e-1, 8e-1, 9e-1,
            1e+0, 2e+0, 3e+0, 4e+0, 5e+0, 6e+0, 7e+0, 8e+0, 9e+0,
            1e+1, 2e+1, 3e+1, 4e+1, 5e+1, 6e+1, 7e+1, 8e+1, 9e+1,
            1e+2, 2e+2, 3e+2, 4e+2, 5e+2, 6e+2, 7e+2, 8e+2, 9e+2,
            1e+3, 2e+3, 3e+3, 4e+3, 5e+3, 6e+3, 7e+3, 8e+3, 9e+3,
            1e+4, 2e+4, 3e+4, 4e+4, 5e+4, 6e+4, 7e+4, 8e+4, 9e+4,
            1e+5, 2e+5, 3e+5, 4e+5, 5e+5, 6e+5, 7e+5, 8e+5, 9e+5
        };
        if (kernel_list_path == L"") {
            SaveKFDRFromBarcodeListFile(left_path, output_path, prm, rprms, kpm, gammas);
        }
        else {
            SaveKFDRFromKernelList(kernel_list_path, output_kfdr, nbegin, nend, gammas);
        }
    }
    else {
        if (left_path != L"" && !fs::exists(left_path)) {
            std::cout << "Not found left path for calculating kernel: " << NStringUtil::_w2s(left_path) << std::endl;
            return nRetCode;
        }
        if (right_path != L"" && !fs::exists(right_path)) {
            std::cout << "Not found right path for calculating kernel: " << NStringUtil::_w2s(right_path) << std::endl;
            return nRetCode;
        }
        SaveKernelResultFromBarcodeListFile(left_path, right_path, output_path, prm, rprms, kpm);
    }
    return nRetCode;
}


#include "geometry.h"

#include <limits>
#include <math.h>
#include <vector>
#include <map>
#include <iostream>
#include <vector_functions.h>
#include <util/helper_math.h>
#include <Eigen/Eigen>

namespace dart {

template <typename Real>
Real distancePointEllipseSpecial (const Real e[2], const Real y[2], Real x[2])
{
    Real distance;
    if (y[1] > (Real)0)
    {
        if (y[0] > (Real)0)
        {
            // Bisect to compute the root of F(t) for t >= -e1*e1.
            Real esqr[2] = { e[0]*e[0], e[1]*e[1] };
            Real ey[2] = { e[0]*y[0], e[1]*y[1] };
            Real t0 = -esqr[1] + ey[1];
            Real t1 = -esqr[1] + sqrt(ey[0]*ey[0] + ey[1]*ey[1]);
            Real t = t0;
            const int imax = 2*std::numeric_limits<Real>::max_exponent;
            for (int i = 0; i < imax; ++i)
            {
                t = ((Real)0.5)*(t0 + t1);
                if (t == t0 || t == t1)
                {
                    break;
                }
                Real r[2] = { ey[0]/(t + esqr[0]), ey[1]/(t + esqr[1]) };
                Real f = r[0]*r[0] + r[1]*r[1] - (Real)1;
                if (f > (Real)0)
                {
                    t0 = t;
                }
                else if (f < (Real)0)
                {
                    t1 = t;
                }
                else
                {
                    break;
                }
            }
            x[0] = esqr[0]*y[0]/(t + esqr[0]);
            x[1] = esqr[1]*y[1]/(t + esqr[1]);
            Real d[2] = { x[0] - y[0], x[1] - y[1] };
            distance = sqrt(d[0]*d[0] + d[1]*d[1]);
        }
        else // y0 == 0
        {
            x[0] = (Real)0;
            x[1] = e[1];
            distance = fabs(y[1] - e[1]);
        }
    }
    else // y1 == 0
    {
        Real denom0 = e[0]*e[0] - e[1]*e[1];
        Real e0y0 = e[0]*y[0];
        if (e0y0 < denom0)
        {
            // y0 is inside the subinterval.
            Real x0de0 = e0y0/denom0;
            Real x0de0sqr = x0de0*x0de0;
            x[0] = e[0]*x0de0;
            x[1] = e[1]*sqrt(fabs((Real)1 - x0de0sqr));
            Real d0 = x[0] - y[0];
            distance = sqrt(d0*d0 + x[1]*x[1]);
        }
        else
        {
            // y0 is outside the subinterval. The closest ellipse point has
            // x1 == 0 and is on the domain-boundary interval (x0/e0)^2 = 1.
            x[0] = e[0];
            x[1] = (Real)0;
            distance = fabs(y[0] - e[0]);
        }
    }
    return distance;
}

template <typename Real>
Real distancePointEllipse (const Real e[2], const Real y[2], Real x[2])
{
    // Determine reflections for y to the first quadrant.
    bool reflect[2];
    int i, j;
    for (i = 0; i < 2; ++i)
    {
        reflect[i] = (y[i] < (Real)0);
    }
    // Determine the axis order for decreasing extents.
    int permute[2];
    if (e[0] < e[1])
    {
        permute[0] = 1; permute[1] = 0;
    }
    else
    {
        permute[0] = 0; permute[1] = 1;
    }
    int invpermute[2];
    for (i = 0; i < 2; ++i)
    {
        invpermute[permute[i]] = i;
    }
    Real locE[2], locY[2];
    for (i = 0; i < 2; ++i)
    {
        j = permute[i];
        locE[i] = e[j];
        locY[i] = y[j];
        if (reflect[j])
        {
            locY[i] = -locY[i];
        }
    }
    Real locX[2];
    Real distance = distancePointEllipseSpecial(locE, locY, locX);
    // Restore the axis order and reflections.
    for (i = 0; i < 2; ++i)
    {
        j = invpermute[i];
        if (reflect[j])
        {
            locX[j] = -locX[j];
        }
        x[i] = locX[j];
    }
    return distance;
}

template <typename Real>
Real distancePointEllipsoidSpecial(const Real e[3], const Real y[3], Real x[3])
{
    Real distance;
    if (y[2] > (Real)0)
    {
        if (y[1] > (Real)0)
        {
            if (y[0] > (Real)0)
            {
                // Bisect to compute the root of F(t) for t >= -e2*e2.
                Real esqr[3] = { e[0]*e[0], e[1]*e[1], e[2]*e[2] };
                Real ey[3] = { e[0]*y[0], e[1]*y[1], e[2]*y[2] };
                Real t0 = -esqr[2] + ey[2];
                Real t1 = -esqr[2] + sqrt(ey[0]*ey[0] + ey[1]*ey[1] +
                                          ey[2]*ey[2]);
                Real t = t0;
                const int imax = 2*std::numeric_limits<Real>::max_exponent;
                for (int i = 0; i < imax; ++i)
                {
                    t = ((Real)0.5)*(t0 + t1);
                    if (t == t0 || t == t1)
                    {
                        break;
                    }
                    Real r[3] = { ey[0]/(t + esqr[0]), ey[1]/(t + esqr[1]),
                                  ey[2]/(t + esqr[2]) };
                    Real f = r[0]*r[0] + r[1]*r[1] + r[2]*r[2] - (Real)1;
                    if (f > (Real)0)
                    {
                        t0 = t;
                    }
                    else if (f < (Real)0)
                    {
                        t1 = t;
                    }
                    else
                    {
                        break;
                    }
                }
                x[0] = esqr[0]*y[0]/(t + esqr[0]);
                x[1] = esqr[1]*y[1]/(t + esqr[1]);
                x[2] = esqr[2]*y[2]/(t + esqr[2]);
                Real d[3] = { x[0] - y[0], x[1] - y[1], x[2] - y[2] };
                distance = sqrt(d[0]*d[0] + d[1]*d[1] + d[2]*d[2]);
            }
            else // y0 == 0
            {
                x[0] = (Real)0;
                Real etmp[2] = { e[1], e[2] };
                Real ytmp[2] = { y[1], y[2] };
                Real xtmp[2];
                distance = distancePointEllipseSpecial<Real>(etmp, ytmp, xtmp);
                x[1] = xtmp[0];
                x[2] = xtmp[1];
            }
        }
        else // y1 == 0
        {
            x[1] = (Real)0;
            if (y[0] > (Real)0)
            {
                Real etmp[2] = { e[0], e[2] };
                Real ytmp[2] = { y[0], y[2] };
                Real xtmp[2];
                distance = distancePointEllipseSpecial<Real>(etmp, ytmp, xtmp);
                x[0] = xtmp[0];
                x[2] = xtmp[1];
            }
            else // y0 == 0
            {
                x[0] = (Real)0;
                x[2] = e[2];
                distance = fabs(y[2] - e[2]);
            }
        }
    }
    else // y2 == 0
    {
        Real denom[2] = { e[0]*e[0] - e[2]*e[2], e[1]*e[1] - e[2]*e[2] };
        Real ey[2] = { e[0]*y[0], e[1]*y[1] };
        if (ey[0] < denom[0] && ey[1] < denom[1])
        {
            // (y0,y1) is inside the axis-aligned bounding rectangle of the
            // subellipse. This intermediate test is designed to guard
            // against the division by zero when e0 == e2 or e1 == e2.
            Real xde[2] = { ey[0]/denom[0], ey[1]/denom[1] };
            Real xdesqr[2] = { xde[0]*xde[0], xde[1]*xde[1] };
            Real discr = (Real)1 - xdesqr[0] - xdesqr[1];
            if (discr > (Real)0)
            {
                // (y0,y1) is inside the subellipse. The closest ellipsoid
                // point has x2 > 0.
                x[0] = e[0]*xde[0];
                x[1] = e[1]*xde[1];
                x[2] = e[2]*sqrt(discr);
                Real d[2] = { x[0] - y[0], x[1] - y[1] };
                distance = sqrt(d[0]*d[0] + d[1]*d[1] + x[2]*x[2]);
            }
            else
            {
                // (y0,y1) is outside the subellipse. The closest ellipsoid
                // point has x2 == 0 and is on the domain-boundary ellipse
                // (x0/e0)^2 + (x1/e1)^2 = 1.
                x[2] = (Real)0;
                distance = distancePointEllipseSpecial<Real>(e, y, x);
            }
        }
        else
        {
            // (y0,y1) is outside the subellipse. The closest ellipsoid
            // point has x2 == 0 and is on the domain-boundary ellipse
            // (x0/e0)^2 + (x1/e1)^2 = 1.
            x[2] = (Real)0;
            distance = distancePointEllipseSpecial<Real>(e, y, x);
        }
    }
    return distance;
}

template <typename Real>
Real distancePointEllipsoid(const Real e[3], const Real y[3], Real x[3])
{
    // Determine reflections for y to the first octant.
    bool reflect[3];
    int i, j;
    for (i = 0; i < 3; ++i)
    {
        reflect[i] = (y[i] < (Real)0);
    }
    // Determine the axis order for decreasing extents.
    int permute[3];
    if (e[0] < e[1])
    {
        if (e[2] < e[0])
        {
            permute[0] = 1; permute[1] = 0; permute[2] = 2;
        }
        else if (e[2] < e[1])
        {
            permute[0] = 1; permute[1] = 2; permute[2] = 0;
        }
        else
        {
            permute[0] = 2; permute[1] = 1; permute[2] = 0;
        }
    }
    else
    {
        if (e[2] < e[1])
        {
            permute[0] = 0; permute[1] = 1; permute[2] = 2;
        }
        else if (e[2] < e[0])
        {
            permute[0] = 0; permute[1] = 2; permute[2] = 1;
        }
        else
        {
            permute[0] = 2; permute[1] = 0; permute[2] = 1;
        }
    }

    int invpermute[3];
    for (i = 0; i < 3; ++i)
    {
        invpermute[permute[i]] = i;
    }
    Real locE[3], locY[3];
    for (i = 0; i < 3; ++i)
    {
        j = permute[i];
        locE[i] = e[j];
        locY[i] = y[j];
        if (reflect[j])
        {
            locY[i] = -locY[i];
        }
    }
    Real locX[3];
    Real distance = distancePointEllipsoidSpecial(locE, locY, locX);

    // Restore the axis order and reflections.
    for (i = 0; i < 3; ++i)
    {
        j = invpermute[i];
        if (reflect[j])
        {
            locX[j] = -locX[j];
        }
        x[i] = locX[j];
    }
    return distance;
}

float distancePointTriangle(const float3 P, const float3 A, const float3 B, const float3 C) {
    float3 D;
    return distancePointTriangle(P,A,B,C,D);
}

float distancePointTriangle(const float3 P, const float3 A, const float3 B, const float3 C, float3 & point) {

    float3 E0 = A-B;
    float3 E1 = C-B;

    float3 D = B-P;
    float a = dot(E0,E0);
    float b = dot(E0,E1);
    float c = dot(E1,E1);
    float d = dot(E0,D);
    float e = dot(E1,D);
    float f = dot(D,D);

    float det = a*c-b*b;
    float s = b*e - c*d;
    float t = b*d - a*e;

    int region;
    if ( s+t <= det) {
        if ( s < 0 ) {
            if ( t < 0 ) {
                region = 4;
            } else {
                region = 3;
            }
        } else if ( t < 0 ) {
            region = 5;
        } else {
            region = 0;
        }
    } else {
        if ( s < 0 ) {
            region = 2;
        } else if ( t < 0) {
            region = 6;
        } else {
            region = 1;
        }
    }

//    std::cout << region << std::endl;

    switch (region) {
        case 0:
            {
                float invDet = 1/det;
                s*= invDet;
                t*= invDet;
            }
            break;
        case 1:
            {
                float numer = c + e - b - d;
                if (numer <= 0) {
                    s = 0;
                } else {
                    float denom = a - 2*b + c;
                    s = ( numer >= denom ? 1 : numer/denom );
                }
                t = 1-s;
            }
            break;
        case 2:
            {
                float tmp0 = b+d;
                float tmp1 = c+e;
                if ( tmp1 > tmp0 ) { // min on edge s+1=1
                    float numer = tmp1 - tmp0;
                    float denom = a - 2*b + c;
                    s = ( numer >= denom ? 1 : numer/denom );
                    t = 1-s;
                } else { // min on edge s=0
                    s = 0;
                    t = ( tmp1 <= 0 ? 1 : ( e >= 0 ? 0 : -e/c ) );
                }
            }
            break;
        case 3:
            s = 0;
            t = ( e >= 0 ? 0 :
                           ( -e >= c ? 1 : -e/c ) );
            break;
        case 4:
            if ( d < 0 ) { // min on edge t=0
                t = 0;
                s = ( d >= 0 ? 0 :
                               ( -d >= a ? 1 : -d/a ) );
            } else { // min on edge s = 0
                s = 0;
                t = ( e >= 0 ? 0 :
                               ( -e >= c ? 1 : -e/c ) );
            }
            break;
        case 5:
            t = 0;
            s = ( d >= 0 ? 0 :
                           ( -d >= a ? 1 : -d/a ) );
            break;
        case 6:
            {
                float tmp0 = a+d;
                float tmp1 = b+e;
                if (tmp0 > tmp1) { // min on edge s+1=1
                    float numer = c + e - b - d;
                    float denom = a -2*b + c;
                    s = ( numer >= denom ? 1 : numer/denom );
                    t = 1-s;
                } else { // min on edge t=1
                    t = 0;
                    s = ( tmp0 <= 0 ? 1 : ( d >= 0 ? 0 : -d/a ));
                }
            }
            break;
    }
    point = B + s*E0 + t*E1;
    float3 v = point-P;
    return dot(v,v);
}

template <typename Real>
Real distancePointLineSegment2D(const typename VectorTypeTemplate<Real>::type2 p,
                                const typename VectorTypeTemplate<Real>::type2 a,
                                const typename VectorTypeTemplate<Real>::type2 b ) {

    typedef typename VectorTypeTemplate<Real>::type2 T2;

    const T2 v = b - a;
    const T2 w = p - a;

    float c1 = dot(w,v);
    if (c1 <= 0) {
        return length(w);
    }

    float c2 = dot(v,v);
    if (c2 <= c1) {
        return length(p - b);
    }

    float t = c1 / c2;
    T2 closest = a + t*v;
    return length(p - closest);

}

template <typename Real>
Real signedDistancePointLineSegment2D(const typename VectorTypeTemplate<Real>::type2 p,
                                      const typename VectorTypeTemplate<Real>::type2 a,
                                      const typename VectorTypeTemplate<Real>::type2 b ) {

    typedef typename VectorTypeTemplate<Real>::type2 T2;

    const T2 v = b - a;
    const T2 w = p - a;
    T2 n;
    n.x = v.y;
    n.y = -v.x;

    int sign = dot(n,w) > 0 ? 1 : -1;

    float c1 = dot(w,v);
    if (c1 <= 0) {
        return sign*length(w);
    }

    float c2 = dot(v,v);
    if (c2 <= c1) {
        return sign*length(p - b);
    }

    float t = c1 / c2;
    T2 closest = a + t*v;
    return sign*length(p - closest);

}

template float distancePointLineSegment2D<float>(const float2, const float2, const float2);

template float signedDistancePointLineSegment2D<float>(const float2, const float2, const float2);

template <typename Real, typename Derived>
inline void rotationMatrixFromRodrigues(const Real w[3], Eigen::MatrixBase<Derived> const & R) {

    Real theta = sqrt(w[0]*w[0] + w[1]*w[1] + w[2]*w[2]);
    typedef typename Derived::Scalar Scalar;

    if (theta == 0) {
        const_cast< Eigen::MatrixBase<Derived>& >(R) = Eigen::Matrix<Scalar,3,3>::Identity(3,3);
        return;
    }

    Real rx = w[0] / theta;
    Real ry = w[1] / theta;
    Real rz = w[2] / theta;

    Eigen::Matrix<Scalar,3,3> H;
    H << 0, -rz, ry, rz, 0, -rx, -ry, rx, 0;

    Eigen::Matrix<Scalar,3,3> H2;
    H2 << rx*rx-1, rx*ry, rx*rz, rx*ry, ry*ry-1, ry*rz, rx*rz, ry*rz, rz*rz-1;

    const_cast< Eigen::MatrixBase<Derived> &>(R) = Eigen::Matrix<Scalar,3,3>::Identity(3,3) + sin(theta)*H + (1-cos(theta))*H2;

}

template <typename Real, typename Derived>
inline void rodriguesFromRotationMatrix(Real w[3], Eigen::MatrixBase<Derived> const & R) {

    Real x = R(2,1) - R(1,2);
    Real y = R(0,2) - R(2,0);
    Real z = R(1,0) - R(0,1);

    Real r = sqrt(x*x + y*y + z*z);

    if (r == 0) {
        memset(w,0,3*sizeof(Real));
        return;
    }

    Real t = R(0,0) + R(1,1) + R(2,2);

    Real theta = atan2(r,t-1);

    w[0] = x/r*theta;
    w[1] = y/r*theta;
    w[2] = z/r*theta;

}

template <typename Real>
void rotationMatrixJacobianFromRodrigues(const Real w[3], Eigen::Matrix<Real,9,3> & J) {

    Real theta = sqrt(w[0]*w[0] + w[1]*w[1] + w[2]*w[2]);

    if (theta == 0) {
        J <<    0, 0, 0, 0, 0, -1, 0, 1, 0,
                0, 0, 1, 0, 0, 0, -1, 0, 0,
                0, -1, 0, 1, 0, 0, 0, 0, 0;
        return;
    }

    Real rx = w[0] / theta;
    Real ry = w[1] / theta;
    Real rz = w[2] / theta;

    Eigen::Matrix<Real,3,3> H;
    H << 0, -rz, ry, rz, 0, -rx, -ry, rx, 0;

    Eigen::Matrix<Real,3,3> H2;
    H2 << rx*rx-1, rx*ry, rx*rz, rx*ry, ry*ry-1, ry*rz, rx*rz, ry*rz, rz*rz-1;

    Eigen::Matrix<Real,3,3> dH_drx, dH_dry, dH_drz;
    dH_drx << 0, 0, 0, 0, 0, -1, 0, 1, 0;
    dH_dry << 0, 0, 1, 0, 0, 0, -1, 0, 0;
    dH_drz << 0, -1, 0, 1, 0, 0, 0, 0, 0;

    Eigen::Matrix<Real,3,3> dH2_drx, dH2_dry, dH2_drz;
    dH2_drx << 2*rx, ry, rz, ry, 0, 0, rz, 0, 0;
    dH2_dry << 0, rx, 0, rx, 2*ry, rz, 0, rz, 0;
    dH2_drz << 0, 0, rx, 0, 0, ry, rx, ry, 2*rz;

    J.block(0,0,3,3) = H*rx*(cos(theta) - sin(theta)/theta) + dH_drx*sin(theta)/theta +
            H*H*rx*(sin(theta)-2*(1-cos(theta))/theta) + (1-cos(theta))/theta*(dH2_drx - 2*rx*Eigen::Matrix<Real,3,3>::Identity(3,3));
    J.block(3,0,3,3) = H*ry*(cos(theta) - sin(theta)/theta) + dH_dry*sin(theta)/theta +
            H*H*ry*(sin(theta)-2*(1-cos(theta))/theta) + (1-cos(theta))/theta*(dH2_dry - 2*ry*Eigen::Matrix<Real,3,3>::Identity(3,3));
    J.block(6,0,3,3) = H*rz*(cos(theta) - sin(theta)/theta) + dH_drz*sin(theta)/theta +
            H*H*rz*(sin(theta)-2*(1-cos(theta))/theta) + (1-cos(theta))/theta*(dH2_drz - 2*rz*Eigen::Matrix<Real,3,3>::Identity(3,3));

}

template <typename Real>
void aabbEllipsoid(const Real e[3], const Real c[3], const Real w[3], Real o[3], Real s[3]) {

    Eigen::Matrix<Real,3,3> R;
    rotationMatrixFromRodrigues<Real>(w,R);

    Real deltax = sqrt(e[0]*e[0]*R(0,0)*R(0,0) + e[1]*e[1]*R(0,1)*R(0,1) + e[2]*e[2]*R(0,2)*R(0,2));
    Real deltay = sqrt(e[0]*e[0]*R(1,0)*R(1,0) + e[1]*e[1]*R(1,1)*R(1,1) + e[2]*e[2]*R(1,2)*R(1,2));
    Real deltaz = sqrt(e[0]*e[0]*R(2,0)*R(2,0) + e[1]*e[1]*R(2,1)*R(2,1) + e[2]*e[2]*R(2,2)*R(2,2));

    o[0] = c[0] - deltax;
    o[1] = c[1] - deltay;
    o[2] = c[2] - deltaz;

    s[0] = 2*deltax;
    s[1] = 2*deltay;
    s[2] = 2*deltaz;
}

template <typename Real>
void aabbEllipticCylinder(const Real e[2], const Real h, const Real c[3], const Real w[3], Real o[3], Real s[3]) {

    Eigen::Matrix<Real,3,3> R;
    rotationMatrixFromRodrigues<Real>(w,R);

    Eigen::Matrix<Real,3,1> u, v, c2;
    u << e[0], 0, 0;
    v << 0, e[1], 0;
    c2 << 0, 0, h;

    u = R*u;
    v = R*v;
    c2 = R*c2;

    Real rx = sqrt(u(0)*u(0) + v(0)*v(0));
    Real ry = sqrt(u(1)*u(1) + v(1)*v(1));
    Real rz = sqrt(u(2)*u(2) + v(2)*v(2));

    o[0] = std::min(c[0]-rx, c[0]+c2(0)-rx);
    o[1] = std::min(c[1]-ry, c[1]+c2(1)-ry);
    o[2] = std::min(c[2]-rz, c[2]+c2(2)-rz);

    s[0] = std::max(c[0]+rx, c[0]+c2(0)+rx) - o[0];
    s[1] = std::max(c[1]+ry, c[1]+c2(1)+ry) - o[1];
    s[2] = std::max(c[2]+rz, c[2]+c2(2)+rz) - o[2];
}

template <typename Real>
void aabbRectangularPrism(const Real l[3], const Real c[3], const Real w[3], Real o[3], Real s[3]) {

    Eigen::Matrix<Real,3,3> R;
    rotationMatrixFromRodrigues<Real>(w,R);

    Eigen::Matrix<Real,3,1> corners[8];
    corners[0] << -l[0], -l[1], -l[2];
    corners[1] << -l[0], -l[1],  l[2];
    corners[2] << -l[0],  l[1], -l[2];
    corners[3] << -l[0],  l[1],  l[2];
    corners[4] <<  l[0], -l[1], -l[2];
    corners[5] <<  l[0], -l[1],  l[2];
    corners[6] <<  l[0],  l[1], -l[2];
    corners[7] <<  l[0],  l[1],  l[2];

    for (int i=0; i<8; i++)
        corners[i] = R*corners[i];

    o[0] = s[0] = corners[0](0);
    o[1] = s[1] = corners[0](1);
    o[2] = s[2] = corners[0](2);

    for (int i=1; i<8; i++) {
        o[0] = std::min(o[0],corners[i](0));
        o[1] = std::min(o[1],corners[i](1));
        o[2] = std::min(o[2],corners[i](2));
        s[0] = std::max(s[0],corners[i](0));
        s[1] = std::max(s[1],corners[i](1));
        s[2] = std::max(s[2],corners[i](2));
    }

    s[0] -= o[0];
    s[1] -= o[1];
    s[2] -= o[2];

    o[0] += c[0];
    o[1] += c[1];
    o[2] += c[2];
}

void generateUnitIcosphere(float3 * &verts, int3 * & indxs, int & nverts, int & nfaces, const int splits) {

    std::vector<float3> * vertVec = new std::vector<float3>();
    std::vector<int3> * faceVec = new std::vector<int3>();

    // generate initial 12 vertices
    float t = (1.0f + sqrtf(5.0f)) / 2.0f;

    vertVec->push_back(normalize(make_float3(-1, t, 0)));
    vertVec->push_back(normalize(make_float3( 1, t, 0)));
    vertVec->push_back(normalize(make_float3(-1,-1, 0)));
    vertVec->push_back(normalize(make_float3( 1,-t, 0)));

    vertVec->push_back(normalize(make_float3( 0,-1, t)));
    vertVec->push_back(normalize(make_float3( 0, 1, t)));
    vertVec->push_back(normalize(make_float3( 0,-1,-t)));
    vertVec->push_back(normalize(make_float3( 0, 1,-t)));

    vertVec->push_back(normalize(make_float3( t, 0,-1)));
    vertVec->push_back(normalize(make_float3( t, 0, 1)));
    vertVec->push_back(normalize(make_float3(-t, 0,-1)));
    vertVec->push_back(normalize(make_float3(-t, 0, 1)));

    // generate intitial 20 faces
    faceVec->push_back(make_int3( 0,11, 5));
    faceVec->push_back(make_int3( 0, 5, 1));
    faceVec->push_back(make_int3( 0, 1, 7));
    faceVec->push_back(make_int3( 0, 7,10));
    faceVec->push_back(make_int3( 0,10,11));

    faceVec->push_back(make_int3( 1, 5, 9));
    faceVec->push_back(make_int3( 5,11, 4));
    faceVec->push_back(make_int3(11,10, 2));
    faceVec->push_back(make_int3(10, 7, 6));
    faceVec->push_back(make_int3( 7, 1, 8));

    faceVec->push_back(make_int3( 3, 9, 4));
    faceVec->push_back(make_int3( 3, 4, 2));
    faceVec->push_back(make_int3( 3, 2, 6));
    faceVec->push_back(make_int3( 3, 6, 8));
    faceVec->push_back(make_int3( 3, 8, 9));

    faceVec->push_back(make_int3( 4, 9, 5));
    faceVec->push_back(make_int3( 2, 4,11));
    faceVec->push_back(make_int3( 6, 2,10));
    faceVec->push_back(make_int3( 8, 6, 7));
    faceVec->push_back(make_int3( 9, 8, 1));

    // map of already split vertices
    std::map<int64_t,int> split_verts;

    for (int i=0; i<splits; i++) {
        std::vector<int3>* new_faces = new std::vector<int3>();

        for (unsigned int f = 0; f < faceVec->size(); f++) {

            const int v1 = faceVec->at(f).x;
            const int v2 = faceVec->at(f).y;
            const int v3 = faceVec->at(f).z;
            int64_t key;
            std::map<int64_t,int>::iterator it;
            int p12, p23, p31;

            // edge 12
            key = (v1 < v2) ? (((int64_t)v1 << 32) | v2) : (((int64_t)v2 << 32) | v1);
            it = split_verts.find(key);
            if (it != split_verts.end()) { // check if we've already split this edge
                p12 = it->second;
            }
            else {
                p12 = vertVec->size();
                split_verts[key] = p12;
                vertVec->push_back(normalize(vertVec->at(v1) + vertVec->at(v2)));
            }

            // edge 23
            key = (v2 < v3) ? (((int64_t)v2 << 32) | v3) : (((int64_t)v3 << 32) | v2);
            it = split_verts.find(key);
            if (it != split_verts.end()) { // check if we've already split this edge
                p23 = it->second;
            }
            else {
                p23 = vertVec->size();
                split_verts[key] = p23;
                vertVec->push_back(normalize(vertVec->at(v2) + vertVec->at(v3)));
            }

            // edge 31
            key = (v3 < v1) ? (((int64_t)v3 << 32) | v1) : (((int64_t)v1 << 32) | v3);
            it = split_verts.find(key);
            if (it != split_verts.end()) { // check if we've already split this edge
                p31 = it->second;
            }
            else {
                p31 = vertVec->size();
                split_verts[key] = p31;
                vertVec->push_back(normalize(vertVec->at(v3) + vertVec->at(v1)));
            }

            // add new faces
            new_faces->push_back(make_int3(v1,p12,p31));
            new_faces->push_back(make_int3(v2,p23,p12));
            new_faces->push_back(make_int3(v3,p31,p23));
            new_faces->push_back(make_int3(p12,p23,p31));

        }

        delete faceVec;
        faceVec = new_faces;
    }

    // convert STL containers to raw arrays
    verts = new float3[vertVec->size()];
    memcpy(verts,vertVec->data(),vertVec->size()*sizeof(float3));
    nverts = vertVec->size();

    indxs = new int3[faceVec->size()];
    memcpy(indxs,faceVec->data(),faceVec->size()*sizeof(int3));
    nfaces = faceVec->size();

    // memory cleanup
    delete vertVec;
    delete faceVec;

}

// generate functions from template for the linker (just float or double for now)
template double distancePointEllipse<double>(const double[], const double[], double[]);
template float distancePointEllipse<float>(const float[], const float[], float[]);

template double distancePointEllipsoid<double>(const double[], const double[], double[]);
template float distancePointEllipsoid<float>(const float[], const float[], float[]);

template void aabbEllipsoid<double>(const double e[3], const double c[3], const double w[3], double o[3], double s[3]);
template void aabbEllipsoid<float>(const float e[3], const float c[3], const float w[3], float o[3], float s[3]);

template void aabbEllipticCylinder<double>(const double e[2], const double h, const double c[3], const double w[3], double o[3], double s[3]);
template void aabbEllipticCylinder<float>(const float e[2], const float h, const float c[3], const float w[3], float o[3], float s[3]);

template void aabbRectangularPrism<double>(const double l[3], const double c[3], const double w[3], double o[3], double s[3]);
template void aabbRectangularPrism<float>(const float l[3], const float c[3], const float w[3], float o[3], float s[3]);

}


#ifndef DIRICHLET_SAMPLER_HPP
#define DIRICHLET_SAMPLER_HPP

#include <Eigen/Dense>
#include <random>
#include <iostream>

namespace kmers {

/**
 * Return Dirichlet random variate with specified positive vector of
 * values.
 */
template <class RNG>
Eigen::VectorXd dirichlet_rng(const Eigen::VectorXd& alpha, RNG& rng) {
  int N = alpha.size();
  Eigen::VectorXd theta(N);
  for (int n = 0; n < N; ++n) {
    std::gamma_distribution<double> gamma_d(alpha(n), 1);
    theta(n) = gamma_d(rng);
  }
  return theta / theta.array().sum();
}

template <class RNG>
Eigen::VectorXd
normal_rng(uint64_t N, double mu, double sigma, RNG& rng) {
  std::normal_distribution<double> normal_d(mu, sigma);
  Eigen::VectorXd y;
  for (int n = 0; n < N; ++n) {
    y(n) = normal_d(rng);
  }
  return y;
}

template <class RNG>
std::vector<uint64_t> multinomial_rng(uint64_t N, const std::vector<double>& theta, RNG& rng) {
  std::vector<uint64_t> y(N, 0);
  std::discrete_distribution<uint64_t> discrete_d(theta.begin(), theta.end());
  for (uint64_t n = 0; n < N; ++n) {
    uint64_t z = discrete_d(rng);
    ++y[z];
  }
  return y;
}
}

#endif


#pragma once
#include "lue/framework/algorithm/aspect.hpp"
#include "lue/framework/algorithm/focal_operation_export.hpp"
#include "lue/framework/algorithm/definition/focal_operation.hpp"
#include "lue/framework/algorithm/serialize/kernel.hpp"
#include "lue/macro.hpp"
#include <boost/math/constants/constants.hpp>

namespace lue {
    namespace detail {

        template<
            typename T>
        T radians_to_compass_direction(
            T angle)
        {
#ifndef NDEBUG
            static T const pi = boost::math::constants::pi<T>();
#endif
            static T const half_pi = boost::math::constants::half_pi<T>();
            static T const two_pi = boost::math::constants::two_pi<T>();

            assert(angle >= -pi && angle <= pi);

            // Input angle, in radians:
            // east :  0.0 pi
            // north:  0.5 pi
            // west :  1.0 pi
            // south: -0.5 pi

            // Output angle, in radians:
            // north: 0.0 pi
            // east : 0.5 pi
            // south: 1.0 pi
            // west : 1.5 pi

            if(angle < T{0})
            {
                angle = half_pi - angle;
            }
            else if(angle > half_pi)
            {
                angle = two_pi - angle + half_pi;
            }
            else
            {
                angle = half_pi - angle;
            }

            assert(angle >= T{0} && angle < T{2} * pi);

            return angle;
        }

        template<
            typename Element>
        class Aspect
        {

            public:

                using OutputElement = Element;

                template<
                    typename Kernel,
                    typename OutputPolicies,
                    typename InputPolicies,
                    typename Subspan>
                OutputElement operator()(
                    [[maybe_unused]] Kernel const& kernel,
                    OutputPolicies const& output_policies,
                    InputPolicies const& input_policies,
                    Subspan const& elevation_window) const
                {
                    static_assert(rank<Kernel> == 2);
                    lue_hpx_assert(kernel.radius() == 1);

                    auto const& indp{input_policies.input_no_data_policy()};
                    auto const& ondp{output_policies.output_no_data_policy()};

                    Element aspect;

                    if(indp.is_no_data(elevation_window(1, 1)))
                    {
                        ondp.mark_no_data(aspect);
                    }
                    else
                    {
                        // TODO: all surrounding cells must have a valid value!!!
                        //       Once done, no-data tests can be removed here. Temp variables
                        //       can be remove too.

                        // west - east
                        // dz_dx = (w[0, 0] + 2 * w[1, 0] + w[2, 0]) - (w[0, 2] + 2 * w[1, 2] + w[2, 2])
                        //       = tmp1 - tmp2

                        // south - north
                        // dz_dy = (w[2, 0] + 2 * w[2, 1] + w[2, 2]) - (w[0, 0] + 2 * w[0, 1] + w[0, 2])
                        //       = tmp3 - tmp4

                        Element tmp1{0};

                        if(!indp.is_no_data(elevation_window(0, 0)))
                        {
                            tmp1 += elevation_window(0, 0);
                        }

                        if(!indp.is_no_data(elevation_window(1, 0)))
                        {
                            tmp1 += 2 * elevation_window(1, 0);
                        }

                        if(!indp.is_no_data(elevation_window(2, 0)))
                        {
                            tmp1 += elevation_window(2, 0);
                        }

                        Element tmp2{0};

                        if(!indp.is_no_data(elevation_window(0, 2)))
                        {
                            tmp2 += elevation_window(0, 2);
                        }

                        if(!indp.is_no_data(elevation_window(1, 2)))
                        {
                            tmp2 += 2 * elevation_window(1, 2);
                        }

                        if(!indp.is_no_data(elevation_window(2, 2)))
                        {
                            tmp2 += elevation_window(2, 2);
                        }

                        Element tmp3{0};

                        if(!indp.is_no_data(elevation_window(2, 0)))
                        {
                            tmp3 += elevation_window(2, 0);
                        }

                        if(!indp.is_no_data(elevation_window(2, 1)))
                        {
                            tmp3 += 2 * elevation_window(2, 1);
                        }

                        if(!indp.is_no_data(elevation_window(2, 2)))
                        {
                            tmp3 += elevation_window(2, 2);
                        }

                        Element tmp4{0};

                        if(!indp.is_no_data(elevation_window(0, 0)))
                        {
                            tmp4 += elevation_window(0, 0);
                        }

                        if(!indp.is_no_data(elevation_window(0, 1)))
                        {
                            tmp4 += 2 * elevation_window(0, 1);
                        }

                        if(!indp.is_no_data(elevation_window(0, 2)))
                        {
                            tmp4 += elevation_window(0, 2);
                        }

                        Element const dz_dx = tmp1 - tmp2;
                        Element const dz_dy = tmp3 - tmp4;

                        aspect = (dz_dx == Element{0} && dz_dy == Element{0})
                            ? -1  // Flat cell, pointing upwards
                            : radians_to_compass_direction(std::atan2(dz_dy, dz_dx))
                            ;
                    }

                    return aspect;
                }

        };

    }  // namespace detail

    template<
        typename Policies,
        typename Element,
        Rank rank>
    PartitionedArray<Element, rank> aspect(
        Policies const& policies,
        PartitionedArray<Element, rank> const& elevation)
    {
        using Functor = detail::Aspect<Element>;

        // Only used for its radius. Weights are not used.
        auto kernel{box_kernel<bool, 2>(1, true)};

        return focal_operation(policies, elevation, std::move(kernel), Functor{});
    }

}  // namespace lue

#define LUE_INSTANTIATE_ASPECT(                         \
    Policies, Element)                                  \
                                                        \
    template LUE_FOCAL_OPERATION_EXPORT                 \
    PartitionedArray<Element, 2> aspect<                \
            ArgumentType<void(Policies)>, Element, 2>(  \
        ArgumentType<void(Policies)> const&,            \
        PartitionedArray<Element, 2> const&);


//此源码被清华学神尹成大魔王专业翻译分析并修改
//尹成QQ77025077
//尹成微信18510341407
//尹成所在QQ群721929980
//尹成邮箱 yinc13@mails.tsinghua.edu.cn
//尹成毕业于清华大学,微软区块链领域全球最有价值专家
//https://mvp.microsoft.com/zh-cn/PublicProfile/4033620
//————————————————————————————————————————————————————————————————————————————————————————————————————————————————
/*
    此文件是Rippled的一部分：https://github.com/ripple/rippled
    版权所有（c）2012-2015 Ripple Labs Inc.

    使用、复制、修改和/或分发本软件的权限
    特此授予免费或不收费的目的，前提是
    版权声明和本许可声明出现在所有副本中。

    本软件按“原样”提供，作者不作任何保证。
    关于本软件，包括
    适销性和适用性。在任何情况下，作者都不对
    任何特殊、直接、间接或后果性损害或任何损害
    因使用、数据或利润损失而导致的任何情况，无论是在
    合同行为、疏忽或其他侵权行为
    或与本软件的使用或性能有关。
**/

//==============================================================

#include <ripple/basics/mulDiv.h>
#include <ripple/basics/contract.h>
#include <boost/multiprecision/cpp_int.hpp>
#include <limits>
#include <utility>

namespace ripple
{

std::pair<bool, std::uint64_t>
mulDiv(std::uint64_t value, std::uint64_t mul, std::uint64_t div)
{
    using namespace boost::multiprecision;

    uint128_t result;
    result = multiply(result, value, mul);

    result /= div;

    auto const limit = std::numeric_limits<std::uint64_t>::max();

    if (result > limit)
        return { false, limit };

    return { true, static_cast<std::uint64_t>(result) };
}

} //涟漪


//
// Copyright 2019 Olzhas Zhumabek <anonymous.from.applecity@gmail.com>
// Copyright 2021 Scramjet911 <36035352+Scramjet911@users.noreply.github.com>
// Use, modification and distribution are subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GIL_IMAGE_PROCESSING_HESSIAN_HPP
#define BOOST_GIL_IMAGE_PROCESSING_HESSIAN_HPP

#include <boost/gil/image_view.hpp>
#include <boost/gil/typedefs.hpp>
#include <boost/gil/extension/numeric/kernel.hpp>
#include <stdexcept>

namespace boost { namespace gil {

/// \brief Computes Hessian response
///
/// Computes Hessian response based on computed entries of Hessian matrix, e.g. second order
/// derivates in x and y, and derivatives in both x, y.
/// d stands for derivative, and x or y stand for derivative direction. For example,
/// ddxx means taking two derivatives (gradients) in horizontal direction.
/// Weights change perception of surroinding pixels.
/// Additional filtering is strongly advised.
template <typename GradientView, typename T, typename Allocator, typename OutputView>
inline void compute_hessian_responses(
    GradientView ddxx,
    GradientView dxdy,
    GradientView ddyy,
    const detail::kernel_2d<T, Allocator>& weights,
    OutputView dst)
{
    if (ddxx.dimensions() != ddyy.dimensions()
        || ddyy.dimensions() != dxdy.dimensions()
        || dxdy.dimensions() != dst.dimensions()
        || weights.center_x() != weights.center_y())
    {
        throw std::invalid_argument("dimensions of views are not the same"
            " or weights don't have equal width and height"
            " or weights' dimensions are not odd");
    }
    // Use pixel type of output, as values will be written to output
    using pixel_t = typename std::remove_reference<decltype(std::declval<OutputView>()(0, 0))>::type;

    using channel_t = typename std::remove_reference
        <
            decltype(std::declval<pixel_t>().at(std::integral_constant<int, 0>{}))
        >::type;

    auto center = weights.center_y();
    for (auto y = center; y < dst.height() - center; ++y)
    {
        for (auto x = center; x < dst.width() - center; ++x)
        {
            auto ddxx_i = channel_t();
            auto ddyy_i = channel_t();
            auto dxdy_i = channel_t();
            for (typename OutputView::coord_t w_y = 0; w_y < static_cast<std::ptrdiff_t>(weights.size()); ++w_y)
            {
                for (typename OutputView::coord_t w_x = 0; w_x < static_cast<std::ptrdiff_t>(weights.size()); ++w_x)
                {
                    ddxx_i += ddxx(x + w_x - center, y + w_y - center)
                        .at(std::integral_constant<int, 0>{}) * weights.at(w_x, w_y);
                    ddyy_i += ddyy(x + w_x - center, y + w_y - center)
                        .at(std::integral_constant<int, 0>{}) * weights.at(w_x, w_y);
                    dxdy_i += dxdy(x + w_x - center, y + w_y - center)
                        .at(std::integral_constant<int, 0>{}) * weights.at(w_x, w_y);
                }
            }
            auto determinant = ddxx_i * ddyy_i - dxdy_i * dxdy_i;
            dst(x, y).at(std::integral_constant<int, 0>{}) = determinant;
        }
    }
}

}} // namespace boost::gil

#endif


/*
 * Copyright Nick Thompson, 2020
 * Use, modification and distribution are subject to the
 * Boost Software License, Version 1.0. (See accompanying file
 * LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
 */
#include <cstdint>
#include <cmath>
#include <boost/math/quadrature/wavelet_transforms.hpp>
#include <Eigen/Dense>

int main()
{
    using boost::math::quadrature::daubechies_wavelet_transform;
    double a = 1.3;
    auto f = [&a](double t) {
        if(t==0) {
            return double(0);
        }
        return std::sin(a/t);
    };

    auto Wf = daubechies_wavelet_transform<decltype(f), double, 8>(f);

    Eigen::MatrixXd grid(512, 512);
    double s = 7;
    double t = 0;
    grid(0,0) = Wf(s, t);

    auto g = [&a](double t)->std::complex<double> {
        if (t==0) {
            return {0.0, 0.0};
        }
        return std::exp(std::complex<double>(0.0, a/t));
    };

    auto Wg = daubechies_wavelet_transform<decltype(g), double, 8>(g);
    std::cout << "W[f](s,t) = " << Wf(s,t) << "\n";
    std::cout << "W[g](s,t) = " << Wg(s, t) << "\n";
    std::cout << Wg(0.0, 3.5) << "\n";
    std::cout << Wf(0.0, 4.8) << "\n";
    std::cout << "W[f](-s,t) = " << Wf(-s, t) << "\n";
    std::cout << "W[g](-s,t) = " << Wg(-s, t) << "\n";

}


#include <cstdlib>
#include <iostream>
#include <complex>
#include <type_traits>
#include <algorithm>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/banded.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/banded.hpp>
#include <boost/numeric/bindings/blas/level1.hpp>
#include <boost/numeric/bindings/blas/level2.hpp>
#include <boost/numeric/bindings/lapack/driver.hpp>
#include "random.hpp"

namespace ublas=boost::numeric::ublas;
namespace blas=boost::numeric::bindings::blas;
namespace lapack=boost::numeric::bindings::lapack;

int main(int argc, char *argv[]) {
  typedef std::complex<double> complex;
  typedef ublas::vector<complex> vector;
  typedef ublas::vector<int> p_vector;
  typedef ublas::banded_matrix<complex, ublas::column_major> matrix;
  typedef typename std::make_signed<vector::size_type>::type size_type;

  rand_normal<complex>::reset();
  size_type n=1024, kl=3, ku=2;
  matrix A(n, n, kl, ku+kl);
  for (size_type j=0; j<n; ++j) {
    for (size_type i=std::max(size_type(0), j-ku-kl); i<std::max(size_type(0), j-ku); ++i)
      A(i, j)=complex(0);
    for (size_type i=std::max(size_type(0), j-ku); i<std::min(n, j+kl+1); ++i)
      A(i, j)=rand_normal<complex>::get();
  }
  vector b(n);
  for (size_type i=0; i<n; ++i)
    b(i)=rand_normal<complex>::get();
  matrix A_bak(A);
  vector x(b);
  p_vector p(n);  // pivots
  int info=lapack::gbsv(A, p, x); // solve
  if (info==0) {
    // res <- A*x - b
    vector res(b);
    blas::gbmv(complex(1, 0), A_bak, x, complex(-1, 0), res);
    std::cout << "norm of residual : " << blas::nrm2(res) << '\n';
  } else
    if (info>0)
      std::cout << "singular matrix\n";
    else
      std::cout << "illegal arguments\n";
  return EXIT_SUCCESS;
}