/* boost random/cauchy_distribution.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * Distributed under 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)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id$
 *
 * Revision history
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_CAUCHY_DISTRIBUTION_HPP
#define BOOST_RANDOM_CAUCHY_DISTRIBUTION_HPP

#include <boost/config/no_tr1/cmath.hpp>
#include <iosfwd>
#include <istream>
#include <boost/limits.hpp>
#include <boost/random/detail/config.hpp>
#include <boost/random/detail/operators.hpp>
#include <boost/random/uniform_01.hpp>

namespace boost {
namespace random {

// Cauchy distribution:

/**
 * The cauchy distribution is a continuous distribution with two
 * parameters, median and sigma.
 *
 * It has \f$\displaystyle p(x) = \frac{\sigma}{\pi(\sigma^2 + (x-m)^2)}\f$
 */
template<class RealType = double>
class cauchy_distribution
{
public:
    typedef RealType input_type;
    typedef RealType result_type;

    class param_type
    {
    public:

        typedef cauchy_distribution distribution_type;

        /** Constructs the parameters of the cauchy distribution. */
        explicit param_type(RealType median_arg = RealType(0.0),
                            RealType sigma_arg = RealType(1.0))
          : _median(median_arg), _sigma(sigma_arg) {}

        // backwards compatibility for Boost.Random

        /** Returns the median of the distribution. */
        RealType median() const { return _median; }
        /** Returns the sigma parameter of the distribution. */
        RealType sigma() const { return _sigma; }

        // The new names in C++0x.

        /** Returns the median of the distribution. */
        RealType a() const { return _median; }
        /** Returns the sigma parameter of the distribution. */
        RealType b() const { return _sigma; }

        /** Writes the parameters to a std::ostream. */
        BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, param_type, parm)
        {
            os << parm._median << " " << parm._sigma;
            return os;
        }

        /** Reads the parameters from a std::istream. */
        BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, param_type, parm)
        {
            is >> parm._median >> std::ws >> parm._sigma;
            return is;
        }

        /** Returns true if the two sets of parameters are equal. */
        BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(param_type, lhs, rhs)
        { return lhs._median == rhs._median && lhs._sigma == rhs._sigma; }

        /** Returns true if the two sets of parameters are different. */
        BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(param_type)

    private:
        RealType _median;
        RealType _sigma;
    };

    /**
     * Constructs a \cauchy_distribution with the paramters @c median
     * and @c sigma.
     */
    explicit cauchy_distribution(RealType median_arg = RealType(0.0),
                                 RealType sigma_arg = RealType(1.0))
      : _median(median_arg), _sigma(sigma_arg) { }

    /**
     * Constructs a \cauchy_distribution from it's parameters.
     */
    explicit cauchy_distribution(const param_type& parm)
      : _median(parm.median()), _sigma(parm.sigma()) { }

    // compiler-generated copy ctor and assignment operator are fine

    // backwards compatibility for Boost.Random

    /** Returns: the "median" parameter of the distribution */
    RealType median() const { return _median; }
    /** Returns: the "sigma" parameter of the distribution */
    RealType sigma() const { return _sigma; }

    // The new names in C++0x

    /** Returns: the "median" parameter of the distribution */
    RealType a() const { return _median; }
    /** Returns: the "sigma" parameter of the distribution */
    RealType b() const { return _sigma; }

    /** Returns the smallest value that the distribution can produce. */
    RealType min BOOST_PREVENT_MACRO_SUBSTITUTION () const
    { return -(std::numeric_limits<RealType>::infinity)(); }

    /** Returns the largest value that the distribution can produce. */
    RealType max BOOST_PREVENT_MACRO_SUBSTITUTION () const
    { return (std::numeric_limits<RealType>::infinity)(); }

    param_type param() const { return param_type(_median, _sigma); }

    void param(const param_type& parm)
    {
        _median = parm.median();
        _sigma = parm.sigma();
    }

    /**
     * Effects: Subsequent uses of the distribution do not depend
     * on values produced by any engine prior to invoking reset.
     */
    void reset() { }

    /**
     * Returns: A random variate distributed according to the
     * cauchy distribution.
     */
    template<class Engine>
    result_type operator()(Engine& eng)
    {
        // Can we have a boost::mathconst please?
        const result_type pi = result_type(3.14159265358979323846);
        using std::tan;
        RealType val = uniform_01<RealType>()(eng)-result_type(0.5);
        return _median + _sigma * tan(pi*val);
    }

    /**
     * Returns: A random variate distributed according to the
     * cauchy distribution with parameters specified by param.
     */
    template<class Engine>
    result_type operator()(Engine& eng, const param_type& parm)
    {
        return cauchy_distribution(parm)(eng);
    }

    /**
     * Writes the distribution to a @c std::ostream.
     */
    BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, cauchy_distribution, cd)
    {
        os << cd._median << " " << cd._sigma;
        return os;
    }

    /**
     * Reads the distribution from a @c std::istream.
     */
    BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, cauchy_distribution, cd)
    {
        is >> cd._median >> std::ws >> cd._sigma;
        return is;
    }

    /**
     * Returns true if the two distributions will produce
     * identical sequences of values, given equal generators.
     */
    BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(cauchy_distribution, lhs, rhs)
    { return lhs._median == rhs._median && lhs._sigma == rhs._sigma; }

    /**
     * Returns true if the two distributions may produce
     * different sequences of values, given equal generators.
     */
    BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(cauchy_distribution)

private:
    RealType _median;
    RealType _sigma;
};

} // namespace random

using random::cauchy_distribution;

} // namespace boost

#endif // BOOST_RANDOM_CAUCHY_DISTRIBUTION_HPP


#include <armadillo>
#include "electronic.hpp"

/*
  From NR 17.1.3 and A&S 25.5.10. For systems of the form $i \hbar
  \dot{c} = H c$, advances c using a 4th order Runge-Kutta integrator
*/
// FIXME: Returns different results than exact propagation
void Electronic::advance_rk4(const arma::cx_mat &H, double dt){
  const std::complex<double> midt(0,-dt);

  k1 = midt * H * amplitudes;
  k2 = midt * H * (amplitudes + (k1 / 2));
  k3 = midt * H * (amplitudes - (k1 / 3) + k2);
  k4 = midt * H * (amplitudes + (k1    ) - k2 + k3);

  amplitudes = amplitudes + (k1 / 6) + (k2 / 3) + (k3 / 3) + (k4 / 6);
}

void Electronic::advance_exact(const arma::cx_mat &H, double dt){
  const std::complex<double> midt(0,-dt);
  amplitudes = arma::expmat(midt * H) * amplitudes;
}

void Electronic::reserve(void){
  auto s = arma::size(amplitudes);
  k1.set_size(s);
  k2.set_size(s);
  k3.set_size(s);
  k4.set_size(s);
}

/*
  Replace U with the closest unitary matrix according to the
  transformation U' = (UU^T)^(-1/2)U.

  For a thorough discussion, see:
  https://en.wikipedia.org/wiki/Kabsch_algorithm
  https://en.wikipedia.org/wiki/Polar_decomposition
*/
void Electronic::unitarize(arma::mat &U){
  // arma::cx_mat R; arma::cx_vec s;
  // arma::eig_gen(s, R, U*U.t());
  // U = arma::real(R * diagmat(arma::pow(s, -0.5)) * R.t()) * U;

  /*
    These algorithms are equivalent but the below has better numerical
    stability properties; namely the above returns 2*I for the
    identity (I) rather than I itself.
  */

  arma::mat W,V; arma::vec s;
  arma::svd(W,s,V,U);
  U = W*V;
}

/*
  Implements Zeyu Zhou et al. JCTC 2020, 16, 835--846

  Approximately minimizes Tr[log(U)^2] via jacobi sweeps while
  enforcing det(U) == 1

  Results agree with the matricies in the paper's Appendix D
*/
void Electronic::phase_match(arma::mat &U, bool do_unitarize){
  // Step 1: det(U) == 1
  if (do_unitarize){
    unitarize(U);
  }
  if (arma::det(U) < 0){
    U.col(0) *= -1.0;
  }

  // Step 2: Jacobi sweeps
  double deljk, Ujj, Ukk;

  const arma::uword N = U.n_rows;
  bool change;
  do{
    change = false;
    for (arma::uword j = 0; j < N; j++){
      for (arma::uword k = j + 1; k < N; k++){
	Ujj = U(j,j);
	Ukk = U(k,k);
	// Eq. 34 with the sum implemented as a dot product
	deljk =
            3*(Ujj*Ujj + Ukk*Ukk)
	  + 6*(U(j,k)*U(k,j))
	  + 8*(Ujj + Ukk)
	  - 3*(arma::as_scalar(U.row(j)*U.col(j) + U.row(k)*U.col(k)));
	if (deljk < 0){
	  U.col(j) *= -1.0;
	  U.col(k) *= -1.0;
          change = true;
	}
      }
    }
  }while(change);
}

void Electronic::phase_match(arma::cx_mat &U, bool do_unitarize){
  (void) U;
  (void) do_unitarize;
  throw std::runtime_error("complex phase matching not implemented");
}


//==================================================================================================
/*!
  @file
  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_ERFCX_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ERFCX_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!
  @ingroup group-euler
    Function object implementing erfcx capabilities

   Computes the  underflow-compensating (scaled) error function:
   \f$\displaystyle e^{x^2}\frac{2}{\sqrt\pi}\int_0^{x} e^{-t^2}\mbox{d}t\f$

    @par Semantic:

    For every parameter of floating type T

    @code
    T r = erfcx(x);
    @endcode

    is similar to:

    @code
    T r = exp(sqr(x))*erfc(x);
    @endcode

    But avoid underflow as much as possible.

    @see erfc, erf

  **/
  const boost::dispatch::functor<tag::erfcx_> erfcx = {};
} }
#endif

#include <boost/simd/function/scalar/erfcx.hpp>
#include <boost/simd/function/simd/erfcx.hpp>

#endif


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 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_HYPERBOLIC_FUNCTIONS_SCALAR_SINHCOSH_HPP_INCLUDED
#define NT2_TOOLBOX_HYPERBOLIC_FUNCTIONS_SCALAR_SINHCOSH_HPP_INCLUDED

#include <nt2/toolbox/hyperbolic/functions/sinhcosh.hpp>
#include <nt2/include/functions/scalar/tofloat.hpp>
#include <nt2/include/functions/scalar/expm1.hpp>
#include <nt2/include/functions/scalar/negif.hpp>
#include <nt2/include/functions/scalar/abs.hpp>
#include <nt2/include/functions/scalar/oneplus.hpp>
#include <nt2/include/functions/scalar/abs.hpp>
#include <nt2/include/functions/scalar/is_negative.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/sdk/meta/as_logical.hpp>
#include <nt2/sdk/meta/as_floating.hpp>
#include <boost/fusion/tuple.hpp>

namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION(  nt2::tag::sinhcosh_, tag::cpu_,(A0)(A1)
                               , ((scalar_<arithmetic_<A0> >))
                                 ((scalar_<floating_<A1> >))
                                 ((scalar_<floating_<A1> >))
                             )
  {
    typedef int result_type;
    inline result_type operator()(A0 const& a0,A1 & a1,A1 & a2) const
    {
      a2 =  nt2::abs(a0);
      a1 = a0;
      if (a2 == Inf<A1>()) return 0;
      const A1 u = expm1(a2);
      const A1 up1 = oneplus(u);
      const A1 tmp =u/up1;
      a1 = negif(is_negative(a0), Half<A1>()*tmp*(oneplus(up1)));
      a2 = oneplus(Half<A1>()*tmp*u);
      return 0;
    }
  };

  NT2_FUNCTOR_IMPLEMENTATION(nt2::tag::sinhcosh_, tag::cpu_,(A0)(A1),
                             ((scalar_ < arithmetic_<A0> > ))
                             ((scalar_ < floating_<A1> > ))
                             )
    {
    typedef A1 result_type;
    inline result_type operator()(A0 const& a0,A1 & a2) const
    {
      A1 a1;
      sinhcosh(tofloat(a0),a1, a2);
      return a1;
    }
  };

  NT2_FUNCTOR_IMPLEMENTATION(nt2::tag::sinhcosh_, tag::cpu_,
                         (A0),
                         ((scalar_<arithmetic_<A0> >))
                        )
  {
      typedef typename meta::as_floating<A0>::type  rtype;
      typedef boost::fusion::tuple<rtype, rtype> result_type;

    NT2_FUNCTOR_CALL(1)
    {
      result_type res;
      sinhcosh(tofloat(a0), boost::fusion::at_c<0>(res),
                 boost::fusion::at_c<1>(res));
      return res;
    }
  };
} }
#endif


//
// Created by Hamza El-Kebir on 5/9/21.
//

#ifndef LODESTAR_DISCRETELQE_HPP
#define LODESTAR_DISCRETELQE_HPP

#include "Lodestar/synthesis/AlgebraicRiccatiEquation.hpp"
#include <Eigen/QR>
#include <vector>

namespace ls {
    namespace filter {
        class DiscreteLQE {
            static Eigen::MatrixXd
            infiniteHorizon(const Eigen::MatrixXd &A,
                            const Eigen::MatrixXd &B,
                            const Eigen::MatrixXd &Q,
                            const Eigen::MatrixXd &C,
                            const Eigen::MatrixXd &R);

            static Eigen::MatrixXd
            infiniteHorizon(const systems::StateSpace<> &sys,
                            const Eigen::MatrixXd &Q,
                            const Eigen::MatrixXd &R);

            static Eigen::MatrixXd
            finiteHorizon(const Eigen::MatrixXd &A,
                          const Eigen::MatrixXd &B,
                          const Eigen::MatrixXd &C,
                          const Eigen::MatrixXd &Q,
                          const Eigen::MatrixXd &R,
                          unsigned int N = 5);

            static Eigen::MatrixXd
            finiteHorizon(const systems::StateSpace<> &sys,
                          const Eigen::MatrixXd &Q,
                          const Eigen::MatrixXd &R,
                          unsigned int N = 5);
        };
    }
}

#endif //LODESTAR_DISCRETELQE_HPP


#ifndef MPP_HAMILTONIAN_CLASSIC_HAMILTONIAN_HPP
#define MPP_HAMILTONIAN_CLASSIC_HAMILTONIAN_HPP

#include <exception>
#include <sstream>
#include <string>
#include <random>
#include <type_traits>
#include <cmath>
#include <functional>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/assert.hpp>
#include <mpp/config.hpp>
#include <mpp/chains/mcmc_chain.hpp>
#include <mpp/hamiltonian/kinetic_energy_multivar_normal.hpp>

namespace mpp { namespace hamiltonian {

template<class real_scalar_type>
class hmc_sampler
{
public:
    typedef boost::numeric::ublas::vector<real_scalar_type> real_vector_type;
    typedef mpp::chains::mcmc_chain<real_scalar_type> chain_type;
    typedef std::mt19937 rng_type;
    typedef std::uniform_real_distribution<real_scalar_type> uni_real_dist_type;
    typedef std::uniform_int_distribution<size_t> uni_int_dist_type;
    typedef mpp::hamiltonian::multivariate_normal<
        real_scalar_type> kinetic_energy_type;

    typedef typename std::function<
        real_scalar_type (real_vector_type const &) > log_post_func_type;

    typedef typename std::function<
        real_vector_type (real_vector_type const &)> grad_log_post_func_type;

    static_assert(
        std::is_floating_point<real_scalar_type>::value,
        "The real scalar is expected to be a floating point type."
    );

    hmc_sampler (
        log_post_func_type & log_posterior,
        grad_log_post_func_type & grad_log_posterior,
        size_t const num_dims,
        size_t const max_num_steps,
        real_scalar_type const max_eps,
        real_vector_type const & inv_mass_mat
    ) throw()
    : m_log_posterior(log_posterior)
    , m_grad_log_posterior(grad_log_posterior)
    , m_num_dims(num_dims)
    , m_max_num_steps(max_num_steps)
    , m_max_eps(max_eps)
    , m_inv_mass_mat(inv_mass_mat)
    , m_beta(1)
    ,m_acc_rate(0) {

        if( m_num_dims == size_t(0) ) {
            std::stringstream msg;
            msg << "The number of dimensions = "
                << m_num_dims
                << " should be greater than zero.";
            throw std::length_error(msg.str());
        }

        BOOST_ASSERT_MSG(
            m_num_dims <= size_t(MPP_MAXIMUM_NUMBER_OF_DIMENSIONS),
            "num_dims too big. Please modify the config.hpp and recompile."
        );

        if( m_max_num_steps == size_t(0) ) {
            std::stringstream msg;
            msg << "Maximum number of steps = "
                << m_max_num_steps
                << " in the discretisation of Hamiltonian should be"
                << " greater than zero.";
            throw std::length_error(msg.str());
        }

        BOOST_ASSERT_MSG(
            m_max_num_steps <= size_t(MPP_CLASSIC_HAMILTONIAN_MAX_NUM_STEPS),
            "max_num_steps too big. Please modify the config.hpp and recompile."
        );

        if( m_max_eps <= real_scalar_type(0)
            or m_max_eps > real_scalar_type(1)) {
            std::stringstream msg;
            msg << "Maximum value of epsilon = "
                << m_max_eps
                << " in the discretisation of Hamiltonian should be"
                << " in the intervale (0,1].";
            throw std::domain_error(msg.str());
        }

    }

    chain_type run_sampler(size_t const num_samples,
        real_vector_type const & start_point) throw() {
        BOOST_ASSERT_MSG(
            num_samples <=
                size_t(MPP_MAXIMUM_NUMBER_OF_SAMPLES_PER_RUN_SAMPLER_CALL),
            "num_samples too big. Please modify the config and recompile."
        );

        chain_type hmc_chain(num_samples,m_num_dims);
        uni_real_dist_type uni_real_dist;
        uni_int_dist_type uni_int_dist( size_t(1),m_max_num_steps+size_t(1) );
        kinetic_energy_type kin_eng(m_inv_mass_mat);
        real_vector_type q_1(start_point);
        size_t num_accepted(0);
        size_t num_rejected(0);
        while( num_accepted < num_samples ) {
            real_vector_type q_0(q_1);
            real_scalar_type const eps = m_max_eps*uni_real_dist(m_rng);
            size_t const num_steps = uni_int_dist(m_rng);
            real_vector_type p_0 = kin_eng.generate_sample(m_rng);

            real_scalar_type const h_0 = -m_log_posterior(q_0)
                -kin_eng.log_posterior(p_0);
            leap_frog(m_grad_log_posterior,kin_eng,q_0,p_0,eps,num_steps);
            real_scalar_type const log_post_val = m_log_posterior(q_0);
            real_scalar_type const h_1 = -log_post_val
                -kin_eng.log_posterior(p_0);
            real_scalar_type const delta_h = h_1 - h_0;

            if( not std::isfinite(delta_h) ) {
                std::stringstream msg;
                msg << "delta(H) value is not finite";
                throw std::out_of_range(msg.str());
            }
            real_scalar_type const uni_rand = uni_real_dist(m_rng);
            if(std::log(uni_rand) < -delta_h*m_beta) {
                q_1 = q_0;
                hmc_chain.set_sample(num_accepted,q_1,log_post_val);
                ++num_accepted;
            }
            else {
                ++num_rejected;
            }
        }

        m_acc_rate = real_scalar_type(num_accepted)
            / real_scalar_type(num_accepted + num_rejected);

        return hmc_chain;
    }

    inline real_scalar_type acc_rate() const {
        return m_acc_rate;
    }

private:

    static void leap_frog(
        grad_log_post_func_type & grad_log_posterior,
        kinetic_energy_type & kin_eng,
        real_vector_type & q,
        real_vector_type & p,
        real_scalar_type const eps,
        size_t const num_steps
    ) {
        BOOST_ASSERT_MSG(
            num_steps <= size_t(MPP_CLASSIC_HAMILTONIAN_MAX_NUM_STEPS),
            "m_max_num_steps too big. Please modify the config and recompile."
        );

        BOOST_ASSERT_MSG(
            eps > real_scalar_type(0) and eps <= real_scalar_type(1),
            "epsilon value should be a real number in [0,1]"
        );

        BOOST_ASSERT_MSG(
            p.size() == q.size(),
            "p and q should have identical dimensions"
        );

        real_vector_type dq = grad_log_posterior(q);
        real_vector_type dp = kin_eng.grad_log_posterior(p);

        p += 0.5*eps*dq;
        for(size_t j=0;j<num_steps;++j) {
            dp = kin_eng.grad_log_posterior(p);
            q -= eps*dp;
            dq = grad_log_posterior(q);
            p += eps*dq;
        }
        p -= 0.5*eps*dq;
    }

    log_post_func_type m_log_posterior;
    grad_log_post_func_type m_grad_log_posterior;
    size_t m_num_dims;
    size_t m_max_num_steps;
    real_scalar_type m_max_eps;
    real_vector_type m_inv_mass_mat;
    rng_type m_rng;
    real_scalar_type m_beta;
    real_scalar_type m_acc_rate;
};

}}

#endif // MPP_HAMILTONIAN_CLASSIC_HAMILTONIAN_HPP


#ifndef _INCLUDED_GEOMETRY_HPP_
#define _INCLUDED_GEOMETRY_HPP_
/**
 * @file   geometry.hpp
 * @author Aron Ahmadia <aron@casiphia.local>
 * @date   Tue Feb 26 14:43:29 2008
 *
 * @brief  Geometry - A set of static methods to assist in hypersphere decomposition
 *
 *
 */

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
//#include "boost_ext/ublas_vector.hpp"
//#include "boost_ext/ublas_matrix.hpp"
#include <boost/version.hpp>

namespace smo {

  const double pi = M_PI;

  typedef double PolarAngle;
  typedef double Area;
  typedef int DimCount;
  typedef int PointCount;
  typedef int RegionCount;

  typedef boost::numeric::ublas::matrix<PolarAngle> Region;
  typedef boost::numeric::ublas::matrix<PolarAngle> SpherePoints;
  typedef boost::numeric::ublas::vector<RegionCount> RegionCounts;
  typedef boost::numeric::ublas::vector<double> CartPoint;
  typedef boost::numeric::ublas::matrix<double> CartPoints;

  typedef struct {
    DimCount d;
    Area capAreaZero;
  } AreaOfCapP;

  class Geometry
  {
  public:
    /**
     * Find the angle that encloses the area of a d-dimensional spherical cap embedded in R^{d+1}
     *
     * @param d dimension of the sphere
     * @param capArea area enclosed by the angle
     *
     * @return angle that ecloses capArea
     */
    static PolarAngle angleOfCap(DimCount d, Area capArea);
    static PolarAngle polarCap(DimCount d, PointCount n);
    static Area regionArea(DimCount d, PointCount n);
    static Area regionArea(DimCount d, Area polarCapArea, PolarAngle phi1, PolarAngle phi2, PointCount n);
    static Area areaOfCap(DimCount d, PolarAngle capPhi);
    static Area areaOfCap(PolarAngle capPhi,AreaOfCapP &p);
    static Area areaOfSphere(DimCount d);
    static Area areaOfCollar(DimCount d, PolarAngle phi1, PolarAngle phi2);
    static void bottomCapRegion(DimCount d, PolarAngle capPhi, Region &r);
    static void topCapRegion(DimCount d, PolarAngle capPhi, Region &r);
    static void sphereRegion(DimCount d, Region &r);
    static PolarAngle circleOffset(PointCount p1, PointCount p2);
    static void toPolar(CartPoints p, SpherePoints &s);
    static void toCart(SpherePoints s, CartPoints &p);
  };
}

#endif


#include "NFmiLambertConformalConicArea.h"
#include "NFmiStringTools.h"
#include <boost/functional/hash.hpp>
#include <fmt/format.h>
#include <macgyver/Exception.h>
#include <cmath>

using namespace std;

// For some reason Boost.Constants does not define quart_pi so we'll just use our own definitions
namespace
{
const double pi = 3.141592653589793238462643383279502884197169399375105820974944592307816406286;
const double half_pi = pi / 2;
const double quart_pi = pi / 4;
}  // namespace

// ----------------------------------------------------------------------
/*!
 * Constructor
 */
// ----------------------------------------------------------------------

NFmiLambertConformalConicArea::NFmiLambertConformalConicArea(const NFmiPoint &theBottomLeftLatLon,
                                                             const NFmiPoint &theTopRightLatLon,
                                                             double theCentralLongitude,
                                                             double theCentralLatitude,
                                                             double theTrueLatitude1,
                                                             double theTrueLatitude2,
                                                             double theRadius,
                                                             bool usePacificView,
                                                             const NFmiPoint &theTopLeftXY,
                                                             const NFmiPoint &theBottomRightXY)
    : NFmiArea(theTopLeftXY, theBottomRightXY, usePacificView),
      itsBottomLeftLatLon(theBottomLeftLatLon),
      itsTopRightLatLon(theTopRightLatLon),
      itsCentralLongitude(theCentralLongitude),
      itsCentralLatitude(theCentralLatitude),
      itsTrueLatitude1(theTrueLatitude1),
      itsTrueLatitude2(theTrueLatitude2),
      itsRadius(theRadius)
{
  try
  {
    Init();
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \param fKeepWorldRect Undocumented
 */
// ----------------------------------------------------------------------

void NFmiLambertConformalConicArea::Init(bool fKeepWorldRect)
{
  try
  {
    const double lat1 = FmiRad(itsTrueLatitude1);
    const double lat2 = FmiRad(itsTrueLatitude2);
    const double lat0 = FmiRad(itsCentralLatitude);

    if (itsTrueLatitude1 != itsTrueLatitude2)
      itsN = log(cos(lat1) / cos(lat2)) / log(tan(quart_pi + lat2 / 2) / tan(quart_pi + lat1 / 2));
    else
      itsN = sin(lat1);

    itsF = cos(lat1) * pow(tan(quart_pi + lat1 / 2), itsN) / itsN;
    itsRho0 = itsRadius * itsF / pow(tan(quart_pi + lat0 / 2), itsN);

    if (!fKeepWorldRect)
      itsWorldRect =
          NFmiRect(LatLonToWorldXY(itsBottomLeftLatLon), LatLonToWorldXY(itsTopRightLatLon));

    itsXScaleFactor = Width() / itsWorldRect.Width();
    itsYScaleFactor = Height() / itsWorldRect.Height();

    const char *fmt =
        "+proj=lcc +lat_1={} +lat_2={} +lat_0={} +lon_0={} +x_0=0 +y_0=0 +R={} +units=m +wktext "
        "+no_defs +type=crs";
    itsProjStr = fmt::format(fmt,
                             itsTrueLatitude1,
                             itsTrueLatitude2,
                             itsCentralLatitude,
                             itsCentralLongitude,
                             itsRadius);
    itsSpatialReference = std::make_shared<Fmi::SpatialReference>(itsProjStr);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \param theBottomLeftLatLon Undocumented
 * \param theTopRightLatLon Undocumented
 * \return Undocumented
 * \todo Should return an boost::shared_ptr instead
 */
// ----------------------------------------------------------------------

NFmiArea *NFmiLambertConformalConicArea::NewArea(const NFmiPoint &theBottomLeftLatLon,
                                                 const NFmiPoint &theTopRightLatLon,
                                                 bool allowPacificFix) const
{
  try
  {
    if (allowPacificFix)
    {
      PacificPointFixerData fixedPointData =
          NFmiArea::PacificPointFixer(theBottomLeftLatLon, theTopRightLatLon);
      return new NFmiLambertConformalConicArea(fixedPointData.itsBottomLeftLatlon,
                                               fixedPointData.itsTopRightLatlon,
                                               itsCentralLongitude,
                                               itsCentralLatitude,
                                               itsTrueLatitude1,
                                               itsTrueLatitude2,
                                               itsRadius,
                                               fixedPointData.fIsPacific,
                                               TopLeft(),
                                               BottomRight());
    }

    return new NFmiLambertConformalConicArea(theBottomLeftLatLon,
                                             theTopRightLatLon,
                                             itsCentralLongitude,
                                             itsCentralLatitude,
                                             itsTrueLatitude1,
                                             itsTrueLatitude2,
                                             itsRadius,
                                             PacificView(),
                                             TopLeft(),
                                             BottomRight());
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \return Undocumented
 * \todo Should return an boost::shared_ptr instead
 */
// ----------------------------------------------------------------------

NFmiArea *NFmiLambertConformalConicArea::Clone() const
{
  try
  {
    return new NFmiLambertConformalConicArea(*this);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * Equality comparison
 *
 * \param theArea The other object being compared to
 * \return True, if the objects are equal
 */
// ----------------------------------------------------------------------

bool NFmiLambertConformalConicArea::operator==(const NFmiLambertConformalConicArea &theArea) const
{
  try
  {
    if ((itsBottomLeftLatLon == theArea.itsBottomLeftLatLon) &&
        (itsTopRightLatLon == theArea.itsTopRightLatLon) &&
        (itsCentralLongitude == theArea.itsCentralLongitude) &&
        (itsCentralLatitude == theArea.itsCentralLatitude) &&
        (itsTrueLatitude1 == theArea.itsTrueLatitude1) &&
        (itsTrueLatitude2 == theArea.itsTrueLatitude2) && (itsRadius == theArea.itsRadius) &&
        (itsWorldRect == theArea.itsWorldRect))
      return true;

    return false;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * Inequality comparison
 *
 * \param theArea The other object being compared to
 * \return True, if the objects are not equal
 */
// ----------------------------------------------------------------------

bool NFmiLambertConformalConicArea::operator!=(const NFmiLambertConformalConicArea &theArea) const
{
  try
  {
    return !(*this == theArea);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * Equality comparison
 *
 * \param theArea The other object being compared to
 * \return True, if the objects are equal
 */
// ----------------------------------------------------------------------

bool NFmiLambertConformalConicArea::operator==(const NFmiArea &theArea) const
{
  try
  {
    return *this == static_cast<const NFmiLambertConformalConicArea &>(theArea);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * Inequality comparison
 *
 * \param theArea The other object being compared to
 * \return True, if the objects are not equal
 */
// ----------------------------------------------------------------------

bool NFmiLambertConformalConicArea::operator!=(const NFmiArea &theArea) const
{
  try
  {
    return !(*this == theArea);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}
// ----------------------------------------------------------------------
/*!
 * Write the object to the given output stream
 *
 * \param file The output stream to write to
 * \return The output stream written to
 */
// ----------------------------------------------------------------------

std::ostream &NFmiLambertConformalConicArea::Write(std::ostream &file) const
{
  try
  {
    NFmiArea::Write(file);

    file << itsBottomLeftLatLon << itsTopRightLatLon << endl
         << itsCentralLongitude << ' ' << itsCentralLatitude << endl
         << itsTrueLatitude1 << ' ' << itsTrueLatitude2 << endl
         << itsRadius << endl;

    int oldPrec = file.precision();
    file.precision(15);
    file << itsWorldRect << endl;

    file.precision(oldPrec);

    return file;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * Read new object contents from the given input stream
 *
 * \param file The input stream to read from
 * \return The input stream read from
 */
// ----------------------------------------------------------------------

std::istream &NFmiLambertConformalConicArea::Read(std::istream &file)
{
  try
  {
    NFmiArea::Read(file);

    file >> itsBottomLeftLatLon >> itsTopRightLatLon;
    PacificView(NFmiArea::IsPacificView(itsBottomLeftLatLon, itsTopRightLatLon));
    file >> itsCentralLongitude >> itsCentralLatitude >> itsTrueLatitude1 >> itsTrueLatitude2 >>
        itsRadius >> itsWorldRect;
    Init();

    return file;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

NFmiArea *NFmiLambertConformalConicArea::CreateNewArea(const NFmiRect &theRect) const
{
  try
  {
    NFmiPoint bottomLeft(ToLatLon(theRect.BottomLeft()));
    NFmiPoint topRight(ToLatLon(theRect.TopRight()));
    NFmiArea *area = new NFmiLambertConformalConicArea(bottomLeft,
                                                       topRight,
                                                       itsCentralLongitude,
                                                       itsCentralLatitude,
                                                       itsTrueLatitude1,
                                                       itsTrueLatitude2,
                                                       itsRadius,
                                                       false,
                                                       TopLeft(),
                                                       BottomRight());
    return area;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

const std::string NFmiLambertConformalConicArea::AreaStr() const
{
  try
  {
    std::ostringstream out;
    out << "lcc," << itsCentralLongitude << ',' << itsCentralLatitude << ',' << itsTrueLatitude1;
    if (itsTrueLatitude2 != itsTrueLatitude2 || itsRadius != kRearth)
      out << ',' << itsTrueLatitude2 << ',' << itsRadius;
    out << ':' << BottomLeftLatLon().X() << ',' << BottomLeftLatLon().Y() << ','
        << TopRightLatLon().X() << ',' << TopRightLatLon().Y();
    return out.str();
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------

const std::string NFmiLambertConformalConicArea::WKT() const
{
  try
  {
    const char *fmt = R"(PROJCS["FMI_LambertConic",)"
                      R"(GEOGCS["Unknown",)"
                      R"(DATUM["Unknown",SPHEROID["Sphere",{:.0f},0]],)"
                      R"(PRIMEM["Greenwich",0],)"
                      R"(UNIT["Degree",0.0174532925199433]],)"
                      R"(PROJECTION["Lambert_Conformal_Conic_2SP"],)"
                      R"(PARAMETER["latitude_of_origin",{}],)"
                      R"(PARAMETER["central_meridian",{}],)"
                      R"(PARAMETER["standard_parallel_1",{}],)"
                      R"(PARAMETER["standard_parallel_2",{}],)"
                      R"(UNIT["Metre",1.0]])";
    return fmt::format(fmt,
                       itsRadius,
                       itsCentralLatitude,
                       itsCentralLongitude,
                       itsTrueLatitude1,
                       itsTrueLatitude2);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

const NFmiPoint NFmiLambertConformalConicArea::LatLonToWorldXY(
    const NFmiPoint &theLatLonPoint) const
{
  try
  {
    const double lat = FmiRad(theLatLonPoint.Y());

    const double theta = itsN * FmiRad(theLatLonPoint.X() - itsCentralLongitude);
    const double rho = itsRadius * itsF * pow(tan(quart_pi + lat / 2), -itsN);

    const double x = rho * sin(theta);
    const double y = itsRho0 - rho * cos(theta);

    return NFmiPoint(x, y);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

const NFmiPoint NFmiLambertConformalConicArea::WorldXYToLatLon(const NFmiPoint &theXYPoint) const
{
  try
  {
    double lambda = 0;
    double phi = 0;

    double x = theXYPoint.X();
    double y = itsRho0 - theXYPoint.Y();

    double rho = std::hypot(x, y);

    if (rho == 0)
    {
      // Either pole
      phi = (itsN > 0 ? half_pi : -half_pi);
    }
    else
    {
      if (itsN < 0)
      {
        rho = -rho;
        x = -x;
        y = -y;
      }
      phi = 2 * atan(pow(itsRadius * itsF / rho, 1 / itsN)) - half_pi;
      lambda = atan2(x, y) / itsN + FmiRad(itsCentralLongitude);
    }

    return NFmiPoint(FmiDeg(lambda), FmiDeg(phi));
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

const NFmiPoint NFmiLambertConformalConicArea::XYToWorldXY(const NFmiPoint &theXYPoint) const
{
  try
  {
    double xWorld = itsWorldRect.Left() + (theXYPoint.X() - Left()) / itsXScaleFactor;
    double yWorld = itsWorldRect.Bottom() - (theXYPoint.Y() - Top()) / itsYScaleFactor;

    return NFmiPoint(xWorld, yWorld);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

const NFmiPoint NFmiLambertConformalConicArea::WorldXYToXY(const NFmiPoint &theWorldXYPoint) const
{
  try
  {
    double x = itsXScaleFactor * (theWorldXYPoint.X() - itsWorldRect.Left()) + Left();
    double y = Top() - itsYScaleFactor * (theWorldXYPoint.Y() - itsWorldRect.Bottom());
    return NFmiPoint(x, y);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

const NFmiPoint NFmiLambertConformalConicArea::ToLatLon(const NFmiPoint &theXYPoint) const
{
  try
  {
    // Transform local xy-coordinates into world xy-coordinates (meters).

    double xWorld = itsWorldRect.Left() + (theXYPoint.X() - Left()) / itsXScaleFactor;
    double yWorld = itsWorldRect.Bottom() - (theXYPoint.Y() - Top()) / itsYScaleFactor;

    // Transform world xy-coordinates into geodetic coordinates.

    return WorldXYToLatLon(NFmiPoint(xWorld, yWorld));
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

const NFmiPoint NFmiLambertConformalConicArea::ToXY(const NFmiPoint &theLatLonPoint) const
{
  try
  {
    double xLocal, yLocal;

    // Transform input geodetic coordinates into world coordinates (meters) on xy-plane.
    NFmiPoint latlon(FixLongitude(theLatLonPoint.X()), theLatLonPoint.Y());
    NFmiPoint xyWorld(LatLonToWorldXY(latlon));

    if (xyWorld == NFmiPoint::gMissingLatlon)
    {
      return xyWorld;
    }

    // Finally, transform world xy-coordinates into local xy-coordinates
    xLocal = Left() + itsXScaleFactor * (xyWorld.X() - itsWorldRect.Left());
    yLocal = Top() + itsYScaleFactor * (itsWorldRect.Bottom() - xyWorld.Y());

    return NFmiPoint(xLocal, yLocal);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Hash value
 */
// ----------------------------------------------------------------------

std::size_t NFmiLambertConformalConicArea::HashValue() const
{
  try
  {
    std::size_t hash = NFmiArea::HashValue();
    boost::hash_combine(hash, itsBottomLeftLatLon.HashValue());
    boost::hash_combine(hash, itsTopRightLatLon.HashValue());
    boost::hash_combine(hash, boost::hash_value(itsCentralLongitude));
    boost::hash_combine(hash, boost::hash_value(itsCentralLatitude));
    boost::hash_combine(hash, boost::hash_value(itsTrueLatitude1));
    boost::hash_combine(hash, boost::hash_value(itsTrueLatitude2));
    boost::hash_combine(hash, boost::hash_value(itsRadius));
    return hash;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}
// ======================================================================


#include <cstdlib>
#include <iostream>
#include <fstream>
#include <iterator>
#include <string>
#include <vector>
#include <boost/lexical_cast.hpp>
#include <CGAL/Exact_predicates_exact_constructions_kernel_with_root_of.h>
#include <CGAL/Construct_theta_graph_2.h>
#include <CGAL/gnuplot_output_2.h>

// select the kernel type
typedef CGAL::Exact_predicates_exact_constructions_kernel_with_root_of   Kernel;
typedef Kernel::Point_2                   Point_2;
typedef Kernel::Direction_2               Direction_2;
/* Note: due to a bug in the boost library, using a directed graph
 * will cause a compilation error with g++ and clang++ when using c++11 standard.
 * See https://lists.boost.org/Archives/boost/2016/05/229458.php.
 */
// define the graph type
typedef boost::adjacency_list<boost::listS,
                              boost::vecS,
                              boost::undirectedS,
                              Point_2
                             > Graph;

int main(int argc, char ** argv)
{
  unsigned int k=4; // By default, no. of cones==4
  std::string filename="data/n9.cin"; // file used by default

  if (argc > 1 &&
      (!strcmp(argv[1],"-h") || !strcmp(argv[1],"--help") || !strcmp(argv[1],"-?")))
  {
    std::cout << "Usage: " << argv[0] << " <no. of cones> <input filename> [<direction-x> <direction-y>]" << std::endl;
    return 1;
  }

  if (argc > 1)
  {
    k = atoi(argv[1]);
    if (k<2) {
      std::cout << "The number of cones should be larger than 1!" << std::endl;
      return 1;
    }
  }

  if (argc > 2)
  { filename=std::string(argv[2]); }

  // open the file containing the vertex list
  std::ifstream inf(filename);
  if (!inf) {
    std::cout << "Cannot open file " << filename << "!" << std::endl;
    return 1;
  }

  Direction_2 initial_direction;
  if (argc == 1 || argc == 3)
    initial_direction = Direction_2(1, 0);  // default initial_direction
  else if (argc == 5)
    initial_direction = Direction_2(atof(argv[3]), atof(argv[4]));
  else {
    std::cout << "Usage: " << argv[0] << " <no. of cones> <input filename> [<direction-x> <direction-y>]" << std::endl;
    return 1;
  }

  // iterators for reading the vertex list file
  std::istream_iterator<Point_2> input_begin( inf );
  std::istream_iterator<Point_2> input_end;

  // initialize the functor
  CGAL::Construct_theta_graph_2<Kernel, Graph> theta(k, initial_direction);
  // create an adjacency_list object
  Graph g;
  // construct the theta graph on the vertex list
  theta(input_begin, input_end, g);

  // obtain the number of vertices in the constructed graph
  boost::graph_traits<Graph>::vertices_size_type n = boost::num_vertices(g);
  // generate gnuplot files for plotting this graph
  std::string file_prefix = "t" + boost::lexical_cast<std::string>(k) + "n" + boost::lexical_cast<std::string>(n);
  CGAL::gnuplot_output_2(g, file_prefix);

  return 0;
}


#include "voronoi.h"

#include <boost/foreach.hpp>

using namespace std;

namespace voronoi
{

template<class Tp_>
void
VoronoiDiagram<Tp_>::clip_infinite_edge(const edge_type& edge,
    point_type& p0, point_type& p1)
  const
{
  const cell_type& cell1 = *edge.cell();
  const cell_type& cell2 = *edge.twin()->cell();
  point_type origin, direction;

  point_type s1 = sites[cell1.source_index()];
  point_type s2 = sites[cell2.source_index()];
  bg::assign_point(origin, s1);
  bg::add_point(origin, s2);
  bg::divide_value(origin, 2);
  bg::set<0>(direction, bg::get<1>(s1) - bg::get<1>(s2));
  bg::set<1>(direction, bg::get<0>(s2) - bg::get<0>(s1));

  coordinate_type koef = boundsx
    / (std::max)(fabs(bg::get<0>(direction)), fabs(bg::get<1>(direction)));
  if (edge.vertex0() == NULL) {
    p0 = point_type(
        bg::get<0>(origin) - bg::get<0>(direction) * koef,
        bg::get<1>(origin) - bg::get<1>(direction) * koef);
  } else {
    p0 = point_type(edge.vertex0()->x(), edge.vertex0()->y());
  }
  if (edge.vertex1() == NULL) {
    p1 = point_type(
        bg::get<0>(origin) + bg::get<0>(direction) * koef,
        bg::get<1>(origin) + bg::get<1>(direction) * koef);
  } else {
    p1 = point_type(edge.vertex1()->x(), edge.vertex1()->y());
  }
}

// Function object to filter-out users whose sites are already known.
template <class Value>
struct is_unknown {
  const vector<int> &index_set;
  is_unknown(const vector<int> &s) : index_set(s) {}
  inline bool operator()(Value const& p) const {
    return index_set[p.second] == -1;
  }
};

template <class Value, class Tp_>
struct inside_cell {
  typedef typename VoronoiDiagram<Tp_>::cell_type cell_type;
  const VoronoiDiagram<Tp_> &vd;
  const cell_type &cell;
  inside_cell(const VoronoiDiagram<Tp_> &v, const cell_type& c)
    : vd(v), cell(c) {}
  inline bool operator()(Value const& p) const {
    return vd.is_inside(cell, p.first); // point inside cell
  }
};

template<class Tp_>
void
VoronoiDiagram<Tp_>::map_knn(void)
{
  // Construct an R-tree for NN(1) queries on the sites.
  // We use this to efficiently find the site that is nearest to each user.
  typedef pair<point_type, int> rvalue_t;
  vector<rvalue_t> svals;
  int site_idx = 0;
  for (auto site = sites.begin(); site != sites.end(); ++site, ++site_idx)
    svals.push_back(make_pair(
          point_type(bg::get<0>(*site), bg::get<1>(*site)),
          site_idx));
  typedef typename bgi::rtree<rvalue_t, bgi::quadratic<16> > voronoi_tree;
  voronoi_tree vtree(svals.begin(), svals.end());

  // The nearest-neighbor is of course the voronoi cell.
  for (size_t user_idx = 0u; user_idx < users.size(); user_idx++)
  {
    const point_type &user = users[user_idx];
    u2s[user_idx] = vtree.qbegin(bgi::nearest(user, 1))->second;
  }
}

// This is an efficient method for mapping user points to their nearest site,
// using a voronoi diagram and range-tree.
// This runs in O(m log m + (m + n) log n) on average for m sites and n users:
//   O(m log m) worst-case to build the voronoi diagram;
//   O(n log n) worst-case to build the user range tree;
//   O(m log n) average-case to find user points in each cell (see below).
// Since Voronoi cells are convex and the Voronoi edge set is O(m), we can
// use the winding check for containment of a query point in amortized O(1)
// time per cell.
// Each containment check in the worst case must consider O(n) users.
// We improve this to O(log n) on average by using a range tree to filter each
// user query down to points which lie in the cell's bounding box. Of course
// this only works when the cells and users are distributed randomly.
// In the worst case, all Voronoi cells may share the same bounding box, and
// all O(n) user points will be checked for all O(m) cells anyway. However, the
// containment check is short-circuited for those users who have already been
// assigned a cell yet, so the overhead is small for the extra comparisons.
template<class Tp_>
void
VoronoiDiagram<Tp_>::map_quick(void)
{
  // Construct an R-tree for range queries on the user points.
  // We use this to efficiently find the user points that lie within each
  // voronoi cell using its bounding box.
  typedef pair<point_type, int> rvalue_t;
  vector<rvalue_t> uvals;
  int user_idx = 0;
  for (auto user = users.begin(); user != users.end(); ++user, ++user_idx)
    uvals.push_back(make_pair(
          point_type(bg::get<0>(*user), bg::get<1>(*user)),
          user_idx));
  bgi::rtree<rvalue_t, bgi::quadratic<16> > utree(uvals.begin(), uvals.end());

  for (auto it = vd.cells().begin(); it != vd.cells().end(); ++it)
  {
    const cell_type &cell = *it;
    // Map the users that lie inside our cell to this site.
    rect_type bbox = boundingRect(cell);
    vector<rvalue_t> query_users;
    utree.query(
           bgi::satisfies(is_unknown<rvalue_t>(u2s))
        && bgi::within(bbox)
        && bgi::satisfies(inside_cell<rvalue_t, Tp_>(*this, cell)),
        back_inserter(query_users));
    BOOST_FOREACH(rvalue_t const& v, query_users)
    {
      int user_idx = v.second;
      u2s[user_idx] = cell.source_index();
    }
  }
}

/*
// This is a brute-force (slow) method for mapping user points to their cells
// simply by associating each user to the site to which it is nearest.
// This runs in O(m*n) for m sites and n users.
template<class Tp_>
void
VoronoiDiagram<Tp_>::map_slow(void)
{
  u2s = vector<int>(users.size(), -1);
  for (size_t user_idx = 0u; user_idx < users.size(); ++user_idx)
  {
    const Point &user = users[user_idx];
    double mindist = std::numeric_limits<double>::infinity();
    int closest_site_idx = -1;
    for (size_t site_idx = 0u; site_idx < sites.size(); ++site_idx)
    {
      const Point &site = sites[site_idx];
      double distance = normalize(site - user);
      if (distance < mindist) {
        mindist = distance;
        closest_site_idx = static_cast<int>(site_idx);
      }
    }
    u2s[user_idx] = closest_site_idx;
  }
}
*/

template<class Tp_>
void
VoronoiDiagram<Tp_>::build(SearchMethod sm)
{
  vd.clear();

  // Always construct the voronoi diagram from the sites so we have edges.
  boost::polygon::construct_voronoi(sites.begin(), sites.end(), &vd);

  // u2s maps user index to nearest facility index
  u2s = vector<int>(users.size(), -1);

  // Now map users to their cells through whichever method we want.
  switch (sm) {
    /*
    case SearchMethod::Slow:
      this->map_slow(); break;
      */
    case SearchMethod::Quick:
      this->map_quick(); break;
    case SearchMethod::KNN:
    case SearchMethod::Default:
      this->map_knn(); break;
    default:
      throw runtime_error("VoronoiDiagram::build(): bad SearchMethod!");
  };
}

// Common instantiations.
template class VoronoiDiagram<double>;

}


#include "stdafx.h"
#include "Triangle.h"
#include <stdexcept>
#include <string>
#include <sstream>
#include <boost/format.hpp>
#include <math.h>

using namespace std;

namespace
{
const double & AssureNotNegative(const double & value, const string & argName = string())
{
	if (value >= 0)
	{
		return value;
	}
	throw invalid_argument(argName.empty() ? "Argument" : argName + " can not be negative.");
}
}

#define ASSURE_NOT_NEGATIVE(value) AssureNotNegative((value), #value)

CTriangle::CTriangle(double side1, double side2, double side3)
	: m_side1(ASSURE_NOT_NEGATIVE(side1))
	, m_side2(AssureNotNegative(side2, "Side 2"))
	, m_side3(AssureNotNegative(side3, "Side 3"))
{
	ValidateSideLength(side1, side2, side3, 1, 2, 3);
	ValidateSideLength(side2, side1, side3, 2, 1, 3);
	ValidateSideLength(side3, side1, side2, 3, 1, 2);
	//if (side1 > side2 + side3)
	//{
	//	throw domain_error("Side 1 can not be greater than sum of side 2 and side 3");
	//}

	//if (side2 > side1 + side3)
	//{
	//	throw domain_error("Side 2 can not be greater than sum of side 1 and side 3");
	//}

	//if (side3 > side1 + side2)
	//{
	//	throw domain_error("Side 3 can not be greater than sum of side 1 and side 2");
	//}
}

double CTriangle::GetSide1()const
{
	return m_side1;
}

double CTriangle::GetSide2()const
{
	return m_side2;
}

double CTriangle::GetSide3()const
{
	return m_side3;
}

double CTriangle::GetPerimeter()const
{
	return m_side1 + m_side2 + m_side3;
}

double CTriangle::GetArea()const
{
	double p = GetPerimeter() / 2;
	return sqrt(p * (p - m_side1) * (p - m_side2) * (p - m_side3));
}

void CTriangle::ValidateSideLength(
	double sideToCheck, double side1, double side2,
	int checkedSideIndex, int side1Index, int side2Index)
{
	if (sideToCheck > side1 + side2)
	{
		throw domain_error((boost::format("Side %1% can not be greater than sum of side %2% and side %3%")
				% checkedSideIndex % side1Index % side2Index).str());

		//ostringstream msg;
		//msg << "Side " << checkedSideIndex << " can not be greater than sum of side "
		//	<< side1Index << " and side " << side2Index;
		//throw domain_error(msg.str());

		//throw domain_error(
		//	"Side " + to_string(checkedSideIndex) +
		//	" can not be greater than sum of side " + to_string(side1Index) +
		//	" and side " + to_string(side2Index));
	}
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_MATRIX_MAX_ABS_POS_INCLUDE
#define MTL_MATRIX_MAX_ABS_POS_INCLUDE

#include <utility>
#include <cmath>
#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/concept/magnitude.hpp>
#include <boost/numeric/mtl/utility/range_generator.hpp>
#include <boost/numeric/mtl/utility/tag.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

namespace mtl {

namespace mat {
///Returns pair (row, col) from absolut maximal entry of %matrix A
    template <typename Matrix>
    typename mtl::traits::enable_if_matrix<Matrix, std::pair<typename Collection<Matrix>::size_type, typename Collection<Matrix>::size_type> >::type
    inline max_abs_pos(const Matrix& A)
    {
	vampir_trace<3024> tracer;
	namespace traits = mtl::traits;
	using std::abs;
	typedef typename Collection<Matrix>::value_type   value_type;
	typedef typename Collection<Matrix>::size_type    size_type;

	typename RealMagnitude<value_type>::type max(abs(A[0][0]));
	size_type r= 0, c= 0;

	typename traits::row<Matrix>::type             row(A);
	typename traits::col<Matrix>::type             col(A);
	typename traits::const_value<Matrix>::type     value(A);
	typedef typename traits::range_generator<tag::major, Matrix>::type  cursor_type;

	for (cursor_type cursor = begin<tag::major>(A), cend = end<tag::major>(A); cursor != cend; ++cursor) {
	    typedef typename traits::range_generator<tag::nz, cursor_type>::type icursor_type;
	    for (icursor_type icursor = begin<tag::nz>(cursor), icend = end<tag::nz>(cursor); icursor != icend; ++icursor)
		if (abs(value(*icursor)) > max) {
		    max= abs(value(*icursor));
		    r= row(*icursor);
		    c= col(*icursor);
		}
	}

	return std::make_pair(r, c);
    }

} // namespace matrix

namespace vec {
///Returns position from absolut maximal entry of %vector v
    template <typename Vector>
    typename mtl::traits::enable_if_vector<Vector, typename Collection<Vector>::size_type>::type
    inline max_abs_pos(const Vector& v)
    {
	vampir_trace<2011> tracer;
	using std::abs;
	typedef typename Collection<Vector>::size_type    size_type;
	typedef typename Collection<Vector>::value_type   value_type;

	size_type i= 0;
	size_type max_col= size(v);
	value_type max(abs(v[0]));

	for(size_type j= 1; j < max_col; j++)
	    if(abs(v[j]) > max) {
		max = abs(v[j]);
		i= j;
	    }
	return i;
    }

} // namespace vector

} // namespace mtl

#endif // MTL_MATRIX_MAX_ABS_POS_INCLUDE


#ifndef SETTINGS_HPP
#define SETTINGS_HPP

#include <boost/lexical_cast.hpp>
#include <clstatphys/physics/todalattice.hpp>
#include <clstatphys/ensemble/hybrid_monte_carlo.hpp>
#include <clstatphys/ensemble/modeoccupancy_ensemble_periodic_boundary_fftw.hpp>
#include <common/settings_common.hpp>

using Ensembler = ensemble::ModeOccupancyEnsemblePeriodicBoundaryFFTW;
using Hamiltonian = hamiltonian::TodaLattice;
using MonteCarloSamplerOrigin = ensemble::HybridMonteCarlo;

class MonteCarloSampler: public MonteCarloSamplerOrigin{
public:
  template<class Hamiltonian>
  MonteCarloSampler(
            int num_particles, double temperture, double dt_relax,
            double relax_time, int total_accept, Hamiltonian hamiltonian)
    : MonteCarloSamplerOrigin(num_particles, temperture, dt_relax, relax_time, total_accept)
  {hamiltonian_ = hamiltonian;}

  template<class Rand>
  void montecarlo(std::vector<double> & z, Rand & mt){
    int counter = 0;
    MonteCarloSamplerOrigin::montecarlo(z, counter, hamiltonian_, mt);
  }

private:
  Hamiltonian hamiltonian_;
}; //end MonteCarloSampler definition

struct Settings : public SettingsCommon{
  int N_normalmode = 5; //numer of time step
  int k_initial = 0; //start wave vector filled by initialization
  double E_initial = 1.0; // initial energy

  int total_accept = 10;
  double relax_time = 10;
  double dt_relax = 0.1;
  double temperture = 1.0;

  double J = 1.0; //interaction constant;
  double alpha = 1.0; //interaction constant;

  Settings(int argc, char **argv, int & input_counter)
    : SettingsCommon(argc, argv, input_counter)
  {
    set(argc, argv, input_counter);
  }
  Settings() = default;

  inline void set(int argc, char **argv, int & input_counter){
    if (argc > input_counter) E_initial =        boost::lexical_cast<double>(argv[input_counter]);++input_counter;
    if (argc > input_counter) k_initial =        boost::lexical_cast<int>(argv[input_counter]);++input_counter;
    if (argc > input_counter) N_normalmode =     boost::lexical_cast<int>(argv[input_counter]);++input_counter;
    if (Ns < N_normalmode + k_initial){
      std::cerr << "k_initial + N_noramalmode should be lower than Ns" << std::endl;
      std::exit(1);
    }

    if (argc > input_counter) total_accept       = boost::lexical_cast<int>(argv[input_counter]);++input_counter;
    if (argc > input_counter) relax_time         = boost::lexical_cast<double>(argv[input_counter]);++input_counter;
    if (argc > input_counter) dt_relax           = boost::lexical_cast<double>(argv[input_counter]);++input_counter;
    if (argc > input_counter) temperture         = boost::lexical_cast<double>(argv[input_counter]);++input_counter;

    if (argc > input_counter) J =     boost::lexical_cast<double>(argv[input_counter]);++input_counter;
    if (argc > input_counter) alpha = boost::lexical_cast<double>(argv[input_counter]);++input_counter;
  }

  template <class Dataput>
  inline void declare(Dataput & dataput){
    SettingsCommon::declare(dataput);

    dataput <<  "<<System Depend Settings>> " << std::endl
            <<  "  " << Hamiltonian::name() << std::endl
            <<  "  " << Ensembler::name() << std::endl
            <<  "  " << MonteCarloSampler::name() << std::endl

            // declare normalmode ensemble
            << "  Energy initial : E_initial =" << E_initial << std::endl
            << "  Number of non-zero energy normal modes : N_normalmode =" << N_normalmode << std::endl
            << "  Start wave vector filled by initialization : k_initial =" << k_initial << std::endl

            // declare hybrid monte carlo
            << "  Number of step for fianally accept : total_accept = " << total_accept << std::endl
            << "  Relaxtion time for update : relax_time = " << relax_time << std::endl
            << "  Interbal of time development : dt_relax  = " << dt_relax << std::endl
            << "  Temperture : temperture = " << temperture << std::endl

            // declare hamiltonian
            << "  Coupling constant : J = " << J << std::endl
            << "  Coupling constant : alpha = " << alpha << std::endl;
  }

  Hamiltonian hamiltonian(){
    Lattice lattice_t(Ns);

    std::vector<std::vector<int> > pair_table(
                                              num_particles,
                                              std::vector<int>(N_adj)
                                              );
    lattice_t.create_table(pair_table);

    Hamiltonian hamiltonian_t(
                              num_particles,
                              J,
                              alpha,
                              pair_table,
                              N_adj
                              );
    return hamiltonian_t;
  }

  MonteCarloSampler monte_carlo_sampler(){
    MonteCarloSampler monte_carlo_sampler_t(
                                            num_particles,
                                            temperture,
                                            dt_relax,
                                            relax_time,
                                            total_accept,
                                            hamiltonian()
                                            );
    return monte_carlo_sampler_t;
  }

  Ensembler ensembler(){
    Ensembler ensembler_t(
                          num_particles,
                          k_initial,
                          N_normalmode,
                          E_initial
                          );
    return ensembler_t;
  }

};

#endif


/**
 * $Id$
 *
 * Copyright (C)
 * 2015 - $Date$
 *     Martin Wolf <ndhist@martin-wolf.org>
 *
 * This file is distributed under the BSD 2-Clause Open Source License
 * (See LICENSE file).
 *
 */
#include <boost/python.hpp>

#include <ndhist/stats/kurtosis.hpp>

namespace bp = boost::python;

namespace ndhist {
namespace stats {

void register_kurtosis()
{
    bp::def("kurtosis"
      , &py::kurtosis
      , ( bp::arg("hist")
        , bp::arg("axis")=bp::object()
        )
      , "Calculates the kurtosis along the given axis of the given           \n"
        "ndhist object. As in statistics, the kurtosis is defined as         \n"
        ":math:`Kurtosis[x] = (E[x^4] - 4 E[x] E[x^3] + 6 E[x]^2 E[x^2] - 3 E[x]^4) / V[x]^2`.\n"
        "This function generates a projection along the given axis and then  \n"
        "calculates the kurtosis.                                            \n"
        "If ``None`` is given as axis argument (the default), the kurtosis   \n"
        "for all individual axes of the ndhist object is calculated and      \n"
        "returned as a tuple. But if the dimensionality of the histogram is  \n"
        "1, a scalar value is returned.                                      \n"
        "                                                                    \n"
        ".. note:: This function is only defined for ndhist objects with POD \n"
        "          type axis values AND POD type weight values.              \n"
    );
}

}// namespace stats
}// namespace ndhist


/*!
  @file glm.hpp
  @author Klaus K. Holst
  @copyright 2018-2020, Klaus Kähler Holst

  @brief Utility functions for Generalized Linear Models

*/

#pragma once

#ifndef ARMA_R
#define MATHLIB_STANDALONE
#include <armadillo>
#endif
#if defined(ARMA_R)
#include <RcppArmadillo.h>
#endif
#include <cmath>
#include <complex>
#include <cfloat>     // precision of double (DBL_MIN)
#include <functional>  // std::bind for using non-static member function as argument to free function
#include <vector>

using cx_dbl  = std::complex<double>;
using cx_func = std::function<arma::cx_mat(arma::cx_vec theta)>;
using matlist = std::vector<arma::mat>;

namespace target {

  arma::mat expit(arma::mat x);
  arma::cx_mat expit(arma::cx_mat x);
  arma::vec softmax(arma::vec u);
  arma::mat softmax(arma::mat lp, bool ref, bool log);

  // template<typename T>
  // arma::Mat<T> expit(const arma::Mat<T> &x) {
  //   return 1.0/(1+exp(-x));
  // }
  arma::mat expit(arma::mat x);
  arma::cx_mat expit(arma::cx_mat x);

  class IID {
  public:
    arma::mat iid;
    arma::mat vcov;
    IID(): iid(arma::zeros(1, 1)), vcov(arma::zeros(1, 1)) {}
    IID(arma::mat score, arma::mat v): iid(score*v), vcov(v) {}
  };

  IID logistic_iid(const arma::vec &y,
		   const arma::vec &p,
		   const arma::mat &x,
		   const arma::vec &w);

  IID linear_iid(const arma::vec &y,
		 const arma::vec &p,
		 const arma::mat &x,
		 const arma::vec &w);

}  // namespace target


/* Copyright (C) 2010-2019, The Regents of The University of Michigan.
 All rights reserved.

 This software was developed as part of the The Vulcan project in the Intelligent Robotics Lab
 under the direction of Benjamin Kuipers, kuipers@umich.edu. Use of this code is governed by an
 MIT-style License that can be found at "https://github.com/h2ssh/Vulcan".
*/

/**
* \file     isovist.cpp
* \author   Collin Johnson
*
* Implementation of classes:
*
*   - Isovist
*   - IsovistField
*/

#include <utils/isovist.h>
#include <math/covariance.h>
#include <math/geometry/convex_hull.h>
#include <math/moments_features.h>
#include <math/trigonometry.h>
#include <math/zernike_moments.h>
#include <math/geometry/shape_fitting.h>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/accumulators/statistics/skewness.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/iterator/counting_iterator.hpp>
#include <boost/iterator/transform_iterator.hpp>
#include <boost/range/iterator_range.hpp>
#include <cmath>

namespace vulcan
{
namespace utils
{

static math::ZernikeMoments<5> zernike;

double area     (Isovist::Iter begin, Isovist::Iter end);
double perimeter(Isovist::Iter begin, Isovist::Iter end);
double circularity(double area, double perimeter);

template <class BinaryOp> // BinaryOp = double func(Point<double>, Point<double>)
double accumulate_pair(Isovist::Iter begin, Isovist::Iter end, BinaryOp op);

std::string Isovist::scalarName(Scalar scalar)
{
    switch(scalar)
    {
        case kArea:
            return "area";
        case kPerimeter:
            return "perimeter";
        case kCircularity:
            return "circularity";
        case kOrientation:
            return "orientation";
        case kWeightedOrientation:
            return "weighted_orientation";
        case kShapeEccentricity:
            return "eccentricity";
        case kShapeCompactness:
            return "shape compactness";
        case kShapeWaviness:
            return "waviness";
        case kMinDistOrientation:
            return "min_dist_orientation";
        case kMaxThroughDist:
            return "max_through_dist";
        case kMaxThroughDistOrientation:
            return "max_through_dist_orientation";
        case kMinLineDist:
            return "min_line_dist";
        case kMinLineNormal:
            return "min_line_normal";
        case kMinNormalDiff:
            return "min_normal_diff";
        case kDmax:
            return "dist_max";
        case kDmin:
            return "dist_min";
        case kDavg:
            return "dist_avg";
        case kDstd:
            return "dist_std";
        case kDVariation:
            return "dist_variation";
        case kDskewness:
            return "dist_skewness";
        case kShapeDistAvg:
            return "shape_dist_avg";
        case kShapeDistStd:
            return "shape_dist_std";
        case kShapeDistVariation:
            return "shape_dist_variation";
        case kShapeDistCompactness:
            return "shape_dist_compactness";
        case kDeltaAvg:
            return "delta_avg";
        case kDeltaStd:
            return "delta_std";
        case kDeltaVariation:
            return "delta_variation";
        case kDistRelationAvg:
            return "dist_relation_avg";
        case kDistRelationStd:
            return "dist_relation_std";
        case kRayCompactness:
            return "ray compactness";
        case kAngleBetweenMinDists:
            return "angle between min dists";
        case kMinHalfArea:
            return "min half area";
        case kAreaBalance:
            return "area balance";
        default:
            return "unknown";
    }

    return "Unknown";
}

void Isovist::calculateScalars(PointIter begin, PointIter end, std::vector<double>& scalars)
{
    scalars_.resize(kNumScalars + zernike.numMoments());

    calculateDistanceScalars(begin, end, scalars);
    calculateDeltaScalars(begin, end, scalars);
    calculatePolygonScalars(begin, end, scalars);
    calculateMomentsScalars(begin, end, scalars);
}

void Isovist::calculateDistanceScalars(PointIter begin, PointIter end, std::vector<double>& scalars)
{
    using namespace boost::accumulators;

    accumulator_set<double, stats<tag::mean, tag::lazy_variance, tag::skewness, tag::max, tag::min>> distanceAcc;
    accumulator_set<double, stats<tag::mean, tag::max, tag::lazy_variance>> distFromCenterAcc;
    accumulator_set<double, stats<tag::mean, tag::lazy_variance, tag::count>> relationAcc;

    auto shapeCenter = std::accumulate(begin, end, Point<float>(0.0f, 0.0f));
    shapeCenter.x /= std::distance(begin, end);
    shapeCenter.y /= std::distance(begin, end);

    double lastDist = distance_between_points(position_, *(end - 1));
    double minDist = std::numeric_limits<double>::max();
    double minDistOrientation = 0.0;

    for(auto p : boost::make_iterator_range(begin, end))
    {
        double dist = distance_between_points(position_, p);
        distanceAcc(dist);
        distFromCenterAcc(distance_between_points(shapeCenter, p));
        relationAcc(std::abs(dist - lastDist));

        // Keeping track of min dist here for the min dist orientation calculation
        if(dist < minDist)
        {
            minDist = dist;
            minDistOrientation = angle_to_point(position_, p);
        }
        lastDist = dist;
    }

    double secondShortestDist = std::numeric_limits<double>::max();
    double secondShortestOrientation = 0.0;

    // Find the other local minimum. Need to see that it's actually a minimum when doing the computation
    // Only relative distances matter here, so we can use the squared_point_distance for efficiency
    double prevDist = squared_point_distance(position_, *(end-2)); // *begin == *end-1 b/c poly wraps around
    double curDist = squared_point_distance(position_, *begin);

    // Want the min diff to be far enough for the discretization error to not be the source of
    // the next min dist
    const double kMinAngleDiff = std::acos(minDist / (minDist + 0.1));

    for(auto pIt = begin, endIt = end - 1; pIt != endIt; ++pIt)
    {
        double nextDist = (pIt + 1 != endIt) ? squared_point_distance(position_, *(pIt + 1)) :
            squared_point_distance(position_, *begin);

        // If this is a local minima cell, check that it is a little far from the min dist
        // then see if its angle diff is greater than the existing, but it is still within 0.1m
        // of the min dist. If so, it's a discretization error that is causing the miss.
        if((curDist < secondShortestDist)
            && (curDist < nextDist) && (curDist < prevDist))
        {
            // Avoid the angle_to_point except when it matters -- atan2 is expensive!
            double orientation = angle_to_point(position_, *pIt);

            if(angle_diff_abs(orientation, minDistOrientation) > kMinAngleDiff)
            {
                secondShortestDist = curDist;
                secondShortestOrientation = orientation;
            }
        }

        prevDist = curDist;
        curDist = nextDist;
    }

    scalars[kMinDistOrientation] = minDistOrientation;
    scalars[kAngleBetweenMinDists] = angle_diff_abs(minDistOrientation, secondShortestOrientation);

    scalars[kDavg] = mean(distanceAcc);
    scalars[kDstd] = std::sqrt(variance(distanceAcc));
    scalars[kDVariation] = (scalars[kDavg] > 0.0) ? scalars[kDstd] / scalars[kDavg] : 0.0;
    scalars[kDskewness] = skewness(distanceAcc);
    scalars[kDmin] = min(distanceAcc);
    scalars[kDmax] = max(distanceAcc);
    scalars[kShapeDistAvg] = mean(distFromCenterAcc);
    scalars[kShapeDistStd] = std::sqrt(variance(distFromCenterAcc));
    scalars[kShapeDistVariation] = (scalars[kShapeDistAvg] > 0.0) ? scalars[kShapeDistStd] / scalars[kShapeDistAvg] : 0.0;
    scalars[kShapeDistCompactness] = (max(distFromCenterAcc) > 0.0) ? scalars[kShapeDistAvg] / max(distFromCenterAcc) : 1e5;

    scalars[kRayCompactness] = (max(distFromCenterAcc) > 0.0) ? (mean(distFromCenterAcc) / max(distFromCenterAcc)) : 1.0;

    if(count(relationAcc) > 1)
    {
        assert(!std::isnan(mean(relationAcc)));
        scalars[kDistRelationAvg] = mean(relationAcc);
        scalars[kDistRelationStd] = std::sqrt(variance(relationAcc));
    }

    double maxDist = 0.0;
    int maxIdx = 0;

    int otherSide = std::distance(begin, end) / 2;
    for(std::size_t n = 0, end = otherSide; n < end; ++n)
    {
        double dist = squared_point_distance(*(begin + n), *(begin + n + otherSide));
        if(dist > maxDist)
        {
            maxDist = dist;
            maxIdx = n;
        }
    }

    scalars[kMaxThroughDist] = std::sqrt(maxDist);
    scalars[kMaxThroughDistOrientation] = angle_to_point(position_, *(begin + maxIdx));
}

void Isovist::calculateDeltaScalars(PointIter begin, PointIter end, std::vector<double>& scalars)
{
    using namespace boost::accumulators;

    accumulator_set<double, stats<tag::mean, tag::lazy_variance>> deltaAcc;

    auto rayDeltaFunc = [begin, &deltaAcc](int n) {
        deltaAcc(distance_between_points(*(begin + n - 1), *(begin + n)));
    };
    auto rayBegin     = boost::make_counting_iterator<std::size_t>(1);
    auto rayEnd       = boost::make_counting_iterator<std::size_t>(std::distance(begin, end));

    std::for_each(rayBegin, rayEnd, rayDeltaFunc);

    scalars[kDeltaAvg]       = mean(deltaAcc);
    scalars[kDeltaStd]       = std::sqrt(variance(deltaAcc));
    scalars[kDeltaVariation] = (scalars[kDeltaAvg] > 0.0) ? scalars[kDeltaStd] / scalars[kDeltaAvg] : 0.0;
}

void Isovist::calculatePolygonScalars(PointIter begin, PointIter end, std::vector<double>& scalars)
{
    scalars[kArea]        = area(begin, end);
    scalars[kPerimeter]   = perimeter(begin, end);
    scalars[kCircularity] = circularity(scalars[kArea], scalars[kPerimeter]);

    if((scalars[kArea] > 0.0) && (scalars[kPerimeter] > 0.0))
    {
        auto shapeFeatures = math::shape_features(begin, end, scalars[kArea], position_);
        scalars[kWeightedOrientation] = shapeFeatures[math::kShapeWeightedOrientation];
        scalars[kShapeEccentricity] = shapeFeatures[math::kShapeEccentricity];
        scalars[kOrientation] = shapeFeatures[math::kShapeOrientation];
        scalars[kShapeCompactness] = shapeFeatures[math::kShapeCompactness];

        auto hull = math::convex_hull<float>(begin, end);
        scalars[kShapeWaviness] = hull.perimeter() / scalars[kPerimeter];
    }
    else
    {
        scalars[kWeightedOrientation] = 0.0;
        scalars[kShapeEccentricity] = 0.0;
        scalars[kOrientation] = 0.0;
        scalars[kShapeCompactness] = 0.0;
        scalars[kShapeWaviness] = 0.0;
    }
}

void Isovist::calculateMomentsScalars(PointIter begin, PointIter end, std::vector<double>& scalars)
{
    if(scalars[kArea] > 0.0)
    {
        auto zernikeFeatures = zernike.moments<float>(begin,
                                                      end,
                                                      position_,
                                                      scalars[kDmax] * 3);
        std::copy(zernikeFeatures.begin(), zernikeFeatures.end(), scalars.begin() + kNumScalars);
    }
    else
    {
        std::fill(scalars.begin() + kNumScalars, scalars.end(), 0.0);
    }
}

void Isovist::calculateDerivs(std::vector<Point<float>>& endpoints)
{
    // Find the two halves separated by the min dist line. Store those at the end of endpoints. Compute the
    // scalars for each side -- except Zernike. The derivative of each can be easily found with abs of difference.

    std::size_t numPoints = endpoints.size();
    double minDist = HUGE_VAL;
    int minIdx = 0;

    int otherSide = numPoints / 2;
    for(int n = 0, end = otherSide; n < end; ++n)
    {
        double dist = squared_point_distance(endpoints[n], endpoints[n + otherSide]);
        if(dist < minDist)
        {
            minDist = dist;
            minIdx = n;
        }
    }

    scalars_[kMinLineDist] = std::sqrt(minDist);
    scalars_[kMinLineNormal] = angle_sum(angle_to_point(endpoints[minIdx],
                                                                    endpoints[minIdx + otherSide]), M_PI_2);

    // Copy the derivative points on to the end of endpoints. Want to continuous regions, so do the [n,n+otherSide)
    // then [n+otherSide,end) [0,n)
    // add one to num points because the ranges need to wrap around
    endpoints.resize((numPoints + 1) * 2);    // resize to ensure the iterators don't change
    auto halfIt = std::copy(endpoints.begin() + minIdx, endpoints.begin() + minIdx + otherSide, endpoints.begin() + numPoints);
    *halfIt++ = *(endpoints.begin() + numPoints);
    auto endIt = std::copy(endpoints.begin() + minIdx + otherSide, endpoints.begin() + numPoints, halfIt);
    std::copy(endpoints.begin(), endpoints.begin() + minIdx, endIt);
    endpoints.back() = *halfIt;

    std::vector<double> halfScalars(kNumScalars);
    std::vector<double> otherHalfScalars(kNumScalars);
    scalarDerivs_.resize(kNumScalars);

    calculateDistanceScalars(endpoints.begin() + numPoints, halfIt, halfScalars);
    calculateDeltaScalars(endpoints.begin() + numPoints, halfIt, halfScalars);
    calculatePolygonScalars(endpoints.begin() + numPoints, halfIt, halfScalars);
    calculateDistanceScalars(halfIt, endpoints.end(), otherHalfScalars);
    calculateDeltaScalars(halfIt, endpoints.end(), otherHalfScalars);
    calculatePolygonScalars(halfIt, endpoints.end(), otherHalfScalars);

    for(int n = 0; n < kNumScalars; ++n)
    {
        scalarDerivs_[n] = std::abs(halfScalars[n] - otherHalfScalars[n]);
    }

    // Handle wraparound for the angles
    scalarDerivs_[kOrientation] = angle_diff_abs_pi_2(halfScalars[kOrientation], otherHalfScalars[kOrientation]);
    scalarDerivs_[kWeightedOrientation] = angle_diff_abs_pi_2(halfScalars[kWeightedOrientation],
                                                                    otherHalfScalars[kWeightedOrientation]);
    scalarDerivs_[kMinDistOrientation] = angle_diff_abs(halfScalars[kMinDistOrientation],
                                                              otherHalfScalars[kMinDistOrientation]);
    scalarDerivs_[kMaxThroughDistOrientation] = angle_diff_abs(halfScalars[kMaxThroughDistOrientation],
                                                                     otherHalfScalars[kMaxThroughDistOrientation]);

    // Assign features based on the half-isovists
    scalars_[kMinHalfArea] = std::min(halfScalars[kArea], otherHalfScalars[kArea]);
    if(scalars_[kMinHalfArea] > 0.0)
    {
        scalars_[kAreaBalance] = scalars_[kMinHalfArea] / std::max(halfScalars[kArea], otherHalfScalars[kArea]);
    }
    else
    {
        scalars_[kAreaBalance] = 1.0;
    }
}

double area(Isovist::Iter begin, Isovist::Iter end)
{
    double sum = accumulate_pair(begin, end, [](const Point<double>& lhs, const Point<double>& rhs) {
        return lhs.x*rhs.y - lhs.y*rhs.x;
    });
    return std::abs(sum / 2.0);
}

double perimeter(Isovist::Iter begin, Isovist::Iter end)
{
    return accumulate_pair(begin, end, [](const Point<double>& lhs, const Point<double>& rhs) {
        return distance_between_points(lhs, rhs);
    });
}

double circularity(double area, double perimeter)
{
    return (perimeter > 0.0) ? (4.0*M_PI*area) / (perimeter*perimeter) : 1.0;
}

template <class BinaryOp>
double accumulate_pair(Isovist::Iter begin, Isovist::Iter end, BinaryOp op)
{
    if(begin == end)
    {
        return 0.0;
    }

    double sum = 0.0;

    for(auto first = begin, second = begin+1; second < end; ++first, ++second)
    {
        sum += op(*first, *second);
    }

    return sum;
}

} // namespace utils
} // namespace vulcan


// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "OhmConfig.h"

// #define GLM_ENABLE_EXPERIMENTAL
#include <glm/gtx/norm.hpp>
#include <glm/mat3x3.hpp>
#include <glm/vec3.hpp>

// Must come after glm includes due to usage on GPU.
#include "CovarianceVoxel.h"

#include "NdtMap.h"
#include "OccupancyMap.h"
#include "Voxel.h"
#include "VoxelMean.h"
#include "VoxelOccupancy.h"

#include <glm/gtc/type_ptr.hpp>
#include <glm/gtx/matrix_factorisation.hpp>

#ifdef OHM_WITH_EIGEN
#include <Eigen/Dense>
#endif  // OHM_WITH_EIGEN

#include <3esservermacros.h>
#ifdef TES_ENABLE
#include <3esserver.h>
#include <shapes/3esshapes.h>
#endif  // TES_ENABLE

#include <array>

namespace ohm
{
#if OHM_COV_DEBUG
namespace
{
unsigned max_iterations = 0;
double max_error = 0;
}  // namespace
#endif  // OHM_COV_DEBUG

namespace
{
#ifdef OHM_WITH_EIGEN
void covarianceEigenDecompositionEigen(const CovarianceVoxel *cov, glm::dmat3 *eigenvectors, glm::dvec3 *eigenvalues)
{
  // This has been noted to be ~3x faster than the GLM iterative version.
  const glm::dmat3 cov_mat = covarianceMatrix(cov);
  // Both GLM and Eigen are column major. Direct mapping is fine.
  const auto cov_eigen = Eigen::Matrix3d::ConstMapType(glm::value_ptr(cov_mat), 3, 3);

  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(cov_eigen);
  Eigen::Vector3d evals = Eigen::Vector3d::Ones();
  Eigen::Matrix3d evecs = Eigen::Matrix3d::Identity();

  if (eigensolver.info() == Eigen::Success)
  {
    evals = eigensolver.eigenvalues();
    evecs = eigensolver.eigenvectors();
  }
  const double det = evecs.determinant();
  if (det < 0.0)
  {
    evecs.col(0) = -evecs.col(0);  // must be valid rotation matrix (determinant=+1)
  }
  else if (det == 0.0)
  {
    evecs = Eigen::Matrix3d::Identity();
  }

  *eigenvalues = glm::dvec3(evals[0], evals[1], evals[2]);

  for (int r = 0; r < 3; ++r)
  {
    for (int c = 0; c < 3; ++c)
    {
      (*eigenvectors)[c][r] = evecs(r, c);
    }
  }
}
#else  // OHM_WITH_EIGEN
void covarianceEigenDecompositionGlm(const CovarianceVoxel *cov, glm::dmat3 *eigenvectors, glm::dvec3 *eigenvalues)
{
  // This has been noted to be ~3x slower than the Eigen solver.
  glm::dmat3 mat = covarianceMatrix(cov);

  *eigenvectors = glm::dmat3(1.0);  // Identity initialisation

  // Use QR decomposition to implement the QR algorithm.
  // For this algorithm, we iterate as follows:
  //
  // P = cov * cov^T  // cov by cov transpose
  // for i = 1 to N
  //   Q, R = qr_decomposition(P)
  //   P = R * Q
  // end
  //
  // The eigenvalues converge in the diagonal of R.
  // Meanwhile the eigenvectors are found by the product all the Q matrices
  //
  // We set a hard iteration limit, but we also check for convergence with last iteration and may early out.
  const unsigned iteration_limit = 20;
  glm::dmat3 q, r;
  glm::dvec3 eigenvalues_last, eigenvalues_current(0);
  const glm::dvec3 delta_threshold(1e-9);
  for (unsigned i = 0; i < iteration_limit; ++i)
  {
    eigenvalues_last = eigenvalues_current;
#if OHM_COV_DEBUG
    max_iterations = std::max(max_iterations, i + 1);
#endif  // OHM_COV_DEBUG

    glm::qr_decompose(mat, q, r);
    // Progressively refine the eigenvectors.
    *eigenvectors = *eigenvectors * q;
    // Update eigenvalues and check for convergence
    eigenvalues_current[0] = r[0][0];
    eigenvalues_current[1] = r[1][1];
    eigenvalues_current[2] = r[2][2];

    mat = r * q;

    const glm::dvec3 eval_delta = glm::abs(eigenvalues_current - eigenvalues_last);
    if (glm::all(glm::lessThanEqual(eval_delta, delta_threshold)))
    {
      break;
    }
  }

#if OHM_COV_DEBUG
  const glm::dvec3 eval_delta = glm::abs(eigenvalues_current - eigenvalues_last);
  max_error = std::max(eval_delta.x, max_error);
  max_error = std::max(eval_delta.y, max_error);
  max_error = std::max(eval_delta.z, max_error);
#endif  // OHM_COV_DEBUG

  *eigenvalues = eigenvalues_current;
}
#endif  // OHM_WITH_EIGEN
}  // namespace

void covarianceEigenDecomposition(const CovarianceVoxel *cov, glm::dmat3 *eigenvectors, glm::dvec3 *eigenvalues)
{
#ifdef OHM_WITH_EIGEN
  covarianceEigenDecompositionEigen(cov, eigenvectors, eigenvalues);
#else   // OHM_WITH_EIGEN
  covarianceEigenDecompositionGlm(cov, eigenvectors, eigenvalues);
#endif  // OHM_WITH_EIGEN
}

void covarianceEstimatePrimaryNormal(const CovarianceVoxel *cov, glm::dvec3 *normal, int preferred_axis)
{
  glm::dmat3 eigenvectors;
  glm::dvec3 eigenvalues;

  covarianceEigenDecomposition(cov, &eigenvectors, &eigenvalues);

  int smallest_eval_index = preferred_axis;
  for (int i = 0; i < 3; ++i)
  {
    if (eigenvalues[i] < eigenvalues[smallest_eval_index])
    {
      smallest_eval_index = i;
    }
  }

  const double length2 = glm::length2(eigenvectors[smallest_eval_index]);
  // const double confidence = 1.0 - length2;
  *normal = (length2 > 0) ? eigenvectors[smallest_eval_index] / glm::sqrt(length2) : eigenvectors[smallest_eval_index];
  // return confidence;
}

bool covarianceUnitSphereTransformation(const CovarianceVoxel *cov, glm::dquat *rotation, glm::dvec3 *scale)
{
  glm::dmat3 eigenvectors;
  glm::dvec3 eigenvalues;

  covarianceEigenDecomposition(cov, &eigenvectors, &eigenvalues);

  const double det = glm::determinant(eigenvectors);
  if (det < 0.0)
  {
    eigenvectors[0] = -eigenvectors[0];  // must be valid rotation matrix (determinant=+1)
  }
  else if (det == 0.0)
  {
    eigenvectors = glm::dmat3(1.0);
  }

  *rotation = glm::dquat(eigenvectors);
  for (int i = 0; i < 3; ++i)
  {
    const double eval = std::abs(eigenvalues[i]);  // abs just in case.
    const double epsilon = 1e-9;
    (*scale)[i] = (eval > epsilon) ? std::sqrt(eval) : eval;
  }

  return true;
}

#if OHM_COV_DEBUG
#include <iostream>
void covDebugStats()
{
  std::cout << "QR algorithm max iterations: " << max_iterations << std::endl;
  std::cout << "QR algorithm max error: " << max_error << std::endl;
}
#endif  // OHM_COV_DEBUG

void integrateNdtHit(NdtMap &map, const Key &key, const glm::dvec3 &sensor, const glm::dvec3 &sample, bool ndt_tm,
                     const float sample_intensity)
{
  OccupancyMap &occupancy_map = map.map();
  Voxel<float> occupancy_voxel(&occupancy_map, occupancy_map.layout().occupancyLayer(), key);
  Voxel<VoxelMean> mean_voxel(occupancy_voxel, occupancy_map.layout().meanLayer());
  Voxel<CovarianceVoxel> cov_voxel(occupancy_voxel, occupancy_map.layout().covarianceLayer());
  const glm::dvec3 voxel_centre = occupancy_map.voxelCentreGlobal(key);

  assert(occupancy_voxel.isValid());
  assert(mean_voxel.isValid());
  assert(cov_voxel.isValid());

  // Keep clang analysis happy
#ifdef __clang_analyzer__
  if (!occupancy_voxel.voxelMemory() || !mean_voxel.voxelMemory() || !cov_voxel.voxelMemory())
  {
    return;
  }
#endif  // __clang_analyzer__

  // NDT-OM
  CovarianceVoxel cov;
  VoxelMean mean;
  float occupancy;
  occupancy_voxel.read(&occupancy);
  mean_voxel.read(&mean);
  cov_voxel.read(&cov);

  float updated_value = occupancy;
  const glm::dvec3 voxel_pos = position(mean, voxel_centre, occupancy_map.resolution());

  // NDT-TM
  if (ndt_tm)
  {
    Voxel<IntensityMeanCov> intensity_voxel(&occupancy_map, occupancy_map.layout().intensityLayer(), key);
    Voxel<HitMissCount> hit_miss_count_voxel(&occupancy_map, occupancy_map.layout().hitMissCountLayer(), key);

    if (intensity_voxel.isLayerValid() && hit_miss_count_voxel.isLayerValid())
    {
      IntensityMeanCov intensity;
      HitMissCount hit_miss_count;
      intensity_voxel.read(&intensity);
      hit_miss_count_voxel.read(&hit_miss_count);

      const bool reinitialise_permeability_with_covariance = true;  // TODO: make a parameter of map
      calculateHitMissUpdateOnHit(&cov, updated_value, &hit_miss_count, sensor, sample, voxel_pos, mean.count,
                                  unobservedOccupancyValue(), reinitialise_permeability_with_covariance,
                                  map.adaptationRate(), map.sensorNoise(), map.reinitialiseCovarianceThreshold(),
                                  map.reinitialiseCovariancePointCount(), map.ndtSampleThreshold());

      hit_miss_count_voxel.write(hit_miss_count);

      calculateIntensityUpdateOnHit(&intensity, updated_value, sample_intensity, map.initialIntensityCovariance(),
                                    mean.count, map.reinitialiseCovarianceThreshold(),
                                    map.reinitialiseCovariancePointCount());

      intensity_voxel.write(intensity);
    }
  }

  if (calculateHitWithCovariance(&cov, &updated_value, sample, voxel_pos, mean.count, occupancy_map.hitValue(),
                                 unobservedOccupancyValue(), float(occupancy_map.resolution()),
                                 map.reinitialiseCovarianceThreshold(), map.reinitialiseCovariancePointCount()))
  {
    // Covariance matrix has reset. Reset the point count to clear the mean value.
    mean.count = 0;
  }

  // Ensure we update the occupancy within the configured map limits.
  occupancyAdjustUp(
    &occupancy, occupancy, updated_value, unobservedOccupancyValue(), occupancy_map.maxVoxelValue(),
    occupancy_map.saturateAtMinValue() ? occupancy_map.minVoxelValue() : std::numeric_limits<float>::lowest(),
    occupancy_map.saturateAtMaxValue() ? occupancy_map.maxVoxelValue() : std::numeric_limits<float>::max(), false);
  occupancy_voxel.write(occupancy);

  // Update the voxel mean.
  updatePosition(&mean, sample, voxel_centre, occupancy_map.resolution());
  mean_voxel.write(mean);
  cov_voxel.write(cov);
}

void integrateNdtMiss(NdtMap &map, const Key &key, const glm::dvec3 &sensor, const glm::dvec3 &sample, bool ndt_tm)
{
  OccupancyMap &occupancy_map = map.map();
  Voxel<float> occupancy_voxel(&occupancy_map, occupancy_map.layout().occupancyLayer(), key);
  Voxel<const VoxelMean> mean_voxel(occupancy_voxel, occupancy_map.layout().meanLayer());
  Voxel<const CovarianceVoxel> cov_voxel(occupancy_voxel, occupancy_map.layout().covarianceLayer());
  const glm::dvec3 voxel_centre = occupancy_map.voxelCentreGlobal(key);

  assert(occupancy_voxel.isValid());
  assert(mean_voxel.isValid());
  assert(cov_voxel.isValid());

  // Keep clang analysis happy
#ifdef __clang_analyzer__
  if (!occupancy_voxel.voxelMemory() || !mean_voxel.voxelMemory() || !cov_voxel.voxelMemory())
  {
    return;
  }
#endif  // __clang_analyzer__

  // NDT-OM
  CovarianceVoxel cov;
  VoxelMean mean;
  float occupancy;
  occupancy_voxel.read(&occupancy);
  mean_voxel.read(&mean);
  cov_voxel.read(&cov);

  float updated_value = occupancy;
#ifdef TES_ENABLE
  const float initial_value = occupancy;
  glm::dvec3 voxel_maximum_likelihood;
#endif  // TES_ENABLE
  const glm::dvec3 voxel_mean = position(mean, voxel_centre, occupancy_map.resolution());
  bool confirm_miss = false;
  calculateMissNdt(&cov, &updated_value, &confirm_miss, sensor, sample, voxel_mean, mean.count,
                   unobservedOccupancyValue(), occupancy_map.missValue(), map.adaptationRate(), map.sensorNoise(),
                   map.ndtSampleThreshold());

  if (ndt_tm && confirm_miss)
  {
    Voxel<HitMissCount> hit_miss_count_voxel(&occupancy_map, occupancy_map.layout().hitMissCountLayer(), key);
    if (hit_miss_count_voxel.isLayerValid())
    {
      HitMissCount hit_miss_count;
      hit_miss_count_voxel.read(&hit_miss_count);
      ++hit_miss_count.miss_count;
      hit_miss_count_voxel.write(hit_miss_count);
    }
  }

  occupancyAdjustDown(
    &occupancy, occupancy, updated_value, unobservedOccupancyValue(), occupancy_map.minVoxelValue(),
    occupancy_map.saturateAtMinValue() ? occupancy_map.minVoxelValue() : std::numeric_limits<float>::lowest(),
    occupancy_map.saturateAtMaxValue() ? occupancy_map.maxVoxelValue() : std::numeric_limits<float>::max(), false);
  occupancy_voxel.write(occupancy);

#ifdef TES_ENABLE
  if (map.trace())
  {
    const glm::dvec3 voxel_centre = occupancy_map.voxelCentreGlobal(key);
    TES_BOX_W(g_tes, TES_COLOUR(OrangeRed), tes::Id(cov_voxel.voxelMemory()),
              tes::Transform(glm::value_ptr(voxel_centre), glm::value_ptr(glm::dvec3(occupancy_map.resolution()))));
    TES_SERVER_UPDATE(g_tes, 0.0f);

    bool drew_surfel = false;
    glm::dquat rot;
    glm::dvec3 scale;
    if (covarianceUnitSphereTransformation(&cov, &rot, &scale))
    {
      TES_SPHERE(g_tes, TES_COLOUR(SeaGreen), tes::Id(cov_voxel.voxelMemory()),
                 tes::Transform(tes::Vector3d(glm::value_ptr(voxel_mean)), tes::Quaterniond(rot.x, rot.y, rot.z, rot.w),
                                tes::Vector3d(glm::value_ptr(scale))));
      drew_surfel = true;
    }

    // Trace the voxel mean, maximum likelihood point and the ellipsoid.
    // Mean
    const float mean_pos_radius = 0.05f;
    const float likely_pos_radius = 0.1f;
    TES_SPHERE(g_tes, TES_COLOUR(OrangeRed), tes::Id(&voxel_mean),
               tes::Spherical(glm::value_ptr(voxel_mean), mean_pos_radius));
    // Maximum likelihood
    TES_SPHERE_W(g_tes, TES_COLOUR(PowderBlue), tes::Id(&voxel_maximum_likelihood),
                 tes::Spherical(glm::value_ptr(voxel_maximum_likelihood), likely_pos_radius));

    std::array<char, 64> text;  // NOLINT(readability-magic-numbers)
    text[0] = '\0';
    sprintf(text.data(), "P %.3f", valueToProbability(occupancy - initial_value));
    TES_TEXT2D_WORLD(g_tes, TES_COLOUR(White), text.data(), tes::Id(),
                     tes::Spherical(tes::Vector3d(glm::value_ptr(voxel_centre))));

    TES_SERVER_UPDATE(g_tes, 0.0f);
    TES_BOX_END(g_tes, tes::Id(cov_voxel.voxelMemory()));
    TES_SPHERE_END(g_tes, tes::Id(&voxel_mean));
    TES_SPHERE_END(g_tes, tes::Id(&voxel_maximum_likelihood));
    if (drew_surfel)
    {
      TES_SPHERE_END(g_tes, tes::Id(cov_voxel.voxelMemory()));
    }
  }
#endif  // TES_ENABLE
}
}  // namespace ohm