/*
    Copyright 2005-2013 Adobe Systems Incorporated
    Distributed under the MIT License (see accompanying file LICENSE_1_0_0.txt
    or a copy at http://stlab.adobe.com/licenses.html)
*/

/*************************************************************************************************/

#ifndef ADOBE_FNV_HPP
#define ADOBE_FNV_HPP

/*************************************************************************************************/

#include <cstddef>

#ifndef ADOBE_FNV_NO_BIGINTS
#include <boost/multiprecision/cpp_int.hpp>
#endif

#include <adobe/functional/operator.hpp>

/*************************************************************************************************/
/**
    \defgroup fnv FNV Hashing Algorithm

    Implementation of the FNV (Fowler Noll Vo) class of hash algorithms. From
    the homepage (http://www.isthe.com/chongo/tech/comp/fnv/):

    The FNV speed allows one to quickly hash lots of data while maintaining a
    reasonable collision rate. The high dispersion of the FNV hashes makes them
    well suited for hashing nearly identical strings such as URLs, hostnames,
    filenames, text, IP addresses, etc.

    There are two implementations of FNV: FNV-1 and FNV-1a. The latter has a
    minor change over the former which results in better avalanche
    characteristics, reducing collisions at no cost to performance. As such
    FNV-1a is implemented here; FNV-1 is not.

    According to the author (on the same website above) these algorithms are
    in the public domain:

    FNV hash algorithms and source code have been released into the public
    domain. The authors of the FNV algorithmm [sic] look deliberate steps to
    disclose the algorhtm [sic] in a public forum soon after it was invented.
    More than a year passed after this public disclosure and the authors
    deliberatly [sic] took no steps to patent the FNV algorithm. Therefore it is
    safe to say that the FNV authors have no patent claims on the FNV algorithm
    as published.

    All versions of the algorithm are available (32, 64, 128, 256, 512, and 1024)
    however only the 32- and 64-bit versions have no external dependencies.
    Beyond 64-bits the implementations leverage the Boost multiprecision
    library. If you would prefer to remove the dependency (and implicitly limit
    yourself to the 32- and 64-bit algorithms), define the
    `ADOBE_FNV_NO_BIGINTS` macro.

    You can also specify a result of any bitsize between 1 and 1024 (or 1 to 64
    with `ADOBE_FNV_NO_BIGINTS` defined), and the algorithm will produce a
    truncated result. When computing a hash that is a bitsize other than those
    explicitly defined, the recommendation from the above website is to use
    `xor_fold`:

    `((value >> (sizeof(T) * 8 - Bits)) ^ value) & ((1 << Bits) - 1)`

    However, `xor_fold` is not really necessary given the excellent distribution
    of the algorithm class, and a straightforward mask will result in bits that
    are as well distributed as result from `xor_fold`. So we mask.

    \warning
    This algorithm class is not (nor was ever intended to be) cryptographically
    secure. Do not use this algorithm where secure algorithms (e.g., \ref sha)
    are required.
*/
/*************************************************************************************************/

namespace adobe {

/*************************************************************************************************/

namespace detail {

/*************************************************************************************************/

constexpr std::size_t rollup(std::size_t n) {
    return n == 0 ? 0 : n <= 32 ? 32 : n <= 64 ? 64 : n <= 128 ? 128 : n <= 256
                                                                           ? 256
                                                                           : n <= 512
                                                                                ? 512
                                                                                : n <= 1024 ? 1024
                                                                                : 0;
}

/*************************************************************************************************/

template <std::size_t Bits>
struct fnv_base_traits {};

/*************************************************************************************************/

template <>
struct fnv_base_traits<32> {
    typedef std::uint32_t value_type;

    static constexpr std::size_t size() { return 32; }

    static constexpr value_type offset_basis() { return 0x811c9dc5; }

    static constexpr value_type prime() { return 0x1000193; }
};

/*************************************************************************************************/

template <>
struct fnv_base_traits<64> {
    typedef std::uint64_t value_type;

    static constexpr std::size_t size() { return 64; }

    static constexpr value_type offset_basis() { return 0xcbf29ce484222325; }

    static constexpr value_type prime() { return 0x100000001b3; }
};

/*************************************************************************************************/
#ifndef ADOBE_FNV_NO_BIGINTS
using namespace boost::multiprecision::literals;

/*************************************************************************************************/

template <>
struct fnv_base_traits<128> {
    typedef boost::multiprecision::uint128_t value_type;

    static constexpr std::size_t size() { return 128; }

    static constexpr value_type offset_basis() {
        return 0x6C62272E07BB014262B821756295C58D_cppui128;
    }

    static constexpr value_type prime() { return 0x1000000000000000000013B_cppui128; }
};

/*************************************************************************************************/

template <>
struct fnv_base_traits<256> {
    typedef boost::multiprecision::uint256_t value_type;

    static constexpr std::size_t size() { return 256; }

    static constexpr value_type offset_basis() {
        return 0xDD268DBCAAC550362D98C384C4E576CCC8B1536847B6BBB31023B4C8CAEE0535_cppui256;
    }

    static constexpr value_type prime() {
        return 0x1000000000000000000000000000000000000000163_cppui256;
    }
};

/*************************************************************************************************/

template <>
struct fnv_base_traits<512> {
    typedef boost::multiprecision::uint512_t value_type;

    static constexpr std::size_t size() { return 512; }

    static constexpr value_type offset_basis() {
        return 0xB86DB0B1171F4416DCA1E50F309990ACAC87D059C90000000000000000000D21E948F68A34C192F62EA79BC942DBE7CE182036415F56E34BAC982AAC4AFE9FD9_cppui512;
    }

    static constexpr value_type prime() {
        return 0x100000000000000000000000000000000000000000000000000000000000000000000000000000000000157_cppui512;
    }
};

/*************************************************************************************************/

template <>
struct fnv_base_traits<1024> {
    typedef boost::multiprecision::uint1024_t value_type;

    static constexpr std::size_t size() { return 1024; }

    static constexpr value_type offset_basis() {
        return 0x5F7A76758ECC4D32E56D5A591028B74B29FC4223FDADA16C3BF34EDA3674DA9A21D9000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000004C6D7EB6E73802734510A555F256CC005AE556BDE8CC9C6A93B21AFF4B16C71EE90B3_cppui1024;
    }

    static constexpr value_type prime() {
        return 0x10000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000018D_cppui1024;
    }
};
#endif
/*************************************************************************************************/

template <std::size_t FromBits, std::size_t ToBits>
struct bitmask {
    template <typename T>
    inline static T mask(T value) {
        return value & ((T(1) << ToBits) - 1);
    }
};

template <std::size_t SameBits>
struct bitmask<SameBits, SameBits> {
    template <typename T>
    inline static T mask(T value) {
        return value;
    }
};

/*************************************************************************************************/

} // namespace detail

/*************************************************************************************************/
/**
    \ingroup fnv

    Traits class used by adobe::fnv1a.

    \note
    In the case the implementation is not defined for the bit size passed, this will bump to the
    next highest implementation and mask the automatically. For example, fnv1a<56> will roll up
    to use fnv_traits<64> and mask the result back down to 56 bits before returning it.
*/
template <std::size_t Bits>
using fnv_traits = typename detail::fnv_base_traits<detail::rollup(Bits)>;

/**
    \ingroup fnv

    Integral type used to return the result of a call to adobe::fnv1a.

    The size of the type depends on the version of fnv called.
*/
template <std::size_t Bits>
using fnvtype = typename fnv_traits<Bits>::value_type;

/*************************************************************************************************/
/**
    \ingroup fnv

    Performs the FNV-1a hash over the advancing iterator until the (sentinel)
    predicate returns true.

    \todo Refactor this variant with the Iterator/Iterator one so there is one
    implementation of the algorithm in this header.
*/
template <std::size_t Bits, typename Iterator, typename Predicate>
fnvtype<Bits> fnv1a(Iterator first, Predicate p) {
    static_assert(sizeof(typename std::iterator_traits<Iterator>::value_type) == 1,
                  "Iterator value_type must be 1 byte.");

    typedef fnvtype<Bits> result_type;

    result_type result(fnv_traits<Bits>::offset_basis());

    while (!p(*first))
        result = (result xor static_cast<result_type>(*first++)) * fnv_traits<Bits>::prime();

    return detail::bitmask<fnv_traits<Bits>::size(), Bits>::template mask(result);
}

/*************************************************************************************************/
/**
    \ingroup fnv

    Performs the FNV-1a hash over the specified range.

    \todo Refactor this variant with the Iterator/Predicate one so there is one
    implementation of the algorithm in this header.
*/
template <std::size_t Bits, typename Iterator>
inline fnvtype<Bits> fnv1a(Iterator first, Iterator last) {
    static_assert(sizeof(typename std::iterator_traits<Iterator>::value_type) == 1,
                  "Iterator value_type must be 1 byte.");

    typedef fnvtype<Bits> result_type;

    result_type result(fnv_traits<Bits>::offset_basis());

    while (first != last)
        result = (result xor static_cast<result_type>(*first++)) * fnv_traits<Bits>::prime();

    return detail::bitmask<fnv_traits<Bits>::size(), Bits>::template mask(result);
}

/*************************************************************************************************/
/**
    \ingroup fnv

    Performs the FNV-1a hash over the specified container.
*/
template <std::size_t Bits, typename Container>
inline fnvtype<Bits> fnv1a(Container c) {
    return fnv1a<Bits>(begin(c), end(c));
}

/*************************************************************************************************/

} // namespace adobe

/*************************************************************************************************/

#endif

/*************************************************************************************************/


#include "cnn/math.h"

#include <random>
#include <vector>
#include <cstring>
#include <boost/random/discrete_distribution.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/mersenne_twister.hpp>

#if HAVE_CUDA
#include "cnn/cuda.h"
#endif

using namespace std;

namespace cnn {

    extern mt19937* rndeng;

    boost::mt19937 boost_rand_gen;

    cnn::real rand01() {
      uniform_real_distribution<cnn::real> distribution(0, 1);
      return distribution(*rndeng);
    }

    int rand0n(int n) {
      assert(n > 0);
      int x = rand01() * n;
      while(n == x) { x = rand01() * n; }
      return x;
    }

    cnn::real rand_normal() {
      normal_distribution<cnn::real> distribution(0, 1);
      return distribution(*rndeng);
    }

    int rand0n_uniform(int n)
    {
        assert(n > 0);
        uniform_int_distribution<> distribution(0, n);
        return distribution(*rndeng);
    }

    std::vector<int> rand0n_uniform(int vecsize, int n_exclusive)
    {
        std::vector<int> res(vecsize);
        for (int i = 0; i < vecsize; i++)
            res[i] = rand0n_uniform(n_exclusive);

        return res;
    }

    std::vector<int> rand0n_uniform(int vecsize, int n_exclusive, const std::vector<cnn::real>& sample_dist)
    {
        std::vector<int> res(vecsize);
        boost::random::discrete_distribution<int> d(sample_dist.begin(), sample_dist.end());
        for (int i = 0; i < vecsize; i++)
            res[i] = d(*rndeng);

        return res;
    }

    int sample_accoding_to_distribution_of(const vector<cnn::real>& probabilities)
    {
        std::vector<cnn::real> cumulative;
        int sz = probabilities.size();
        std::partial_sum(&probabilities[0], &probabilities[0] + sz,
            std::back_inserter(cumulative));
        boost::uniform_real<> dist(0, cumulative.back());
        boost::variate_generator<boost::mt19937&, boost::uniform_real<> > die(boost_rand_gen, dist);
        return (std::lower_bound(cumulative.begin(), cumulative.end(), die()) - cumulative.begin());
    }
} // namespace cnn


/**
 * @file
 * @copyright This code is licensed under the 3-clause BSD license.\n
 *            Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.\n
 *            See LICENSE.txt for details.
 */

#include "Utils/GeometricDerivatives/NormalModeAnalysis.h"
#include "Utils/Constants.h"
#include "Utils/GeometricDerivatives/HessianUtilities.h"
#include "Utils/GeometricDerivatives/NormalMode.h"
#include "Utils/GeometricDerivatives/NormalModesContainer.h"
#include <Eigen/Core>
#include <vector>

namespace Scine {
namespace Utils {
namespace NormalModeAnalysis {

inline NormalModesContainer calculate(HessianUtilities& diagonalizer, int nAtoms) {
  Eigen::VectorXd eigenvalues = diagonalizer.getInternalEigenvalues();
  Eigen::MatrixXd cartesianDisplacements = diagonalizer.getBackTransformedInternalEigenvectors();

  NormalModesContainer modesContainer;
  DisplacementCollection dc(nAtoms, 3);
  for (int i = 0; i < cartesianDisplacements.cols(); ++i) {
    for (int j = 0; j < nAtoms; ++j) {
      dc.row(j) = cartesianDisplacements.col(i).segment(3 * j, 3);
    }

    double freq = getWaveNumber(eigenvalues[i]);
    NormalMode m(freq, dc);
    modesContainer.add(std::move(m));
  }
  return modesContainer;
}

NormalModesContainer calculateNormalModes(const HessianMatrix& hessian, const AtomCollection& atoms) {
  return calculateNormalModes(hessian, atoms.getElements(), atoms.getPositions());
}

NormalModesContainer calculateNormalModes(const HessianMatrix& hessian, const ElementTypeCollection& elements,
                                          const PositionCollection& positions) {
  int nAtoms = elements.size();

  HessianUtilities diagonalizer(hessian, elements, positions, true);

  return calculate(diagonalizer, nAtoms);
}

NormalModesContainer calculateOrthogonalNormalModes(const HessianMatrix& hessian, const ElementTypeCollection& elements,
                                                    const PositionCollection& positions, const GradientCollection& gradient) {
  assert(gradient.size() == hessian.rows() && "Gradient dimension and hessian dimension do not match! (must be 3*N)");
  int nAtoms = elements.size();

  HessianUtilities diagonalizer(hessian, elements, positions, gradient, true);

  return calculate(diagonalizer, nAtoms);
}

double getWaveNumber(double value) {
  double f1 = Constants::invCentimeter_per_hartree * sqrt(Constants::u_per_electronRestMass); // for conversion to cm^-1
  double f2 = value < 0 ? -1 : 1; // for imaginary frequencies, will be shown as negative frequencies

  return f1 * f2 * std::sqrt(f2 * value);
}

} // namespace NormalModeAnalysis
} // namespace Utils
} // namespace Scine


// Project Includes

// Common Library
#include "Exception.h"
#include "MathUtility.h"

// Boost Includes
#include <boost/math/special_functions/next.hpp>

// GTest Includes
#include <gtest/gtest.h>

//--------------------------------------------------------------------------------------------------
class MathUtilityTests : public ::testing::Test
{

protected:

    //--------------------------------------------------------------------------------------------------
    void SetUp()
    {
    }

    //--------------------------------------------------------------------------------------------------
    static void SetUpTestCase()
    {
    }

    //--------------------------------------------------------------------------------------------------
    void TearDown()
    {
    }

    //--------------------------------------------------------------------------------------------------
    static void TearDownTestCase()
    {
    }
};

//--------------------------------------------------------------------------------------------------
TEST_F(MathUtilityTests, Compare)
{
    // Let's assume we were comparing monetary values
    // Let's also assume we're not concerned with the implications of truncating the values
    const size_t significantDigits  = FLT_DIG;
    const float  originalNumber     = 100.000f;
    const float  nextAvailavleFloat = boost::math::float_next(originalNumber);

    EXPECT_TRUE (Common::AreAlmostEqual(originalNumber, originalNumber + 0.001f, 0.01f));
    EXPECT_TRUE (Common::AreAlmostEqual(originalNumber, originalNumber + 0.009f, 0.01f));
    EXPECT_FALSE(Common::AreAlmostEqual(originalNumber, originalNumber + 0.010f, 0.01f));
}


#pragma once

#include <vector>

#include <boost/range.hpp>

#include "bp_vector.hpp"
#include "util.hpp"

namespace succinct {

    // This class implements a cartesian-tree-based RMQ data
    // structure, using the 2d-Min-Heap DFUDS representation described
    // in "Space-Efficient Preprocessing Schemes for Range Minimum
    // Queries on Static Arrays", Johannes Fischer and Volker Heun,
    // SIAM J. Comput., 40(2), 465–492.

    // We made a few variations:
    //
    // - The rmq() operation in the paper checks whether x is parent
    //   of w - 1, which can be written as select0(x - 1) <
    //   find_open(w - 1). We use instead the fact that the excess
    //   between x and w (both excluded) is strictly greater than the
    //   excess of w, so the formula above holds iff excess(select0(x
    //   - 1) + 1) <= excess(w). This is faster because a select0 is
    //   faster than find_open+rank0.
    //
    // - The construction is done in reverse order so that the input
    //   array can be traversed left-to-right. This involves
    //   re-mapping all the indices at query time. Since the array is
    //   reversed, in ties the leftmost element wins
    //
    // - Our data structures have 0-based indices, so the operations
    //   are slightly different from those in the paper

    class cartesian_tree : boost::noncopyable {
    public:

        template <typename T>
        class builder {
        public:
            builder(uint64_t expected_size = 0)
            {
                if (expected_size) {
                    m_bp.reserve(2 * expected_size + 2);
                }
            }

            template <typename Comparator>
            void push_back(T const& val, Comparator const& comp)
            {
                m_bp.push_back(0);

                while (!m_stack.empty()
                       && comp(val, m_stack.back())) { // val < m_stack.back()
                    m_stack.pop_back();
                    m_bp.push_back(1);
                }

                m_stack.push_back(val);
            }

            bit_vector_builder& finalize()
            {
                // super-root
                m_bp.push_back(0);
                while (!m_stack.empty()) {
                    m_stack.pop_back();
                    m_bp.push_back(1);
                }
                m_bp.push_back(1);

                m_bp.reverse();
                return m_bp;
            }

            friend class cartesian_tree;
        private:
            std::vector<T> m_stack;
            bit_vector_builder m_bp;
        };

        cartesian_tree() {}

        template <typename T>
        cartesian_tree(builder<T>* b)
        {
            bp_vector(&b->finalize(), false, true).swap(m_bp);
        }

        template <typename Range>
        cartesian_tree(Range const& v)
        {
            build_from_range(v, std::less<typename boost::range_value<Range>::type>());
        }

        template <typename Range, typename Comparator>
        cartesian_tree(Range const& v, Comparator const& comp)
        {
            build_from_range(v, comp);
        }

        // NOTE: this is RMQ in the interval [a, b], b inclusive
        // XXX(ot): maybe change this to [a, b), for consistency with
        // the rest of the library?
        uint64_t rmq(uint64_t a, uint64_t b) const
        {
            typedef bp_vector::excess_t excess_t;

            assert(a <= b);
            if (a == b) return a;

            uint64_t n = size();

            uint64_t t = m_bp.select0(n - b - 1);
            excess_t exc_t = excess_t(t - 2 * (n - b - 1));
            assert(exc_t - 1 == m_bp.excess(t + 1));

            uint64_t x = m_bp.select0(n - b);
            uint64_t y = m_bp.select0(n - a);

            excess_t exc_w;
            uint64_t w = m_bp.excess_rmq(x, y, exc_w);
            uint64_t rank0_w = (w - uint64_t(exc_w)) / 2;
            assert(m_bp[w - 1] == 0);

            uint64_t ret;
            if (exc_w >= exc_t - 1) {
                ret = b;
            } else {
                ret = n - rank0_w;
            }

            assert(ret >= a);
            assert(ret <= b);
            return ret;
        }

        bp_vector const& get_bp() const
        {
            return m_bp;
        }

        uint64_t size() const
        {
            return m_bp.size() / 2 - 1;
        }

        template <typename Visitor>
        void map(Visitor& visit)
        {
            visit
                (m_bp, "m_bp");
        }

        void swap(cartesian_tree& other)
        {
            other.m_bp.swap(m_bp);
        }

    protected:

        template <typename Range, typename Comparator>
        void build_from_range(Range const& v, Comparator const& comp)
        {
            typedef typename
                boost::range_value<Range>::type value_type;
            typedef typename
                boost::range_iterator<const Range>::type iter_type;

            builder<value_type> b;
            for (iter_type it = boost::begin(v); it != boost::end(v); ++it) {
                b.push_back(*it, comp);
            }
            cartesian_tree(&b).swap(*this);
        }

        bp_vector m_bp;
    };

}


#ifndef SKYLARK_QRLT_DATA_HPP
#define SKYLARK_QRLT_DATA_HPP

#ifndef SKYLARK_SKETCH_HPP
#error "Include top-level sketch.hpp instead of including individuals headers"
#endif

#include <boost/math/special_functions/erf.hpp>
#include <vector>

namespace skylark { namespace sketch {

/**
 * Random Laplace Transform (data)
 *
 * Sketch transform into Eucledian space of fuctions in an RKHS
 * implicitly defined by a vector and a semigroup kernel.
 *
 * Use quasi-random features.
 *
 * See:
 *
 * Jiyan Yang, Vikas Sindhwani, Quanfu Fan, Haim Avron, Michael Mahoney
 * Random Laplace Feature Maps for Semigroup Kernels on Histograms
 * CVPR 2014
 *
 * Yang, Sindhawni, Avron and Mahoney
 * Quasi-Monte Carlo Feature Maps for Shift-Invariant Kernels
 * ICML 2014
 *
 */
template <template <typename, typename> class KernelDistribution,
          template <typename> class QMCSequenceType>
struct QRLT_data_t : public sketch_transform_data_t {

    typedef double value_type;
    typedef quasi_dense_transform_data_t<KernelDistribution, QMCSequenceType>
    underlying_data_type;
    typedef QMCSequenceType<value_type> sequence_type;
    typedef sketch_transform_data_t base_t;

    static size_t qmc_sequence_dim(size_t N)  { return N; }

    QRLT_data_t (int N, int S, double inscale, double outscale,
        const sequence_type& sequence, int skip,  base::context_t& context)
        : base_t(N, S, context, "QRLT"), _inscale(inscale),
          _outscale(outscale), _sequence(sequence), _skip(skip) {

        context = build();
    }

    /**
     *  Serializes a sketch to a string.
     *
     *  @return property_tree describing the sketch.
     */
    virtual boost::property_tree::ptree to_ptree() const {
        SKYLARK_THROW_EXCEPTION (
          base::sketch_exception()
              << base::error_msg(
                 "Do not yet support serialization of generic QRLT transform"));

        return boost::property_tree::ptree();
    }

    virtual sketch_transform_t<boost::any, boost::any> *get_transform() const {
        SKYLARK_THROW_EXCEPTION (
          base::sketch_exception()
              << base::error_msg(
                 "Trying to create concrete transform of QRLT_data_t"));

        return nullptr;
    }

protected:

    typedef typename underlying_data_type::value_accessor_type accessor_type;

    QRLT_data_t (int N, int S, double inscale, double outscale,
        const sequence_type& sequence, int skip,
        const base::context_t& context, std::string type)
        : base_t(N, S, context, type), _inscale(inscale),
          _outscale(outscale), _sequence(sequence), _skip(skip) {

    }

   base::context_t build() {
       base::context_t ctx = base_t::build();
       _underlying_data = boost::shared_ptr<underlying_data_type>(new
           underlying_data_type(base_t::_N, base_t::_S, _inscale,
               _sequence, _skip, ctx));
       return ctx;
   }

    double _inscale;
    double _outscale; /** Scaling for exponential factor */
    boost::shared_ptr<underlying_data_type> _underlying_data;
    /**< Data of the underlying dense transformation */
    sequence_type _sequence;
    const int _skip;
};

namespace internal {

// There is no boost::math class for levy's distribution.
// However, we need only the quantile function, which
// we implement in this skeleton.

template <class ValueType = double,
          class Policy = boost::math::policies::policy<> >
struct levy_distribution_t
{
    typedef ValueType value_type;
    typedef Policy policy_type;

    levy_distribution_t(value_type mean = 0, value_type scale = 1)
        : _mean(mean), _scale(scale) {

    }

    value_type get_mean() const {
        return _mean;
    }

    value_type get_scale() const {
        return _scale;
    }

private:
    value_type _mean;
    value_type _scale;
};

// I have no idea how the compiler/boost finds this function!
// It works on my machine, but I hope it will not break in others...
template <class RealType, class Policy>
inline RealType quantile(const internal::levy_distribution_t<RealType,
    Policy>& dist,
    const RealType& p) {
    RealType v = boost::math::erfc_inv(p, Policy());
    return dist.get_scale() / (2 * v * v) + dist.get_mean();
}

}

/**
 * Quasi Random Features for Exponential Semigroup
 */
template<template <typename> class QMCSequenceType>
struct ExpSemigroupQRLT_data_t :
        public QRLT_data_t<internal::levy_distribution_t,
                           QMCSequenceType> {

    typedef QRLT_data_t<internal::levy_distribution_t,
                        QMCSequenceType> base_t;

    typedef typename base_t::sequence_type sequence_type;

    /// Params structure
    struct params_t : public sketch_params_t {

        params_t(double beta, const sequence_type& sequence, int skip) :
            beta(beta), sequence(sequence), skip(skip) {

        }

        const double beta;
        const sequence_type sequence;
        const int skip;
    };

    ExpSemigroupQRLT_data_t(int N, int S, double beta,
        const sequence_type& sequence, int skip,
        base::context_t& context)
        : base_t(N, S, beta * beta / 2, std::sqrt(1.0 / S),
            sequence, skip, context, "ExpSemigroupQRLT"),
          _beta(beta), _sequence(sequence), _skip(skip) {

        context = base_t::build();
    }

    ExpSemigroupQRLT_data_t(int N, int S, const params_t& params,
        base::context_t& context)
        : base_t(N, S, params.beta * params.beta / 2, std::sqrt(1.0 / S),
            params.sequence, params.skip, context, "ExpSemigroupQRLT"),
          _beta(params.beta), _sequence(params.sequence),
          _skip(params.skip) {

        context = base_t::build();
    }

    ExpSemigroupQRLT_data_t(const boost::property_tree::ptree &pt) :
        base_t(pt.get<int>("N"), pt.get<int>("S"),
            pt.get<double>("beta") * pt.get<double>("beta") / 2,
            std::sqrt(1.0 / pt.get<double>("S")),
            sequence_type(pt.get_child("sequence")), pt.get<int>("skip"),
            base::context_t(pt.get_child("creation_context")), "ExpSemiGroupQRLT"),
        _beta(pt.get<double>("beta")),
        _sequence(pt.get_child("sequence")),  _skip(pt.get<int>("skip")) {

        base_t::build();
    }

    /**
     *  Serializes a sketch to a string.
     *
     *  @return property_tree describing the sketch.
     */
    virtual boost::property_tree::ptree to_ptree() const {
        boost::property_tree::ptree pt;
        sketch_transform_data_t::add_common(pt);
        pt.put_child("sequence", _sequence.to_ptree());
        pt.put("beta", _beta);
        pt.put("skip", _skip);
        return pt;
    }

    /**
     * Get a concrete sketch transform based on the data
     */
    virtual sketch_transform_t<boost::any, boost::any> *get_transform() const;

protected:
    ExpSemigroupQRLT_data_t(int N, int S, double beta,
        const sequence_type& sequence, int skip,
        const skylark::base::context_t& context, std::string type)
        : base_t(N, S,  beta * beta / 2, std::sqrt(1.0 / S), context, type),
          _beta(beta), _sequence(sequence), _skip(skip) {

    }

private:
    const double _beta;
    const sequence_type _sequence;
    const int _skip;
};

} } /** namespace skylark::sketch */

#endif /** SKYLARK_QRLT_DATA_HPP */


// Boost Includes ==============================================================
//#define PYUBLAS_HAVE_BOOST_BINDINGS
#include <boost/python.hpp>
#include <boost/python/numeric.hpp>
#include <boost/cstdint.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>

// Includes ====================================================================
#include "profile.hpp"
#include "jeans.hpp"
#include "orbit_integrator.hpp"

//#define PY_ARRAY_UNIQUE_SYMBOL PyArrayHandle
//#include <numpy/arrayobject.h>
//#include <numpyconv.hpp>

// Using =======================================================================
using namespace boost::python;
using namespace gd;

// Declarations ================================================================
namespace  {

}// namespace

// Module ======================================================================
BOOST_PYTHON_MODULE(gdfast)
{
	//import_array();
	//boost::python::numeric::array::set_module_and_type("numpy", "ndarray");

	class_< Profile, boost::noncopyable >("Profile", no_init)
		.def("densityr", pure_virtual(&Profile::densityr))
	;
	class_<Plummer, bases<Profile> >("Plummer", init<double, double, double>())
		.def("densityr", (&Profile::densityr))
		.def("dphidr", (&Profile::dphidr))
		.def("dphidr2", (&Plummer::dphidr2))
	;
	class_<Hernquist, bases<Profile> >("Hernquist", init<double, double, double>())
		.def("densityr", (&Profile::densityr))
		.def("dphidr", (&Profile::dphidr))
	;

	class_< OrbitIntegrator, boost::noncopyable >("OrbitIntegrator", no_init)
	;
	class_<OrbitIntegratorEuler, bases<OrbitIntegrator> >("OrbitIntegratorEuler", init<double, double, double, double>())
		.def("integrate", (&OrbitIntegratorEuler::integrate))
	;
	class_<OrbitIntegratorLeapFrog, bases<OrbitIntegrator> >("OrbitIntegratorLeapFrog", init<double, double, double, double>())
		.def("integrate", (&OrbitIntegratorLeapFrog::integrate))
	;

	//mab::numpy::init();

}


///////////////////////////////////////////////////////////////////////////////
// ars::parameter.hpp                                                        //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_ARS_PARAMETER_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_ARS_PARAMETER_HPP_ER_2009
#include <boost/math/tools/precision.hpp>
#include <boost/ars/constant.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace ars{

// These parameters are used as defaults, but they can be changed at
// runtime with sampler's interface.
//
// max_data_count:
// The algorithm switches to standard rejection sampling once this
// number is reached. Complex distributions may require a higher number.
//
// max_log:
// The higher this number, the greater the precision, but it should not
// exceed const_::lmax_.
// A lower number may be needed to ensure finiteness of certain
// quantities (?) such as cum_sum.
template<typename T>
struct parameter{
    static unsigned max_data_count;
    static T max_log;
    static std::size_t reserve;
    static unsigned n_max_reject;
};
template<typename T> unsigned parameter<T>::max_data_count = 20;
template<typename T>
T parameter<T>::max_log = constant<T>::lmax_/static_cast<T>(max_data_count);
template<typename T> std::size_t parameter<T>::reserve = 5;
template<typename T> unsigned parameter<T>::n_max_reject = 100;

}// ars
}// detail
}// statistics
}// boost

#endif


///////////////////////////////////////////////////////////////////////////////
// statistics::survival::data::algorithm::include.hpp                        //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_SURVIVAL_DATA_ALGORTIHM_INCLUDE_HPP_ER_2009
#define BOOST_STATISTICS_SURVIVAL_DATA_ALGORTIHM_INCLUDE_HPP_ER_2009

#include <boost/statistics/survival/data/algorithm/events.hpp>
#include <boost/statistics/survival/data/algorithm/failure_times.hpp>
#include <boost/statistics/survival/data/algorithm/vectorize_events.hpp>
#include <boost/statistics/survival/data/algorithm/logit_log.hpp>
#include <boost/statistics/survival/data/algorithm/mean_events_by_covariate.hpp>

#endif


#ifndef _CELERITE2_INTERFACE_HPP_DEFINED_
#define _CELERITE2_INTERFACE_HPP_DEFINED_

#include <Eigen/Core>
#include "forward.hpp"

namespace celerite2 {
namespace core {

#define MakeEmptyWork(BaseType)                                                                                \
  typedef typename BaseType::Scalar Scalar;                                                                                \
  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Empty;                                                              \
  Empty

/**
 * \brief Compute the Cholesky factorization of the system
 *
 * This computes `d` and `W` such that:
 *
 * `diag(a) + tril(U*V^T) + triu(V*U^T) = L*diag(d)*L^T`
 *
 * where
 *
 * `L = 1 + tril(U*W^T)`
 *
 * This can be safely applied in place: `d_out` can point to `a` and `W_out` can
 * point to `V`, and the memory will be reused.
 *
 * @param t     (N,): The input coordinates (must be sorted)
 * @param c     (J,): The transport coefficients
 * @param a     (N,): The diagonal component
 * @param U     (N, J): The first low rank matrix
 * @param V     (N, J): The second low rank matrix
 * @param d_out (N,): The diagonal component of the Cholesky factor
 * @param W_out (N, J): The second low rank component of the Cholesky factor
 */
template <typename Input, typename Coeffs, typename Diag, typename LowRank, typename DiagOut, typename LowRankOut>
Eigen::Index factor(const Eigen::MatrixBase<Input> &t,         // (N,)
                    const Eigen::MatrixBase<Coeffs> &c,        // (J,)
                    const Eigen::MatrixBase<Diag> &a,          // (N,)
                    const Eigen::MatrixBase<LowRank> &U,       // (N, J)
                    const Eigen::MatrixBase<LowRank> &V,       // (N, J)
                    Eigen::MatrixBase<DiagOut> const &d_out,   // (N,)
                    Eigen::MatrixBase<LowRankOut> const &W_out // (N, J)
) {
  MakeEmptyWork(Diag) S;
  return factor<false>(t, c, a, U, V, d_out, W_out, S);
}

/**
 * \brief Compute the solution of a lower triangular linear equation
 *
 * This computes `Z` such that:
 *
 * `Y = L * Y`
 *
 * where
 *
 * `L = 1 + tril(U*W^T)`
 *
 * This can be safely applied in place.
 *
 * @param t     (N,): The input coordinates (must be sorted)
 * @param c     (J,): The transport coefficients
 * @param U     (N, J): The first low rank matrix
 * @param W     (N, J): The second low rank matrix
 * @param Y     (N, Nrhs): The right hand side
 * @param Z_out (N, Nrhs): The solution of this equation
 */
template <typename Input, typename Coeffs, typename LowRank, typename RightHandSide, typename RightHandSideOut>
void solve_lower(const Eigen::MatrixBase<Input> &t,               // (N,)
                 const Eigen::MatrixBase<Coeffs> &c,              // (J,)
                 const Eigen::MatrixBase<LowRank> &U,             // (N, J)
                 const Eigen::MatrixBase<LowRank> &W,             // (N, J)
                 const Eigen::MatrixBase<RightHandSide> &Y,       // (N, nrhs)
                 Eigen::MatrixBase<RightHandSideOut> const &Z_out // (N, nrhs)
) {
  MakeEmptyWork(Input) F;
  solve_lower<false>(t, c, U, W, Y, Z_out, F);
}

/**
 * \brief Compute the solution of a upper triangular linear equation
 *
 * This computes `Z` such that:
 *
 * `Y = L^T * Y`
 *
 * where
 *
 * `L = 1 + tril(U*W^T)`
 *
 * This can be safely applied in place.
 *
 * @param t     (N,): The input coordinates (must be sorted)
 * @param c     (J,): The transport coefficients
 * @param U     (N, J): The first low rank matrix
 * @param W     (N, J): The second low rank matrix
 * @param Y     (N, Nrhs): The right hand side
 * @param Z_out (N, Nrhs): The solution of this equation
 */
template <typename Input, typename Coeffs, typename LowRank, typename RightHandSide, typename RightHandSideOut>
void solve_upper(const Eigen::MatrixBase<Input> &t,               // (N,)
                 const Eigen::MatrixBase<Coeffs> &c,              // (J,)
                 const Eigen::MatrixBase<LowRank> &U,             // (N, J)
                 const Eigen::MatrixBase<LowRank> &W,             // (N, J)
                 const Eigen::MatrixBase<RightHandSide> &Y,       // (N, nrhs)
                 Eigen::MatrixBase<RightHandSideOut> const &Z_out // (N, nrhs)
) {
  MakeEmptyWork(Input) F;
  solve_upper<false>(t, c, U, W, Y, Z_out, F);
}

/**
 * \brief Apply a strictly lower matrix multiply
 *
 * This computes:
 *
 * `Z += tril(U * V^T) * Y`
 *
 * where `tril` is the strictly lower triangular function.
 *
 * Note that this will *update* the value of `Z`.
 *
 * @param t     (N,): The input coordinates (must be sorted)
 * @param c     (J,): The transport coefficients
 * @param U     (N, J): The first low rank matrix
 * @param V     (N, J): The second low rank matrix
 * @param Y     (N, Nrhs): The matrix to be multiplied
 * @param Z_out (N, Nrhs): The matrix to be updated
 */
template <typename Input, typename Coeffs, typename LowRank, typename RightHandSide, typename RightHandSideOut>
void matmul_lower(const Eigen::MatrixBase<Input> &t,               // (N,)
                  const Eigen::MatrixBase<Coeffs> &c,              // (J,)
                  const Eigen::MatrixBase<LowRank> &U,             // (N, J)
                  const Eigen::MatrixBase<LowRank> &V,             // (N, J)
                  const Eigen::MatrixBase<RightHandSide> &Y,       // (N, nrhs)
                  Eigen::MatrixBase<RightHandSideOut> const &Z_out // (N, nrhs)
) {
  MakeEmptyWork(Input) F;
  matmul_lower<false>(t, c, U, V, Y, Z_out, F);
}

/**
 * \brief Apply a strictly upper matrix multiply
 *
 * This computes:
 *
 * `Z += triu(V * U^T) * Y`
 *
 * where `triu` is the strictly lower triangular function.
 *
 * Note that this will *update* the value of `Z`.
 *
 * @param t     (N,): The input coordinates (must be sorted)
 * @param c     (J,): The transport coefficients
 * @param U     (N, J): The first low rank matrix
 * @param V     (N, J): The second low rank matrix
 * @param Y     (N, Nrhs): The matrix to be multiplied
 * @param Z_out (N, Nrhs): The matrix to be updated
 */
template <typename Input, typename Coeffs, typename LowRank, typename RightHandSide, typename RightHandSideOut>
void matmul_upper(const Eigen::MatrixBase<Input> &t,               // (N,)
                  const Eigen::MatrixBase<Coeffs> &c,              // (J,)
                  const Eigen::MatrixBase<LowRank> &U,             // (N, J)
                  const Eigen::MatrixBase<LowRank> &V,             // (N, J)
                  const Eigen::MatrixBase<RightHandSide> &Y,       // (N, nrhs)
                  Eigen::MatrixBase<RightHandSideOut> const &Z_out // (N, nrhs)
) {
  MakeEmptyWork(Input) F;
  matmul_upper<false>(t, c, U, V, Y, Z_out, F);
}

/**
 * \brief The general lower-triangular dot product of a rectangular celerite system
 *
 * @param t1     (N,): The left input coordinates (must be sorted)
 * @param t2     (M,): The right input coordinates (must be sorted)
 * @param c      (J,): The transport coefficients
 * @param U      (N, J): The first low rank matrix
 * @param V      (M, J): The second low rank matrix
 * @param Y      (M, Nrhs): The matrix that will be multiplied
 * @param Z_out  (N, Nrhs): The result of the operation
 */
template <typename Input, typename Coeffs, typename LowRank, typename RightHandSide, typename RightHandSideOut>
void general_matmul_lower(const Eigen::MatrixBase<Input> &t1,              // (N,)
                          const Eigen::MatrixBase<Input> &t2,              // (M,)
                          const Eigen::MatrixBase<Coeffs> &c,              // (J,)
                          const Eigen::MatrixBase<LowRank> &U,             // (N, J)
                          const Eigen::MatrixBase<LowRank> &V,             // (M, J)
                          const Eigen::MatrixBase<RightHandSide> &Y,       // (M, nrhs)
                          Eigen::MatrixBase<RightHandSideOut> const &Z_out // (N, nrhs)
) {
  MakeEmptyWork(Input) F;
  general_matmul_lower<false>(t1, t2, c, U, V, Y, Z_out, F);
}

/**
 * \brief The general upper-triangular dot product of a rectangular celerite system
 *
 * @param t1     (N,): The left input coordinates (must be sorted)
 * @param t2     (M,): The right input coordinates (must be sorted)
 * @param c      (J,): The transport coefficients
 * @param U      (N, J): The first low rank matrix
 * @param V      (M, J): The second low rank matrix
 * @param Y      (M, Nrhs): The matrix that will be multiplied
 * @param Z_out  (N, Nrhs): The result of the operation
 */
template <typename Input, typename Coeffs, typename LowRank, typename RightHandSide, typename RightHandSideOut>
void general_matmul_upper(const Eigen::MatrixBase<Input> &t1,              // (N,)
                          const Eigen::MatrixBase<Input> &t2,              // (M,)
                          const Eigen::MatrixBase<Coeffs> &c,              // (J,)
                          const Eigen::MatrixBase<LowRank> &U,             // (N, J)
                          const Eigen::MatrixBase<LowRank> &V,             // (M, J)
                          const Eigen::MatrixBase<RightHandSide> &Y,       // (M, nrhs)
                          Eigen::MatrixBase<RightHandSideOut> const &Z_out // (N, nrhs)
) {
  MakeEmptyWork(Input) F;
  general_matmul_upper<false>(t1, t2, c, U, V, Y, Z_out, F);
}

} // namespace core
} // namespace celerite2

#endif // _CELERITE2_INTERFACE_HPP_DEFINED_


/******************************************************************************
 * Copyright (C) 2015 by Jerome Maye                                          *
 * jerome.maye@gmail.com                                                      *
 ******************************************************************************/

#include "aslam/calibration/egomotion/simulation/simulationEngine.h"

#include <cmath>

#include <utility>
#include <vector>
#include <limits>
#include <unordered_map>

#include <boost/make_shared.hpp>

#include <bsplines/BSplineFitter.hpp>

#include <sm/timing/NsecTimeUtilities.hpp>

#include <sm/kinematics/Transformation.hpp>
#include <sm/kinematics/quaternion_algebra.hpp>

#include <aslam/calibration/statistics/NormalDistribution.h>

#include "aslam/calibration/egomotion/simulation/Trajectory.h"
#include "aslam/calibration/egomotion/simulation/SimulationData.h"
#include "aslam/calibration/egomotion/simulation/SimulationParams.h"
#include "aslam/calibration/egomotion/algo/bestQuat.h"

using namespace sm::timing;
using namespace sm::kinematics;
using namespace bsplines;

namespace aslam {
  namespace calibration {

/******************************************************************************/
/* Methods                                                                    */
/******************************************************************************/

    void simulate(const SimulationParams& params, SimulationData& data) {
      const auto type = params.trajectoryParams.type;
      const auto f = params.trajectoryParams.f;
      const auto dt = params.dt;
      const auto A = params.trajectoryParams.A;
      const auto T = params.T;

      NsecTime timestamp = 0;
      std::vector<NsecTime> timestamps;
      timestamps.push_back(timestamp);
      std::vector<Eigen::Vector3d> transPoses;
      transPoses.push_back(params.trajectoryParams.w_T_v_0.t());
      std::vector<Eigen::Vector4d> rotPoses;
      rotPoses.push_back(params.trajectoryParams.w_T_v_0.q());

      if (type == "combined") {
        // straight
        double w_phi_v_km1 = 0;
        Eigen::Vector2d w_r_wv_km1 = Eigen::Vector2d::Zero();
        for (double t = dt; t < T / 3; t += dt) {
          Eigen::Vector2d v_v_om_wv_k = genSineBodyVel2d(w_phi_v_km1,
            w_r_wv_km1, t, dt, 0.0, f);
          Eigen::Vector3d v_v_wv_k(v_v_om_wv_k(0), 0.0, 0.0);
          Eigen::Vector3d v_om_wv_k(0.0, 0.0, v_v_om_wv_k(1));
          const auto w_T_v_t = integrateMotionModel(
            Transformation(rotPoses.back(), transPoses.back()), v_v_wv_k,
            v_om_wv_k, dt);
          timestamp += secToNsec(dt);
          timestamps.push_back(timestamp);
          transPoses.push_back(w_T_v_t.t());
          auto q = w_T_v_t.q();
          if (!rotPoses.empty())
            q = bestQuat(rotPoses.back(), q);
          rotPoses.push_back(q);
        }

        // sinePlanar
        w_phi_v_km1 = std::atan(2 * M_PI * f * A * cos(2 * M_PI * f * dt));
        w_r_wv_km1 = Eigen::Vector2d::Zero();
        auto lastTimestamp = nsecToSec(timestamps.back());
        for (double t = lastTimestamp + dt; t < 2.0 / 3.0 * T; t += dt) {
          Eigen::Vector2d v_v_om_wv_k = genSineBodyVel2d(w_phi_v_km1,
            w_r_wv_km1, t, dt, A, f);
          Eigen::Vector3d v_v_wv_k(v_v_om_wv_k(0), 0.0, 0.0);
          Eigen::Vector3d v_om_wv_k(0.0, 0.0, v_v_om_wv_k(1));
          const auto w_T_v_t = integrateMotionModel(
            Transformation(rotPoses.back(), transPoses.back()), v_v_wv_k,
            v_om_wv_k, dt);
          timestamp += secToNsec(dt);
          timestamps.push_back(timestamp);
          transPoses.push_back(w_T_v_t.t());
          auto q = w_T_v_t.q();
          if (!rotPoses.empty())
            q = bestQuat(rotPoses.back(), q);
          rotPoses.push_back(q);
        }

        // random
        w_phi_v_km1 = std::atan(2 * M_PI * f * A * cos(2 * M_PI * f * dt));
        w_r_wv_km1 = Eigen::Vector2d::Zero();
        lastTimestamp = nsecToSec(timestamps.back());
        auto trajDist = NormalDistribution<3>(
          Eigen::Matrix<double, 3, 1>::Zero(),
          Eigen::Matrix<double, 3, 3>::Identity() * 2);
        for (double t = lastTimestamp + dt; t < T; t += dt) {
          Eigen::Vector2d v_v_om_wv_k = genSineBodyVel2d(w_phi_v_km1,
            w_r_wv_km1, t, dt, A, f);
          auto sample = trajDist.getSample();
          Eigen::Vector3d v_v_wv_k(v_v_om_wv_k(0), v_v_om_wv_k(0),
            v_v_om_wv_k(0));
          v_v_wv_k = v_v_wv_k + sample;
          sample = trajDist.getSample();
          Eigen::Vector3d v_om_wv_k(v_v_om_wv_k(1), v_v_om_wv_k(1),
            v_v_om_wv_k(1));
          v_om_wv_k = v_om_wv_k + sample;
          const auto w_T_v_t = integrateMotionModel(
            Transformation(rotPoses.back(), transPoses.back()), v_v_wv_k,
            v_om_wv_k, dt);
          timestamp += secToNsec(dt);
          timestamps.push_back(timestamp);
          transPoses.push_back(w_T_v_t.t());
          auto q = w_T_v_t.q();
          if (!rotPoses.empty())
            q = bestQuat(rotPoses.back(), q);
          rotPoses.push_back(q);
        }
      }
      else {
        double w_phi_v_km1;
        if (type == "straight")
          w_phi_v_km1 = 0;
        else
          w_phi_v_km1 = std::atan(2 * M_PI * f * A * cos(2 * M_PI * f * dt));
        Eigen::Vector2d w_r_wv_km1 = Eigen::Vector2d::Zero();

        auto trajDist = NormalDistribution<3>(
          Eigen::Matrix<double, 3, 1>::Zero(),
          Eigen::Matrix<double, 3, 3>::Identity() * 2);

        // generate trajectory
        for (double t = dt; t < T; t += dt) {
          Eigen::Vector2d v_v_om_wv_k;
          if (type == "straight")
            v_v_om_wv_k =
              genSineBodyVel2d(w_phi_v_km1, w_r_wv_km1, t, dt, 0.0, f);
          else
            v_v_om_wv_k = genSineBodyVel2d(w_phi_v_km1, w_r_wv_km1, t, dt, A,
              f);
          Eigen::Vector3d v_v_wv_k;
          Eigen::Vector3d v_om_wv_k;
          if (type == "random") {
            auto sample = trajDist.getSample();
            v_v_wv_k = Eigen::Vector3d(v_v_om_wv_k(0), v_v_om_wv_k(0),
              v_v_om_wv_k(0));
            v_v_wv_k = v_v_wv_k + sample;
            sample = trajDist.getSample();
            v_om_wv_k = Eigen::Vector3d(v_v_om_wv_k(1), v_v_om_wv_k(1),
              v_v_om_wv_k(1));
            v_om_wv_k = v_om_wv_k + sample;
          }
          else {
            v_v_wv_k = Eigen::Vector3d(v_v_om_wv_k(0), 0.0, 0.0);
            v_om_wv_k = Eigen::Vector3d(0.0, 0.0, v_v_om_wv_k(1));
          }
          const auto w_T_v_t = integrateMotionModel(
            Transformation(rotPoses.back(), transPoses.back()), v_v_wv_k,
            v_om_wv_k, dt);
          timestamp += secToNsec(dt);
          timestamps.push_back(timestamp);
          transPoses.push_back(w_T_v_t.t());
          auto q = w_T_v_t.q();
          if (!rotPoses.empty())
            q = bestQuat(rotPoses.back(), q);
          rotPoses.push_back(q);
        }
      }

      // fit splines
      const auto elapsedTime = (timestamps.back() - timestamps.front()) /
        static_cast<double>(NsecTimePolicy::getOne());
      const auto numMeasurements = timestamps.size();
      const auto measPerSec = std::lround(numMeasurements / elapsedTime);
      int numSegments;
      if (measPerSec > params.trajectoryParams.splineKnotsPerSecond)
        numSegments = std::ceil(params.trajectoryParams.splineKnotsPerSecond *
          elapsedTime);
      else
        numSegments = numMeasurements;

      data.trajectory.translationSpline =
        boost::make_shared<Trajectory::TranslationSpline>(
        EuclideanBSpline<Eigen::Dynamic, 3, NsecTimePolicy>::CONF(
        EuclideanBSpline<Eigen::Dynamic, 3,
        NsecTimePolicy>::CONF::ManifoldConf(3),
        params.trajectoryParams.transSplineOrder));
      BSplineFitter<Trajectory::TranslationSpline>::initUniformSpline(
        *data.trajectory.translationSpline, timestamps, transPoses, numSegments,
        params.trajectoryParams.transSplineLambda);

      data.trajectory.rotationSpline =
        boost::make_shared<Trajectory::RotationSpline>(
        UnitQuaternionBSpline<Eigen::Dynamic, NsecTimePolicy>::CONF(
        UnitQuaternionBSpline<Eigen::Dynamic,
        NsecTimePolicy>::CONF::ManifoldConf(),
        params.trajectoryParams.rotSplineOrder));
      BSplineFitter<Trajectory::RotationSpline>::initUniformSpline(
        *data.trajectory.rotationSpline, timestamps, rotPoses, numSegments,
        params.trajectoryParams.rotSplineLambda);

      // generate sensor data
      auto prevTransformation = Transformation();
      const auto referenceSensor = params.referenceSensor;
      bool firstTime = true;
      std::unordered_map<size_t, NormalDistribution<6> > normDists;
      for (const auto& cov : params.sigma2)
        normDists[cov.first] = NormalDistribution<6>(
          Eigen::Matrix<double, 6, 1>::Zero(), cov.second);
      for (auto t = data.trajectory.translationSpline->getMinTime();
          t <= data.trajectory.translationSpline->getMaxTime();
          t += secToNsec(dt)) {
        auto translationEvaluator =
          data.trajectory.translationSpline->getEvaluatorAt<0>(t);
        auto rotationEvaluator =
          data.trajectory.rotationSpline->getEvaluatorAt<0>(t);
        const auto currentTransformation =
          Transformation(rotationEvaluator.eval(), translationEvaluator.eval());
        if (!firstTime) {
          for (const auto& cov : params.sigma2) {
            if (cov.first == referenceSensor) {
              auto w_T_s = prevTransformation.inverse() * currentTransformation;
              MotionMeasurement motion;
              motion.motion = w_T_s;
              motion.duration = secToNsec(dt);
              motion.sigma2 = cov.second;
              data.motionData[cov.first].push_back(std::make_pair(t, motion));
              auto normSample = normDists[cov.first].getSample();
              motion.motion = w_T_s * Transformation(qexp(normSample.tail<3>()),
                normSample.head<3>());
              data.motionDataNoisy[cov.first].push_back(std::make_pair(t,
                motion));
            }
            else {
              auto r_T_s = params.sensorCalibration.at(cov.first).second;
              auto timeDelay = params.sensorCalibration.at(cov.first).first;
              auto w_T_s = r_T_s.inverse() * prevTransformation.inverse() *
                currentTransformation * r_T_s;
              MotionMeasurement motion;
              motion.motion = w_T_s;
              motion.duration = secToNsec(dt);
              motion.sigma2 = cov.second;
              data.motionData[cov.first].push_back(std::make_pair(t + timeDelay,
                motion));
              auto normSample = normDists[cov.first].getSample();
              motion.motion = w_T_s * Transformation(qexp(normSample.tail<3>()),
                normSample.head<3>());
              data.motionDataNoisy[cov.first].push_back(std::make_pair(t +
                timeDelay, motion));
            }
          }
        }
        prevTransformation = currentTransformation;
        firstTime = false;
      }
    }

    Transformation integrateMotionModel(const Transformation& w_T_v_km1, const
        Eigen::Vector3d& v_v_wv_km1, const Eigen::Vector3d& v_om_wv_km1, double
        dt) {
      const Eigen::Vector4d w_q_v_km1 = w_T_v_km1.q();
      const Eigen::Vector3d w_r_wv_k = w_T_v_km1.t() + dt *
        quatRotate(w_q_v_km1, v_v_wv_km1);
      Eigen::Vector4d v_q_w_k = quatInv(w_q_v_km1);
      const double v_om_wv_km1_norm2 = v_om_wv_km1.norm();
      if (v_om_wv_km1_norm2 > std::numeric_limits<double>::epsilon()) {
        const double c = std::cos(v_om_wv_km1_norm2 / 2.0 * dt);
        const double s = std::sin(v_om_wv_km1_norm2 / 2.0 * dt);
        Eigen::Vector4d sigma;
        sigma.head<3>() = v_om_wv_km1 / v_om_wv_km1_norm2 * s;
        sigma(3) = c;
        v_q_w_k = qplus(sigma, v_q_w_k);
      }

      return Transformation(quatInv(v_q_w_k), w_r_wv_k);
    }

    Eigen::Vector2d genSineBodyVel2d(double& w_phi_v_km1, Eigen::Vector2d&
        w_r_wv_km1, double t, double dt, double A, double f) {
      const double w_phi_v_k =
        std::atan(2.0 * M_PI * f * A * cos(2 * M_PI * f * t));
      const double v_om_wv_k = (w_phi_v_k - w_phi_v_km1) / dt;
      const Eigen::Vector2d w_r_wv_k(t, A * sin(2.0 * M_PI * f * t));
      const Eigen::Vector2d w_r_v_k_km1v_k = w_r_wv_k - w_r_wv_km1;
      const Eigen::Vector2d w_v_wv_k = w_r_v_k_km1v_k / dt;
      const double c = std::cos(w_phi_v_k);
      const double s = std::sin(w_phi_v_k);
      Eigen::Matrix2d w_R_v_k;
      w_R_v_k << c, -s, s, c;
      Eigen::Vector2d v_v_wv_k = w_R_v_k.transpose() * w_v_wv_k;
      w_phi_v_km1 = w_phi_v_k;
      w_r_wv_km1 = w_r_wv_k;
      return Eigen::Vector2d(v_v_wv_k(0), v_om_wv_k);
    }

  }
}