//  Boost common_factor_rt.hpp header file  ----------------------------------//

//  (C) Copyright Daryle Walker and Paul Moore 2001-2002.  Permission to copy,
//  use, modify, sell and distribute this software is granted provided this
//  copyright notice appears in all copies.  This software is provided "as is"
//  without express or implied warranty, and with no claim as to its suitability
//  for any purpose.

// boostinspect:nolicense (don't complain about the lack of a Boost license)
// (Paul Moore hasn't been in contact for years, so there's no way to change the
// license.)

//  See http://www.boost.org for updates, documentation, and revision history.

#ifndef BOOST_MATH_COMMON_FACTOR_RT_HPP
#define BOOST_MATH_COMMON_FACTOR_RT_HPP

#include <boost/math_fwd.hpp>  // self include

#include <boost/config.hpp>  // for BOOST_NESTED_TEMPLATE, etc.
#include <boost/limits.hpp>  // for std::numeric_limits
#include <climits>           // for CHAR_MIN
#include <boost/detail/workaround.hpp>

#ifdef BOOST_MSVC
#pragma warning(push)
#pragma warning(disable:4127 4244)  // Conditional expression is constant
#endif

namespace boost
{
namespace math
{

//  Forward declarations for function templates  -----------------------------//

template < typename IntegerType >
    IntegerType  gcd( IntegerType const &a, IntegerType const &b );

template < typename IntegerType >
    IntegerType  lcm( IntegerType const &a, IntegerType const &b );

//  Greatest common divisor evaluator class declaration  ---------------------//

template < typename IntegerType >
class gcd_evaluator
{
public:
    // Types
    typedef IntegerType  result_type, first_argument_type, second_argument_type;

    // Function object interface
    result_type  operator ()( first_argument_type const &a,
     second_argument_type const &b ) const;

};  // boost::math::gcd_evaluator

//  Least common multiple evaluator class declaration  -----------------------//

template < typename IntegerType >
class lcm_evaluator
{
public:
    // Types
    typedef IntegerType  result_type, first_argument_type, second_argument_type;

    // Function object interface
    result_type  operator ()( first_argument_type const &a,
     second_argument_type const &b ) const;

};  // boost::math::lcm_evaluator

//  Implementation details  --------------------------------------------------//

namespace detail
{
    // Greatest common divisor for rings (including unsigned integers)
    template < typename RingType >
    RingType
    gcd_euclidean
    (
        RingType a,
        RingType b
    )
    {
        // Avoid repeated construction
        #ifndef __BORLANDC__
        RingType const  zero = static_cast<RingType>( 0 );
        #else
        RingType  zero = static_cast<RingType>( 0 );
        #endif

        // Reduce by GCD-remainder property [GCD(a,b) == GCD(b,a MOD b)]
        while ( true )
        {
            if ( a == zero )
                return b;
            b %= a;

            if ( b == zero )
                return a;
            a %= b;
        }
    }

    // Greatest common divisor for (signed) integers
    template < typename IntegerType >
    inline
    IntegerType
    gcd_integer
    (
        IntegerType const &  a,
        IntegerType const &  b
    )
    {
        // Avoid repeated construction
        IntegerType const  zero = static_cast<IntegerType>( 0 );
        IntegerType const  result = gcd_euclidean( a, b );

        return ( result < zero ) ? static_cast<IntegerType>(-result) : result;
    }

    // Greatest common divisor for unsigned binary integers
    template < typename BuiltInUnsigned >
    BuiltInUnsigned
    gcd_binary
    (
        BuiltInUnsigned  u,
        BuiltInUnsigned  v
    )
    {
        if ( u && v )
        {
            // Shift out common factors of 2
            unsigned  shifts = 0;

            while ( !(u & 1u) && !(v & 1u) )
            {
                ++shifts;
                u >>= 1;
                v >>= 1;
            }

            // Start with the still-even one, if any
            BuiltInUnsigned  r[] = { u, v };
            unsigned         which = static_cast<bool>( u & 1u );

            // Whittle down the values via their differences
            do
            {
#if BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x582))
                while ( !(r[ which ] & 1u) )
                {
                    r[ which ] = (r[which] >> 1);
                }
#else
                // Remove factors of two from the even one
                while ( !(r[ which ] & 1u) )
                {
                    r[ which ] >>= 1;
                }
#endif

                // Replace the larger of the two with their difference
                if ( r[!which] > r[which] )
                {
                    which ^= 1u;
                }

                r[ which ] -= r[ !which ];
            }
            while ( r[which] );

            // Shift-in the common factor of 2 to the residues' GCD
            return r[ !which ] << shifts;
        }
        else
        {
            // At least one input is zero, return the other
            // (adding since zero is the additive identity)
            // or zero if both are zero.
            return u + v;
        }
    }

    // Least common multiple for rings (including unsigned integers)
    template < typename RingType >
    inline
    RingType
    lcm_euclidean
    (
        RingType const &  a,
        RingType const &  b
    )
    {
        RingType const  zero = static_cast<RingType>( 0 );
        RingType const  temp = gcd_euclidean( a, b );

        return ( temp != zero ) ? ( a / temp * b ) : zero;
    }

    // Least common multiple for (signed) integers
    template < typename IntegerType >
    inline
    IntegerType
    lcm_integer
    (
        IntegerType const &  a,
        IntegerType const &  b
    )
    {
        // Avoid repeated construction
        IntegerType const  zero = static_cast<IntegerType>( 0 );
        IntegerType const  result = lcm_euclidean( a, b );

        return ( result < zero ) ? static_cast<IntegerType>(-result) : result;
    }

    // Function objects to find the best way of computing GCD or LCM
#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
    template < typename T, bool IsSpecialized, bool IsSigned >
    struct gcd_optimal_evaluator_helper_t
    {
        T  operator ()( T const &a, T const &b )
        {
            return gcd_euclidean( a, b );
        }
    };

    template < typename T >
    struct gcd_optimal_evaluator_helper_t< T, true, true >
    {
        T  operator ()( T const &a, T const &b )
        {
            return gcd_integer( a, b );
        }
    };

    template < typename T >
    struct gcd_optimal_evaluator
    {
        T  operator ()( T const &a, T const &b )
        {
            typedef ::std::numeric_limits<T>  limits_type;

            typedef gcd_optimal_evaluator_helper_t<T,
             limits_type::is_specialized, limits_type::is_signed>  helper_type;

            helper_type  solver;

            return solver( a, b );
        }
    };
#else // BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
    template < typename T >
    struct gcd_optimal_evaluator
    {
        T  operator ()( T const &a, T const &b )
        {
            return gcd_integer( a, b );
        }
    };
#endif

    // Specialize for the built-in integers
#define BOOST_PRIVATE_GCD_UF( Ut )                  \
    template < >  struct gcd_optimal_evaluator<Ut>  \
    {  Ut  operator ()( Ut a, Ut b ) const  { return gcd_binary( a, b ); }  }

    BOOST_PRIVATE_GCD_UF( unsigned char );
    BOOST_PRIVATE_GCD_UF( unsigned short );
    BOOST_PRIVATE_GCD_UF( unsigned );
    BOOST_PRIVATE_GCD_UF( unsigned long );

#ifdef BOOST_HAS_LONG_LONG
    BOOST_PRIVATE_GCD_UF( boost::ulong_long_type );
#elif defined(BOOST_HAS_MS_INT64)
    BOOST_PRIVATE_GCD_UF( unsigned __int64 );
#endif

#if CHAR_MIN == 0
    BOOST_PRIVATE_GCD_UF( char ); // char is unsigned
#endif

#undef BOOST_PRIVATE_GCD_UF

#define BOOST_PRIVATE_GCD_SF( St, Ut )                            \
    template < >  struct gcd_optimal_evaluator<St>                \
    {  St  operator ()( St a, St b ) const  { Ut const  a_abs =   \
    static_cast<Ut>( a < 0 ? -a : +a ), b_abs = static_cast<Ut>(  \
    b < 0 ? -b : +b ); return static_cast<St>(                    \
    gcd_optimal_evaluator<Ut>()(a_abs, b_abs) ); }  }

    BOOST_PRIVATE_GCD_SF( signed char, unsigned char );
    BOOST_PRIVATE_GCD_SF( short, unsigned short );
    BOOST_PRIVATE_GCD_SF( int, unsigned );
    BOOST_PRIVATE_GCD_SF( long, unsigned long );

#if CHAR_MIN < 0
    BOOST_PRIVATE_GCD_SF( char, unsigned char ); // char is signed
#endif

#ifdef BOOST_HAS_LONG_LONG
    BOOST_PRIVATE_GCD_SF( boost::long_long_type, boost::ulong_long_type );
#elif defined(BOOST_HAS_MS_INT64)
    BOOST_PRIVATE_GCD_SF( __int64, unsigned __int64 );
#endif

#undef BOOST_PRIVATE_GCD_SF

#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
    template < typename T, bool IsSpecialized, bool IsSigned >
    struct lcm_optimal_evaluator_helper_t
    {
        T  operator ()( T const &a, T const &b )
        {
            return lcm_euclidean( a, b );
        }
    };

    template < typename T >
    struct lcm_optimal_evaluator_helper_t< T, true, true >
    {
        T  operator ()( T const &a, T const &b )
        {
            return lcm_integer( a, b );
        }
    };

    template < typename T >
    struct lcm_optimal_evaluator
    {
        T  operator ()( T const &a, T const &b )
        {
            typedef ::std::numeric_limits<T>  limits_type;

            typedef lcm_optimal_evaluator_helper_t<T,
             limits_type::is_specialized, limits_type::is_signed>  helper_type;

            helper_type  solver;

            return solver( a, b );
        }
    };
#else // BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
    template < typename T >
    struct lcm_optimal_evaluator
    {
        T  operator ()( T const &a, T const &b )
        {
            return lcm_integer( a, b );
        }
    };
#endif

    // Functions to find the GCD or LCM in the best way
    template < typename T >
    inline
    T
    gcd_optimal
    (
        T const &  a,
        T const &  b
    )
    {
        gcd_optimal_evaluator<T>  solver;

        return solver( a, b );
    }

    template < typename T >
    inline
    T
    lcm_optimal
    (
        T const &  a,
        T const &  b
    )
    {
        lcm_optimal_evaluator<T>  solver;

        return solver( a, b );
    }

}  // namespace detail

//  Greatest common divisor evaluator member function definition  ------------//

template < typename IntegerType >
inline
typename gcd_evaluator<IntegerType>::result_type
gcd_evaluator<IntegerType>::operator ()
(
    first_argument_type const &   a,
    second_argument_type const &  b
) const
{
    return detail::gcd_optimal( a, b );
}

//  Least common multiple evaluator member function definition  --------------//

template < typename IntegerType >
inline
typename lcm_evaluator<IntegerType>::result_type
lcm_evaluator<IntegerType>::operator ()
(
    first_argument_type const &   a,
    second_argument_type const &  b
) const
{
    return detail::lcm_optimal( a, b );
}

//  Greatest common divisor and least common multiple function definitions  --//

template < typename IntegerType >
inline
IntegerType
gcd
(
    IntegerType const &  a,
    IntegerType const &  b
)
{
    gcd_evaluator<IntegerType>  solver;

    return solver( a, b );
}

template < typename IntegerType >
inline
IntegerType
lcm
(
    IntegerType const &  a,
    IntegerType const &  b
)
{
    lcm_evaluator<IntegerType>  solver;

    return solver( a, b );
}

}  // namespace math
}  // namespace boost

#ifdef BOOST_MSVC
#pragma warning(pop)
#endif

#endif  // BOOST_MATH_COMMON_FACTOR_RT_HPP


#include <iostream>
#include <boost/format.hpp>
#include <cmath>

bool bkt_is_ok(float bkt) {
  return (bkt != 60.0)
    && (bkt != -60.0)
    && (bkt != 120.0)
    && (bkt != -120.0)
    && (bkt != 50.0)
    && (bkt != -50.0)
    && (bkt != 100.0)
    && (bkt != -100.0);
}

int main(int argc, char * argv[])
{
  float bkt;
  float prop;
  int count = 0;
  float sum = 0.0;
  float sumsq = 0.0;
  bool flag = true;
  while(flag) {
    std::cin >> bkt >> prop;
    flag = !std::cin.eof();
    if(flag) {
      if(bkt_is_ok(bkt)) {
	count++;
	sum += bkt * prop;
	sumsq += (bkt * bkt * prop);
      }
    }
  }

  float fcount = float(count);
  // variance is E[X^2] - (E[X])^2

  float var = sumsq - sum*sum;
  float sdev = sqrt(var);
  std::cout << boost::format("mean = %g  var = %g  sdev = %g\n")
    % sum % var % sdev;
}


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  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_ACOTPI_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ACOTPI_HPP_INCLUDED

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

  /*!
    @ingroup group-trigonometric
    This function object returns the inverse cotangent in pi multiples.

    @par Header <boost/simd/function/acotpi.hpp>

    @par Note

      For every parameter of floating type `acotpi(x)`
      returns the arc @c r in the interval  \f$[0, 1[\f$ such
      that <tt>cotpi(r) == x</tt>.

    @see acot, acotd, cotpi

    @par Example:

      @snippet acotpi.cpp acotpi

    @par Possible output:

      @snippet acotpi.txt acotpi

  **/
  IEEEValue acotpi(IEEEValue const & x);
} }
#endif

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

#endif


#include "element.h"

#include <Eigen/Eigen>

Element::Element(const Eigen::MatrixXd& V, const Eigen::VectorXi& indices, const Eigen::VectorXi& faces)
: _indices(indices), _faces(faces)
{
	_X = extract(V);
}

Element::~Element()
{

}

double Element::getEnergy() const
{
	return _energy;
}

const Eigen::VectorXi& Element::getVertices() const
{
	return _indices;
}

/////////////////////////////////////////////////////

void Element::addGradient(Eigen::MatrixXd& G) const
{
	const int32_t n = (int32_t)_indices.size();
	for (int32_t i = 0; i < n; i++)
	{
		G.row(_indices[i]) += _G.row(i);
	}
}

void Element::addHessian(Eigen::MatrixXd& H) const
{
	const int32_t n = (int32_t)_indices.size();
	for (int32_t i = 0; i < n; i++)
	{
		H.row(_indices[i]) += _H.row(i);
	}
}

void Element::addFiniteGradient(Eigen::MatrixXd& G) const
{
	const int32_t n = (int32_t)_indices.size();
	for (int32_t i = 0; i < n; i++)
	{
		G.row(_indices[i]) += _fG.row(i);
	}
}

void Element::colorFaces(const Eigen::Vector3d& rgb, Eigen::MatrixXd& C) const
{
	for (int i = 0; i < _faces.size(); i++)
	{
		C.row(_faces[i]) = rgb;
	}
}

/////////////////////////////////////////////////////

Eigen::MatrixXd Element::extract(const Eigen::MatrixXd& V)
{
	const int32_t n = (int32_t)_indices.size();
	Eigen::MatrixXd x = Eigen::MatrixXd::Zero(n, 3);
	for (int32_t i = 0; i < n; i++)
	{
		x.row(i) = V.row(_indices[i]);
	}
	return x;
}


/**
 * @file kf_node.cpp
 * @brief !Valgrind output
 *  Memcheck, a memory error detector
 *  Copyright (C) 2002-2017, and GNU GPL'd, by Julian Seward et al.
 *  Using Valgrind-3.15.0 and LibVEX; rerun with -h for copyright info
 *  Command: ./src/signal_processing/signal_processing_kf_example
 *
 *  HEAP SUMMARY:
 *      in use at exit: 0 bytes in 0 blocks
 *    total heap usage: 2,127 allocs, 2,127 frees, 128,736 bytes allocated
 *
 *  All heap blocks were freed -- no leaks are possible
 *
 *  For lists of detected and suppressed errors, rerun with: -s
 *  ERROR SUMMARY: 0 errors from 0 contexts (suppressed: 0 from 0)
 */

#include <kalman_filter.hpp>
#include <Eigen/Dense>
#include <memory>
#include <iostream>
#include <vector>
#include <ctime>

using namespace filter;

int main(int argc, char* argv[])
{
    int n = 3; // Number of states [position, velocity, acceleration].
    int m = 1; // Number of measurements.
    double dt = 1.0/30; // Timestamp.

    // Declare MAT for Kalman filter computation.
    Eigen::MatrixXd A(n, n); // System dynamics matrix
    Eigen::MatrixXd C(m, n); // Output matrix
    Eigen::MatrixXd Q(n, n); // Process noise covariance
    Eigen::MatrixXd R(m, m); // Measurement noise covariance
    Eigen::MatrixXd P(n, n); // Estimate error covariance

    // Measure the position only.
    A <<
        1, dt, 0,
        0, 1, dt,
        0, 0, 1;

    C << 1, 0, 0;

    // Reasonable covariance matrices
    Q <<
        .05, .05, .0,
        .05, .05, .0,
        .0, .0, .0;

    R << 1;

    P <<
        .1, .1, .1,
        .1, 10000, 10,
        .1, 10, 100;

    // Generate random values from 0 - 1.
    std::srand(std::time(0));
    std::vector<double> measurements;

    for(unsigned int i = 0; i < 100 ; i++)
    {
        double noise = ((double)std::rand() / (double)RAND_MAX);
        measurements.push_back(noise);
    }

    // Initialize Kalman filter.
    auto KF = std::unique_ptr<KalmanFilter>(new KalmanFilter(A, C, Q, R, P));

    // Start the Kalman filter with zero initial parameters.
    KF->init();

    // Feed the measurements to the filter and get estimated states of a system.
    Eigen::VectorXd y(m);
    for(unsigned int i = 0; i < measurements.size(); i++)
    {
        y << measurements[i];
        std::cout << "KF Result 結果: " << KF->compute(y, dt).transpose() << std::endl;

        /** !Output.
         * @brief KF Result 結果:  0.513434 -0.150251 -0.213232
         *
         * @brief [position, velocity, acceleration]
         */
    }

    return EXIT_SUCCESS;
}


// This scripts runs the simulations with two populations (Section 6.1)

#include <src/algorithms/neal2_algorithm.h>
#include <src/algorithms/semihdp_sampler.h>
#include <src/collectors/file_collector.h>
#include <src/collectors/memory_collector.h>
#include <src/includes.h>
#include <src/utils/rng.h>

#include <Eigen/Dense>
#include <stan/math/prim.hpp>
#include <vector>

#include "utils.hpp"

using Eigen::MatrixXd;

std::vector<MatrixXd> simulate_data(double m1, double s1, double m2, double s2,
                                    double w1, double m3, double s3, double m4,
                                    double s4, double w2, int n1, int n2) {
  auto& rng = bayesmix::Rng::Instance().get();
  std::vector<MatrixXd> out(2);
  out[0] = MatrixXd::Zero(n1, 1);
  out[1] = MatrixXd::Zero(n2, 1);

  for (int i = 0; i < n1; i++) {
    if (stan::math::uniform_rng(0, 1, rng) < w1) {
      out[0](i, 0) = stan::math::normal_rng(m1, s1, rng);
    } else {
      out[0](i, 0) = stan::math::normal_rng(m2, s2, rng);
    }
  }

  for (int i = 0; i < n2; i++) {
    if (stan::math::uniform_rng(0, 1, rng) < w2) {
      out[1](i, 0) = stan::math::normal_rng(m3, s3, rng);
    } else {
      out[1](i, 0) = stan::math::normal_rng(m4, s4, rng);
    }
  }
  return out;
}

int main() {
  // Scenario I
  std::vector<MatrixXd> data1 = simulate_data(0.0, 1.0, 5.0, 1.0, 0.5, 0.0,
                                              1.0, 5.0, 1.0, 0.5, 100, 100);

  // Scenario II
  std::vector<MatrixXd> data2 = simulate_data(5.0, 0.6, 10.0, 0.6, 0.9, 5.0,
                                              0.6, 0.0, 0.6, 0.1, 100, 100);

  // Scenario III
  std::vector<MatrixXd> data3 = simulate_data(0.0, 1.0, 5.0, 1.0, 0.8, 0.0,
                                              1.0, 5.0, 1.0, 0.2, 100, 100);

  // data1[1] = data1[0];

  run_semihdp(data1,
              "/home/mario/PhD/exchangeability/semihdp-scripts/"
              "new_chains/s1e1_v2.recordio",
              "/Users/marioberaha/research/bnp/semihdp/semihdp-scripts/semihdp_params.asciipb");
  run_semihdp(data2,
              "/home/mario/PhD/exchangeability/semihdp-scripts/"
              "new_chains/s1e2_v2.recordio",
              "/Users/marioberaha/research/bnp/semihdp/semihdp-scripts/semihdp_params.asciipb");
  run_semihdp(data3,
              "/home/mario/PhD/exchangeability/semihdp-scripts/"
              "new_chains/s1e3_v2.recordio",
              "/Users/marioberaha/research/bnp/semihdp/semihdp-scripts/semihdp_params.asciipb");
}


#include <cmath>
#include <iostream>
#include <boost/multiprecision/float128.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/lambert_w.hpp>
#include <boost/core/demangle.hpp>
#include "quicksvg/ulp_plot.hpp"

using boost::math::lambert_w0;
using boost::math::lambert_wm1;
using boost::math::lambert_w0_prime;
using boost::math::lambert_wm1_prime;
using boost::multiprecision::float128;
using boost::math::constants::exp_minus_one;

int main()
{
    using PreciseReal = float128;
    using CoarseReal = float;
    CoarseReal divider = -0.3667;
    int samples = 15000;
    CoarseReal a = -exp_minus_one<CoarseReal>();
    CoarseReal b = divider;
    std::string title = "ULP accuracy of " + boost::core::demangle(typeid(CoarseReal).name()) + " precision Lambert W₀ on (-1/e, -0.3667)";
    std::cout << title << "\n";
    //title = "";
    std::string filename = "examples/ulp_lambert_w0_1e_3667.svg";
    auto flo = [](CoarseReal x)->CoarseReal { return lambert_w0<CoarseReal>(x); };
    auto fhi = [](PreciseReal x)->PreciseReal { return lambert_w0<PreciseReal>(x); };

    int clip = 3;
    int horizontal_lines = 5;
    int vertical_lines = 5;
    auto ulp_plot = quicksvg::ulp_plot<decltype(fhi), PreciseReal, CoarseReal>(fhi, a, b, true, samples);
    ulp_plot.add_fn(flo);
    ulp_plot.write(filename, clip, true, title, 1100, horizontal_lines, vertical_lines);
    clip = 100;
    filename = "examples/ulp_lambert_w0_1e_3667_clip_" + std::to_string(clip) + ".svg";
    ulp_plot.write(filename, clip, true, title, 1100, horizontal_lines, vertical_lines);

}


#ifndef SM_READER_HH
#define SM_READER_HH

#include <Eigen/Dense>

#include <pcl/common/transforms.h>
#include <pcl/octree/octree_search.h>  // for occlusion detection
// #include <pcl/surface/texture_mapping.h>  // for occlusion detection

/*
You need first 3 parameters that define your camera: the focal length f, and the center of the projection plane: cx, cy. With this you create a 3x3 matrix (I will use matlab syntax):

A = [ fx 0 cx;
	  0 fy cy;
	  0 0  1 ];
You can use something like cx = 0.5 * image_width, cy = 0.5 * image_height, and some value as fx/fy = 800 (try some of them to check how the image looks better).

Then, a 3x4 matrix with the transformation from the camera frame to the point cloud frame:

T = [ r11 r12 r13 tx;
	  r21 r22 r23 ty;
	  r31 r32 r33 tz ];
And finally, your point cloud in homogeneous coordinates, i.e. in a 4xN matrix for a point cloud with N points:

P = [ x1 x2 ... xN;
	  y1 y2 ... yN;
	  z1 z2 ... zN;
	   1  1 ...  1 ];
Now you can project the points:

S = A * T * P;
S is a 3xN matrix where the pixel coordinates of each i-th 3D point are:

x = S(1, i) / S(3, i);
y = S(2, i) / S(3, i);
*/

template <typename PointT>
class PinholeCamera
{
	typedef typename pcl::PCLBase<PointT>::PointCloud PointCloud;
	typedef typename pcl::PCLBase<PointT>::PointCloudConstPtr PointCloudConstPtr;
	typedef typename pcl::octree::OctreePointCloudSearch<PointT> Octree;
	typedef typename Octree::Ptr OctreePtr;

public:
	PinholeCamera(float fx, float fy, float cx, float cy, const Eigen::Matrix4f& camera_pose = Eigen::Matrix4f::Identity()):
		T_(3,4)
	{
		fx_ = fx;
		fy_ = fy;
		cx_ = cx;
		cy_ = cy;

		set_camera_pose(camera_pose);
	}

	bool isPointOccluded (const PointT &pt, OctreePtr octree, double dist_thresh = 0)
	{
	  Eigen::Vector3f direction;
	  direction (0) = pt.x;
	  direction (1) = pt.y;
	  direction (2) = pt.z;

	  std::vector<int> indices;

	  PointCloudConstPtr cloud (new PointCloud());
	  cloud = octree->getInputCloud();

	  double distance_threshold = octree->getResolution();
	  if (dist_thresh > 0)
	  {
	  	distance_threshold = dist_thresh;
	  }

	  // raytrace
	  octree->getIntersectedVoxelIndices(direction, -direction, indices);

	  int nbocc = static_cast<int> (indices.size ());
	  for (size_t j = 0; j < indices.size (); j++)
	  {
	   // if intersected point is on the over side of the camera
	   if (pt.z * cloud->points[indices[j]].z < 0)
	   {
	     nbocc--;
	     continue;
	   }

	   if (std::fabs (cloud->points[indices[j]].z - pt.z) <= distance_threshold)
	   {
	     // points are very close to each-other, we do not consider the occlusion
	     nbocc--;
	   }
	  }

	  if (nbocc == 0)
	   return (false);
	  else
	   return (true);
	}

	// setter methods
	inline void set_fx(float fx)
	{
		fx_ = fx;
	}
	inline void set_fy(float fy)
	{
		fy_ = fy;
	}
	inline void set_cx(float cx)
	{
		cx_ = cx;
	}
	inline void set_cy(float cy)
	{
		cy_ = cy;
	}

	inline void set_image_width(int width)
	{
		image_width_ = width;
	}
	inline void set_image_height(int height)
	{
		image_height_ = height;
	}

	inline void set_camera_pose(const Eigen::Matrix4f& camera_pose)
	{
		camera_pose_ = camera_pose;
	}

	inline void set_input_cloud(const PointCloudConstPtr &cloud)
	{
		cloud_ = cloud;

		has_cloud = true;
	}

	// getter methods
	inline float get_fx() const
	{
		return fx_;
	}
	inline float get_fy() const
	{
		return fy_;
	}
	inline float get_cx() const
	{
		return cx_;
	}
	inline float get_cy() const
	{
		return cy_;
	}
	inline int get_image_width() const
	{
		return image_width_;
	}
	inline int get_image_height() const
	{
		return image_height_;
	}
	inline size_t get_cloud_size() const
	{
		return cloud_->size();
	}
	inline Eigen::Matrix4f get_camera_pose() const
	{
		return camera_pose_;
	}

	void project(cv::Mat& proj_img, Eigen::MatrixXi& uv_idx_map)
	{
		if (!has_cloud)
		{
			printf("Cloud has not yet been added!\n");
			return;
		}
		compute_A();
		compute_T();
		compute_P();

		Eigen::MatrixXf S = A_ * T_ * P_;

		proj_img = cv::Mat::zeros(image_height_, image_width_, CV_8UC3);
		uv_idx_map = Eigen::MatrixXi::Constant(image_height_, image_width_, -1);

		typename pcl::PointCloud<PointT>::Ptr rot_cloud (new pcl::PointCloud<PointT>);
		pcl::transformPointCloud(*cloud_, *rot_cloud, camera_pose_);

		const int N = cloud_->size();
		// store a cache of the distance of all the points to origin
		std::vector<float> pts_distance_cache(N);
		for (int i = 0; i < N; ++i)
		{
			pts_distance_cache[i] = get_pt_distance(rot_cloud->points[i]);
		}

		for (int i = 0; i < N; ++i)
		{
			// std::cout << S(0, i) << std::endl;
			float x = S(0, i) / S(2, i);
			float y = S(1, i) / S(2, i);

			if (x >= image_width_ || x < 0 || y >= image_height_ || y < 0)
				continue;

			const PointT& pt = cloud_->points[i];
			if (!pt_has_nan(pt))
			{
				// printf("%.3f %.3f,", y, x);
				bool updated = false;
				int prev_map_idx = uv_idx_map(y, x);
				if (prev_map_idx == -1)
				{
					uv_idx_map(y, x) = i;
					updated = true;
				} else {
					// if another point has the same uv mapping pixel, compare distance from camera to point and use the closest
					if (pts_distance_cache[i] < pts_distance_cache[prev_map_idx])
					{
						uv_idx_map(y, x) = i;
						updated = true;
					}
				}

				if (updated)
				{
					auto& px = proj_img.at<cv::Vec3b>(y,x);
					px[0] = pt.b;
					px[1] = pt.g;
					px[2] = pt.r;
				}
			}
		}
	}

	void project(cv::Mat& proj_img, std::vector<std::vector<std::vector<int>>>& uv_idx_map)
	{
		if (!has_cloud)
		{
			printf("Cloud has not yet been added!\n");
			return;
		}
		compute_A();
		compute_T();
		compute_P();

		Eigen::MatrixXf S = A_ * T_ * P_;

		proj_img = cv::Mat::zeros(image_height_, image_width_, CV_8UC3);
		uv_idx_map.resize(image_height_);
		for (int y = 0; y < image_height_; ++y)
		{
			uv_idx_map[y].resize(image_width_);
			for (int x = 0; x < image_width_; ++x)
			{
				uv_idx_map[y][x] = {};
			}
		}

		typename pcl::PointCloud<PointT>::Ptr rot_cloud (new pcl::PointCloud<PointT>);
		pcl::transformPointCloud(*cloud_, *rot_cloud, camera_pose_);

		const int N = cloud_->size();
		// store a cache of the distance of all the points to origin
		std::vector<float> pts_distance_cache(N);
		for (int i = 0; i < N; ++i)
		{
			pts_distance_cache[i] = get_pt_distance(rot_cloud->points[i]);
		}

		// Store a cache (as Matrix) of the minimum point distance for each pixel point
		Eigen::MatrixXf uv_min_distance_cache = Eigen::MatrixXf::Constant(image_height_, image_width_, -1);

		for (int i = 0; i < N; ++i)
		{
			// std::cout << S(0, i) << std::endl;
			float x = S(0, i) / S(2, i);
			float y = S(1, i) / S(2, i);

			if (x >= image_width_ || x < 0 || y >= image_height_ || y < 0)
				continue;

			const PointT& pt = cloud_->points[i];
			if (!pt_has_nan(pt))
			{
				uv_idx_map[y][x].push_back(i);

				float pt_dist = pts_distance_cache[i];
				float min_cache_dist = uv_min_distance_cache(y,x);

				if (pt_dist < min_cache_dist || min_cache_dist == -1)
				{
					auto& px = proj_img.at<cv::Vec3b>(y,x);
					px[0] = pt.b;
					px[1] = pt.g;
					px[2] = pt.r;

					uv_min_distance_cache(y,x) = pt_dist;
				}
			}
		}
	}

	/*

	*/
	void project_surface(cv::Mat& proj_img, std::vector<std::vector<std::vector<int>>>& uv_idx_map, float dist_tolerance=0)
	{
		if (!has_cloud)
		{
			printf("Cloud has not yet been added!\n");
			return;
		}
		compute_A();
		compute_T();
		compute_P();

		Eigen::MatrixXf S = A_ * T_ * P_;

		proj_img = cv::Mat::zeros(image_height_, image_width_, CV_8UC3);
		uv_idx_map.resize(image_height_);
		for (int y = 0; y < image_height_; ++y)
		{
			uv_idx_map[y].resize(image_width_);
			for (int x = 0; x < image_width_; ++x)
			{
				uv_idx_map[y][x] = {};
			}
		}

		typename pcl::PointCloud<PointT>::Ptr rot_cloud (new pcl::PointCloud<PointT>);
		pcl::transformPointCloud(*cloud_, *rot_cloud, camera_pose_);

		const int N = cloud_->size();
		// store a cache of the distance of all the points to origin
		std::vector<float> pts_distance_cache(N);
		for (int i = 0; i < N; ++i)
		{
			pts_distance_cache[i] = get_pt_distance(rot_cloud->points[i]);
		}

		// Store a cache (as Matrix) of the minimum point distance for each pixel point
		Eigen::MatrixXf uv_min_distance_cache = Eigen::MatrixXf::Constant(image_height_, image_width_, -1);

		for (int i = 0; i < N; ++i)
		{
			// std::cout << S(0, i) << std::endl;
			float x = S(0, i) / S(2, i);
			float y = S(1, i) / S(2, i);

			if (x >= image_width_ || x < 0 || y >= image_height_ || y < 0)
				continue;

			const PointT& pt = cloud_->points[i];
			if (!pt_has_nan(pt))
			{
				float pt_dist = pts_distance_cache[i];
				float min_cache_dist = uv_min_distance_cache(y,x);

				bool updated = false;
				if (min_cache_dist == -1 || pt_dist < min_cache_dist)
				{
					uv_min_distance_cache(y,x) = pt_dist;
					// update (check previous added points and remove if exceeds dist tolerance) then add this
					auto& cur_indices = uv_idx_map[y][x]; // naturally sorted
					int r = 0;
					for (int ix = 0; ix < cur_indices.size(); ++ix)
					{
						int idx = ix - r;
						if (pts_distance_cache[cur_indices[idx]] > pt_dist + dist_tolerance)
						{
							cur_indices.erase(cur_indices.begin() + idx);
							++r;
						}
					}
					cur_indices.push_back(i);

					auto& px = proj_img.at<cv::Vec3b>(y,x);
					px[0] = pt.b;
					px[1] = pt.g;
					px[2] = pt.r;
				} else if (pt_dist < min_cache_dist + dist_tolerance)
				{
					// add
					uv_idx_map[y][x].push_back(i);
				}
			}
		}
	}

	void project_non_occluded_surface(cv::Mat& proj_img, std::vector<std::vector<std::vector<int>>>& uv_idx_map, float octree_resolution=0.02, float dist_threshold = 0)
	{
		if (!has_cloud)
		{
			printf("Cloud has not yet been added!\n");
			return;
		}
		compute_A();
		compute_T();
		compute_P();

		Eigen::MatrixXf S = A_ * T_ * P_;

		proj_img = cv::Mat::zeros(image_height_, image_width_, CV_8UC3);
		uv_idx_map.resize(image_height_);
		for (int y = 0; y < image_height_; ++y)
		{
			uv_idx_map[y].resize(image_width_);
			for (int x = 0; x < image_width_; ++x)
			{
				uv_idx_map[y][x] = {};
			}
		}

		typename pcl::PointCloud<PointT>::Ptr rot_cloud (new pcl::PointCloud<PointT>);
		pcl::transformPointCloud(*cloud_, *rot_cloud, camera_pose_);

		OctreePtr octree (new Octree(octree_resolution));
		octree->setInputCloud (rot_cloud);
		octree->addPointsFromInputCloud ();

		const int N = cloud_->size();

		for (int i = 0; i < N; ++i)
		{
			// std::cout << S(0, i) << std::endl;
			float x = S(0, i) / S(2, i);
			float y = S(1, i) / S(2, i);

			if (x >= image_width_ || x < 0 || y >= image_height_ || y < 0)
				continue;

			const PointT& pt = cloud_->points[i];
			if (!pt_has_nan(pt))
			{
				// check occlusion
				bool is_occ = isPointOccluded(rot_cloud->points[i], octree, dist_threshold);
				if (!is_occ)
				{
					uv_idx_map[y][x].push_back(i);

					// TODO: only update pixel to closest point
					auto& px = proj_img.at<cv::Vec3b>(y,x);
					px[0] = pt.b;
					px[1] = pt.g;
					px[2] = pt.r;
				}
			}
		}
	}

private:
	template <typename PT>
	static inline bool pt_has_nan(const PT& pt)
	{
		return std::isnan(pt.x) || std::isnan(pt.y) || std::isnan(pt.z);
	}

	inline void compute_A()
	{
		A_ << fx_, 0, cx_, 0, fy_, cy_, 0, 0, 1;
	}
	inline void compute_T()
	{
		for (int i = 0; i < 3; ++i)
		{
			T_(i,0) = camera_pose_(i,0);
			T_(i,1) = camera_pose_(i,1);
			T_(i,2) = camera_pose_(i,2);
			T_(i,3) = camera_pose_(i,3);
		}
	}
	inline void compute_P()
	{
		const int N = cloud_->size();

		P_.resize(4, N);
		for (int i = 0; i < N; ++i)
		{
			const PointT& pt = cloud_->points[i];
			P_(0,i) = pt.x;
			P_(1,i) = pt.y;
			P_(2,i) = pt.z;
			P_(3,i) = 1;
		}
	}

	template <typename Pt>
	inline double get_pt_distance(const Pt& pt)
	{
		return Eigen::Vector3f(pt.x, pt.y, pt.z).squaredNorm();
	}

	float fx_;
	float fy_;
	float cx_;
	float cy_;

	int image_height_ = 800;
	int image_width_ = 800;

	Eigen::Matrix4f camera_pose_;
	PointCloudConstPtr cloud_;

	// computed
	Eigen::Matrix3f A_;
	Eigen::MatrixXf T_;
	Eigen::MatrixXf P_;

	// state
	bool has_cloud = false;

	// algorithms
	// typename pcl::TextureMapping<PointT> tm_;
};

#endif


#ifndef BART_SRC_FORMULATION_SCALAR_DIFFUSION_I_HPP_
#define BART_SRC_FORMULATION_SCALAR_DIFFUSION_I_HPP_

#include <deal.II/lac/full_matrix.h>
#include <deal.II/dofs/dof_accessor.h>

#include "formulation/common/rhs_constant_i.hpp"
#include "system/moments/spherical_harmonic_types.h"
#include "utility/has_description.h"

//! Scalar (non-angular) formulations of the transport equation.
namespace bart::formulation::scalar {

/*! \brief Interface for classes that provide the diffusion formulation of the transport equation.
 *
 * The diffusion formulation is a common non-angular version of the transport equation that models neutron propegation
 * diffusively, much like the heat equation. Therefore, this is naturally a second-order formulation of the transport
 * equation. The diffusion equation is derived by integrating the first-order transport equation over angle and
 * using Fick's law as a closure for the current. The multigroup diffusion equation for a multiplying medium
 * in _k_-eigenvalue form is,
 *
 * \f[
 * -D_g(\vec{r})\vec{\nabla} \cdot \vec{\nabla} \phi_g(\vec{r}) + (\Sigma_{t,g} - \Sigma_{s}^{g\to g})\phi_g(\vec{r})
 * = \sum_{g' \neq g}\Sigma_{g}^{g' \to g}\phi_{g'}(\vec{r}) + \frac{\chi_g}{k}\sum_{g'}\nu_{g'}\Sigma_{f,g'}\phi_{g'}(\vec{r})
 * \f]
 *
 * With weak formulation,
 *
 * \f[
 * \bigg(\nabla \cdot v(\vec{r}), D_g\nabla\cdot\phi_{g}(\vec{r})\bigg)_{K} + \bigg(v(\vec{r}), (\Sigma_{t,g} - \Sigma_{s}^{g\to g})\phi_g(\vec{r})\bigg)_{K}
 * + \bigg(v(\vec{r}), \frac{1}{2}\phi_g(\vec{r})\bigg)_{\partial K, \text{vacuum}} =
 * \bigg(v(\vec{r}),\sum_{g' \neq g}\Sigma_{g}^{g' \to g}\phi_{g'}(\vec{r})\bigg)_{K} +
 * \bigg(v(\vec{r}), \frac{\chi_g}{k}\sum_{g'}\nu_{g'}\Sigma_{f,g'}\phi_{g'}(\vec{r})\bigg)_{K}\;.
 * \f]
 *
 * Each of these terms are stamped individually on the system matrix, \f$\mathbf{A}\f$ or vector \f$\vec{b}\f$
 * by the member functions of classes that derive from this interface. The integration over the element \f$K\f$ is done
 * using the cell quadrature and the right-hand-side scalar fluxes are treated as sources from the previous iteration.
 *
 * For further information about this derivation see any neutron transport text such as
 * Lewis and Miller.
 * @tparam dim spatial dimension
 */
template <int dim>
class DiffusionI : public common::RHSConstantI<dim>, public utility::HasDescription {
 public:
  //! Types of boundaries for the diffusion equation
  enum class BoundaryType {
    kVacuum,
    kReflective
  };

  //! Pointer to a cell iterator returned by a dof object.
  using CellPtr = typename dealii::DoFHandler<dim>::active_cell_iterator;
  using Matrix = dealii::FullMatrix<double>;
  using Vector = dealii::Vector<double>;
  using GroupNumber = int;
  using FaceNumber = int;

  virtual ~DiffusionI() = default;

  /*! \brief Precalculate many of the shape functions.
   *
   * The bilinear terms require the shape-funtion or gradient of the shape function squared. This function precalculates
   * those, reducing the number of times the underlying finite element object needs to be called. The shape functions
   * for each cell are identical, and the jacobian is used to translate from the base cell.
   *
   * @param cell_ptr an arbitrary cell to use, often just the beginning of active cells.
   */
  virtual auto Precalculate(const CellPtr& cell_ptr) -> void = 0;

  /*! \brief Integrates the bilinear streaming term over a cell and fills a given matrix.
   *
   * For a given cell in the triangulation, \f$K \in T_K\f$, with basis functions
   * \f$\varphi\f$, this function integrates the bilinear SAAF streaming term for
   * one group using the cell quadrature and adds them to the provided
   * local cell matrix, \f$\mathbf{A}\f$:
   *
   * \f[
   * \mathbf{A}(i,j)_{K,g}' = \mathbf{A}(i,j)_{K,g} + \int_{K}D_g\nabla\varphi_i(\vec{r})\nabla\varphi_j(\vec{r})dV
   * \f]
   *
   * @param to_fill
   */
  virtual auto FillCellStreamingTerm(Matrix& to_fill, const CellPtr&, GroupNumber) const -> void = 0;
  /*! \brief Integrates the bilinear collision term over a cell and fills a given matrix.
   *
   * For a given cell in the triangulation, \f$K \in T_K\f$, with basis functions
   * \f$\varphi\f$, this function integrates the bilinear SAAF streaming term for
   * one group using the cell quadrature and adds them to the provided
   * local cell matrix, \f$\mathbf{A}\f$:
   *
   * \f[
   * \mathbf{A}(i,j)_{K,g}' = \mathbf{A}(i,j)_{K,g} + \int_{K}\varphi_i(\vec{r})(\Sigma_{t,g} - \Sigma_{s, g \to g})\varphi_j(\vec{r})dV
   * \f]
   *
   * @param to_fill
   */
  virtual auto FillCellCollisionTerm(Matrix& to_fill, const CellPtr&, GroupNumber) const -> void = 0;
/*! \brief Integrates the bilinear boundary term over a cell and fills a given matrix.
   *
   * For a given cell and face in the triangulation, \f$\partial K \in \partial T_K\f$, with basis functions
   * \f$\varphi\f$, this function integrates the bilinear diffusion boundary term for
   * one group using the cell quadrature and adds them to the provided
   * local cell matrix, \f$\mathbf{A}\f$. For reflective boundary conditions, nothing is added, for vacuum:
   *
   * \f[
   * \mathbf{A}(i,j)_{K,g}' = \mathbf{A}(i,j)_{K,g} + \frac{1}{2}\int_{\partial K}\varphi_i(\vec{r})\varphi_j(\vec{r})dV
   * \f]
   *
   * @param to_fill
   */
  virtual auto FillBoundaryTerm(Matrix& to_fill, const CellPtr&, FaceNumber, BoundaryType) const -> void = 0;

  /*! \brief Integrates the fixed source term and fills a given vector.
   *
   * For a given cell in the triangulation, \f$K \in T_K\f$, with basis functions
   * \f$\varphi\f$, this function integrates the diffusion fixed-source term and
   * adds it to the cell right-hand side vector.
   * \f[
   * \vec{b}(i)_{K,g}' = \vec{b}(i)_{K,g} + \int_{K}q_{g}(\vec{r})\varphi_i(\vec{r})dV
   * \f]
   *
   * where \f$\phi\f$ is the scalar flux. Adds the result per cell DOFF to the
   * input-output vector cell_rhs.
   *
   *
   * @param to_fill cell vector to fill
   */
  virtual auto FillCellFixedSource(Vector& to_fill, const CellPtr&, GroupNumber) const -> void = 0;

  /*! \brief Integrates the fission source term and fills a given cell vector.
   *
   * For a given cell in the triangulation, \f$K \in T_K\f$, with basis functions
   * \f$\varphi\f$, this function integrates the diffusion fission-source term and
   * adds it to the cell right-hand side vector.
   * \f[
   * \vec{b}(i)_{K,g}' = \vec{b}(i)_{K,g} + \frac{\chi_g}{k}\sum_{g'}\int_{K}\varphi_i(\vec{r})\Sigma_{f,g'}\nu_{g'}\phi_{g'}(\vec{r})dV
   * \f]
   *
   * @param to_fill cell vector to fill
   * @param k_eigenvalue value of the _k_-eigenvalue
   * @param in_group_moment scalar flux moment for the current group
   * @param group_moments scalar flux moment for all other groups
   */
  virtual auto FillCellFissionSource(Vector& to_fill, const CellPtr&, GroupNumber, double k_eigenvalue,
                                     const system::moments::MomentVector& in_group_moment,
                                     const system::moments::MomentsMap& group_moments) const -> void = 0;

  /*! \brief Integrates the scattering source term and fills a given cell vector.
   *
   * For a given cell in the triangulation, \f$K \in T_K\f$, with basis functions
   * \f$\varphi\f$, this function integrates the diffusion scattering-source term and
   * adds it to the cell right-hand side vector.
   * \f[
   * \vec{b}(i)_{K,g}' = \vec{b}(i)_{K,g} + \sum_{g' \neq g}\int_{K}\varphi_i(\vec{r})\Sigma_{s,g' \to g}\phi_{g'}(\vec{r})dV
   * \f]
   *
   * @param to_fill cell vector to fill
   * @param k_eigenvalue value of the _k_-eigenvalue
   * @param group_moments scalar flux moment for all groups
   */
  virtual auto FillCellScatteringSource(Vector& to_fill, const CellPtr&, GroupNumber,
                                        const system::moments::MomentsMap& group_moments) const -> void = 0;

  /*! \brief Returns a bool indicating if Precalculate been called. */
  virtual auto is_initialized() const -> bool = 0;
};

} // namespace bart::formulation::scalar

#endif //BART_SRC_FORMULATION_SCALAR_DIFFUSION_I_HPP_


//  Copyright 2020 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt

#include <iostream>
#include <benchmark/benchmark.h>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/multiprecision/gmp.hpp>
#include <boost/multiprecision/integer.hpp>
#include <boost/random.hpp>
#include <cmath>

#include <immintrin.h>

using namespace boost::multiprecision;
using namespace boost::random;

template <class Integer>
BOOST_MP_CXX14_CONSTEXPR Integer sqrt_old(const Integer& x, Integer& r)
{
   //
   // This is slow bit-by-bit integer square root, see for example
   // http://en.wikipedia.org/wiki/Methods_of_computing_square_roots#Binary_numeral_system_.28base_2.29
   // There are better methods such as http://hal.inria.fr/docs/00/07/28/54/PDF/RR-3805.pdf
   // and http://hal.inria.fr/docs/00/07/21/13/PDF/RR-4475.pdf which should be implemented
   // at some point.
   //
   Integer s = 0;
   if (x == 0)
   {
      r = 0;
      return s;
   }
   int g = msb(x);
   if (g == 0)
   {
      r = 1;
      return s;
   }

   Integer t = 0;
   r         = x;
   g /= 2;
   bit_set(s, g);
   bit_set(t, 2 * g);
   r = x - t;
   --g;
   do
   {
      t = s;
      t <<= g + 1;
      bit_set(t, 2 * g);
      if (t <= r)
      {
         bit_set(s, g);
         r -= t;
      }
      --g;
   } while (g >= 0);
   return s;
}

template <class Integer>
BOOST_MP_CXX14_CONSTEXPR Integer sqrt_old(const Integer& x)
{
   Integer r(0);
   return sqrt_old(x, r);
}

template <class T>
std::tuple<std::vector<T>, std::vector<T> >& get_test_vector(unsigned bits)
{
   static std::map<unsigned, std::tuple<std::vector<T>, std::vector<T> > > data;

   std::tuple<std::vector<T>, std::vector<T> >& result = data[bits];

   if (std::get<0>(result).size() == 0)
   {
      mt19937                     mt;
      uniform_int_distribution<T> ui(T(1) << (bits - 1), T(1) << bits);

      std::vector<T>& a = std::get<0>(result);
      std::vector<T>& b = std::get<1>(result);

      for (unsigned i = 0; i < 1000; ++i)
      {
         a.push_back(ui(mt));
         b.push_back(0);
      }
   }
   return result;
}

template <class T>
std::vector<T>& get_test_vector_a(unsigned bits)
{
   return std::get<0>(get_test_vector<T>(bits));
}
template <class T>
std::vector<T>& get_test_vector_b(unsigned bits)
{
   return std::get<1>(get_test_vector<T>(bits));
}

template <typename T>
static void BM_sqrt_old(benchmark::State& state)
{
   int                         bits = state.range(0);

   std::vector<T>& a = get_test_vector_a<T>(bits);
   std::vector<T>& b = get_test_vector_b<T>(bits);

   for (auto _ : state)
   {
      for (unsigned i = 0; i < a.size(); ++i)
         b[i] = sqrt_old(a[i]);
   }
   state.SetComplexityN(bits);
}

template <typename T>
static void BM_sqrt_current(benchmark::State& state)
{
   int bits = state.range(0);

   std::vector<T>& a = get_test_vector_a<T>(bits);
   std::vector<T>& b = get_test_vector_b<T>(bits);

   for (auto _ : state)
   {
      for (unsigned i = 0; i < a.size(); ++i)
         b[i] = sqrt(a[i]);
   }
   state.SetComplexityN(bits);
}

constexpr unsigned lower_range = 512;
constexpr unsigned upper_range = 1 << 15;

BENCHMARK_TEMPLATE(BM_sqrt_old, cpp_int)->RangeMultiplier(2)->Range(lower_range, upper_range)->Unit(benchmark::kMillisecond)->Complexity();
BENCHMARK_TEMPLATE(BM_sqrt_current, cpp_int)->RangeMultiplier(2)->Range(lower_range, upper_range)->Unit(benchmark::kMillisecond)->Complexity();
BENCHMARK_TEMPLATE(BM_sqrt_current, mpz_int)->RangeMultiplier(2)->Range(lower_range, upper_range)->Unit(benchmark::kMillisecond)->Complexity();

BENCHMARK_MAIN();


/*
 * Copyright 2020-2021 CNRS-UM LIRMM, CNRS-AIST JRL
 */

#pragma once

#include <Eigen/Core>

namespace cdm {

/*! \brief Generate a block diagonal matrix.
 * \tparam NBlock Number of the block of the matrix.
 * \param blockMat Block matrix to repeat.
 * \return Block diagonal matrix.
 */
template <int NBlock>
Eigen::MatrixXd makeDiag(const Eigen::MatrixXd& blockMat)
{
    static_assert(NBlock >= 0, "Not yet ready for dynamic");
    constexpr int N = NBlock;
    Eigen::MatrixXd out = Eigen::MatrixXd::Zero(N * blockMat.rows(), N * blockMat.cols());
    for (int i = 0; i < N; ++i)
        out.block(i * blockMat.rows(), i * blockMat.cols(), blockMat.rows(), blockMat.cols()) = blockMat;

    return out;
}

template <int NVec>
Eigen::MatrixXd generateD(const CrossN<NVec>& cx)
{
    using mat_t = Eigen::Matrix<double, 6 * NVec, 6 * NVec>;
    using sub_mat_t = Eigen::Matrix<double, 6, 6 * NVec>;
    Eigen::MatrixXd D_N = Eigen::MatrixXd::Zero(6 * (NVec - 1), 6 * (NVec - 1));
    for (int i = 0; i < NVec - 1; ++i)
        D_N.block<6, 6>(6 * i, 6 * i) = Eigen::Matrix6d::Identity() / (i + 1);
    return mat_t::Identity() + (mat_t() << sub_mat_t::Zero(), D_N * cx.dualMatrix().template topRows<6 * (NVec - 1)>()).finished();
}

// M = I + Cd * I * C
template <int Order>
std::vector<Eigen::MatrixXd> getSubTreeInertia(const Model& m, const ModelConfig<Order>& mc)
{
    static_assert(Order > 0, "Not yet ready for dynamic");
    std::vector<Eigen::MatrixXd> M(static_cast<size_t>(m.nLinks()), Eigen::MatrixXd::Zero(6 * Order, 6 * Order));
    for (Index i = m.nLinks() - 1; i >= 0; --i) {
        size_t ui = static_cast<size_t>(i);
        M[ui] += makeDiag<Order>(m.body(i).inertia().matrix());
        Index p = m.jointParent(i);
        if (p != -1) {
            size_t up = static_cast<size_t>(p);
            auto C_p_b = mc.bodyMotions[up].inverse() * mc.bodyMotions[ui];
            M[up] += C_p_b.template dualMatrix<Order>() * M[ui] * C_p_b.inverse().template matrix<Order>();
        }
    }

    return M;
}

} // namespace cdm


// Copyright (C) 2019 David Harmon and Artificial Necessity
// This code distributed under zlib, see LICENSE.txt for terms.

#include "triangle_energies.hpp"

#include <Eigen/Dense>
#include <Eigen/SVD>

TriangleOrthoStrain::TriangleOrthoStrain(const Eigen::Vector3i& idxs,
                                         const std::vector<Eigen::Vector3d>& x,
                                         double ksx, double ksy,
                                         bool ignore_compression)
    : idxs_(idxs), ignore_compression_(ignore_compression)  {

    Matrix3x2 Ds;
    Ds.col(0) = x[1] - x[0];
    Ds.col(1) = x[2] - x[0];

    // Take n1 as just the first edge, n2 as the orthogonal vector closest to second edge
    Eigen::Vector3d n1 = Ds.col(0).normalized();
    Eigen::Vector3d n2 = (Ds.col(1) - Ds.col(1).dot(n1) * n1).normalized();

    Matrix3x2 Dm;
    Dm.col(0) = n1;
    Dm.col(1) = n2;

    Eigen::Matrix2d F = Dm.transpose() * Ds;
    rest_ = F.inverse();

    A_ = F.determinant() * 0.5;

    ksx = std::sqrt(ksx * A_);
    ksy = std::sqrt(ksy * A_);

    weights_ = Eigen::VectorXd(6);
    weights_ << ksx, ksx, ksx, ksy, ksy, ksy;

    S_.setZero();
    S_(0,0) = -1; S_(0,1) = -1;
    S_(1,0) =  1; S_(2,1) =  1;
}

void TriangleOrthoStrain::get_reduction(std::vector<Eigen::Triplet<double>> &triplets) const {
    const int cols[3] = { 3*idxs_[0], 3*idxs_[1], 3*idxs_[2] };

    Matrix3x2 D = S_ * rest_;
    for (int i=0; i<3; ++i) {
        for (int j=0; j<3; ++j) {
            triplets.emplace_back(i, cols[j]+i, D(j,0));
            triplets.emplace_back(3+i, cols[j]+i, D(j,1));
        }
    }
}

Eigen::VectorXd TriangleOrthoStrain::reduce(const Eigen::VectorXd& x) const {
    const int cols[3] = { 3*idxs_[0], 3*idxs_[1], 3*idxs_[2] };

    Vector6d z = Vector6d::Zero();

    Matrix3x2 D = S_ * rest_;
    for (int i=0; i<3; ++i ) {
        for (int j=0; j<3; j++) {
            z[i]   += D(j,0) * x[cols[j]+i];
            z[3+i] += D(j,1) * x[cols[j]+i];
        }
    }

    return z;
}

void TriangleOrthoStrain::project(Eigen::VectorXd& zi) const {
    Eigen::JacobiSVD<Matrix3x2>
        svd(Eigen::Map<Matrix3x2>(zi.data()), Eigen::ComputeThinU | Eigen::ComputeThinV);

    //Eigen::Vector2d S = Eigen::Vector2d::Ones();
    //if (ignore_compression_) {
    //    for (int i=0; i<2; i++) {
    //        S[i] = std::min(S[i], svd.singularValues()[i]);
    //    }
    //}

    //Matrix3x2 P = svd.matrixU().leftCols(2) * S.asDiagonal() * svd.matrixV().transpose();

    Matrix3x2 P = svd.matrixU().leftCols(2) * svd.matrixV().transpose();
    zi = 0.5 * (Eigen::Map<Vector6d>(P.data()) + zi);
}


//! \file examples/Arrangement_on_surface_2/circular_line_arc.cpp
// Using the circular line arc traits.

#include <CGAL/Cartesian.h>
#include <CGAL/point_generators_2.h>
#include <CGAL/MP_Float.h>
#include <CGAL/Algebraic_kernel_for_circles_2_2.h>
#include <CGAL/intersections.h>
#include <CGAL/Circular_kernel_2.h>
#include <CGAL/Arr_circular_line_arc_traits_2.h>
#include <CGAL/Arrangement_2.h>
#include <CGAL/Arr_naive_point_location.h>
#include <boost/variant.hpp>

#include <CGAL/Random.h>

typedef CGAL::Quotient<CGAL::MP_Float>                      NT;
typedef CGAL::Cartesian<NT>                                 Linear_k;
typedef CGAL::Algebraic_kernel_for_circles_2_2<NT>          Algebraic_k;
typedef CGAL::Circular_kernel_2<Linear_k,Algebraic_k>       Circular_k;

typedef Circular_k::Point_2                                 Point_2;
typedef Circular_k::Circle_2                                Circle_2;
typedef Circular_k::Circular_arc_2                          Circular_arc_2;
typedef Circular_k::Line_arc_2                              Line_arc_2;

typedef boost::variant< Circular_arc_2, Line_arc_2>         Arc_2;
typedef std::vector< Arc_2>                                 ArcContainer;

typedef CGAL::Arr_circular_line_arc_traits_2<Circular_k>    Traits;

typedef CGAL::Arrangement_2<Traits>                         Arrangement;
typedef CGAL::Arr_naive_point_location<Arrangement>         Point_location;

int main() {
  CGAL::Random generatorOfgenerator;
  int random_seed = generatorOfgenerator.get_int(0, 123456);
  std::cout << "random_seed = " << random_seed << std::endl;
  CGAL::Random theRandom(random_seed);
  int random_max = 128;
  int random_min = -128;
  ArcContainer ac;
  int x1, y1, x2, y2;

  for (int i = 0; i < 10; i++) {
    x1 = theRandom.get_int(random_min,random_max);
    y1 = theRandom.get_int(random_min,random_max);
    do{
      x2 = theRandom.get_int(random_min,random_max);
      y2 = theRandom.get_int(random_min,random_max);
    } while((x1 == x2) && (y1 == y2));

    std::cout << x1 << " " << y1 << " " << x2 << " " << y2 << std::endl;
    boost::variant< Circular_arc_2, Line_arc_2 > v =
      Line_arc_2(Point_2(x1,y1), Point_2(x2,y2));
    ac.push_back( v);
  }

  for (int i = 0; i < 10; i++) {
    do{
      x1 = theRandom.get_int(random_min,random_max);
      y1 = theRandom.get_int(random_min,random_max);
    }
    while(x1==0 && y1==0);
    boost::variant< Circular_arc_2, Line_arc_2 > v =
      Circle_2( Point_2(x1,y1), x1*x1 + y1*y1);
    ac.push_back(v);
  }

  Arrangement arr;
  Point_location _pl(arr);
  for (ArcContainer::const_iterator it = ac.begin(); it != ac.end(); ++it) {
    //insert(arr,_pl,*it);
    insert(arr, *it, _pl);
  };

  return 0;
}


#include <NTL/mat_GF2.h>
#include <NTL/vec_long.h>

NTL_START_IMPL

void add(mat_GF2& X, const mat_GF2& A, const mat_GF2& B)
{
   long n = A.NumRows();
   long m = A.NumCols();

   if (B.NumRows() != n || B.NumCols() != m)
      LogicError("matrix add: dimension mismatch");

   X.SetDims(n, m);

   long mw = (m + NTL_BITS_PER_LONG - 1)/NTL_BITS_PER_LONG;

   long i;
   for (i = 0; i < n; i++) {
      _ntl_ulong *xp = X[i].rep.elts();
      const _ntl_ulong *ap = A[i].rep.elts();
      const _ntl_ulong *bp = B[i].rep.elts();
      long j;
      for (j = 0; j < mw; j++)
         xp[j] = ap[j] ^ bp[j];
   }
}

static
void mul_aux(vec_GF2& x, const mat_GF2& A, const vec_GF2& b)
{
   long n = A.NumRows();
   long l = A.NumCols();

   if (l != b.length())
      LogicError("matrix mul: dimension mismatch");

   x.SetLength(n);

   long i;

   for (i = 0; i < n; i++) {
      x.put(i, A[i] * b);
   }
}

void mul(vec_GF2& x, const mat_GF2& A, const vec_GF2& b)
{
   if (&b == &x || A.alias(x)) {
      vec_GF2 tmp;
      mul_aux(tmp, A, b);
      x = tmp;
   }
   else
      mul_aux(x, A, b);
}

static
void mul_aux(vec_GF2& x, const vec_GF2& a, const mat_GF2& B)
{
   long n = B.NumRows();
   long l = B.NumCols();

   if (n != a.length())
      LogicError("matrix mul: dimension mismatch");

   x.SetLength(l);
   clear(x);

   const _ntl_ulong *ap = a.rep.elts();
   _ntl_ulong a_mask = 1;

   _ntl_ulong *xp = x.rep.elts();

   long lw = (l + NTL_BITS_PER_LONG - 1)/NTL_BITS_PER_LONG;

   long i;
   for (i = 0; i < n; i++) {
      if (*ap & a_mask) {
         const _ntl_ulong *bp = B[i].rep.elts();
         long j;
         for (j = 0; j < lw; j++)
            xp[j] ^= bp[j];
      }

      a_mask <<= 1;
      if (!a_mask) {
         a_mask = 1;
         ap++;
      }
   }
}

void mul(vec_GF2& x, const vec_GF2& a, const mat_GF2& B)
{
   if (&a == &x || B.alias(x)) {
      vec_GF2 tmp;
      mul_aux(tmp, a, B);
      x = tmp;
   }
   else
      mul_aux(x, a, B);
}

void mul_aux(mat_GF2& X, const mat_GF2& A, const mat_GF2& B)
{
   long n = A.NumRows();
   long l = A.NumCols();
   long m = B.NumCols();

   if (l != B.NumRows())
      LogicError("matrix mul: dimension mismatch");

   X.SetDims(n, m);

   long i;

   for (i = 1; i <= n; i++) {
      mul_aux(X(i), A(i), B);
   }
}

void mul(mat_GF2& X, const mat_GF2& A, const mat_GF2& B)
{
   if (&X == &A || &X == &B) {
      mat_GF2 tmp;
      mul_aux(tmp, A, B);
      X = tmp;
   }
   else
      mul_aux(X, A, B);
}

void ident(mat_GF2& X, long n)
{
   X.SetDims(n, n);
   clear(X);
   long i;

   for (i = 0; i < n; i++)
      X.put(i, i, to_GF2(1));
}

void determinant(ref_GF2 d, const mat_GF2& M_in)
{
   long k, n;
   long i, j;
   long pos;

   n = M_in.NumRows();

   if (M_in.NumCols() != n)
      LogicError("determinant: nonsquare matrix");

   if (n == 0) {
      set(d);
      return;
   }

   mat_GF2 M;

   M = M_in;

   long wn = (n + NTL_BITS_PER_LONG - 1)/NTL_BITS_PER_LONG;

   for (k = 0; k < n; k++) {
      long wk = k/NTL_BITS_PER_LONG;
      long bk = k - wk*NTL_BITS_PER_LONG;
      _ntl_ulong k_mask = 1UL << bk;

      pos = -1;
      for (i = k; i < n; i++) {
         if (M[i].rep.elts()[wk] & k_mask) {
            pos = i;
            break;
         }
      }

      if (pos != -1) {
         if (k != pos) {
            swap(M[pos], M[k]);
         }

         _ntl_ulong *y = M[k].rep.elts();

         for (i = k+1; i < n; i++) {
            // M[i] = M[i] + M[k]*M[i,k]

            if (M[i].rep.elts()[wk] & k_mask) {
               _ntl_ulong *x = M[i].rep.elts();

               for (j = wk; j < wn; j++)
                  x[j] ^= y[j];
            }

         }
      }
      else {
         clear(d);
         return;
      }
   }

   set(d);
   return;
}

static
long IsUnitVector(const vec_GF2& a, long i)
{
   long wi = i/NTL_BITS_PER_LONG;
   long bi = i - wi*NTL_BITS_PER_LONG;

   const _ntl_ulong *p = a.rep.elts();
   long wdlen = a.rep.length();

   long j;

   for (j = 0; j < wi; j++)
      if (p[j] != 0) return 0;

   if (p[wi] != (1UL << bi))
      return 0;

   for (j = wi+1; j < wdlen; j++)
      if (p[j] != 0) return 0;

   return 1;
}

long IsIdent(const mat_GF2& A, long n)
{
   if (A.NumRows() != n || A.NumCols() != n)
      return 0;

   if (n == 0) return 1;

   long i;

   for (i = 0; i < n; i++)
      if (!IsUnitVector(A[i], i))
         return 0;

   return 1;
}

void AddToCol(mat_GF2& x, long j, const vec_GF2& a)
// add a to column j of x
// ALIAS RESTRICTION: a should not alias any row of x
{
   long n = x.NumRows();
   long m = x.NumCols();

   if (a.length() != n || j < 0 || j >= m)
      LogicError("AddToCol: bad args");

   long wj = j/NTL_BITS_PER_LONG;
   long bj = j - wj*NTL_BITS_PER_LONG;
   _ntl_ulong j_mask = 1UL << bj;

   const _ntl_ulong *ap = a.rep.elts();
   _ntl_ulong a_mask = 1;

   long i;
   for (i = 0; i < n; i++) {
      if (*ap & a_mask)
         x[i].rep.elts()[wj] ^= j_mask;

      a_mask <<= 1;
      if (!a_mask) {
         a_mask = 1;
         ap++;
      }
   }
}

void transpose_aux(mat_GF2& X, const mat_GF2& A)
{
   long n = A.NumRows();
   long m = A.NumCols();

   X.SetDims(m, n);
   clear(X);

   long i;
   for (i = 0; i < n; i++)
      AddToCol(X, i, A[i]);
}

void transpose(mat_GF2& X, const mat_GF2& A)
{
   if (&X == &A) {
      mat_GF2 tmp;
      transpose_aux(tmp, A);
      X = tmp;
   }
   else
      transpose_aux(X, A);
}

static
void solve_impl(ref_GF2 d, vec_GF2& X, const mat_GF2& A, const vec_GF2& b, bool trans)

{
   long n = A.NumRows();
   if (A.NumCols() != n)
      LogicError("solve: nonsquare matrix");

   if (b.length() != n)
      LogicError("solve: dimension mismatch");

   if (n == 0) {
      X.SetLength(0);
      set(d);
      return;
   }

   long i, j, k, pos;

   mat_GF2 M;
   M.SetDims(n, n+1);

   if (trans) {
      for (i = 0; i < n; i++) {
	 AddToCol(M, i, A[i]);
      }
   }
   else {
      for (i = 0; i < n; i++) {
         VectorCopy(M[i], A[i], n+1);
      }
   }

   AddToCol(M, n, b);

   long wn = ((n+1) + NTL_BITS_PER_LONG - 1)/NTL_BITS_PER_LONG;

   for (k = 0; k < n; k++) {
      long wk = k/NTL_BITS_PER_LONG;
      long bk = k - wk*NTL_BITS_PER_LONG;
      _ntl_ulong k_mask = 1UL << bk;

      pos = -1;
      for (i = k; i < n; i++) {
         if (M[i].rep.elts()[wk] & k_mask) {
            pos = i;
            break;
         }
      }

      if (pos != -1) {
         if (k != pos) {
            swap(M[pos], M[k]);
         }

         _ntl_ulong *y = M[k].rep.elts();

         for (i = k+1; i < n; i++) {
            // M[i] = M[i] + M[k]*M[i,k]

            if (M[i].rep.elts()[wk] & k_mask) {
               _ntl_ulong *x = M[i].rep.elts();

               for (j = wk; j < wn; j++)
                  x[j] ^= y[j];
            }

         }
      }
      else {
         clear(d);
         return;
      }
   }

   vec_GF2 XX;
   XX.SetLength(n+1);
   XX.put(n, 1);

   for (i = n-1; i >= 0; i--) {
      XX.put(i, XX*M[i]);
   }

   XX.SetLength(n);
   X = XX;

   set(d);
   return;
}

void solve(ref_GF2 d, vec_GF2& x, const mat_GF2& A, const vec_GF2& b)
{
   solve_impl(d, x, A, b, true);
}

void solve(ref_GF2 d, const mat_GF2& A, vec_GF2& x,  const vec_GF2& b)
{
   solve_impl(d, x, A, b, false);
}

void inv(ref_GF2 d, mat_GF2& X, const mat_GF2& A)
{
   long n = A.NumRows();
   if (A.NumCols() != n)
      LogicError("solve: nonsquare matrix");

   if (n == 0) {
      X.SetDims(0, 0);
      set(d);
   }

   long i, j, k, pos;

   mat_GF2 M;
   M.SetDims(n, 2*n);

   vec_GF2 aa;
   aa.SetLength(2*n);

   for (i = 0; i < n; i++) {
      aa = A[i];
      aa.SetLength(2*n);
      aa.put(n+i, 1);
      M[i] = aa;
   }

   long wn = ((2*n) + NTL_BITS_PER_LONG - 1)/NTL_BITS_PER_LONG;

   for (k = 0; k < n; k++) {
      long wk = k/NTL_BITS_PER_LONG;
      long bk = k - wk*NTL_BITS_PER_LONG;
      _ntl_ulong k_mask = 1UL << bk;

      pos = -1;
      for (i = k; i < n; i++) {
         if (M[i].rep.elts()[wk] & k_mask) {
            pos = i;
            break;
         }
      }

      if (pos != -1) {
         if (k != pos) {
            swap(M[pos], M[k]);
         }

         _ntl_ulong *y = M[k].rep.elts();

         for (i = k+1; i < n; i++) {
            // M[i] = M[i] + M[k]*M[i,k]

            if (M[i].rep.elts()[wk] & k_mask) {
               _ntl_ulong *x = M[i].rep.elts();

               for (j = wk; j < wn; j++)
                  x[j] ^= y[j];
            }

         }
      }
      else {
         clear(d);
         return;
      }
   }

   vec_GF2 XX;
   XX.SetLength(2*n);

   X.SetDims(n, n);
   clear(X);

   for (j = 0; j < n; j++) {
      XX.SetLength(n+j+1);
      clear(XX);
      XX.put(n+j, to_GF2(1));

      for (i = n-1; i >= 0; i--) {
         XX.put(i, XX*M[i]);
      }

      XX.SetLength(n);
      AddToCol(X, j, XX);
   }

   set(d);
   return;
}

long gauss(mat_GF2& M, long w)
{
   long k, l;
   long i, j;
   long pos;

   long n = M.NumRows();
   long m = M.NumCols();

   if (w < 0 || w > m)
      LogicError("gauss: bad args");

   long wm = (m + NTL_BITS_PER_LONG - 1)/NTL_BITS_PER_LONG;

   l = 0;
   for (k = 0; k < w && l < n; k++) {
      long wk = k/NTL_BITS_PER_LONG;
      long bk = k - wk*NTL_BITS_PER_LONG;
      _ntl_ulong k_mask = 1UL << bk;

      pos = -1;
      for (i = l; i < n; i++) {
         if (M[i].rep.elts()[wk] & k_mask) {
            pos = i;
            break;
         }
      }

      if (pos != -1) {
         if (l != pos)
            swap(M[pos], M[l]);

         _ntl_ulong *y = M[l].rep.elts();

         for (i = l+1; i < n; i++) {
            // M[i] = M[i] + M[l]*M[i,k]

            if (M[i].rep.elts()[wk] & k_mask) {
               _ntl_ulong *x = M[i].rep.elts();

               for (j = wk; j < wm; j++)
                  x[j] ^= y[j];
            }
         }

         l++;
      }
   }

   return l;
}

long gauss(mat_GF2& M)
{
   return gauss(M, M.NumCols());
}

void image(mat_GF2& X, const mat_GF2& A)
{
   mat_GF2 M;
   M = A;
   long r = gauss(M);
   M.SetDims(r, M.NumCols());
   X = M;
}

void kernel(mat_GF2& X, const mat_GF2& A)
{
   long m = A.NumRows();
   long n = A.NumCols();

   mat_GF2 M;
   long r;

   transpose(M, A);
   r = gauss(M);

   X.SetDims(m-r, m);
   clear(X);

   long i, j, k;

   vec_long D;
   D.SetLength(m);
   for (j = 0; j < m; j++) D[j] = -1;

   j = -1;
   for (i = 0; i < r; i++) {
      do {
         j++;
      } while (M.get(i, j) == 0);

      D[j] = i;
   }

   for (k = 0; k < m-r; k++) {
      vec_GF2& v = X[k];
      long pos = 0;
      for (j = m-1; j >= 0; j--) {
         if (D[j] == -1) {
            if (pos == k) {
               v[j] = 1;
               // v.put(j, to_GF2(1));
            }
            pos++;
         }
         else {
            v[j] = v*M[D[j]];
            // v.put(j, v*M[D[j]]);
         }
      }
   }
}

void mul(mat_GF2& X, const mat_GF2& A, GF2 b)
{
   X = A;
   if (b == 0)
      clear(X);
}

void diag(mat_GF2& X, long n, GF2 d)
{
   if (d == 1)
      ident(X, n);
   else {
      X.SetDims(n, n);
      clear(X);
   }
}

long IsDiag(const mat_GF2& A, long n, GF2 d)
{
   if (A.NumRows() != n || A.NumCols() != n)
      return 0;

   if (d == 1)
      return IsIdent(A, n);
   else
      return IsZero(A);
}

long IsZero(const mat_GF2& a)
{
   long n = a.NumRows();
   long i;

   for (i = 0; i < n; i++)
      if (!IsZero(a[i]))
         return 0;

   return 1;
}

void clear(mat_GF2& x)
{
   long n = x.NumRows();
   long i;
   for (i = 0; i < n; i++)
      clear(x[i]);
}

mat_GF2 operator+(const mat_GF2& a, const mat_GF2& b)
{
   mat_GF2 res;
   add(res, a, b);
   NTL_OPT_RETURN(mat_GF2, res);
}

mat_GF2 operator*(const mat_GF2& a, const mat_GF2& b)
{
   mat_GF2 res;
   mul_aux(res, a, b);
   NTL_OPT_RETURN(mat_GF2, res);
}

mat_GF2 operator-(const mat_GF2& a, const mat_GF2& b)
{
   mat_GF2 res;
   add(res, a, b);
   NTL_OPT_RETURN(mat_GF2, res);
}

vec_GF2 operator*(const mat_GF2& a, const vec_GF2& b)
{
   vec_GF2 res;
   mul_aux(res, a, b);
   NTL_OPT_RETURN(vec_GF2, res);
}

vec_GF2 operator*(const vec_GF2& a, const mat_GF2& b)
{
   vec_GF2 res;
   mul_aux(res, a, b);
   NTL_OPT_RETURN(vec_GF2, res);
}

void inv(mat_GF2& X, const mat_GF2& A)
{
   GF2 d;
   inv(d, X, A);
   if (d == 0) ArithmeticError("inv: non-invertible matrix");
}

void power(mat_GF2& X, const mat_GF2& A, const ZZ& e)
{
   if (A.NumRows() != A.NumCols()) LogicError("power: non-square matrix");

   if (e == 0) {
      ident(X, A.NumRows());
      return;
   }

   mat_GF2 T1, T2;
   long i, k;

   k = NumBits(e);
   T1 = A;

   for (i = k-2; i >= 0; i--) {
      sqr(T2, T1);
      if (bit(e, i))
         mul(T1, T2, A);
      else
         T1 = T2;
   }

   if (e < 0)
      inv(X, T1);
   else
      X = T1;
}

void random(mat_GF2& x, long n, long m)
{
   x.SetDims(n, m);
   for (long i = 0; i < n; i++) random(x[i], m);
}

NTL_END_IMPL


//==============================================================================
//         Copyright 2003 - 2013   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   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
//==============================================================================
#include <nt2/combinatorial/include/functions/cnp.hpp>

#include <nt2/sdk/functor/meta/call.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>
#include <boost/simd/sdk/simd/io.hpp>

#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/functions/splat.hpp>

NT2_TEST_CASE_TPL ( cnp_real,  NT2_SIMD_REAL_TYPES)
{
  using nt2::cnp;
  using nt2::tag::cnp_;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_ULP_EQUAL(cnp(nt2::Inf<vT>(), nt2::Inf<vT>()), nt2::Nan<vT>(), 0);
  NT2_TEST_ULP_EQUAL(cnp(nt2::Nan<vT>(), nt2::Nan<vT>()), nt2::Nan<vT>(), 0);
#endif
  NT2_TEST_ULP_EQUAL(cnp(nt2::splat<vT>(10),nt2::splat<vT>(1)), nt2::splat<vT>(10), 0);
  NT2_TEST_ULP_EQUAL(cnp(nt2::splat<vT>(10),nt2::splat<vT>(2)), nt2::splat<vT>(45), 0);
  NT2_TEST_ULP_EQUAL(cnp(nt2::splat<vT>(10),nt2::splat<vT>(8)), nt2::splat<vT>(45), 0);
  NT2_TEST_ULP_EQUAL(cnp(nt2::splat<vT>(2),nt2::splat<vT>(1)), nt2::splat<vT>(2), 0);
  NT2_TEST_ULP_EQUAL(cnp(nt2::splat<vT>(2),nt2::splat<vT>(2)), nt2::splat<vT>(1), 0);
  NT2_TEST_ULP_EQUAL(cnp(nt2::One<vT>(), nt2::One<vT>()), nt2::One<vT>(), 0);
  NT2_TEST_ULP_EQUAL(cnp(nt2::Zero<vT>(), nt2::Zero<vT>()), nt2::One<vT>(), 0);
}


/*
 * MultipolyCloser.cpp
 *
 *  Created on: Jan 14, 2016
 *      Author: jcassidy
 */

#include <array>
#include "LatLon.h"
#include "MultipolyCloser.hpp"

#include <boost/range/algorithm.hpp>

#include <boost/math/constants/constants.hpp>

using namespace std;

/// Convenience function for cosine in degrees
float cosd(float theta){ return cos(boost::math::constants::two_pi<float>()/360.0f*theta); }

template<std::size_t I>struct CompareTupleElement
{
	template<typename T>static const T& extract(const T& t){ return t; }
	template<typename... Types>static const tuple_element<I,tuple<Types...>>& extract(const tuple<Types...>& t){ return get<I>(t); }

	template<typename T,typename U>bool operator()(const T& lhs,const U& rhs) const
	{
		return extract(lhs) < extract(rhs);
	}
};

/** Computes the distance (metres) from a point to the north/east/south/west bounds (in that order)
 * 		@param		p		The point to compute
 * 		@param		bounds	The latlon bounds (SW, NE corners)
 */

array<float,4> MultipolyCloser::distanceToBounds(LatLon p) const
{
	float cosphi=cosd(p.lat);
	return array<float,4>
	{
		metresPerDegreeLat*fabs(m_bounds.second.lat - p.lat),			// north edge
		metresPerDegreeLat*fabs((m_bounds.second.lon - p.lon)*cosphi),	// east edge
		metresPerDegreeLat*fabs(m_bounds.first.lat - p.lat),				// south edge
		metresPerDegreeLat*fabs((m_bounds.first.lon - p.lon)*cosphi)		// west edge
	};
}

/** Closes the end of a poly to the specified boundary.
 * 		@param		p 		The point to close
 * 		@param		which	The boundary to close to
 * 		@param		bounds	The (min,max) bounds
 */

LatLon MultipolyCloser::closeToBoundary(LatLon p,CompassCardinal which) const
{
	switch(which)
	{
		case CompassCardinal::N:	return(LatLon(m_bounds.second.lat,p.lon));
		case CompassCardinal::E: 	return(LatLon(p.lat,m_bounds.second.lon));
		case CompassCardinal::S: 	return(LatLon(m_bounds.first.lat,p.lon));
		case CompassCardinal::W:	return(LatLon(p.lat,m_bounds.first.lon));
	}

	throw std::logic_error("closeToBoundary: invalid boundary requested");
}

/** Computes the nearest point on the boundary */

MultipolyCloser::EdgePoint MultipolyCloser::nearestPointOnEdge(LatLon p) const
{
	array<float,4> dToBounds = distanceToBounds(p);
	const auto nearestBound = boost::min_element(dToBounds,CompareTupleElement<0>());

	CompassCardinal edge(nearestBound-dToBounds.begin());

	LatLon pBound = closeToBoundary(p,edge);

	float ew=m_bounds.second.lon-m_bounds.first.lon;
	float ns=m_bounds.second.lat-m_bounds.first.lat;

	float cwd=0;		// clockwise distance around bounds from NW

	switch(edge)
	{
		case CompassCardinal::N: cwd=pBound.lon-m_bounds.first.lon; break;
		case CompassCardinal::E: cwd=ew+m_bounds.second.lat-pBound.lat; break;
		case CompassCardinal::S: cwd=ew+ns+m_bounds.second.lon-pBound.lon; break;
		case CompassCardinal::W: cwd=2*ew+ns+pBound.lat-m_bounds.first.lat; break;
	}

	return { pBound, *nearestBound, edge, cwd };
}

LatLon MultipolyCloser::corner(CompassIntercardinal c) const
{
	switch(c)
	{
		case CompassIntercardinal::NE: return m_bounds.second;
		case CompassIntercardinal::SE: return LatLon(m_bounds.first.lat,m_bounds.second.lon);
		case CompassIntercardinal::SW: return m_bounds.first;
		case CompassIntercardinal::NW: return LatLon(m_bounds.second.lat,m_bounds.first.lon);
	}

	throw std::logic_error("corner called with an invalid intercardinal point");
}

/** Returns clockwise "distance" (dlat+dlon along bounds) from NW to corner point
 */

float MultipolyCloser::cornerCWDistance(CompassIntercardinal c) const
{
	float ew=m_bounds.second.lon-m_bounds.first.lon;
	float ns=m_bounds.second.lat-m_bounds.first.lat;

	switch(c)
	{
	case CompassIntercardinal::NE: return ew;
	case CompassIntercardinal::SE: return ew+ns;
	case CompassIntercardinal::SW: return 2.0f*ew+ns;
	case CompassIntercardinal::NW: return 0.0f;
	}
	throw std::logic_error("cornerCWDistance: invalid intercardinal point");
}

/** Returns clockwise "distance" (dlat+dlon along bounds) from NW to corner point
 */

float cornerCWDistance(CompassPrincipal p,pair<LatLon,LatLon> bounds)
{
	unsigned u = unsigned(p);
	if((u%2)!=1)
		throw std::logic_error("cornerCWDistance: invalid compass principal point");
	return cornerCWDistance(CompassIntercardinal(u>>1),bounds);
}

/** Joins a set of ways end-to-end, and closes to the boundary where they terminate at a boundary.
 * Returns a vector of pair<vector,bool>. The bool indicates if the way way originally closed (ie. did not need bounding points added to close)
 *
 */

MultipolyCloser::MultipolyCloser(const OSMDatabase& db,const vector<const OSMWay*>& ways) :
		m_db(db),
		m_ways(ways),
		m_bounds(db.bounds()),
		m_wsAdded(ways.size(),false),
		m_waySegments(ways.size())
{
	////// Extract polygons and form list of endpoints

	for(unsigned i=0;i<ways.size();++i)
	{
		// extract the polygons
		WaySegment& ws = m_waySegments[i] = WaySegment(m_db, m_ways[i], i);

		// insert endpoints into list
		m_endpoints.push_back(Endpoint { &ws, ws.nodeRefFirst(), true,  i});
		m_endpoints.push_back(Endpoint { &ws, ws.nodeRefLast(),  false, i});
	}

	matchEndpointsByNodeID();
	closeEndpointsToBoundary();
	closeAroundBoundary();
	buildLoops();
}

void MultipolyCloser::matchEndpointsByNodeID()
{
	m_endpoints.sort(Endpoint::NodeIDOrder());
	list<Endpoint>::iterator l=m_endpoints.begin(),m,u=m_endpoints.begin();

	while(l != m_endpoints.end())
	{
		// advance u until it is a different node ID
		unsigned d;
		for(d=1,u++; u != m_endpoints.end() && Endpoint::NodeIDEqual(*u,*l); ++u,++d)
			{}

		if (d > 2)
			cout << "Not sure what to do where >2 points match!" << endl;
		else if (d == 2)
		{
			m = l;
			m++;
			if (l->waySegment == m->waySegment)		// segment closes itself
			{
				l->waySegment->linkSelf();
				cout << "Way " << l->way << " closes itself" << endl;

				m_endpoints.erase(l,u);
			}
			else						// end of way l->way -> start of way m->way
			{
				// ordering choice: endpoints before startpoints (==> l is end || both are starts)
				assert(!l->isWayFirst || m->isWayFirst);

				if (!l->isWayFirst && m->isWayFirst)		// normal case: l is an end, m is a start
				{
					cout << " INFO: Connecting ways " << setw(3) << l->way << " and " << setw(3) << m->way << endl;
					l->waySegment->append(m->waySegment);
				}
				else if (l->isWayFirst == m->isWayFirst)	// abnormal: start-start or end-end connection
				{
					// l startpoint ==> m startpoint due to ordering choice (endpoints order before startpoints)

					cout << "  WARNING: Orientation change between ways " << setw(3) << l->way << " and " << setw(3) << m->way;
					if (l->isWayFirst)
						cout << " (start->start connection)" << endl;
					else
						cout << " (  end->end   connection)" << endl;

					// each must have space to link at either next or prev
					assert(!l->waySegment->hasNext() || !l->waySegment->hasPrev());
					assert(!m->waySegment->hasNext() || !m->waySegment->hasPrev());

					if (l->isWayFirst)
					{
						l->waySegment->reverse(true);
						l->waySegment->append(m->waySegment);
						cout << "         : Reversing way " << setw(3) << l->way << endl;
					}
					else
					{
						m->waySegment->reverse(true);
						l->waySegment->prepend(m->waySegment);
						cout << "         : Reversing way " << setw(3) << m->way << endl;
					}
				}
				else
					assert(false);	// impossible case

				m_endpoints.erase(l,u);
			}
		}
		else if (d == 1)
			cout << "Segment terminal " << l->way << " " << (l->isWayFirst ? "start":"end") << "  is lonely" << endl;

		l=u;			// u is past-the-end, so points to next endpoint
	}
}

void MultipolyCloser::closeEndpointsToBoundary()
{
	////// Map unresolved terminals (points where segments start/end at boundary)
	if(m_endpoints.size())
		cout << "Unresolved terminals with bounds " << m_bounds.first.lat << ',' << m_bounds.first.lon << "  " << m_bounds.second.lat << ',' << m_bounds.second.lon << ": " << endl;
	else
		cout << "All terminals resolved" << endl;

	for(auto it = m_endpoints.begin(); it != m_endpoints.end();)
	{
		assert(it->way != -1U);
		assert(m_ways[it->way]);

		LatLon p = it->pos();
		auto res = nearestPointOnEdge(p);

		// create new boundary point at edge
		BoundaryPointSegment bps(res.edge,res.p,res.edgeDistance,res.cwDistance,it->isWayFirst);

		LatLon pEdge = bps.first();

		cout << "  Segment " << setw(3) << it->way << " " << (it->isWayFirst ? "starts":"ends  ") << " at " << p.lat << ',' << p.lon << endl;
		cout << "    Boundary point " << pEdge.lat << "," << pEdge.lon << " (" << bps.m_edgeDistance << "m from " << bps.m_location << ")" << endl;

		if (bps.m_edgeDistance < m_maxBoundaryDistance)
		{
			auto newIt = m_boundpoints.insert(m_boundpoints.begin(),bps);
			if (it->isWayFirst)
				newIt->append(it->waySegment);
			else
				newIt->prepend(it->waySegment);

			m_endpoints.erase(it++);
		}
		else
		{
			cout << "WARNING: Distance too far to edge (maxBoundaryDistance=" << m_maxBoundaryDistance << ")" << endl;
			it++;
		}
	}
}

void MultipolyCloser::closeAroundBoundary()
{
	// sort boundary points in clockwise order
	m_boundpoints.sort(BoundaryPointSegment::OrientedOrder(m_dir));

	// insert corner points as needed
	int delta = m_dir == CW ? 2 : -2;
	int edgeToCorner = m_dir == CW ? 1 : -1;		// value to add to edge to get the corner after the edge
	CompassPrincipal currentCorner(CompassPrincipal::NW);

	list<BoundaryPointSegment>::iterator it=m_boundpoints.begin();

	// put the NW corner in
	m_boundpoints.insert(it,BoundaryPointSegment(currentCorner,corner(currentCorner),cornerCWDistance(currentCorner)));
	currentCorner=currentCorner+delta;

	while(it != m_boundpoints.end() || currentCorner != CompassPrincipal::NW)
	{
		if (it == m_boundpoints.end())
			cout << "Done with boundary points, currently at " << currentCorner << " corner" << endl;
		else
			cout << "Boundary point is on " << it->m_location << " edge, looking at " << currentCorner << " corner" << endl;

		for(; currentCorner != CompassPrincipal::NW && (it == m_boundpoints.end() || (it->m_location+edgeToCorner != currentCorner)); currentCorner=currentCorner+delta)
		{
			m_boundpoints.insert(it,BoundaryPointSegment(currentCorner,corner(currentCorner),cornerCWDistance(currentCorner)));
		}

		if (it != m_boundpoints.end())
		{
			++it;
			if (it != m_boundpoints.end())
				cout << "Moving to point on " << it->m_location << " edge" << endl;
		}
	}

	cout << "Boundary points: " << endl;
	for(const auto bp : m_boundpoints)
		cout << bp << endl;

	// advance to the first endpoint
	Segment* last=nullptr;
	for(it = m_boundpoints.begin(); it != m_boundpoints.end() && !it->isEnd(); ++it){}

	// make connections around edge
	for(; it != m_boundpoints.end(); ++it)
	{
		if (it->isStart())
		{
			if (last)					// hit start of segment -> prepend last segment to it
			{
				cout << "INFO: Prepending to " << *it << endl;
				if (!it->prepend(last))
					cout << "ERROR: Failed to prepend!" << endl;
				last=nullptr;
			}
			else
			{
				cout << "WARNING: Found a segment start but no curve currently being processed (incorrect orientation?)" << endl;
				cout << "       : " << *it << endl;
			}
		}
		else if (it->isEnd())
		{
			if (last)
			{
				cout << "WARNING: Found a segment end while traversing boundary, but there is already a segment being processed" << endl;
				cout << "       : " << *it << endl;
			}
			else
			{
				cout << "INFO: Picked up new endpoint " << *it << endl;
				last=&(*it);
			}
		}
		else if (last)
		{
			cout << "INFO: Traversing boundary point " << *it << " and appending" << endl;
			if (last->append(&(*it)))
				last = &(*it);
			else
				cout << "ERROR: Failed to add!" << endl;
		}
		else
			cout << "INFO: Skipping corner point " << *it << endl;
	}

	cout << "Continuing through north to get last startpoints" << endl;

	// finish off last few connections (must be startpoints or would have been)
	for(it = m_boundpoints.begin(); it != m_boundpoints.end() && !it->isEnd(); ++it)
	{
		if (it->isStart())
		{
			if (last)					// hit start of segment -> prepend last segment to it
			{
				cout << "INFO: Prepending to " << *it << endl;
				if (!it->prepend(last))
					cout << "ERROR: Failed to prepend!" << endl;
				last=nullptr;
			}
			else
			{
				cout << "WARNING: Found a segment start but no curve currently being processed (incorrect orientation?)" << endl;
				cout << "       : " << *it << endl;
			}
		}
		else if (last)
		{
			cout << "INFO: Traversing boundary point " << *it << " and appending" << endl;
			if (last->append(&(*it)))
				last = &(*it);
			else
				cout << "ERROR: Failed to add!" << endl;
		}
		else
			cout << "INFO: Skipping corner point " << *it << endl;
	}
}

void MultipolyCloser::buildLoops()
{
	m_loops.clear();
	m_wsAdded.clear();
	m_wsAdded.resize(m_waySegments.size(),false);

	// build loop entries serially
	cout << "Building loop indices" << endl;
	for(unsigned i=0;i<m_waySegments.size();++i)
	{
		if (m_wsAdded.at(i))
			continue;

		cout << "checking segment " << i << endl;

		if (m_waySegments[i].isMemberOfClosedLoop())
		{
			for(const Segment* s=&m_waySegments[i]; s != nullptr; s=s->nextEndingAt(&m_waySegments[i]))
			{
				if (const WaySegment* ws = dynamic_cast<const WaySegment*>(s))
				{
					if (m_wsAdded.at(ws->index()))
						cout << "WARNING: Way segment " << ws->index() << " multiple inclusion" << endl;
					m_wsAdded[ws->index()] = true;
				}
			}
			m_loops.push_back(&m_waySegments[i]);
		}
		else
			cout << "WARNING: Segment " << i << " is not part of a closed loop" << endl;
	}
}

vector<pair<vector<LatLon>,bool>> MultipolyCloser::loops(Boundedness b) const
{
	cout << " Constructing return loops" << endl;

	vector<pair<vector<LatLon>,bool>> o;
	vector<LatLon> loop;
	bool bounded;

	for(const Segment* l : m_loops)
	{
		loop.clear();
		bounded=true;
		for(const Segment* s=l; s != nullptr; s=s->nextEndingAt(l))
		{
			s->add(loop);
			if (dynamic_cast<const BoundaryPointSegment*>(s))
				bounded=false;
		}
		if (b == All || (b==Unbounded && !bounded) || (b==Bounded && bounded) )
			o.emplace_back(std::move(loop),bounded);
	}
	return o;
}


#ifndef __PICSAR_MULTIPHYSICS_UTILITIES__
#define __PICSAR_MULTIPHYSICS_UTILITIES__

//This .hpp file contains general purpose functions to perform useful
//operations

#include <vector>
#include <cmath>
#include <functional>
#include <algorithm>
#include <utility>

#ifndef PXRMP_CORE_ONLY
    #include <limits>
    //Uses the root finding algorithms provided by boost
    #include <boost/math/tools/roots.hpp>
#endif //PXRMP_CORE_ONLY

//Should be included by all the src files of the library
#include "qed_commons.h"

//############################################### Declaration

namespace picsar{
    namespace multi_physics{

        //Generates a linearly spaced vector
        template<typename _REAL>
        std::vector<_REAL> generate_lin_spaced_vec
        (_REAL min, _REAL max, size_t size);

        //Generates a logarithmically spaced vector
        template<typename _REAL>
        std::vector<_REAL> generate_log_spaced_vec
        (_REAL min, _REAL max, size_t size);

        //Generates log_lin_log spaced vector
        template<typename _REAL>
        std::vector<_REAL> generate_log_lin_log_spaced_vec
        (_REAL min, _REAL max, size_t size);

        //GPU-friendly replacement of "std::upper_bound"
        template<typename T>
        PXRMP_GPU
        PXRMP_FORCE_INLINE
        const T* picsar_upper_bound(const T* first, const T* last, const T& val);

        //A function providing values extracted from a poisson distribution,
        //given lambda and a number in the interval [0,1)
        template<typename T>
        PXRMP_GPU
        PXRMP_FORCE_INLINE
        size_t poisson_distrib(T lambda, T unf_zero_one_minus_epsi);

#ifndef PXRMP_CORE_ONLY
        //A wrapper around the function provided by Boost library
        template<typename _REAL>
        _REAL bracket_and_solve_root
        (const std::function<_REAL(_REAL)>& f, _REAL guess, bool rising);
#endif
    }
}

//############################################### Implementation

template<typename _REAL>
std::vector<_REAL>
picsar::multi_physics::generate_log_spaced_vec
(_REAL min, _REAL max, size_t size)
{
    //Return empty vector upon error
    if( min >= max ||
        min < static_cast<_REAL>(0) || max< static_cast<_REAL>(0) ||
        size < 2)
        return std::vector<_REAL>(0);

    std::vector<_REAL> vec(size);

    _REAL val = min;
    _REAL mul = pow(max/min, static_cast<_REAL>(1.0/(size-1)));

    for(size_t i = 0; i < size-1; ++i){
        vec[i] = val;
        val*=mul;
    }
    vec.back() = max; //Enforces this exactly
    return vec;
}

//Generates a linearly spaced vector
template<typename _REAL>
std::vector<_REAL>
picsar::multi_physics::generate_lin_spaced_vec
(_REAL min, _REAL max, size_t size)
{
    //Return empty vector upon error
    if( min >= max || size < 2)
        return std::vector<_REAL>(0);

    std::vector<_REAL> vec(size);

    for(size_t i = 0; i < size-1; ++i){
        vec[i] = static_cast<_REAL>(i*(max-min)/(size-1.0)) + min;
    }
    vec.back() = max; //Enforces this exactly
    return vec;
}

//Generates log_lin_log spaced vector
template<typename _REAL>
std::vector<_REAL>
picsar::multi_physics::generate_log_lin_log_spaced_vec
(_REAL min, _REAL max, size_t size)
{
    std::vector<_REAL> vec(size);
    size_t size_first = size/3;
    size_t size_second = size/3;
    size_t size_third = size/3;

    _REAL first_val = max*static_cast<_REAL>(1.0/10.0);
    _REAL second_val = max*static_cast<_REAL>(9.0/10.0);

    size_t n = 0;
    std::generate(vec.begin(), vec.begin()+size_first,
    [=] () mutable { return min*exp((n++)*log(first_val/min)/(size_first)); });

    std::generate(vec.begin()+size_first, vec.begin()+size_first+size_second,
    [=] () mutable { return first_val + (second_val-first_val)*(n++)/(size_second); });

    std::generate(vec.begin()+size_first+size_second, vec.end(),
    [=] () mutable { return max*exp((size_third-1-(n++))*log(second_val/max)/(size_third-1)); });

    vec.front() = min;
    vec.back() = max;

    return vec;
}

template<typename T>
PXRMP_GPU
PXRMP_FORCE_INLINE
const T*
picsar::multi_physics::picsar_upper_bound
(const T* first, const T* last, const T& val)
{
    const T* it;
    size_t count, step;
    count = last-first;
    while(count>0){
        it = first;
        step = count/2;
        it += step;
         if (!(val<*it)){
             first = ++it;
             count -= step + 1;
         }
         else{
             count = step;
         }
    }
    return first;
}

//A function providing values extracted from a poisson distribution,
//given lambda and a number in the interval [0,1)
template<typename T>
PXRMP_GPU
PXRMP_FORCE_INLINE
size_t picsar::multi_physics::poisson_distrib
(T lambda, T unf_zero_one_minus_epsi)
{
    size_t k = 0;
    T p = exp(-lambda);
    T s = p;
    T old_s;
    while (unf_zero_one_minus_epsi > s){
        old_s = s;
        p = p*lambda/(++k);
        s += p;
        //If this is true we have reached the limit of the floating
        //point number that we are using
        if(s <= old_s)
            break;
    }
    return k;
}

#ifndef PXRMP_CORE_ONLY
    //A wrapper around the function provided by Boost library
    template<typename _REAL>
    _REAL picsar::multi_physics::bracket_and_solve_root
    (const std::function<_REAL(_REAL)>& f, _REAL guess, bool rising)
    {
        size_t digits = std::numeric_limits<_REAL>::digits;
        size_t precision_digits = digits - 2;
        boost::math::tools::eps_tolerance<_REAL> tol(precision_digits);

        _REAL factor = static_cast<_REAL>(2.0);

        size_t max_iter = 32;

        std::pair<_REAL, _REAL> r =
            boost::math::tools::bracket_and_solve_root
            (f, guess, factor, rising, tol, max_iter);

        return r.first + (r.second - r.first)/static_cast<_REAL>(2.0);
    }
#endif //PXRMP_CORE_ONLY

#endif // __PICSAR_MULTIPHYSICS_UTILITIES__


#include <cfloat>
#include <boost/format.hpp>
#include "Utils.h"

bool build_even_motion_relative_time_samples(float           i_relative_shutter_open,
		float           i_relative_shutter_close,
		Alembic::Abc::uint8_t          i_motion_sample_count,
		FloatContainer& o_earlier_sampling_time_vector,
		FloatContainer& o_later_sampling_time_vector)
{
	if (i_motion_sample_count<2)
		return false;
	float shutter_delta = (i_relative_shutter_close - i_relative_shutter_open)/(i_motion_sample_count-1);
	if (shutter_delta < FLT_EPSILON)
		return false;
	o_earlier_sampling_time_vector.clear();
	o_later_sampling_time_vector.clear();
	for (Alembic::Abc::uint8_t sample_index=0;sample_index<i_motion_sample_count;sample_index++)
	{
		float time_sample  = i_relative_shutter_open + sample_index * shutter_delta;
		std::cout << boost::format("time_sample = %1%") % time_sample << std::endl;
		if (time_sample>=0.0f)
			o_later_sampling_time_vector.push_back(time_sample);
		else
			o_earlier_sampling_time_vector.push_back(time_sample);
	}
	return true;

}

// == Emacs ================
// -------------------------
// Local variables:
// tab-width: 4
// indent-tabs-mode: t
// c-basic-offset: 4
// end:
//
// == vi ===================
// -------------------------
// Format block
// ex:ts=4:sw=4:expandtab
// -------------------------


#pragma once

#include "tools.hpp"
#include <Eigen/Dense>

namespace kde1d {

namespace interp {

//! A class for cubic spline interpolation in one dimension
//!
//! The class is used for implementing kernel estimators. It makes storing the
//! observations obsolete and allows for fast numerical integration.
class InterpolationGrid
{
  public:
    InterpolationGrid() {}

    InterpolationGrid(const Eigen::VectorXd& grid_points,
                        const Eigen::VectorXd& values,
                        int norm_times);

    void normalize(int times);

    Eigen::VectorXd interpolate(const Eigen::VectorXd& x) const;

    Eigen::VectorXd integrate(const Eigen::VectorXd& u,
                              bool normalize = false) const;

    Eigen::VectorXd get_values() const { return values_; }
    Eigen::VectorXd get_grid_points() const { return grid_points_; }
    double get_grid_max() const
    {
        return grid_points_[grid_points_.size() - 1];
    }
    double get_grid_min() const { return grid_points_[0]; }

  private:
    // Utility functions for spline Interpolation
    double cubic_poly(const double& x, const Eigen::VectorXd& a) const;
    double cubic_indef_integral(const double& x,
                                const Eigen::VectorXd& a) const;
    double cubic_integral(const double& lower,
                          const double& upper,
                          const Eigen::VectorXd& a) const;
    int find_cell(const double& x0) const;
    Eigen::VectorXd find_cell_coefs(const int& k) const;

    Eigen::VectorXd grid_points_;
    Eigen::VectorXd values_;
};

//! Constructor
//!
//! @param grid_points an ascending sequence of grid points.
//! @param values a vector of values of same length as grid_points.
//! @param norm_times how many times the normalization routine should run.
inline InterpolationGrid::InterpolationGrid(
  const Eigen::VectorXd& grid_points,
  const Eigen::VectorXd& values,
  int norm_times)
{
    if (grid_points.size() != values.size())
        throw std::runtime_error(
          "grid_points and values must be of equal length");

    grid_points_ = grid_points;
    values_ = values;
    this->normalize(norm_times);
}

//! renormalizes the estimate to integrate to one
//!
//! @param times how many times the normalization routine should run.
inline void
InterpolationGrid::normalize(int times)
{
    double x_max = grid_points_(grid_points_.size() - 1);
    double int_max;
    for (int k = 0; k < times; ++k) {
        int_max = this->integrate(Eigen::VectorXd::Constant(1, x_max))(0);
        values_ /= int_max;
    }
}

//! Interpolation
//! @param x vector of evaluation points.
inline Eigen::VectorXd
InterpolationGrid::interpolate(const Eigen::VectorXd& x) const
{
    Eigen::VectorXd tmp_coefs(4);
    auto interpolate_one = [&](const double& xx) {
        int k = find_cell(xx);
        double xev =
          (xx - grid_points_(k)) / (grid_points_(k + 1) - grid_points_(k));

        // use Gaussian tail for extrapolation
        if (xev <= 0) {
            return values_(k) * std::exp(-0.5 * xev * xev);
        } else if (xev >= 1) {
            return values_(k + 1) * std::exp(-0.5 * xev * xev);
        }

        return cubic_poly(xev, find_cell_coefs(k));
    };

    return tools::unaryExpr_or_nan(x, interpolate_one);
}

//! Integration along the grid
//!
//! @param x a vector  of evaluation points
//! @param normalize whether to normalize the integral to a maximum value of 1.
inline Eigen::VectorXd
InterpolationGrid::integrate(const Eigen::VectorXd& x, bool normalize) const
{
    Eigen::VectorXd res(x.size());
    auto ord = tools::get_order(x);

    // temporaries for the loop
    Eigen::VectorXd tmp_coefs(4);
    double new_int, tmp_eps, cum_int = 0.0;
    int k = 0, m = grid_points_.size();
    tmp_coefs = find_cell_coefs(0);
    tmp_eps = (grid_points_(1) - grid_points_(0));

    for (long i = 0; i < x.size(); ++i) {
        double upr = x(ord(i));

        if (std::isnan(upr)) {
            res(ord(i)) = upr;
            continue;
        }
        if (upr <= grid_points_(0)) {
            res(ord(i)) = 0.0;
            continue;
        }

        // go up the grid and integrate
        while (k < m - 1) {
            // halt loop if integration limit is in kth cell
            if (upr < grid_points_(k + 1))
                break;
            // integrate over full cell
            tmp_coefs = find_cell_coefs(k);
            tmp_eps = (grid_points_(k + 1) - grid_points_(k));
            cum_int += cubic_integral(0.0, 1.0, tmp_coefs) * tmp_eps;
            k++;
        }

        // integrate over partial cell
        if (upr < grid_points_(m - 1)) { // only if still in interior
            tmp_coefs = find_cell_coefs(k);
            tmp_eps = (grid_points_(k + 1) - grid_points_(k));
            upr = (upr - grid_points_(k)) / tmp_eps;
            new_int = cubic_integral(0.0, upr, tmp_coefs) * tmp_eps;
        } else {
            new_int = 0.0;
        }

        res(ord(i)) = cum_int + new_int;
    }

    if (!normalize)
        return res;

    // integrate until end
    while (k < m - 1) {
        tmp_coefs = find_cell_coefs(k);
        tmp_eps = (grid_points_(k + 1) - grid_points_(k));
        cum_int += cubic_integral(0.0, 1.0, tmp_coefs) * tmp_eps;
        k++;
    }
    return res / cum_int;
}

// ---------------- Utility functions for spline interpolation ----------------

//! Evaluate a cubic polynomial
//!
//! @param x evaluation point.
//! @param a polynomial coefficients
inline double
InterpolationGrid::cubic_poly(const double& x, const Eigen::VectorXd& a) const
{
    double x2 = x * x;
    double x3 = x2 * x;
    return a(0) + a(1) * x + a(2) * x2 + a(3) * x3;
}

//! Indefinite integral of a cubic polynomial
//!
//! @param x evaluation point.
//! @param a polynomial coefficients.
inline double
InterpolationGrid::cubic_indef_integral(const double& x,
                                          const Eigen::VectorXd& a) const
{
    double x2 = x * x;
    double x3 = x2 * x;
    double x4 = x3 * x;
    return a(0) * x + a(1) / 2.0 * x2 + a(2) / 3.0 * x3 + a(3) / 4.0 * x4;
}

//! Definite integral of a cubic polynomial
//!
//! @param lower lower limit of the integral.
//! @param upper upper limit of the integral.
//! @param a polynomial coefficients.
inline double
InterpolationGrid::cubic_integral(const double& lower,
                                    const double& upper,
                                    const Eigen::VectorXd& a) const
{
    return cubic_indef_integral(upper, a) - cubic_indef_integral(lower, a);
}

inline int
InterpolationGrid::find_cell(const double& x0) const
{
    int low = 0, high = grid_points_.size() - 1;
    int mid;
    while (low < high - 1) {
        mid = low + (high - low) / 2;
        if (x0 < grid_points_(mid))
            high = mid;
        else
            low = mid;
    }

    return low;
}

//! Calculate coefficients for cubic intrpolation spline
//!
//! @param k the cell index.
inline Eigen::VectorXd
InterpolationGrid::find_cell_coefs(const int& k) const
{
    // indices for cell and neighboring grid points
    int k0 = std::max(k - 1, 0);
    int k2 = k + 1;
    int k3 = std::min(k + 2, static_cast<int>(grid_points_.size()) - 1);

    double dt0 = grid_points_(k) - grid_points_(k0);
    double dt1 = grid_points_(k2) - grid_points_(k);
    double dt2 = grid_points_(k3) - grid_points_(k2);

    // compute tangents when parameterized in (t1,t2)
    // for smooth extrapolation, derivative is set to zero at boundary
    double dx1 = 0.0, dx2 = 0.0;
    if (dt0 > 0) {
        dx1 = (values_(k) - values_(k0)) / dt0;
        dx1 -= (values_(k2) - values_(k0)) / (dt0 + dt1);
        dx1 += (values_(k2) - values_(k)) / dt1;
    }
    if (dt2 > 0) {
        dx2 = (values_(k2) - values_(k)) / dt1;
        dx2 -= (values_(k3) - values_(k)) / (dt1 + dt2);
        dx2 += (values_(k3) - values_(k2)) / dt2;
    }

    // rescale tangents for parametrization in (0,1)
    dx1 *= dt1;
    dx2 *= dt1;

    // ensure positivity (Schmidt and Hess, DOI:10.1007/bf01934097)
    dx1 = std::max(dx1, -3 * values_(k));
    dx2 = std::min(dx2, 3 * values_(k2));

    // compute coefficents
    Eigen::VectorXd a(4);
    a(0) = values_(k);
    a(1) = dx1;
    a(2) = -3 * (values_(k) - values_(k2)) - 2 * dx1 - dx2;
    a(3) = 2 * (values_(k) - values_(k2)) + dx1 + dx2;

    return a;
}

} // end kde1d::interp

} // end kde1d