// test_owens_t.cpp

// Copyright Paul A. Bristow 2012.
// Copyright Benjamin Sobotta 2012.

// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

// Tested using some 30 decimal digit accuracy values from:
// Fast and accurate calculation of Owen's T-function
// Mike Patefield, and David Tandy
// Journal of Statistical Software, 5 (5), 1-25 (2000).
// http://www.jstatsoft.org/v05/a05/paper  Table 3, page 15
// Values of T(h,a) accurate to thirty figures were calculated using 128 bit arithmetic by
// evaluating (9) with m = 48, the summation over k being continued until additional terms did
// not alter the result. The resultant values Tacc(h,a) say, were validated by evaluating (8) with
// m = 48 (i.e. 96 point Gaussian quadrature).

#define BOOST_MATH_OVERFLOW_ERROR_POLICY ignore_error

#ifdef _MSC_VER
#  pragma warning (disable : 4127) // conditional expression is constant
#  pragma warning (disable : 4305) // 'initializing' : truncation from 'double' to 'const float'
// ?? TODO get rid of these warnings?
#endif

#include <boost/math/concepts/real_concept.hpp> // for real_concept.
using ::boost::math::concepts::real_concept;

#include <boost/math/special_functions/owens_t.hpp> // for owens_t function.
using boost::math::owens_t;
#include <boost/math/distributions/normal.hpp>

#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/array.hpp>

#include "libs/math/test/handle_test_result.hpp"
#include "libs/math/test/table_type.hpp"
#include "libs/math/test/functor.hpp"
#include "test_owens_t.hpp"

//
// Defining TEST_CPP_DEC_FLOAT enables testing of multiprecision support.
// This requires the multiprecision library from sandbox/big_number.
// Note that these tests *do not pass*, but they do give an idea of the
// error rates that can be expected....
//
#ifdef TEST_CPP_DEC_FLOAT
#include <boost/multiprecision/cpp_dec_float.hpp>

template <class R>
inline R convert_to(const char* s)
{
   try{
      return boost::lexical_cast<R>(s);
   }
   catch(const boost::bad_lexical_cast&)
   {
      return 0;
   }
}

#define SC_(x) convert_to<T>(BOOST_STRINGIZE(x))
#endif

#include "owens_t_T7.hpp"

#include <iostream>
using std::cout;
using std::endl;
#include <limits>
using std::numeric_limits;

void expected_results()
{
   //
   // Define the max and mean errors expected for
   // various compilers and platforms.
   //
   const char* largest_type;
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   if(boost::math::policies::digits<double, boost::math::policies::policy<> >() == boost::math::policies::digits<long double, boost::math::policies::policy<> >())
   {
      largest_type = "(long\\s+)?double|real_concept";
   }
   else
   {
      largest_type = "long double|real_concept";
   }
#else
   largest_type = "(long\\s+)?double";
#endif

   //
   // Catch all cases come last:
   //
   if(std::numeric_limits<long double>::digits > 60)
   {
      add_expected_result(
         ".*",                            // compiler
         ".*",                            // stdlib
         ".*",                            // platform
         largest_type,                    // test type(s)
         ".*",      // test data group
         "owens_t", 500, 100);  // test function
   }
   else
   {
      add_expected_result(
         ".*",                            // compiler
         ".*",                            // stdlib
         ".*",                            // platform
         largest_type,                    // test type(s)
         ".*",      // test data group
         "owens_t", 60, 5);  // test function
   }
   //
   // Finish off by printing out the compiler/stdlib/platform names,
   // we do this to make it easier to mark up expected error rates.
   //
   std::cout << "Tests run with " << BOOST_COMPILER << ", "
      << BOOST_STDLIB << ", " << BOOST_PLATFORM << std::endl;
}

BOOST_AUTO_TEST_CASE( test_main )
{
  BOOST_MATH_CONTROL_FP;

  expected_results();

  // Basic sanity-check spot values.

  // (Parameter value, arbitrarily zero, only communicates the floating point type).
  test_spots(0.0F); // Test float.
  test_spots(0.0); // Test double.
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
  test_spots(0.0L); // Test long double.
#if !BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x582))
  test_spots(boost::math::concepts::real_concept(0.)); // Test real concept.
#endif
#endif

  check_against_T7(0.0F); // Test float.
  check_against_T7(0.0); // Test double.
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
  check_against_T7(0.0L); // Test long double.
#if !BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x582))
  check_against_T7(boost::math::concepts::real_concept(0.)); // Test real concept.
#endif
#endif

  test_owens_t(0.0F, "float"); // Test float.
  test_owens_t(0.0, "double"); // Test double.
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
  test_owens_t(0.0L, "long double"); // Test long double.
#if !BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x582))
  test_owens_t(boost::math::concepts::real_concept(0.), "real_concept"); // Test real concept.
#endif
#endif
#ifdef TEST_CPP_DEC_FLOAT
  typedef boost::multiprecision::mp_number<boost::multiprecision::cpp_dec_float<35> > cpp_dec_float_35;
  test_owens_t(cpp_dec_float_35(0), "cpp_dec_float_35"); // Test real concept.
  test_owens_t(boost::multiprecision::cpp_dec_float_50(0), "cpp_dec_float_50"); // Test real concept.
  test_owens_t(boost::multiprecision::cpp_dec_float_100(0), "cpp_dec_float_100"); // Test real concept.
#endif

} // BOOST_AUTO_TEST_CASE( test_main )

/*

Output:

  Description: Autorun "J:\Cpp\MathToolkit\test\Math_test\Debug\test_owens_t.exe"
  Running 1 test case...
  Tests run with Microsoft Visual C++ version 10.0, Dinkumware standard library version 520, Win32
  Tolerance = 3.57628e-006.
  Tolerance = 6.66134e-015.
  Tolerance = 6.66134e-015.
  Tolerance = 6.66134e-015.
  Tolerance = 1.78814e-005.
  Tolerance = 3.33067e-014.
  Tolerance = 3.33067e-014.
  Tolerance = 3.33067e-014.
  Testing Owens T (medium small values) with type float
  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  boost::math::owens_t<float> Max = 0 RMS Mean=0

  Testing Owens T (large and diverse values) with type float
  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  boost::math::owens_t<float> Max = 0 RMS Mean=0

  Testing Owens T (medium small values) with type double
  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  boost::math::owens_t<double> Max = 4.375 RMS Mean=0.9728
      worst case at row: 81
      { 4.4206809997558594, 0.1269868016242981, 1.0900281236140834e-006 }

  Testing Owens T (large and diverse values) with type double
  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  boost::math::owens_t<double> Max = 3.781 RMS Mean=0.6206
      worst case at row: 430
      { 3.4516773223876953, 0.0010718167759478092, 4.413983645332431e-007 }

  Testing Owens T (medium small values) with type long double
  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  boost::math::owens_t<long double> Max = 4.375 RMS Mean=0.9728
      worst case at row: 81
      { 4.4206809997558594, 0.1269868016242981, 1.0900281236140834e-006 }

  Testing Owens T (large and diverse values) with type long double
  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  boost::math::owens_t<long double> Max = 3.781 RMS Mean=0.6206
      worst case at row: 430
      { 3.4516773223876953, 0.0010718167759478092, 4.413983645332431e-007 }

  Testing Owens T (medium small values) with type real_concept
  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  boost::math::owens_t<real_concept> Max = 4.375 RMS Mean=1.032
      worst case at row: 81
      { 4.4206809997558594, 0.1269868016242981, 1.0900281236140834e-006 }

  Testing Owens T (large and diverse values) with type real_concept
  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  boost::math::owens_t<real_concept> Max = 21.04 RMS Mean=1.102
      worst case at row: 439
      { 3.4516773223876953, 0.98384737968444824, 0.00013923002576038691 }

  *** No errors detected

*/


#ifndef LABELME_IO_HH
#define LABELME_IO_HH

#include <string>
#include <vector>       // std::vector

#include <Eigen/Dense>

namespace labelme
{

struct LabelMeData
{
	std::vector<Eigen::Vector3i> colors; // available annotation colors
	std::vector<Eigen::Vector4i> data;   // each representing: idx of original cloud, color_b, color_g, color_r
	std::vector<Eigen::Matrix4f> poses;  //
	std::string pcd_file;
};

bool read_data(const std::string json_file, LabelMeData& data);
bool save_data(const std::string json_file, const LabelMeData& data);

}

#endif


// (C) Copyright Andrew Sutton 2007
//
// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0 (See accompanying file
// LICENSE_1_0.txt or http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GRAPH_GEODESIC_DISTANCE_HPP
#define BOOST_GRAPH_GEODESIC_DISTANCE_HPP

#include <boost/graph/detail/geodesic.hpp>
#include <boost/graph/exterior_property.hpp>

namespace boost
{
template <typename Graph,
          typename DistanceType,
          typename ResultType,
          typename Divides = std::divides<ResultType> >
struct mean_geodesic_measure
    : public geodesic_measure<Graph, DistanceType, ResultType>
{
    typedef geodesic_measure<Graph, DistanceType, ResultType> base_type;
    typedef typename base_type::distance_type distance_type;
    typedef typename base_type::result_type result_type;

    result_type operator ()(distance_type d, const Graph& g)
    {
        function_requires< VertexListGraphConcept<Graph> >();
        function_requires< NumericValueConcept<DistanceType> >();
        function_requires< NumericValueConcept<ResultType> >();
        function_requires< AdaptableBinaryFunctionConcept<Divides,ResultType,ResultType,ResultType> >();

        return (d == base_type::infinite_distance())
            ? base_type::infinite_result()
            : div(result_type(d), result_type(num_vertices(g) - 1));
    }
    Divides div;
};

template <typename Graph, typename DistanceMap>
inline mean_geodesic_measure<Graph, typename property_traits<DistanceMap>::value_type, double>
measure_mean_geodesic(const Graph&, DistanceMap)
{
    return mean_geodesic_measure<Graph, typename property_traits<DistanceMap>::value_type, double>();
}

template <typename T, typename Graph, typename DistanceMap>
inline mean_geodesic_measure<Graph, typename property_traits<DistanceMap>::value_type, T>
measure_mean_geodesic(const Graph&, DistanceMap)
{
    return mean_geodesic_measure<Graph, typename property_traits<DistanceMap>::value_type, T>();
}

// This is a little different because it's expected that the result type
// should (must?) be the same as the distance type. There's a type of
// transitivity in this thinking... If the average of distances has type
// X then the average of x's should also be type X. Is there a case where this
// is not true?
//
// This type is a little under-genericized... It needs generic parameters
// for addition and division.
template <typename Graph, typename DistanceType>
struct mean_graph_distance_measure
    : public geodesic_measure<Graph, DistanceType, DistanceType>
{
    typedef geodesic_measure<Graph, DistanceType, DistanceType> base_type;
    typedef typename base_type::distance_type distance_type;
    typedef typename base_type::result_type result_type;

    inline result_type operator ()(distance_type d, const Graph& g)
    {
        function_requires< VertexListGraphConcept<Graph> >();
        function_requires< NumericValueConcept<DistanceType> >();

        if(d == base_type::infinite_distance()) {
            return base_type::infinite_result();
        }
        else {
            return d / result_type(num_vertices(g));
        }
    }
};

template <typename Graph, typename DistanceMap>
inline mean_graph_distance_measure<Graph, typename property_traits<DistanceMap>::value_type>
measure_graph_mean_geodesic(const Graph&, DistanceMap)
{
    typedef typename property_traits<DistanceMap>::value_type T;
    return mean_graph_distance_measure<Graph, T>();
}

template <typename Graph,
          typename DistanceMap,
          typename Measure,
          typename Combinator>
inline typename Measure::result_type
mean_geodesic(const Graph& g,
                DistanceMap dist,
                Measure measure,
                Combinator combine)
{
    function_requires< DistanceMeasureConcept<Measure,Graph> >();
    typedef typename Measure::distance_type Distance;

    Distance n = detail::combine_distances(g, dist, combine, Distance(0));
    return measure(n, g);
}

template <typename Graph,
            typename DistanceMap,
            typename Measure>
inline typename Measure::result_type
mean_geodesic(const Graph& g, DistanceMap dist, Measure measure)
{
    function_requires< DistanceMeasureConcept<Measure,Graph> >();
    typedef typename Measure::distance_type Distance;

    return mean_geodesic(g, dist, measure, std::plus<Distance>());
}

template <typename Graph, typename DistanceMap>
inline double
mean_geodesic(const Graph& g, DistanceMap dist)
{ return mean_geodesic(g, dist, measure_mean_geodesic(g, dist)); }

template <typename T, typename Graph, typename DistanceMap>
inline T
mean_geodesic(const Graph& g, DistanceMap dist)
{ return mean_geodesic(g, dist, measure_mean_geodesic<T>(g, dist)); }

template <typename Graph,
            typename DistanceMatrixMap,
            typename GeodesicMap,
            typename Measure>
inline typename property_traits<GeodesicMap>::value_type
all_mean_geodesics(const Graph& g,
                    DistanceMatrixMap dist,
                    GeodesicMap geo,
                    Measure measure)
{
    function_requires< VertexListGraphConcept<Graph> >();
    typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
    typedef typename graph_traits<Graph>::vertex_iterator VertexIterator;
    function_requires< ReadablePropertyMapConcept<DistanceMatrixMap,Vertex> >();
    typedef typename property_traits<DistanceMatrixMap>::value_type DistanceMap;
    function_requires< DistanceMeasureConcept<Measure,Graph> >();
    typedef typename Measure::result_type Result;
    function_requires< WritablePropertyMapConcept<GeodesicMap,Vertex> >();
    function_requires< NumericValueConcept<Result> >();

    // NOTE: We could compute the mean geodesic here by performing additional
    // computations (i.e., adding and dividing). However, I don't really feel
    // like fully genericizing the entire operation yet so I'm not going to.

    Result inf = numeric_values<Result>::infinity();
    Result sum = numeric_values<Result>::zero();
    VertexIterator i, end;
    for(tie(i, end) = vertices(g); i != end; ++i) {
        DistanceMap dm = get(dist, *i);
        Result r = mean_geodesic(g, dm, measure);
        put(geo, *i, r);

        // compute the sum along with geodesics
        if(r == inf) {
            sum = inf;
        }
        else if(sum != inf) {
            sum += r;
        }
    }

    // return the average of averages.
    return sum / Result(num_vertices(g));
}

template <typename Graph, typename DistanceMatrixMap, typename GeodesicMap>
inline typename property_traits<GeodesicMap>::value_type
all_mean_geodesics(const Graph& g, DistanceMatrixMap dist, GeodesicMap geo)
{
    function_requires< GraphConcept<Graph> >();
    typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
    function_requires< ReadablePropertyMapConcept<DistanceMatrixMap,Vertex> >();
    typedef typename property_traits<DistanceMatrixMap>::value_type DistanceMap;
    function_requires< WritablePropertyMapConcept<GeodesicMap,Vertex> >();
    typedef typename property_traits<GeodesicMap>::value_type Result;

    return all_mean_geodesics(g, dist, geo, measure_mean_geodesic<Result>(g, DistanceMap()));
}

template <typename Graph, typename GeodesicMap, typename Measure>
inline typename Measure::result_type
small_world_distance(const Graph& g, GeodesicMap geo, Measure measure)
{
    function_requires< DistanceMeasureConcept<Measure,Graph> >();
    typedef typename Measure::result_type Result;

    Result sum = detail::combine_distances(g, geo, std::plus<Result>(), Result(0));
    return measure(sum, g);
}

template <typename Graph, typename GeodesicMap>
inline typename property_traits<GeodesicMap>::value_type
small_world_distance(const Graph& g, GeodesicMap geo)
{ return small_world_distance(g, geo, measure_graph_mean_geodesic(g, geo)); }

}

#endif


#pragma once

#include <cstddef>
#include <string>

#include <boost/optional.hpp>

inline
std::size_t
operator "" _B(unsigned long long size)
{
  return size;
}

inline
std::size_t
operator "" _kB(unsigned long long size)
{
  return size * 1000;
}

inline
std::size_t
operator "" _MB(unsigned long long size)
{
  return size * 1'000'000;
}

inline
std::size_t
operator "" _GB(unsigned long long size)
{
  return size * 1000'000'000;
}

inline
std::size_t
operator "" _KiB(unsigned long long size)
{
  return size * 1024;
}

inline
std::size_t
operator "" _MiB(unsigned long long size)
{
  return size * 1024 * 1024;
}

inline
std::size_t
operator "" _GiB(unsigned long long size)
{
  return size * 1024 * 1024 * 1024;
}

namespace elle
{
  /// Pretty-print a number of bytes.
  ///
  /// @param bytes  The number of bytes.
  /// @param si     Whether to use scientific notation (KB vs KiB, etc.).
  std::string
  human_data_size(double bytes, bool si = true);

  /// Convert an optional capacity in string into an optional capacity.
  boost::optional<int64_t>
  convert_capacity(boost::optional<std::string> capacity);

  /// Parse a capacity with embeded unit.
  ///
  /// For instance "10KB" => 10000, "10KiB" => 10240.
  int64_t convert_capacity(std::string value);

  /// Parse a capacity, with its unit on the side.
  long double
  convert_capacity(long double value, std::string unit);
}


/**
 * @file concat_performance.hpp
 * @author Marcus Edel
 *
 * Definition of the ConcatPerformance class.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#ifndef MLPACK_METHODS_ANN_LAYER_CONCAT_PERFORMANCE_HPP
#define MLPACK_METHODS_ANN_LAYER_CONCAT_PERFORMANCE_HPP

#include <mlpack/prereqs.hpp>

#include <boost/ptr_container/ptr_vector.hpp>

#include "layer_types.hpp"

namespace mlpack {
namespace ann /** Artificial Neural Network. */ {

/**
 * Implementation of the concat performance class. The class works as a
 * feed-forward fully connected network container which plugs performance layers
 * together.
 *
 * @tparam InputDataType Type of the input data (arma::colvec, arma::mat,
 *         arma::sp_mat or arma::cube).
 * @tparam OutputDataType Type of the output data (arma::colvec, arma::mat,
 *         arma::sp_mat or arma::cube).
 */
template <
    typename OutputLayerType = NegativeLogLikelihood<>,
    typename InputDataType = arma::mat,
    typename OutputDataType = arma::mat
>
class ConcatPerformance
{
 public:
  /**
   * Create the ConcatPerformance object.
   *
   * @param inSize The number of inputs.
   * @param outputLayer Output layer used to evaluate the network.
   */
  ConcatPerformance(const size_t inSize = 0,
                    OutputLayerType&& outputLayer = OutputLayerType());

  /*
   * Computes the Negative log likelihood.
   *
   * @param input Input data used for evaluating the specified function.
   * @param output Resulting output activation.
   */
  template<typename eT>
  double Forward(const arma::Mat<eT>& input, arma::Mat<eT>& target);

  /**
   * Ordinary feed backward pass of a neural network. The negative log
   * likelihood layer expectes that the input contains log-probabilities for
   * each class. The layer also expects a class index, in the range between 1
   * and the number of classes, as target when calling the Forward function.
   *
   * @param input The propagated input activation.
   * @param target The target vector, that contains the class index in the range
   *        between 1 and the number of classes.
   * @param output The calculated error.
   */
  template<typename eT>
  void Backward(const arma::Mat<eT>& input,
                const arma::Mat<eT>& target,
                arma::Mat<eT>& output);

  //! Get the output parameter.
  OutputDataType& OutputParameter() const { return outputParameter; }
  //! Modify the output parameter.
  OutputDataType& OutputParameter() { return outputParameter; }

  //! Get the delta.
  OutputDataType& Delta() const { return delta; }
  //! Modify the delta.
  OutputDataType& Delta() { return delta; }

  /**
   * Serialize the layer
   */
  template<typename Archive>
  void serialize(Archive& /* ar */, const unsigned int /* version */);

 private:
  //! Locally-stored number of inputs.
  size_t inSize;

  //! Instantiated outputlayer used to evaluate the network.
  OutputLayerType outputLayer;

  //! Locally-stored delta object.
  OutputDataType delta;

  //! Locally-stored output parameter object.
  OutputDataType outputParameter;
}; // class ConcatPerformance

} // namespace ann
} // namespace mlpack

// Include implementation.
#include "concat_performance_impl.hpp"

#endif


//---------------------------------------------------------------------------//
// Copyright (c) 2021 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2021 Ilias Khairullin <ilias@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_RANDOM_ALGEBRAIC_ENGINE_HPP
#define CRYPTO3_RANDOM_ALGEBRAIC_ENGINE_HPP

#include <type_traits>

#include <boost/type_traits.hpp>

#include <boost/random/random_device.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <boost/random/mersenne_twister.hpp>

#include <nil/crypto3/algebra/type_traits.hpp>

namespace nil {
    namespace crypto3 {
        namespace random {
            /*!
             * @brief
             * @tparam AlgebraicType denote an some algebraic type (field, curve group types).
             * @tparam Engine denote an some base \RandomNumberEngine generating random numbers
             *
             * The class template algebraic_engine is a pseudo-random number engine adaptor that generate random values
             * of algebraic type using data produced by the base engine. It models (not fully) a
             * \RandomNumberEngine. https://en.cppreference.com/w/cpp/named_req/RandomNumberEngine
             *
             * @warning The class template algebraic_engine differs from \RandomNumberEngine as it doesn't have
             * constructor and seed function with parameter of result_type. This is due to the fact that
             * algebraic_engine is adapter wrapping some base \RandomNumberEngine (Engine), so instead it has
             * constructor and seed function with parameter of Engine::result_type.
             */
            template<typename AlgebraicType, typename Engine = boost::random::mt19937, typename = void>
            struct algebraic_engine;

            template<typename AlgebraicType, typename Engine>
            struct algebraic_engine<
                AlgebraicType,
                Engine,
                typename std::enable_if<algebra::is_field<AlgebraicType>::value &&
                                        !algebra::is_extended_field<AlgebraicType>::value &&
                                        boost::is_integral<typename Engine::result_type>::value>::type> {
            protected:
                typedef AlgebraicType field_type;
                typedef typename field_type::value_type field_value_type;
                typedef typename field_type::integral_type integral_type;

                typedef Engine internal_generator_type;
                typedef boost::random::uniform_int_distribution<integral_type> internal_distribution_type;

                constexpr static integral_type _min = 0;
                constexpr static integral_type _max = field_type::modulus - 1;

            public:
                typedef field_value_type result_type;

                /**
                 * Constructs a @c algebraic_engine and calls @c seed().
                 */
                algebraic_engine() {
                    seed();
                }
                /**
                 * Constructs a @c algebraic_engine and calls @c seed(value).
                 */
                BOOST_RANDOM_DETAIL_ARITHMETIC_CONSTRUCTOR(algebraic_engine, typename Engine::result_type, value) {
                    seed(value);
                }
                /**
                 * Constructs a algebraic_engine and calls @c seed(seq).
                 *
                 * @xmlnote
                 * The copy constructor will always be preferred over
                 * the templated constructor.
                 * @endxmlnote
                 */
                BOOST_RANDOM_DETAIL_SEED_SEQ_CONSTRUCTOR(algebraic_engine, SeedSeq, seq) {
                    seed(seq);
                }

                /** Calls @c seed(default_seed) of base Engine. */
                void seed() {
                    gen.seed();
                }
                /** Calls @c seed(value) of base Engine. */
                BOOST_RANDOM_DETAIL_ARITHMETIC_SEED(algebraic_engine, typename Engine::result_type, value) {
                    gen.seed(value);
                }
                /** Calls @c seed(seq) of base Engine. */
                BOOST_RANDOM_DETAIL_SEED_SEQ_SEED(algebraic_engine, SeeqSeq, seq) {
                    gen.seed(seq);
                }

                /** Returns the smallest value that the \algebraic_random_device can produce. */
                constexpr static inline result_type min() {
                    constexpr result_type min_value(_min);
                    return min_value;
                }

                /** Returns the largest value that the \algebraic_random_device can produce. */
                constexpr static inline result_type max() {
                    constexpr result_type max_value(_max);
                    return max_value;
                }

                /** Returns a random value in the range [min, max]. */
                result_type operator()() {
                    return dist(gen);
                }

                /**
                 * Advances the state of the generator by @c z steps.  Equivalent to
                 */
                void discard(std::size_t z) {
                    while (z--) {
                        (*this)();
                    }
                }

                /** Writes a algebraic_engine to a @c std::ostream */
                template<class CharT, class Traits>
                friend std::basic_ostream<CharT, Traits>& operator<<(std::basic_ostream<CharT, Traits>& os,
                                                                     const algebraic_engine& ae) {
                    os << ae.gen;
                    return os;
                }

                /** Reads a algebraic_engine from a @c std::istream */
                template<class CharT, class Traits>
                friend std::basic_istream<CharT, Traits>& operator>>(std::basic_istream<CharT, Traits>& is,
                                                                     algebraic_engine& ae) {
                    is >> ae.gen;
                    return is;
                }

                /**
                 * Returns true if the two generators are in the same state,
                 * and will thus produce identical sequences.
                 */
                friend bool operator==(const algebraic_engine& x_, const algebraic_engine& y_) {
                    return x_.gen == y_.gen && x_.dist == y_.dist;
                }

                /**
                 * Returns true if the two generators are in different states.
                 */
                friend bool operator!=(const algebraic_engine& x_, const algebraic_engine& y_) {
                    return !(x_ == y_);
                }

            protected:
                internal_generator_type gen;
                internal_distribution_type dist = internal_distribution_type(_min, _max);
            };

            template<typename AlgebraicType, typename Engine>
            struct algebraic_engine<
                AlgebraicType,
                Engine,
                typename std::enable_if<algebra::is_field<AlgebraicType>::value &&
                                        algebra::is_extended_field<AlgebraicType>::value &&
                                        boost::is_integral<typename Engine::result_type>::value>::type> {
            protected:
                typedef AlgebraicType extended_field_type;
                typedef typename extended_field_type::value_type extended_field_value_type;
                typedef typename extended_field_type::underlying_field_type underlying_field_type;

                typedef algebraic_engine<underlying_field_type, Engine> internal_generator_type;

            public:
                typedef extended_field_value_type result_type;

                /**
                 * Constructs a @c algebraic_engine and calls @c seed().
                 */
                algebraic_engine() {
                    seed();
                }
                /**
                 * Constructs a @c algebraic_engine and calls @c seed(value).
                 */
                BOOST_RANDOM_DETAIL_ARITHMETIC_CONSTRUCTOR(algebraic_engine, typename Engine::result_type, value) {
                    seed(value);
                }
                /**
                 * Constructs a algebraic_engine and calls @c seed(seq).
                 *
                 * @xmlnote
                 * The copy constructor will always be preferred over
                 * the templated constructor.
                 * @endxmlnote
                 */
                BOOST_RANDOM_DETAIL_SEED_SEQ_CONSTRUCTOR(algebraic_engine, SeedSeq, seq) {
                    seed(seq);
                }

                /** Calls @c seed(default_seed) of base Engine. */
                void seed() {
                    gen.seed();
                }
                /** Calls @c seed(value) of base Engine. */
                BOOST_RANDOM_DETAIL_ARITHMETIC_SEED(algebraic_engine, typename Engine::result_type, value) {
                    gen.seed(value);
                }
                /** Calls @c seed(seq) of base Engine. */
                BOOST_RANDOM_DETAIL_SEED_SEQ_SEED(algebraic_engine, SeeqSeq, seq) {
                    gen.seed(seq);
                }

                /** Returns the smallest value that the \algebraic_random_device can produce. */
                // TODO: evaluate min_value at compile-time
                constexpr static inline result_type min() {
                    result_type min_value;
                    for (auto& coord : min_value.data) {
                        coord = internal_generator_type::min();
                    }

                    return min_value;
                }

                /** Returns the largest value that the \algebraic_random_device can produce. */
                // TODO: evaluate max_value at compile-time
                constexpr static inline result_type max() {
                    result_type max_value;
                    for (auto& coord : max_value.data) {
                        coord = internal_generator_type::max();
                    }

                    return max_value;
                }

                /** Returns a random value in the range [min, max]. */
                result_type operator()() {
                    result_type result;
                    for (auto& coord : result.data) {
                        coord = gen();
                    }

                    return result;
                }

                /**
                 * Advances the state of the generator by @c z steps.  Equivalent to
                 */
                void discard(std::size_t z) {
                    while (z--) {
                        (*this)();
                    }
                }

                /** Writes a algebraic_engine to a @c std::ostream */
                template<class CharT, class Traits>
                friend std::basic_ostream<CharT, Traits>& operator<<(std::basic_ostream<CharT, Traits>& os,
                                                                     const algebraic_engine& ae) {
                    os << ae.gen;
                    return os;
                }

                /** Reads a algebraic_engine from a @c std::istream */
                template<class CharT, class Traits>
                friend std::basic_istream<CharT, Traits>& operator>>(std::basic_istream<CharT, Traits>& is,
                                                                     algebraic_engine& ae) {
                    is >> ae.gen;
                    return is;
                }

                /**
                 * Returns true if the two generators are in the same state,
                 * and will thus produce identical sequences.
                 */
                friend bool operator==(const algebraic_engine& x_, const algebraic_engine& y_) {
                    return x_.gen == y_.gen;
                }

                /**
                 * Returns true if the two generators are in different states.
                 */
                friend bool operator!=(const algebraic_engine& x_, const algebraic_engine& y_) {
                    return !(x_ == y_);
                }

            protected:
                internal_generator_type gen;
            };

            template<typename AlgebraicType, typename Engine>
            struct algebraic_engine<
                AlgebraicType,
                Engine,
                typename std::enable_if<algebra::is_curve_group<AlgebraicType>::value &&
                                        boost::is_integral<typename Engine::result_type>::value>::type> {
            protected:
                typedef AlgebraicType group_type;
                typedef typename group_type::value_type group_value_type;
                typedef typename group_type::curve_type::scalar_field_type scalar_field_type;

                typedef algebraic_engine<scalar_field_type, Engine> internal_generator_type;

            public:
                typedef group_value_type result_type;

                /**
                 * Constructs a @c algebraic_engine and calls @c seed().
                 */
                algebraic_engine() {
                    seed();
                }
                /**
                 * Constructs a @c algebraic_engine and calls @c seed(value).
                 */
                BOOST_RANDOM_DETAIL_ARITHMETIC_CONSTRUCTOR(algebraic_engine, typename Engine::result_type, value) {
                    seed(value);
                }
                /**
                 * Constructs a algebraic_engine and calls @c seed(seq).
                 *
                 * @xmlnote
                 * The copy constructor will always be preferred over
                 * the templated constructor.
                 * @endxmlnote
                 */
                BOOST_RANDOM_DETAIL_SEED_SEQ_CONSTRUCTOR(algebraic_engine, SeedSeq, seq) {
                    seed(seq);
                }

                /** Calls @c seed(default_seed) of base Engine. */
                void seed() {
                    gen.seed();
                }
                /** Calls @c seed(value) of base Engine. */
                BOOST_RANDOM_DETAIL_ARITHMETIC_SEED(algebraic_engine, typename Engine::result_type, value) {
                    gen.seed(value);
                }
                /** Calls @c seed(seq) of base Engine. */
                BOOST_RANDOM_DETAIL_SEED_SEQ_SEED(algebraic_engine, SeeqSeq, seq) {
                    gen.seed(seq);
                }

                /** Returns the smallest value that the \algebraic_random_device can produce. */
                // TODO: evaluate returned value at compile-time
                constexpr static inline result_type min() {
                    return result_type::zero();
                }

                /** Returns the largest value that the \algebraic_random_device can produce. */
                // TODO: evaluate max_value at compile-time
                constexpr static inline result_type max() {
                    return result_type::one() * (scalar_field_type::modulus - 1);
                }

                /**
                 * Returns a random value in the range [min, max]. Elements of group are ordered in exponent growing
                 * order with respect to group base element.
                 */
                // TODO: check correctness of the generation method
                result_type operator()() {
                    return result_type::one() * gen();
                }

                /**
                 * Advances the state of the generator by @c z steps.  Equivalent to
                 */
                void discard(std::size_t z) {
                    while (z--) {
                        (*this)();
                    }
                }

                /** Writes a algebraic_engine to a @c std::ostream */
                template<class CharT, class Traits>
                friend std::basic_ostream<CharT, Traits>& operator<<(std::basic_ostream<CharT, Traits>& os,
                                                                     const algebraic_engine& ae) {
                    os << ae.gen;
                    return os;
                }

                /** Reads a algebraic_engine from a @c std::istream */
                template<class CharT, class Traits>
                friend std::basic_istream<CharT, Traits>& operator>>(std::basic_istream<CharT, Traits>& is,
                                                                     algebraic_engine& ae) {
                    is >> ae.gen;
                    return is;
                }

                /**
                 * Returns true if the two generators are in the same state,
                 * and will thus produce identical sequences.
                 */
                friend bool operator==(const algebraic_engine& x_, const algebraic_engine& y_) {
                    return x_.gen == y_.gen;
                }

                /**
                 * Returns true if the two generators are in different states.
                 */
                friend bool operator!=(const algebraic_engine& x_, const algebraic_engine& y_) {
                    return !(x_ == y_);
                }

            protected:
                internal_generator_type gen;
            };
        }    // namespace random
    }        // namespace crypto3
}    // namespace nil

#endif    // CRYPTO3_RANDOM_ALGEBRAIC_ENGINE_HPP


#ifndef STAN_MATH_PRIM_SCAL_PROB_LOGNORMAL_RNG_HPP
#define STAN_MATH_PRIM_SCAL_PROB_LOGNORMAL_RNG_HPP

#include <boost/random/lognormal_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <stan/math/prim/scal/err/check_consistent_sizes.hpp>
#include <stan/math/prim/scal/err/check_finite.hpp>
#include <stan/math/prim/scal/err/check_nonnegative.hpp>
#include <stan/math/prim/scal/err/check_not_nan.hpp>
#include <stan/math/prim/scal/err/check_positive_finite.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/fun/value_of.hpp>
#include <stan/math/prim/scal/fun/square.hpp>
#include <stan/math/prim/scal/meta/include_summand.hpp>

namespace stan {
  namespace math {

    template <class RNG>
    inline double
    lognormal_rng(double mu,
                  double sigma,
                  RNG& rng) {
      using boost::variate_generator;
      using boost::random::lognormal_distribution;

      static const char* function("lognormal_rng");

      check_finite(function, "Location parameter", mu);
      check_positive_finite(function, "Scale parameter", sigma);

      variate_generator<RNG&, lognormal_distribution<> >
        lognorm_rng(rng, lognormal_distribution<>(mu, sigma));
      return lognorm_rng();
    }

  }
}
#endif


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2018-2020, LAAS-CNRS, University of Edinburgh
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#ifndef CROCODDYL_CORE_SOLVERS_DDP_HPP_
#define CROCODDYL_CORE_SOLVERS_DDP_HPP_

#include <Eigen/Cholesky>
#include <vector>
#include "crocoddyl/core/solver-base.hpp"

namespace crocoddyl {

class SolverDDP : public SolverAbstract {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  explicit SolverDDP(boost::shared_ptr<ShootingProblem> problem);
  ~SolverDDP();

  virtual bool solve(const std::vector<Eigen::VectorXd>& init_xs = DEFAULT_VECTOR,
                     const std::vector<Eigen::VectorXd>& init_us = DEFAULT_VECTOR, const std::size_t& maxiter = 100,
                     const bool& is_feasible = false, const double& regInit = 1e-9);
  virtual void computeDirection(const bool& recalc = true);
  virtual double tryStep(const double& steplength = 1);
  virtual double stoppingCriteria();
  virtual const Eigen::Vector2d& expectedImprovement();
  virtual double calc();
  virtual void backwardPass();
  virtual void forwardPass(const double& stepLength);

  virtual void computeGains(const std::size_t& t);
  void increaseRegularization();
  void decreaseRegularization();
  virtual void allocateData();

  const double& get_regfactor() const;
  const double& get_regmin() const;
  const double& get_regmax() const;
  const std::vector<double>& get_alphas() const;
  const double& get_th_stepdec() const;
  const double& get_th_stepinc() const;
  const double& get_th_grad() const;
  const std::vector<Eigen::MatrixXd>& get_Vxx() const;
  const std::vector<Eigen::VectorXd>& get_Vx() const;
  const std::vector<Eigen::MatrixXd>& get_Qxx() const;
  const std::vector<Eigen::MatrixXd>& get_Qxu() const;
  const std::vector<Eigen::MatrixXd>& get_Quu() const;
  const std::vector<Eigen::VectorXd>& get_Qx() const;
  const std::vector<Eigen::VectorXd>& get_Qu() const;
  const std::vector<Eigen::MatrixXd>& get_K() const;
  const std::vector<Eigen::VectorXd>& get_k() const;
  const std::vector<Eigen::VectorXd>& get_fs() const;

  void set_regfactor(const double& reg_factor);
  void set_regmin(const double& regmin);
  void set_regmax(const double& regmax);
  void set_alphas(const std::vector<double>& alphas);
  void set_th_stepdec(const double& th_step);
  void set_th_stepinc(const double& th_step);
  void set_th_grad(const double& th_grad);

 protected:
  double regfactor_;  //!< Regularization factor used to decrease / increase it
  double regmin_;     //!< Minimum allowed regularization value
  double regmax_;     //!< Maximum allowed regularization value

  double cost_try_;                      //!< Total cost computed by line-search procedure
  std::vector<Eigen::VectorXd> xs_try_;  //!< State trajectory computed by line-search procedure
  std::vector<Eigen::VectorXd> us_try_;  //!< Control trajectory computed by line-search procedure
  std::vector<Eigen::VectorXd> dx_;

  // allocate data
  std::vector<Eigen::MatrixXd> Vxx_;  //!< Hessian of the Value function
  std::vector<Eigen::VectorXd> Vx_;   //!< Gradient of the Value function
  std::vector<Eigen::MatrixXd> Qxx_;  //!< Hessian of the Hamiltonian
  std::vector<Eigen::MatrixXd> Qxu_;  //!< Hessian of the Hamiltonian
  std::vector<Eigen::MatrixXd> Quu_;  //!< Hessian of the Hamiltonian
  std::vector<Eigen::VectorXd> Qx_;   //!< Gradient of the Hamiltonian
  std::vector<Eigen::VectorXd> Qu_;   //!< Gradient of the Hamiltonian
  std::vector<Eigen::MatrixXd> K_;    //!< Feedback gains
  std::vector<Eigen::VectorXd> k_;    //!< Feed-forward terms
  std::vector<Eigen::VectorXd> fs_;   //!< Gaps/defects between shooting nodes

  Eigen::VectorXd xnext_;
  Eigen::MatrixXd FxTVxx_p_;
  std::vector<Eigen::MatrixXd> FuTVxx_p_;
  Eigen::VectorXd fTVxx_p_;
  std::vector<Eigen::LLT<Eigen::MatrixXd> > Quu_llt_;
  std::vector<Eigen::VectorXd> Quuk_;
  std::vector<double> alphas_;  //!< Set of step lengths using by the line-search procedure
  double th_grad_;              //!< Tolerance of the expected gradient used for testing the step
  double th_stepdec_;           //!< Step-length threshold used to decrease regularization
  double th_stepinc_;           //!< Step-length threshold used to increase regularization
  bool was_feasible_;           //!< Label that indicates in the previous iterate was feasible
};

}  // namespace crocoddyl

#endif  // CROCODDYL_CORE_SOLVERS_DDP_HPP_


#include "vtkCarGeometricCalibration.h"

#include <cmath>
#include <numeric>
#include <random>
#include <iostream>
#include <fstream>

#include <vtkMath.h>

#include <Eigen/Geometry>

#include "vtkConversions.h"
#include "vtkEigenTools.h"

Eigen::Vector3d GetXYZ(const vtkSmartPointer<vtkVelodyneTransformInterpolator> trajectory,
                       double time)
{
  vtkSmartPointer<vtkTransform> transform = vtkSmartPointer<vtkTransform>::New();
  trajectory->InterpolateTransform(time, transform);
  return PositionVectorFromTransform(transform);
}

Eigen::Matrix3d GetR(const vtkSmartPointer<vtkVelodyneTransformInterpolator> trajectory,
                     double time)
{
  vtkSmartPointer<vtkTransform> transform = vtkSmartPointer<vtkTransform>::New();
  trajectory->InterpolateTransform(time, transform);
  return RotationMatrixFromTransform(transform);
}

// method to identify a directions in straight lines
enum class DIRECTION_METHOD
{
  TWO_POINTS,
};

// method to identify the normal of the plane containing the turn
enum class NORMAL_METHOD
{
  CROSS_PRODUCT,
};

// method to optimize the direction after the normal has been computed
enum class DIRECTION_OPTIMIZATION_METHOD
{
  NONE,
};

// method to compute an average orientation
// in a zone where the orientation should be constant
// (but could contain noise)
enum class AVERAGE_ORIENTATION_METHOD
{
  SINGLE_POINT
};

std::vector<std::vector<double>> ComputeTurns(
        const vtkSmartPointer<vtkVelodyneTransformInterpolator> poseTrajectory,
        double timeWindow,
        double curveTreshold,
        bool emptyIntersection,
        std::string debugCSV,
        bool verbose
        )
{
  int minPtsInDirectionSets = 3; // could be as low as 2
  int minPtsInTurn = 3; // could be as low as 1
  std::ofstream debugFile;
  bool debugFileEnabled = !debugCSV.empty();
  if (debugFileEnabled)
  {
    debugFile.open(debugCSV);
  }
  // Compute local curvature, in rad/second
  // for each sample: is it inside a turn ?
  // Using the curveTreshold treshold
  std::vector<int> sampleId =
    std::vector<int>();
  sampleId.reserve(poseTrajectory->GetNumberOfTransforms());
  std::vector<double> sampleTime =
    std::vector<double>();
  sampleTime.reserve(poseTrajectory->GetNumberOfTransforms());
  std::vector<bool> sampleStatus =
    std::vector<bool>();
  sampleStatus.reserve(poseTrajectory->GetNumberOfTransforms());

  if (verbose)
  {
    std::cout << "Computing per-sample status for "
              << poseTrajectory->GetNumberOfTransforms()
              << " samples" << std::endl;
  }
  for (int i = 0; i < poseTrajectory->GetNumberOfTransforms(); i++)
  {
    vtkSmartPointer<vtkTransform> sample = vtkSmartPointer<vtkTransform>::New();
    double t;
    poseTrajectory->GetSample(i, sample, t);
    if (t - 0.5 * timeWindow < poseTrajectory->GetMinimumT()
        || t + 0.5 * timeWindow > poseTrajectory->GetMaximumT())
    {
      continue;
    }
    sampleId.push_back(i);
    sampleTime.push_back(t);

    vtkSmartPointer<vtkTransform> prev = vtkSmartPointer<vtkTransform>::New();
    poseTrajectory->InterpolateTransform(t - 0.5 * timeWindow, prev);
    vtkSmartPointer<vtkTransform> next = vtkSmartPointer<vtkTransform>::New();
    poseTrajectory->InterpolateTransform(t + 0.5 * timeWindow, next);
    Eigen::AngleAxisd aa = Eigen::AngleAxisd(RotationMatrixFromTransform(next)
		    * RotationMatrixFromTransform(prev).transpose());
    double curvature = std::abs(aa.angle()) / timeWindow;
    sampleStatus.push_back(curvature >= curveTreshold);
  }

  if (verbose)
  {
    std::cout << "Aggregating samples" << std::endl;
  }
  // Compute turn limits (exprimed in samples indexes)
  unsigned int i = 0; // ! warning that is not the id of a sample
  // if the first samples are inside a turn, discard this turn
  // and start searching turns after its end.
  while (i < sampleStatus.size() && sampleStatus[i] == true)
  {
    i = i + 1;
  }
  // we are ready to search turns and know that sample i
  // is not inside a turn
  bool inside = false;

  // turn_limits[n][0] will contain the time of the first sample inside turn n
  // turn_limits[n][1] will contain the time of the last sample inside turn n
  // We have turn_limits[n][0] <= turn_limits[n][1]
  std::vector<std::vector<int>> turnLimits = std::vector<std::vector<int>>();
  for (unsigned int j = i; j < sampleStatus.size(); j++)
  {
    if (!inside && sampleStatus[j])
    {
      // entering turn
      inside = true;
      turnLimits.push_back(std::vector<int>());
      turnLimits[turnLimits.size() - 1].push_back(j);
    }
    else if (inside && !sampleStatus[j])
    {
      // leaving turn
      inside = false;
      turnLimits[turnLimits.size() - 1].push_back(j - 1);
    }
  }
  // if the end of last turn is not seen, discard this turn
  if (inside)
  {
    if (verbose)
    {
      std::cout << "pop_back" << std::endl;
    }
    turnLimits.pop_back();
  }

  // convert indexes to times:
  if (verbose)
  {
    std::cout << "Converting indexes to time" << std::endl;
  }
  std::vector<std::vector<double>> turnTimes = std::vector<std::vector<double>>();

  for (unsigned int i = 0; i < turnLimits.size(); i++) {
    if (turnLimits[i][1] - turnLimits[i][0] + 1 < minPtsInTurn)
    {
      if (verbose)
      {
        std::cout << "warning: skipped one turn that did not contain enough points" << std::endl;
      }
      continue;
    }
    std::vector<double> times = std::vector<double>(2);
    times[0] = sampleTime[turnLimits[i][0]];
    times[1] = sampleTime[turnLimits[i][1]];
    turnTimes.push_back(times);
  }

  // Now we want to get a direction vector before and after the curve,
  // if that is possible.
  // ! we cannot use the time to look N seconds before/after,
  // because turns are ofen preceded by a traffic light
  // which can be red, in this case there is a stop in the trajectory,
  // right before the turn. Sometimes there is a stop
  // just at the end of a turn if there is a traffic jam.
  // In both cases the risk is that the set of points selected with time
  // before/after the turn will be just 1 point,
  // or will be a set of points very centered locally.
  // (So this set of points will not permit a reliable estimation of
  // a direction vector)

  // precompute a way to tell the length of the curve at any time
  // (will be used by dichotomy search)
  if (verbose)
  {
    std::cout << "Precomputing curve length" << std::endl;
  }
  double currentLength = 0.0;
  std::vector<double> length = std::vector<double>();
  length.reserve(poseTrajectory->GetNumberOfTransforms());
  std::vector<double> lengthTimes = std::vector<double>();
  lengthTimes.reserve(poseTrajectory->GetNumberOfTransforms());
  vtkSmartPointer<vtkTransform> sample = vtkSmartPointer<vtkTransform>::New();
  double time;
  poseTrajectory->GetSample(0, sample, time);
  Eigen::Vector3d previousPos = PositionVectorFromTransform(sample);
  for (int i = 0; i < poseTrajectory->GetNumberOfTransforms(); i++)
  {
    poseTrajectory->GetSample(i, sample, time);
    Eigen::Vector3d pos = PositionVectorFromTransform(sample);
    currentLength += (pos - previousPos).norm();
    lengthTimes.push_back(time);
    length.push_back(currentLength);
    previousPos = pos;
  }

  if (verbose)
  {
    std::cout << "Placing bounds" << std::endl;
  }
  std::vector<std::vector<double>> bounds = std::vector<std::vector<double>>();
  for (unsigned int i = 0; i < turnTimes.size(); i++)
  {
    int iStart = std::distance(lengthTimes.begin(),
		    std::lower_bound(lengthTimes.begin(), lengthTimes.end(), turnTimes[i][0]));
    int iEnd = std::distance(lengthTimes.begin(),
		    std::lower_bound(lengthTimes.begin(), lengthTimes.end(), turnTimes[i][1]));
    double turnLength = length[iEnd] - length[iStart];
    if (verbose)
    {
      std::cout << "turn length is: " << turnLength << " (scale unknown)" << std::endl;
    }
    int iBefore = std::distance(length.begin(),
		    std::lower_bound(length.begin(), length.end(), length[iStart] - turnLength));
    int iAfter = std::distance(length.begin(),
		    std::lower_bound(length.begin(), length.end(), length[iEnd] + turnLength));
    // if it was not possible to find iBefore/iAfter
    // it means that the turn is either at the very start
    // or very end of the trajectory, so it is hard to tell which
    // was the direction before the turn
    if (iBefore == 0 || iAfter == static_cast<int>(length.size()))
    {
      continue;
    }
    // if there is not enough points in sets "before" or "after",
    // discard this turn:
    if (iStart - iBefore + 1 < minPtsInDirectionSets
        || iAfter - iEnd + 1 < minPtsInDirectionSets)
    {
      continue;
    }
    std::vector<double> bound = std::vector<double>(4);
    bound[0] = lengthTimes[iBefore];
    bound[1] = turnTimes[i][0];
    bound[2] = turnTimes[i][1];
    bound[3] = lengthTimes[iAfter];
    bounds.push_back(bound);
  }

  // look for bug in my algo/my code/the data:
  std::vector<int> misbounded = std::vector<int>();
  for (unsigned int i = 0; i < bounds.size(); i++)
  {
    if (!(bounds[i][0] < bounds[i][1]
          && bounds[i][1] < bounds[i][2]
          && bounds[i][2] < bounds[i][3]))
    {
      if (verbose)
      {
        std::cout << "warning: turn " << i
          << "(" << bounds[i][0] << ", "
          << bounds[i][1] << ", "
          << bounds[i][2] << ", "
          << bounds[i][3] << ") is not bounded correctly, discarding it."
          << std::endl;
      }
      misbounded.push_back(i);
    }
  }
  // delete misbounded turns:
  if (verbose)
  {
    std::cout << "Discarding mis-bounded turns" << std::endl;
  }
  for (int i = misbounded.size() - 1; i >= 0; i--)
  {
    bounds.erase(bounds.begin() + misbounded[i]);
  }

  // handle the cases where two turns are close
  // if possible, make the bounds intersection empty
  // this is not mandatory but it help visualization a lot
  if (emptyIntersection)
  {
    if (verbose)
    {
      std::cout << "Changing bounds to empty intersections" << std::endl;
    }
    std::vector<int> mixedUpTurns = std::vector<int>();
    for (unsigned int i = 1; i < bounds.size(); i++)
    {
      if (bounds[i - 1][3] > bounds[i][0])
      {
        double newLimit = 0.5 * (bounds[i - 1][3] + bounds[i][0]);
        // check if this new_limit solves the problem:
        if (newLimit <= bounds[i - 1][2])
        {
          // cannot solve easily, discard the first turn
          if (verbose)
          {
          std::cout << "warning: could not fix (first) turn " << i - 1
            << " discarding it" << std::endl;
          }
          mixedUpTurns.push_back(i - 1);
        }
        else if (newLimit >= bounds[i][1])
        {
          // cannot solve easily, discard the second turn
          if (verbose)
          {
            std::cout << "warning: could not fix (second) turn " << i
              << " discarding it" << std::endl;
          }
          mixedUpTurns.push_back(i);
        }
        else
        {
          // one could argue that we should not do this fix
          // because the lengths of the curve in the new after/before
          // parts will not be equal to the lengths inside the turns
          if (verbose)
          {
            std::cout << "warning: fixing bounds of turns " << i - 1
              << " and " << i << " that are touching each other" << std::endl;
          }
          bounds[i - 1][3] = newLimit;
          bounds[i][0] = newLimit;
        }
      }
    }

    if (verbose)
    {
      std::cout << "Discarding turns with no empty intersection" << std::endl;
    }
    // delete turns whose bounds intersection could not be made empty:
    for (int i = mixedUpTurns.size() - 1; i >= 0; i--)
    {
      bounds.erase(bounds.begin() + mixedUpTurns[i]);
    }
  }

  // write debug file:
  // may not work correctly if emptyIntersection != true
  if (debugFileEnabled)
  {
    if (verbose)
    {
      std::cout << "Writing debug file" << std::endl;
    }
    int currentTurnToConsider = 0;
    for (int i = 0; i < poseTrajectory->GetNumberOfTransforms(); i++)
    {
      vtkSmartPointer<vtkTransform> sample = vtkSmartPointer<vtkTransform>::New();
      double time;
      poseTrajectory->GetSample(i, sample, time);
      double status;
      if (currentTurnToConsider == static_cast<int>(bounds.size()))
      {
        status = 0.0; // will stay outside from now on
      }
      else if (time < bounds[currentTurnToConsider][0])
      {
        status = 0.0; // before
      }
      else if (time < bounds[currentTurnToConsider][1])
      {
        status = 0.4; // inside "before" set of samples
      }
      else if (time < bounds[currentTurnToConsider][2])
      {
        status = 1.0; // inside turn samples
      }
      else if (time < bounds[currentTurnToConsider][3])
      {
        status = 0.2; // inside "after" set of samples
      }
      else
      {
        status = 0.0; // after
        currentTurnToConsider = currentTurnToConsider + 1;
      }
      if (debugFileEnabled)
      {
        debugFile << PositionVectorFromTransform(sample)[0]
                  << "," << PositionVectorFromTransform(sample)[1]
                  << "," << PositionVectorFromTransform(sample)[2]
                  << "," << status << std::endl;
      }
    }
  }

  if (debugFileEnabled)
  {
    if (verbose)
    {
      std::cout << "Closing debug file" << std::endl;
    }
    debugFile.close();
  }

  return bounds;
}

bool ProcessTurn(
        const vtkSmartPointer<vtkVelodyneTransformInterpolator> reference,
        const vtkSmartPointer<vtkVelodyneTransformInterpolator> aligned,
        double t0, double t1, double t2, double t3,
        DIRECTION_METHOD directionMethod,
        NORMAL_METHOD normalMethod,
        DIRECTION_OPTIMIZATION_METHOD normalBasedDirectionOptimisation,
        AVERAGE_ORIENTATION_METHOD orientationMethod,
        Eigen::Matrix3d& rotationBefore, // 1st result
        Eigen::Matrix3d& rotationAfter, // 2nd result
        double& scaleBefore,
        double& scaleAfter
        )
{
  if (!(t0 < t1 && t1 < t2 && t2 < t3))
  {
    std::cerr << "WARNING: times not valid in ProcessTurn(). Bug in implementation" << std::endl;
    return false; // rotationBefore/After are not valid
  }

  Eigen::Vector3d vBeforeReference = GetXYZ(reference, t1) - GetXYZ(reference, t0);
  Eigen::Vector3d vAfterReference = GetXYZ(reference, t3) - GetXYZ(reference, t2);
  Eigen::Vector3d vBeforeAligned = GetXYZ(aligned, t1) - GetXYZ(aligned, t0);
  Eigen::Vector3d vAfterAligned = GetXYZ(aligned, t3) - GetXYZ(aligned, t2);

  scaleBefore = vBeforeAligned.norm() / vBeforeReference.norm();
  scaleAfter = vAfterAligned.norm() / vAfterReference.norm();

  // depending on options, these values will be recomputed later
  // using a more robust method
  Eigen::Vector3d directionBeforeReference = vBeforeReference.normalized();
  Eigen::Vector3d directionAfterReference = vAfterReference.normalized();
  Eigen::Vector3d directionBeforeAligned = vBeforeAligned.normalized();
  Eigen::Vector3d directionAfterAligned = vAfterAligned.normalized();

  if (directionMethod == DIRECTION_METHOD::TWO_POINTS)
  {
    // no need to recompute direction{Before,After}{Reference,Aligned}
  }
  else
  {
    std::cerr << "WARNING: unimplemented option passed to ProcessTurn()" << std::endl;
    return false; // rotationBefore/After are not valid
  }

  // compute normal to plane containing the trajectory
  Eigen::Vector3d normalOrientationReference;
  Eigen::Vector3d normalOrientationAligned;
  if (normalMethod == NORMAL_METHOD::CROSS_PRODUCT)
  {
    normalOrientationReference = directionBeforeReference.cross(directionAfterReference);
    normalOrientationAligned = directionBeforeAligned.cross(directionAfterAligned);
  }
  else
  {
    std::cerr << "WARNING: unimplemented option passed to ProcessTurn()" << std::endl;
    return false; // rotationBefore/After are not valid
  }

  if (normalBasedDirectionOptimisation == DIRECTION_OPTIMIZATION_METHOD::NONE)
  {
    // nothing to do
  }
  else
  {
    std::cerr << "WARNING: unimplemented option passed to ProcessTurn()" << std::endl;
    return false; // rotationBefore/After are not valid
  }

  // compute orientation of trajectories
  Eigen::Matrix3d trajectoryOrientationBeforeReference;
  // operator "<<" stacks the vectors horizontally
  // (Eigen vectors are vertical i.e. single-column matrices)
  trajectoryOrientationBeforeReference <<
      directionBeforeReference,
      normalOrientationReference,
      directionBeforeReference.cross(normalOrientationReference);

  Eigen::Matrix3d trajectoryOrientationAfterReference;
  trajectoryOrientationAfterReference <<
      directionAfterReference,
      normalOrientationReference,
      directionAfterReference.cross(normalOrientationReference);

  Eigen::Matrix3d trajectoryOrientationBeforeAligned;
  trajectoryOrientationBeforeAligned <<
      directionBeforeAligned,
      normalOrientationAligned,
      directionBeforeAligned.cross(normalOrientationAligned);

  Eigen::Matrix3d trajectoryOrientationAfterAligned;
  trajectoryOrientationAfterAligned <<
      directionAfterAligned,
      normalOrientationAligned,
      directionAfterAligned.cross(normalOrientationAligned);

  Eigen::Matrix3d RRemapBefore = trajectoryOrientationBeforeReference
      * trajectoryOrientationBeforeAligned.transpose();

  Eigen::Matrix3d RRemapAfter = trajectoryOrientationAfterReference
      * trajectoryOrientationAfterAligned.transpose();

  Eigen::Matrix3d orientationBeforeReference;
  Eigen::Matrix3d orientationAfterReference;
  Eigen::Matrix3d orientationBeforeAligned;
  Eigen::Matrix3d orientationAfterAligned;
  // compute remapped orientation of sensors
  if (orientationMethod == AVERAGE_ORIENTATION_METHOD::SINGLE_POINT)
  {
        orientationBeforeReference = GetR(reference, t0);
        orientationAfterReference = GetR(reference, t3);
        orientationBeforeAligned = GetR(aligned, t0);
        orientationAfterAligned = GetR(aligned, t3);
  }
  else
  {
    std::cerr << "WARNING: unimplemented option passed to ProcessTurn()" << std::endl;
    return false; // rotationBefore/After are not valid
  }

  // compute rotation between sensors
  // so that R_rotation * orientation_aligned = orientation_reference
  rotationBefore = orientationBeforeReference.transpose()
      * RRemapBefore * orientationBeforeAligned;
  rotationAfter = orientationAfterReference.transpose()
      * RRemapAfter * orientationAfterAligned;
  return true; // rotations are valid
}

bool ProcessTurn(
        const vtkSmartPointer<vtkVelodyneTransformInterpolator> reference,
        const vtkSmartPointer<vtkVelodyneTransformInterpolator> aligned,
        double t0, double t1, double t2, double t3,
        Eigen::Matrix3d& rotationBefore, // 1st result
        Eigen::Matrix3d& rotationAfter, // 2nd result
        double scaleBefore,
        double scaleAfter
        )
{
  return ProcessTurn(
        reference,
        aligned,
        t0, t1, t2, t3,
        DIRECTION_METHOD::TWO_POINTS,
        NORMAL_METHOD::CROSS_PRODUCT,
        DIRECTION_OPTIMIZATION_METHOD::NONE,
        AVERAGE_ORIENTATION_METHOD::SINGLE_POINT,
        rotationBefore,
        rotationAfter,
        scaleBefore,
        scaleAfter
        );
}

enum class ROTATION_ESTIMATOR
{
  ESTIMATOR_UNDEFINED = 0,
  ESTIMATOR_L2_CHORDAL_SVD = 0
};

void estimateL2ChordalSVD(const std::vector<Eigen::Matrix3d>& rotations,
                    Eigen::Matrix3d& result,
                    bool& valid)
{
  if (rotations.size() == 0)
  {
    result = Eigen::Matrix3d::Identity();
    valid = false;
  }

  Eigen::Matrix3d S = Eigen::Matrix3d::Zero();
  for (unsigned int i = 0; i < rotations.size(); i++)
  {
    S += rotations[i];
  }

  Eigen::JacobiSVD<Eigen::Matrix3d> svd(S, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Matrix3d U = svd.matrixU();
  Eigen::Matrix3d V = svd.matrixV();
  // we should have S == U * D * V.transpose();

  Eigen::Matrix3d R = U *  V.transpose(); // not using singular values
  if (R.determinant() < 0.0) // i.e. close to -1
  {
    Eigen::Vector3d D;
    D << 1.0, 1.0, -1.0;
    R = U * D.asDiagonal() * V.transpose();
  }

  result = R;
  valid = true;
}

void RansacRotation(const std::vector<Eigen::Matrix3d>& rotations,
                    ROTATION_ESTIMATOR estimator,
                    int maxIterations,
                    int sampleToEstimate,
                    int sampleToValidate,
                    float maxAngleToFit,
                    Eigen::Matrix3d& result,
                    bool& valid,
                    int& samplesUsed)
{
  std::default_random_engine rng = std::default_random_engine();
  std::vector<int> shuffled(rotations.size());
  std::iota(std::begin(shuffled), std::end(shuffled), 0); //0 is the starting number

  if (estimator != ROTATION_ESTIMATOR::ESTIMATOR_L2_CHORDAL_SVD)
  {
    std::cerr << "Unknown estimator passed to RansacRotation" << std::endl;
    result = Eigen::Matrix3d::Identity();
    valid = false;
    return;
  }
  for (int i = 0; i < maxIterations; i++)
  {
    // re-shuffle
    std::shuffle(std::begin(shuffled), std::end(shuffled), rng);
    std::vector<Eigen::Matrix3d> samples = std::vector<Eigen::Matrix3d>(sampleToEstimate);
    for (int j = 0; j < sampleToEstimate; j++)
    {
      samples[j] = rotations[shuffled[j]];
    }

    Eigen::Matrix3d estimation;
    bool estimationValid = false;
    estimateL2ChordalSVD(samples, estimation, estimationValid);
    if (!estimationValid)
    {
      continue;
    }

    // count how many rotations are close enough:
    std::vector<Eigen::Matrix3d> samplesFitting = std::vector<Eigen::Matrix3d>();
    samplesFitting.reserve(rotations.size());
    for (unsigned int j = 0; j < rotations.size(); j++)
    {
      double angle = Eigen::AngleAxisd(estimation.transpose() * rotations[j]).angle();
      if (angle <= maxAngleToFit)
      {
        samplesFitting.push_back(rotations[j]);
      }
    }

    if (static_cast<int>(samplesFitting.size()) >= sampleToValidate)
    {
      // do a final estimation, then return
      samplesUsed = samplesFitting.size();
      Eigen::Matrix3d estimation;
      bool estimationValid = false;
      estimateL2ChordalSVD(samplesFitting, estimation, estimationValid);
      result = estimation;
      valid = estimationValid;
      return;
    }
  }

  result = Eigen::Matrix3d::Identity();
  valid = false;
}

void ComputeCarCalibrationRotationScale(
        const vtkSmartPointer<vtkTemporalTransforms> reference,
        const vtkSmartPointer<vtkTemporalTransforms> aligned,
        double curveTreshold,
        int ransacMaxIter,
        double ransacMaxAngleToFit,
        double ransacFittingRatio,
        double ransacValidationRatio,
        DIRECTION_METHOD directionMethod,
        NORMAL_METHOD normalMethod,
        DIRECTION_OPTIMIZATION_METHOD normalBasedDirectionOptimisation,
        AVERAGE_ORIENTATION_METHOD orientationMethod,
        Eigen::Matrix3d& result,
        double& scale,
        bool& validResult,
        bool verbose
        )
{
  vtkSmartPointer<vtkVelodyneTransformInterpolator> referenceI
      = reference->CreateInterpolator();
  referenceI->SetInterpolationTypeToLinear();
  vtkSmartPointer<vtkVelodyneTransformInterpolator> alignedI
      = aligned->CreateInterpolator();
  alignedI->SetInterpolationTypeToLinear();
  if (verbose)
  {
    std::cout << "Detecting turns" << std::endl;
  }
  std::vector<std::vector<double>> turns =
      ComputeTurns(referenceI, 0.8, curveTreshold, false, "", verbose);
  if (verbose)
  {
    std::cout << "Processing " << turns.size() << " turns" << std::endl;
  }

  std::vector<Eigen::Matrix3d> rotations = std::vector<Eigen::Matrix3d>();
  std::vector<double> scales;
  for (unsigned int i = 0; i < turns.size(); i++)
  {
    if (turns[i][0] < alignedI->GetMinimumT()
                    || turns[i][3] > alignedI->GetMaximumT())
    {
      // if this turn (that was seen in "reference")
      // is not contained in aligned, skip it
      continue;
    }
    Eigen::Matrix3d RBefore;
    Eigen::Matrix3d RAfter;
    double scaleBefore, scaleAfter;
    ProcessTurn(referenceI, alignedI,
                turns[i][0], turns[i][1], turns[i][2], turns[i][3],
                    directionMethod,
                    normalMethod,
                    normalBasedDirectionOptimisation,
                    orientationMethod,
                    RBefore,
                    RAfter,
                    scaleBefore,
                    scaleAfter);

    scales.push_back(scaleBefore);
    scales.push_back(scaleAfter);

    Eigen::Vector3d yprBefore = (180.0 / vtkMath::Pi()) * RBefore.eulerAngles(2,1,0);
    Eigen::Vector3d yprAfter = (180.0 / vtkMath::Pi()) * RAfter.eulerAngles(2,1,0);
    if (verbose)
    {
      std::cout << "rotation before: " << yprBefore[2]
                << ", " << yprBefore[1]
                << ", " << yprBefore[0] << std::endl;
      std::cout << "rotation after: " << yprAfter[2]
                << ", " << yprAfter[1]
                << ", " << yprAfter[0] << std::endl;
    }

    if (RBefore.allFinite())
    {
      rotations.push_back((RBefore));
    }
    if (RAfter.allFinite())
    {
      rotations.push_back((RAfter));
    }
  }

  if (verbose)
  {
    // must be displayed before any call of median on the std::vector, else the
    // order is changed
    for (unsigned int i = 0; i < scales.size() / 2; i++)
    {
      std::cout << "scale before: " << scales[2*i]
                << ", scale after: " << scales[2*i+1] << std::endl;
    }
  }

  scale = ComputeMedian(scales);

  if (verbose)
  {
    std::cout << "median of scales is: " << ComputeMedian(scales) << std::endl;
  }

  Eigen::Matrix3d R;
  bool valid;
  int sampleUsed = 0;
  RansacRotation(rotations,
                 ROTATION_ESTIMATOR::ESTIMATOR_L2_CHORDAL_SVD,
                 ransacMaxIter,
                 std::max(1, static_cast<int>(vtkMath::Round(ransacFittingRatio * static_cast<double>(rotations.size())))),
                 std::max(1, static_cast<int>(vtkMath::Round(ransacValidationRatio * static_cast<double>(rotations.size())))),
                 (vtkMath::Pi() / 180.0) * ransacMaxAngleToFit,
                 R,
                 valid,
                 sampleUsed);

  if (verbose)
  {
    if (valid)
    {
      std::cout << "Ransac result is valid" << std::endl;
    }
    else
    {
      std::cout << "Ransac result not valid" << std::endl;
    }
  }
  Eigen::Vector3d rotationYPR = (180.0 / vtkMath::Pi()) * R.eulerAngles(2,1,0);
  if (verbose)
  {
    std::cout << "Rotation found using " << sampleUsed << " samples among " << rotations.size() << ": "
              << rotationYPR[2] << ", " << rotationYPR[1] << ", " << rotationYPR[0] << std::endl;
  }

  if (valid)
  {
    result = R;
  }
  else
  {
    result = Eigen::Matrix3d::Identity();
  }

  validResult = valid;
}

void ComputeCarCalibrationRotationScale(
        const vtkSmartPointer<vtkTemporalTransforms> reference,
        const vtkSmartPointer<vtkTemporalTransforms> aligned,
        double curveTreshold,
        int ransacMaxIter,
        double ransacMaxAngleToFit,
        double ransacFittingRatio,
        double ransacValidationRatio,
        Eigen::Matrix3d& result,
        double& scale,
        bool& validResult,
        bool verbose
        )
{
  ComputeCarCalibrationRotationScale(reference, aligned, curveTreshold,
        ransacMaxIter, ransacMaxAngleToFit,
        ransacFittingRatio, ransacValidationRatio,
        DIRECTION_METHOD::TWO_POINTS,
        NORMAL_METHOD::CROSS_PRODUCT,
        DIRECTION_OPTIMIZATION_METHOD::NONE,
        AVERAGE_ORIENTATION_METHOD::SINGLE_POINT,
        result, scale, validResult, verbose);
}


/**
 * @author Andre Anjos <andre.anjos@idiap.ch>
 * @date Sun 27 Oct 09:02:32 2013
 *
 * @brief LogNormal distributions (with floating point numbers)
 */

#define BOB_CORE_RANDOM_MODULE
#include <bob.core/random_api.h>
#include <bob.blitz/cppapi.h>
#include <bob.blitz/cleanup.h>
#include <bob.extension/documentation.h>
#include <boost/make_shared.hpp>

#include <bob.core/random.h>

static auto lognormal_doc = bob::extension::ClassDoc(
  BOB_EXT_MODULE_PREFIX ".lognormal",
  "Models a random lognormal distribution",
  "This distribution produces random numbers :math:`x` distributed with the probability density function\n\n"
  ".. math::\n\n   p(x) = \\frac{1}{x \\sigma_N \\sqrt{2\\pi}} e^{\\frac{-\\left(\\log(x)-\\mu_N\\right)^2}{2\\sigma_N^2}}\n\n"
  "for :math:`x > 0` and :math:`\\sigma_N = \\sqrt{\\log\\left(1 + \\frac{\\sigma^2}{\\mu^2}\\right)}`, "
  "where the ``mean`` (:math:`\\mu`) and ``sigma`` (:math:`\\sigma`, the standard deviation) are the parameters of this distribution class."
)
.add_constructor(bob::extension::FunctionDoc(
  "lognormal",
  "Constructs a new lognormal distribution object"
)
.add_prototype("dtype, [mean], [sigma]", "")
.add_parameter("dtype", ":py:class:`numpy.dtype` or anything that converts to a dtype", "The data type to get the distribution for; only real-valued types are supported")
.add_parameter("mean", "dtype", "[Default: 0.] The mean :math:`\\mu` of the lognormal distibution")
.add_parameter("sigma", "dtype", "[Default: 1.] The standard deviation :math:`\\sigma` of the lognormal distributiuon")
);

/* How to create a new PyBoostLogNormalObject */
static PyObject* PyBoostLogNormal_New(PyTypeObject* type, PyObject*, PyObject*) {

  /* Allocates the python object itself */
  PyBoostLogNormalObject* self = (PyBoostLogNormalObject*)type->tp_alloc(type, 0);
  self->type_num = NPY_NOTYPE;
  self->distro.reset();

  return Py_BuildValue("N", self);
}

/* How to delete a PyBoostLogNormalObject */
static void PyBoostLogNormal_Delete (PyBoostLogNormalObject* o) {
  o->distro.reset();
  Py_TYPE(o)->tp_free((PyObject*)o);
}

template <typename T>
boost::shared_ptr<void> make_lognormal(PyObject* mean, PyObject* sigma) {
  T cmean = 0.;
  if (mean) cmean = PyBlitzArrayCxx_AsCScalar<T>(mean);
  T csigma = 1.;
  if (sigma) csigma = PyBlitzArrayCxx_AsCScalar<T>(sigma);
  return boost::make_shared<bob::core::random::lognormal_distribution<T>>(cmean, csigma);
}

PyObject* PyBoostLogNormal_SimpleNew (int type_num, PyObject* mean, PyObject* sigma) {
BOB_TRY
  PyBoostLogNormalObject* retval = (PyBoostLogNormalObject*)PyBoostLogNormal_New(&PyBoostLogNormal_Type, 0, 0);
  if (!retval) return 0;
  auto retval_ = make_safe(retval);

  retval->type_num = type_num;

  switch(type_num) {
    case NPY_FLOAT32:
      retval->distro = make_lognormal<float>(mean, sigma);
      break;
    case NPY_FLOAT64:
      retval->distro = make_lognormal<double>(mean, sigma);
      break;
    default:
      PyErr_Format(PyExc_NotImplementedError, "cannot create %s(T) with T having an unsupported numpy type number of %d (it only supports numpy.float32 or numpy.float64)", Py_TYPE(retval)->tp_name, retval->type_num);
      return 0;
  }

  if (!retval->distro) { // a problem occurred
    return 0;
  }

  return Py_BuildValue("O", retval);
BOB_CATCH_FUNCTION("SimpleNew", 0)
}

/* Implements the __init__(self) function */
static int PyBoostLogNormal_Init(PyBoostLogNormalObject* self, PyObject *args, PyObject* kwds) {
BOB_TRY
  /* Parses input arguments in a single shot */
  char** kwlist = lognormal_doc.kwlist();

  PyObject* m = 0;
  PyObject* s = 0;

  if (!PyArg_ParseTupleAndKeywords(args, kwds, "O&|OO", kwlist, &PyBlitzArray_TypenumConverter, &self->type_num, &m, &s)) return -1; ///< FAILURE

  switch(self->type_num) {
    case NPY_FLOAT32:
      self->distro = make_lognormal<float>(m, s);
      break;
    case NPY_FLOAT64:
      self->distro = make_lognormal<double>(m, s);
      break;
    default:
      PyErr_Format(PyExc_NotImplementedError, "cannot create %s(T) with T having an unsupported numpy type number of %d (it only supports numpy.float32 or numpy.float64)", Py_TYPE(self)->tp_name, self->type_num);
      return -1;
  }

  if (!self->distro) { // a problem occurred
    return -1;
  }

  return 0; ///< SUCCESS
BOB_CATCH_MEMBER("constructor", -1)
}

int PyBoostLogNormal_Check(PyObject* o) {
  if (!o) return 0;
  return PyObject_IsInstance(o, reinterpret_cast<PyObject*>(&PyBoostLogNormal_Type));
}

int PyBoostLogNormal_Converter(PyObject* o, PyBoostLogNormalObject** a) {
  if (!PyBoostLogNormal_Check(o)) return 0;
  Py_INCREF(o);
  (*a) = reinterpret_cast<PyBoostLogNormalObject*>(o);
  return 1;
}

static auto mean_doc = bob::extension::VariableDoc(
  "mean",
  "dtype",
  "The mean value the distribution will produce."
);
template <typename T> PyObject* get_mean(PyBoostLogNormalObject* self) {
  return PyBlitzArrayCxx_FromCScalar(boost::static_pointer_cast<bob::core::random::lognormal_distribution<T>>(self->distro)->m());
}

static PyObject* PyBoostLogNormal_GetMean(PyBoostLogNormalObject* self) {
BOB_TRY
  switch (self->type_num) {
    case NPY_FLOAT32:
      return get_mean<float>(self);
    case NPY_FLOAT64:
      return get_mean<double>(self);
    default:
      PyErr_Format(PyExc_NotImplementedError, "cannot get m of %s(T) with T having an unsupported numpy type number of %d (DEBUG ME)", Py_TYPE(self)->tp_name, self->type_num);
      return 0;
  }
BOB_CATCH_MEMBER("mean", 0)
}

static auto sigma_doc = bob::extension::VariableDoc(
  "sigma",
  "dtype",
  "The standard deviation the distribution will have"
);
template <typename T> PyObject* get_sigma(PyBoostLogNormalObject* self) {
  return PyBlitzArrayCxx_FromCScalar(boost::static_pointer_cast<bob::core::random::lognormal_distribution<T>>(self->distro)->s());
}

static PyObject* PyBoostLogNormal_GetSigma(PyBoostLogNormalObject* self) {
BOB_TRY
  switch (self->type_num) {
    case NPY_FLOAT32:
      return get_sigma<float>(self);
    case NPY_FLOAT64:
      return get_sigma<double>(self);
    default:
      PyErr_Format(PyExc_NotImplementedError, "cannot get s of %s(T) with T having an unsupported numpy type number of %d (DEBUG ME)", Py_TYPE(self)->tp_name, self->type_num);
      return 0;
  }
BOB_CATCH_MEMBER("sigma", 0)
}

static auto dtype_doc = bob::extension::VariableDoc(
  "dtype",
  ":py:class:`numpy.dtype`",
  "The type of scalars produced by this normal distribution"
);
static PyObject* PyBoostLogNormal_GetDtype(PyBoostLogNormalObject* self) {
BOB_TRY
  return Py_BuildValue("N", PyArray_DescrFromType(self->type_num));
BOB_CATCH_MEMBER("dtype", 0)
}

static PyGetSetDef PyBoostLogNormal_getseters[] = {
    {
      dtype_doc.name(),
      (getter)PyBoostLogNormal_GetDtype,
      0,
      dtype_doc.doc(),
      0,
    },
    {
      mean_doc.name(),
      (getter)PyBoostLogNormal_GetMean,
      0,
      mean_doc.doc(),
      0,
    },
    {
      sigma_doc.name(),
      (getter)PyBoostLogNormal_GetSigma,
      0,
      sigma_doc.doc(),
      0,
    },
    {0}  /* Sentinel */
};

static auto reset_doc = bob::extension::FunctionDoc(
  "reset",
  "Resets this distribution",
  "After calling this method, subsequent uses of the distribution do not depend on values produced by any random number generator prior to invoking reset",
  true
)
.add_prototype("")
;
template <typename T> PyObject* reset(PyBoostLogNormalObject* self) {
  boost::static_pointer_cast<bob::core::random::lognormal_distribution<T>>(self->distro)->reset();
  Py_RETURN_NONE;
}

static PyObject* PyBoostLogNormal_Reset(PyBoostLogNormalObject* self) {
BOB_TRY
  switch (self->type_num) {
    case NPY_FLOAT32:
      return reset<float>(self);
    case NPY_FLOAT64:
      return reset<double>(self);
    default:
      PyErr_Format(PyExc_NotImplementedError, "cannot reset %s(T) with T having an unsupported numpy type number of %d (DEBUG ME)", Py_TYPE(self)->tp_name, self->type_num);
      return 0;
  }
BOB_CATCH_MEMBER("reset", 0)
}

static auto call_doc = bob::extension::FunctionDoc(
  "draw",
  "Draws one random number from this distribution using the given ``rng``",
  ".. note:: The :py:meth:`__call__` function is a synonym for this ``draw``.",
  true
)
.add_prototype("rng", "value")
.add_parameter("rng", ":py:class:`mt19937`", "The random number generator to use")
.add_return("value", "dtype", "A random value that follows the lognormal distribution")
;
template <typename T> PyObject* call(PyBoostLogNormalObject* self, PyBoostMt19937Object* rng) {
  typedef bob::core::random::lognormal_distribution<T> distro_t;
  return PyBlitzArrayCxx_FromCScalar((*boost::static_pointer_cast<distro_t>(self->distro))(*rng->rng));
}

/**
 * Calling a PyBoostLogNormalObject to generate a random number
 */
static PyObject* PyBoostLogNormal_Call(PyBoostLogNormalObject* self, PyObject *args, PyObject* kwds) {
BOB_TRY
  char** kwlist = call_doc.kwlist();

  PyBoostMt19937Object* rng;

  if (!PyArg_ParseTupleAndKeywords(args, kwds, "O!", kwlist, &PyBoostMt19937_Type, &rng)) return 0; ///< FAILURE

  switch(self->type_num) {
    case NPY_FLOAT32:
      return call<float>(self, rng);
      break;
    case NPY_FLOAT64:
      return call<double>(self, rng);
      break;
    default:
      PyErr_Format(PyExc_NotImplementedError, "cannot call %s(T) with T having an unsupported numpy type number of %d (DEBUG ME)", Py_TYPE(self)->tp_name, self->type_num);
  }

  return 0; ///< FAILURE
BOB_CATCH_MEMBER("call", 0)
}

static PyMethodDef PyBoostLogNormal_methods[] = {
    {
      call_doc.name(),
      (PyCFunction)PyBoostLogNormal_Call,
      METH_VARARGS|METH_KEYWORDS,
      call_doc.doc(),
    },
    {
      reset_doc.name(),
      (PyCFunction)PyBoostLogNormal_Reset,
      METH_NOARGS,
      reset_doc.doc(),
    },
    {0}  /* Sentinel */
};

extern PyObject* scalar_to_bytes(PyObject* s);

/**
 * String representation and print out
 */
static PyObject* PyBoostLogNormal_Repr(PyBoostLogNormalObject* self) {
  PyObject* smean = scalar_to_bytes(PyBoostLogNormal_GetMean(self));
  if (!smean) return 0;
  auto smean_ = make_safe(smean);

  PyObject* ssigma = scalar_to_bytes(PyBoostLogNormal_GetSigma(self));
  if (!ssigma) return 0;
  auto ssigma_ = make_safe(ssigma);

  return
    PyString_FromFormat
      (
       "%s(dtype='%s', mean=%s, sigma=%s)",
       Py_TYPE(self)->tp_name, PyBlitzArray_TypenumAsString(self->type_num),
       PyString_AS_STRING(smean), PyString_AS_STRING(ssigma)
      );
}

PyTypeObject PyBoostLogNormal_Type = {
  PyVarObject_HEAD_INIT(0,0)
  0
};

bool init_BoostLogNormal(PyObject* module)
{
  // initialize the type struct
  PyBoostLogNormal_Type.tp_name = lognormal_doc.name();
  PyBoostLogNormal_Type.tp_basicsize = sizeof(PyBoostLogNormalObject);
  PyBoostLogNormal_Type.tp_flags = Py_TPFLAGS_DEFAULT;
  PyBoostLogNormal_Type.tp_doc = lognormal_doc.doc();
  PyBoostLogNormal_Type.tp_str = reinterpret_cast<reprfunc>(PyBoostLogNormal_Repr);
  PyBoostLogNormal_Type.tp_repr = reinterpret_cast<reprfunc>(PyBoostLogNormal_Repr);

  // set the functions
  PyBoostLogNormal_Type.tp_new = PyBoostLogNormal_New;
  PyBoostLogNormal_Type.tp_init = reinterpret_cast<initproc>(PyBoostLogNormal_Init);
  PyBoostLogNormal_Type.tp_dealloc = reinterpret_cast<destructor>(PyBoostLogNormal_Delete);
  PyBoostLogNormal_Type.tp_methods = PyBoostLogNormal_methods;
  PyBoostLogNormal_Type.tp_getset = PyBoostLogNormal_getseters;
  PyBoostLogNormal_Type.tp_call = reinterpret_cast<ternaryfunc>(PyBoostLogNormal_Call);

  // check that everything is fine
  if (PyType_Ready(&PyBoostLogNormal_Type) < 0) return false;

  // add the type to the module
  return PyModule_AddObject(module, "lognormal", Py_BuildValue("O", &PyBoostLogNormal_Type)) >= 0;
}