// This file is part of the dune-xt project:
//   https://zivgitlab.uni-muenster.de/ag-ohlberger/dune-community/dune-xt
// Copyright 2009-2021 dune-xt developers and contributors. All rights reserved.
// License: Dual licensed as BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)
//      or  GPL-2.0+ (http://opensource.org/licenses/gpl-license)
//          with "runtime exception" (http://www.dune-project.org/license.html)
// Authors:
//   Felix Schindler (2014 - 2017, 2020)
//   René Fritze     (2014 - 2020)
//   Tim Keil        (2018)
//   Tobias Leibner  (2014, 2016 - 2020)

#ifndef DUNE_XT_COMMON_FVECTOR_26_HH
#define DUNE_XT_COMMON_FVECTOR_26_HH

#include <functional>
#include <initializer_list>
#include <numeric>
#include <type_traits>
#include <vector>

#include <boost/functional/hash.hpp>

#include <dune/common/fvector.hh>
#include <dune/common/fmatrix.hh>
#include <dune/common/promotiontraits.hh>

#include <dune/xt/common/debug.hh>
#include <dune/xt/common/densevector.hh>
#include <dune/xt/common/exceptions.hh>
#include <dune/xt/common/float_cmp.hh>
#include <dune/xt/common/numeric.hh>
#include <dune/xt/common/type_traits.hh>
#include <dune/xt/common/vector.hh>

namespace Dune {
namespace XT::Common {

/**
 * \todo We need to implement all operators from the base which return the base, to rather return ourselfes!
 */
template <class K, int SIZE>
class FieldVector : public Dune::FieldVector<K, SIZE>
{
  static_assert(SIZE >= 0, "Really?");

  using BaseType = Dune::FieldVector<K, SIZE>;
  using ThisType = FieldVector<K, SIZE>;

public:
  FieldVector(const K& kk = suitable_default<K>::value())
    : BaseType(kk)
  {}

  FieldVector(const ThisType& other) = default;

  FieldVector(const BaseType& other)
    : BaseType(other)
  {}

  /* FieldMatrix< K, 1, 1 > is convertible to K, which in turn is convertible to FieldVector< K, 1 >. Without the
   * following constructor, this leads to an "ambiguous constructor" error (candidates are copy constructor and
   * constructor taking a K) */
  template <class Type = K>
  FieldVector(const typename std::enable_if<SIZE == 1 && std::is_same<K, Type>::value,
                                            typename Dune::FieldMatrix<K, 1, 1>>::type& mat)
    : BaseType(mat[0][0])
  {}

  FieldVector(const std::vector<K>& vec)
    : BaseType()
  {
#ifndef NDEBUG
    if (vec.size() != SIZE)
      DUNE_THROW(Exceptions::wrong_input_given,
                 "You are trying to construct a FieldVector< ..., " << SIZE << " > (of "
                                                                    << "static size) from a vector of size "
                                                                    << vec.size() << "!");
#endif // NDEBUG
    for (size_t ii = 0; ii < SIZE; ++ii)
      this->operator[](ii) = vec[ii];
  } // FieldVector(...)

  FieldVector(std::initializer_list<K> list)
    : BaseType()
  {
#ifndef NDEBUG
    if (list.size() != SIZE)
      DUNE_THROW(Exceptions::wrong_input_given,
                 "You are trying to construct a FieldVector< ..., " << SIZE << " > (of "
                                                                    << "static size) from a list of size "
                                                                    << list.size() << "!");
#endif // NDEBUG
    size_t ii = 0;
    for (auto element : list)
      this->operator[](ii++) = element;
  } // FieldVector(...)

  template <class C>
  FieldVector(
      const DenseVector<C>& x,
      typename std::enable_if<IsFieldVectorSizeCorrect<C, SIZE>::value
                              && std::is_convertible<typename DenseVector<C>::value_type, K>::value>::type* /*dummy*/
      = nullptr)
    : BaseType(x)
  {}

  operator std::vector<K>() const
  {
    std::vector<K> ret(SIZE);
    for (size_t ii = 0; ii < SIZE; ++ii)
      ret[ii] = this->operator[](ii);
    return ret;
  }

  operator Dune::FieldMatrix<K, 1, SIZE>() const
  {
    Dune::FieldMatrix<K, 1, SIZE> ret;
    ret[0] = *this;
    return ret;
  }

  template <int S = SIZE>
  operator typename std::enable_if<(S == SIZE) && (SIZE != 1), Dune::FieldMatrix<K, S, 1>>::type() const
  {
    Dune::FieldMatrix<K, SIZE, 1> ret;
    for (size_t ii = 0; ii < SIZE; ++ii)
      ret[ii][0] = this->operator[](ii);
    return ret;
  }

  operator std::array<K, SIZE>() const
  {
    std::array<K, SIZE> ret;
    for (size_t ii = 0; ii < SIZE; ++ii)
      ret[ii] = this->operator[](ii);
    return ret;
  }

  template <class K_Other>
  typename multiplication_promotion<K, K_Other>::type operator*(const Dune::FieldVector<K_Other, SIZE>& other) const
  {
    typename multiplication_promotion<K, K_Other>::type result(0.);
    for (size_t ii = 0; ii < SIZE; ++ii)
      result += (*this)[ii] * other[ii];
    return result;
  }

  template <int R>
  typename std::enable_if<SIZE != 1 && R == SIZE, K>::type operator*(const Dune::FieldMatrix<K, R, 1>& mat) const
  {
    K ret = 0;
    for (size_t ii = 0; ii < SIZE; ++ii)
      ret += (*this)[ii] * mat[ii][0];
    return ret;
  }

  //! This op is not redundant
  ThisType operator*(const K& scalar) const
  {
    ThisType ret(*this);
    ret *= scalar;
    return ret;
  }

  ThisType operator/(const K& scalar) const
  {
    ThisType ret(*this);
    ret /= scalar;
    return ret;
  }
}; // class FieldVector

template <class K, size_t block_num, size_t size_block>
class BlockedFieldVector
{
  using ThisType = BlockedFieldVector;

public:
  static constexpr size_t num_blocks = block_num;
  static constexpr size_t block_size = size_block;
  static constexpr size_t static_size = num_blocks * block_size;
  using VectorType = Dune::FieldVector<K, static_size>;
  using XtVectorType = FieldVector<K, static_size>;
  using BlockType = FieldVector<K, block_size>;

  BlockedFieldVector(const K& val = K(0.))
    : backend_(BlockType(val))
  {}

  template <class OtherVectorType>
  BlockedFieldVector(const OtherVectorType& other,
                     std::enable_if_t<is_vector<OtherVectorType>::value, int> /*dummy*/ = 0)
  {
    *this = other;
  }

  BlockedFieldVector(const BlockType& block)
    : backend_(block)
  {}

  BlockedFieldVector(const std::initializer_list<K>& init_list)
  {
    assert(init_list.size() == static_size || init_list.size() == num_blocks);
    if (init_list.size() == static_size) {
      *this = VectorType(init_list);
    } else {
      size_t jj = 0;
      for (const auto& block_init : init_list)
        block(jj++) = block_init;
    }
  }

  template <class OtherVectorType>
  std::enable_if_t<is_vector<OtherVectorType>::value, ThisType>& operator=(const OtherVectorType& other)
  {
    assert(other.size() == static_size);
    for (size_t jj = 0; jj < num_blocks; ++jj)
      for (size_t ii = 0; ii < block_size; ++ii)
        backend_[jj][ii] = other[jj * block_size + ii];
    return *this;
  }

  size_t size() const
  {
    return static_size;
  }

  K& operator[](const size_t ii)
  {
    assert(ii < static_size);
    return backend_[ii / block_size][ii % block_size];
  }

  const K& operator[](const size_t ii) const
  {
    assert(ii < static_size);
    return backend_[ii / block_size][ii % block_size];
  }

  K& get_entry(const size_t jj, const size_t ii)
  {
    assert(jj < num_blocks);
    assert(ii < block_size);
    return backend_[jj][ii];
  }

  const K& get_entry(const size_t jj, const size_t ii) const
  {
    assert(jj < num_blocks);
    assert(ii < block_size);
    return backend_[jj][ii];
  }

  BlockType& block(const size_t jj)
  {
    assert(jj < num_blocks);
    return backend_[jj];
  }

  const BlockType& block(const size_t jj) const
  {
    assert(jj < num_blocks);
    return backend_[jj];
  }

  K one_norm() const
  {
    K ret(0);
    for (const auto& block_vec : backend_)
      for (const auto& entry : block_vec)
        ret += std::abs(entry);
    return ret;
  }

  K two_norm() const
  {
    return std::sqrt(two_norm2());
  }

  K two_norm2() const
  {
    K ret(0);
    for (const auto& block_vec : backend_)
      for (const auto& entry : block_vec)
        ret += std::pow(entry, 2);
    return ret;
  }

  operator XtVectorType() const
  {
    XtVectorType ret(0.);
    for (size_t jj = 0; jj < num_blocks; ++jj)
      for (size_t ii = 0; ii < block_size; ++ii)
        ret[jj * block_size + ii] = backend_[jj][ii];
    return ret;
  }

  K* data()
  {
    return &(backend_[0][0]);
  }

  const K* data() const
  {
    return &(backend_[0][0]);
  }

  K* begin()
  {
    return data();
  }

  const K* begin() const
  {
    return data();
  }

  K* end()
  {
    return &(backend_[num_blocks - 1][block_size]);
  }

  const K* end() const
  {
    return &(backend_[num_blocks - 1][block_size]);
  }

  ThisType& operator*=(const K& val)
  {
    for (size_t jj = 0; jj < num_blocks; ++jj)
      backend_[jj] *= val;
    return *this;
  }

  ThisType operator*(const K& val) const
  {
    auto ret = *this;
    ret *= val;
    return ret;
  }

  K operator*(const ThisType& other) const
  {
    return Common::transform_reduce(begin(), end(), other.begin(), 0.);
  }

  ThisType& operator+=(const ThisType& other)
  {
    backend_ += other.backend_;
    return *this;
  }

  ThisType operator+(const ThisType& other) const
  {
    auto ret = *this;
    ret += other;
    return ret;
  }

  ThisType& operator-=(const ThisType& other)
  {
    backend_ -= other.backend_;
    return *this;
  }

  ThisType operator-(const ThisType& other) const
  {
    auto ret = *this;
    ret -= other;
    return ret;
  }

private:
  FieldVector<BlockType, num_blocks> backend_;
};

//! this allows to set the init value of the FieldVector at compile time
template <class K, int SIZE, K value>
class ValueInitFieldVector : public Dune::XT::Common::FieldVector<K, SIZE>
{
  using BaseType = Dune::XT::Common::FieldVector<K, SIZE>;

public:
  ValueInitFieldVector()
    : BaseType(value)
  {}
}; // class ValueInitFieldVector

//! struct to be used as comparison function e.g. in a std::map<FieldVector<...>, ..., FieldVectorLess>
struct FieldVectorLess
{
  template <class FieldType, int dimDomain>
  bool operator()(const Dune::FieldVector<FieldType, dimDomain>& a,
                  const Dune::FieldVector<FieldType, dimDomain>& b) const
  {
    for (size_t dd = 0; dd < dimDomain; ++dd) {
      if (a[dd] < b[dd])
        return true;
      if (a[dd] > b[dd])
        return false;
    }
    return false;
  }
};

struct FieldVectorFloatLess
{
  template <class FieldType, int dimDomain>
  bool operator()(const Dune::FieldVector<FieldType, dimDomain>& a,
                  const Dune::FieldVector<FieldType, dimDomain>& b) const
  {
    for (size_t dd = 0; dd < dimDomain; ++dd) {
      if (XT::Common::FloatCmp::lt(a[dd], b[dd]))
        return true;
      if (XT::Common::FloatCmp::gt(a[dd], b[dd]))
        return false;
    }
    return false;
  }
};

//! Specialization of VectorAbstraction for Dune::XT::Common::FieldVector
template <class K, int SIZE>
struct VectorAbstraction<Dune::XT::Common::FieldVector<K, SIZE>>
  : public internal::VectorAbstractionBase<Dune::XT::Common::FieldVector<K, SIZE>, K>
  , public internal::HasSubscriptOperatorForVectorAbstraction<Dune::XT::Common::FieldVector<K, SIZE>,
                                                              typename Dune::FieldTraits<K>::field_type>
{
  static constexpr bool has_static_size = true;
  static constexpr size_t static_size = SIZE;
  static constexpr bool is_contiguous = true;

  template <size_t SZ = SIZE, class Field = K>
  using VectorTypeTemplate = Dune::XT::Common::FieldVector<Field, SZ>;

  template <size_t SZ = SIZE>
  static inline VectorTypeTemplate<SZ> create(const size_t sz, const K& val = suitable_default<K>::value())
  {
    if (sz != SZ)
      DUNE_THROW(Exceptions::wrong_input_given,
                 "You are trying to construct a FieldVector< ..., " << SZ << " > (of "
                                                                    << "static size) with " << sz << " elements!");
    return VectorTypeTemplate<SZ>(val);
  }
};

//! Specialization of VectorAbstraction for Dune::XT::Common::BlockedFieldVector
template <class K, size_t num_blocks, size_t block_size>
struct VectorAbstraction<Dune::XT::Common::BlockedFieldVector<K, num_blocks, block_size>>
  : public internal::VectorAbstractionBase<Dune::XT::Common::BlockedFieldVector<K, num_blocks, block_size>, K>
  , public internal::HasSubscriptOperatorForVectorAbstraction<
        Dune::XT::Common::BlockedFieldVector<K, num_blocks, block_size>,
        typename Dune::FieldTraits<K>::field_type>
{
  using VectorType = Dune::XT::Common::BlockedFieldVector<K, num_blocks, block_size>;
  static constexpr bool has_static_size = true;
  static constexpr size_t static_size = VectorType::static_size;
  static constexpr bool is_contiguous = true;

  template <size_t SZ = static_size>
  static inline VectorType create(const size_t sz, const K& val = suitable_default<K>::value())
  {
    static_assert(SZ == static_size, "Creation of Vector with different size not implemented!");
    if (sz != SZ)
      DUNE_THROW(Exceptions::wrong_input_given,
                 "You are trying to construct a FieldVector< ..., " << SZ << " > (of "
                                                                    << "static size) with " << sz << " elements!");
    return VectorType(val);
  }
};

template <class V>
typename std::enable_if<
    is_vector<V>::value && VectorAbstraction<V>::has_static_size,
    std::unique_ptr<FieldVector<typename VectorAbstraction<V>::S, VectorAbstraction<V>::static_size>>>::type
make_field_container_ptr(const V& vec)
{
  auto ret = std::make_unique<FieldVector<typename VectorAbstraction<V>::S, VectorAbstraction<V>::static_size>>;
  for (size_t ii = 0; ii < ret->size(); ++ii)
    (*ret)[ii] = vec[ii];
  return std::move(ret);
}

template <class V>
typename std::enable_if<is_vector<V>::value && VectorAbstraction<V>::has_static_size,
                        FieldVector<typename VectorAbstraction<V>::S, VectorAbstraction<V>::static_size>>::type
make_field_container(const V& vec)
{
  FieldVector<typename VectorAbstraction<V>::S, VectorAbstraction<V>::static_size> ret;
  for (size_t ii = 0; ii < ret.size(); ++ii)
    ret[ii] = vec[ii];
  return ret;
}

template <class K, int SIZE>
FieldVector<K, SIZE> make_field_container(Dune::FieldVector<K, SIZE>&& vec)
{
  return std::move(vec);
}

template <class K, int SIZE>
FieldVector<K, SIZE> make_field_container(FieldVector<K, SIZE>&& vec)
{
  return std::move(vec);
}

template <class K>
FieldVector<K, 3> cross_product(const FieldVector<K, 3>& u, const FieldVector<K, 3>& v)
{
  FieldVector<K, 3> ret;
  ret[0] = u[1] * v[2] - u[2] * v[1];
  ret[1] = u[2] * v[0] - u[0] * v[2];
  ret[2] = u[0] * v[1] - u[1] * v[0];
  return ret;
}

namespace internal {

/**
 * Most likely, you do not want to use this class directly, but \sa hstack instead.
 */
template <class K>
struct hstack_decay
{
  static_assert(is_arithmetic<K>::value);
  using type = K;
};

template <class K, int SIZE>
struct hstack_decay<Dune::FieldVector<K, SIZE>>
{
  using type = Dune::XT::Common::FieldVector<K, SIZE>;
};

template <class K, int SIZE>
struct hstack_decay<const Dune::FieldVector<K, SIZE>&> : public hstack_decay<Dune::FieldVector<K, SIZE>>
{};

template <class K, int SIZE>
struct hstack_decay<Dune::XT::Common::FieldVector<K, SIZE>> : public hstack_decay<Dune::FieldVector<K, SIZE>>
{};

template <class K, int SIZE>
struct hstack_decay<const Dune::XT::Common::FieldVector<K, SIZE>&> : public hstack_decay<Dune::FieldVector<K, SIZE>>
{};

/**
 * Most likely, you do not want to use this, but \sa hstack instead.
 */
template <class T>
using hstack_decay_t = typename hstack_decay<T>::type;

/**
 * Most likely, you do not want to use this class directly, but \sa hstack instead.
 */
template <class... Vectors>
struct hstack_helper;

template <class L, class R, class... Vectors>
struct hstack_helper<L, R, Vectors...>
{
  using type =
      typename hstack_helper<typename hstack_helper<hstack_decay_t<L>, hstack_decay_t<R>>::type, Vectors...>::type;
};

template <class K, int SIZE>
struct hstack_helper<Dune::FieldVector<K, SIZE>>
{
  using type = hstack_decay_t<Dune::FieldVector<K, SIZE>>;
};

template <class K, int SIZE>
struct hstack_helper<Dune::XT::Common::FieldVector<K, SIZE>> : public hstack_helper<Dune::FieldVector<K, SIZE>>
{};

template <class KL, class KR, int r>
struct hstack_helper<KL, Dune::FieldVector<KR, r>>
{
  using type = hstack_decay_t<Dune::FieldVector<typename PromotionTraits<KL, KR>::PromotedType, 1 + r>>;
};

template <class KL, class KR, int r>
struct hstack_helper<KL, Dune::XT::Common::FieldVector<KR, r>> : public hstack_helper<KL, Dune::FieldVector<KR, r>>
{};

template <class KL, int l, class KR>
struct hstack_helper<Dune::FieldVector<KL, l>, KR>
{
  using type = hstack_decay_t<Dune::FieldVector<typename PromotionTraits<KL, KR>::PromotedType, l + 1>>;
};

template <class KL, int l, class KR>
struct hstack_helper<Dune::XT::Common::FieldVector<KL, l>, KR> : public hstack_helper<Dune::FieldVector<KL, l>, KR>
{};

template <class KL, int l, class KR, int r>
struct hstack_helper<Dune::FieldVector<KL, l>, Dune::FieldVector<KR, r>>
{
  using type = hstack_decay_t<Dune::FieldVector<typename PromotionTraits<KL, KR>::PromotedType, l + r>>;
};

template <class KL, int l, class KR, int r>
struct hstack_helper<Dune::XT::Common::FieldVector<KL, l>, Dune::FieldVector<KR, r>>
  : public hstack_helper<Dune::FieldVector<KL, l>, Dune::FieldVector<KR, r>>
{};

template <class KL, int l, class KR, int r>
struct hstack_helper<Dune::FieldVector<KL, l>, Dune::XT::Common::FieldVector<KR, r>>
  : public hstack_helper<Dune::FieldVector<KL, l>, Dune::FieldVector<KR, r>>
{};

template <class KL, int l, class KR, int r>
struct hstack_helper<Dune::XT::Common::FieldVector<KL, l>, Dune::XT::Common::FieldVector<KR, r>>
  : public hstack_helper<Dune::FieldVector<KL, l>, Dune::FieldVector<KR, r>>
{};

// not sure why this is required, would've hoped the decay above would take care of it
template <class KL, int l, class KR, int r>
struct hstack_helper<Dune::FieldVector<KL, l>, const Dune::FieldVector<KR, r>&>
  : public hstack_helper<Dune::FieldVector<KL, l>, Dune::FieldVector<KR, r>>
{};

// not sure why this is required, would've hoped the decay above would take care of it
template <class KL, int l, class KR, int r>
struct hstack_helper<Dune::FieldVector<KL, l>, const Dune::XT::Common::FieldVector<KR, r>&>
  : public hstack_helper<Dune::FieldVector<KL, l>, Dune::FieldVector<KR, r>>
{};

} // namespace internal

/// \brief Specialization of \sa hstack to stop the recursion.
template <class K, int SIZE>
Dune::XT::Common::FieldVector<K, SIZE> hstack(Dune::FieldVector<K, SIZE>&& vec)
{
  return std::move(vec);
}

/// \brief Specialization of \sa hstack to stop the recursion.
template <class K, int SIZE>
Dune::XT::Common::FieldVector<K, SIZE> hstack(Dune::XT::Common::FieldVector<K, SIZE>&& vec)
{
  return std::move(vec);
}

/// \brief Specialization of \sa hstack to stop the recursion.
template <class K, int SIZE>
Dune::XT::Common::FieldVector<K, SIZE> hstack(const Dune::FieldVector<K, SIZE>& vec)
{
  return vec;
}

/// \brief Specialization of \sa hstack to stop the recursion.
template <class K, int SIZE>
Dune::XT::Common::FieldVector<K, SIZE> hstack(const Dune::XT::Common::FieldVector<K, SIZE>& vec)
{
  return vec;
}

/// \brief Specialization of \sa hstack for two arguments.
template <class KL, class KR, int r>
typename std::enable_if<
    is_arithmetic<KL>::value && !is_field_vector<KL>::value,
    typename internal::hstack_helper<internal::hstack_decay_t<KL>,
                                     internal::hstack_decay_t<Dune::FieldVector<KR, r>>>::type>::type
hstack(const KL& left, const Dune::FieldVector<KR, r>& right)
{
  typename internal::hstack_helper<internal::hstack_decay_t<KL>,
                                   internal::hstack_decay_t<Dune::FieldVector<KR, r>>>::type ret;
  ret[0] = left;
  for (size_t ii = 0; ii < r; ++ii)
    ret[ii + 1] = right[ii];
  return ret;
} // ... hstack<1, r>(...)

/// \brief Specialization of \sa hstack for two arguments.
template <class KL, class KR, int r>
typename std::enable_if<
    is_arithmetic<KL>::value && !is_field_vector<KL>::value,
    typename internal::hstack_helper<internal::hstack_decay_t<KL>,
                                     internal::hstack_decay_t<Dune::FieldVector<KR, r>>>::type>::type
hstack(const KL& left, const Dune::XT::Common::FieldVector<KR, r>& right)
{
  return hstack(left, static_cast<const Dune::FieldVector<KR, r>&>(right));
}

/// \brief Specialization of \sa hstack for two arguments.
template <class KL, int l, class KR>
typename std::enable_if<is_arithmetic<KR>::value && !is_field_vector<KR>::value,
                        typename internal::hstack_helper<internal::hstack_decay_t<Dune::FieldVector<KL, l>>,
                                                         internal::hstack_decay_t<KR>>::type>::type
hstack(const Dune::FieldVector<KL, l>& left, const KR& right)
{
  typename internal::hstack_helper<internal::hstack_decay_t<Dune::FieldVector<KL, l>>,
                                   internal::hstack_decay_t<KR>>::type ret;
  for (size_t ii = 0; ii < l; ++ii)
    ret[ii] = left[ii];
  ret[l] = right;
  return ret;
} // ... hstack<l, 1>(...)

/// \brief Specialization of \sa hstack for two arguments.
template <class KL, int l, class KR>
typename std::enable_if<is_arithmetic<KR>::value && !is_field_vector<KR>::value,
                        typename internal::hstack_helper<internal::hstack_decay_t<Dune::FieldVector<KL, l>>,
                                                         internal::hstack_decay_t<KR>>::type>::type
hstack(const Dune::XT::Common::FieldVector<KL, l>& left, const KR& right)
{
  return hstack(static_cast<const Dune::FieldVector<KL, l>&>(left), right);
}

/// \brief Specialization of \sa hstack for two arguments.
template <class KL, int l, class KR, int r>
typename internal::hstack_helper<internal::hstack_decay_t<Dune::FieldVector<KL, l>>,
                                 internal::hstack_decay_t<Dune::FieldVector<KR, r>>>::type
hstack(const Dune::FieldVector<KL, l>& left, const Dune::FieldVector<KR, r>& right)
{
  typename internal::hstack_helper<internal::hstack_decay_t<Dune::FieldVector<KL, l>>,
                                   internal::hstack_decay_t<Dune::FieldVector<KR, r>>>::type ret;
  size_t ii = 0;
  for (size_t jj = 0; jj < l; ++jj, ++ii)
    ret[ii] = left[jj];
  for (size_t jj = 0; jj < r; ++jj, ++ii)
    ret[ii] = right[jj];
  return ret;
} // ... hstack<l, r>(...)

/// \brief Specialization of \sa hstack for two arguments.
template <class KL, int l, class KR, int r>
typename internal::hstack_helper<internal::hstack_decay_t<Dune::FieldVector<KL, l>>,
                                 internal::hstack_decay_t<Dune::FieldVector<KR, r>>>::type
hstack(const Dune::XT::Common::FieldVector<KL, l>& left, const Dune::FieldVector<KR, r>& right)
{
  return hstack(static_cast<const Dune::FieldVector<KL, l>&>(left), right);
}

/// \brief Specialization of \sa hstack for two arguments.
template <class KL, int l, class KR, int r>
typename internal::hstack_helper<internal::hstack_decay_t<Dune::FieldVector<KL, l>>,
                                 internal::hstack_decay_t<Dune::FieldVector<KR, r>>>::type
hstack(const Dune::FieldVector<KL, l>& left, const Dune::XT::Common::FieldVector<KR, r>& right)
{
  return hstack(left, static_cast<const Dune::FieldVector<KR, r>&>(right));
}

/// \brief Specialization of \sa hstack for two arguments.
template <class KL, int l, class KR, int r>
typename internal::hstack_helper<internal::hstack_decay_t<Dune::FieldVector<KL, l>>,
                                 internal::hstack_decay_t<Dune::FieldVector<KR, r>>>::type
hstack(const Dune::XT::Common::FieldVector<KL, l>& left, const Dune::XT::Common::FieldVector<KR, r>& right)
{
  return hstack(static_cast<const Dune::FieldVector<KL, l>&>(left),
                static_cast<const Dune::FieldVector<KR, r>&>(right));
}

/**
 * \brief Stacks an arbitrary number of numbers and FieldVectors horizontally into one appropriate FieldVector.
 * \note  At least one argument must be a FieldVector!
 */
template <class L, class R, class... Vectors>
typename std::enable_if<
    is_field_vector<L>::value || is_field_vector<R>::value,
    typename internal::hstack_helper<internal::hstack_decay_t<L>, internal::hstack_decay_t<R>, Vectors...>::type>::type
hstack(const L& left, const R& right, Vectors&&... vectors)
{
  return hstack(hstack(left, right), std::forward<Vectors>(vectors)...);
}

} // namespace XT::Common

template <class K, int SIZE>
typename std::enable_if<SIZE != 1, K>::type operator*(const Dune::FieldVector<K, SIZE>& vec,
                                                      const Dune::FieldMatrix<K, SIZE, 1>& mat)
{
  K ret = 0;
  for (size_t ii = 0; ii < SIZE; ++ii)
    ret += vec[ii] * mat[ii][0];
  return ret;
}

template <class K, int SIZE>
struct FieldTraits<XT::Common::FieldVector<K, SIZE>>
{
  using field_type = typename FieldTraits<K>::field_type;
  using real_type = typename FieldTraits<K>::real_type;
};

} // namespace Dune
namespace std {

/**
 * \sa http://en.cppreference.com/w/cpp/utility/hash
 * \sa http://www.boost.org/doc/libs/1_55_0/doc/html/hash/combine.html
 */
template <class K, int SIZE>
struct hash<Dune::FieldVector<K, SIZE>>
{
  using argument_type = Dune::FieldVector<K, SIZE>;
  using result_type = std::size_t;

  result_type operator()(argument_type const& s) const noexcept
  {
    std::size_t seed = 0;
    for (size_t ii = 0; ii < s.size(); ++ii)
      boost::hash_combine(seed, s[ii]);
    return seed;
  }
}; // struct hash<Dune::FieldVector<...>>

/**
 * \sa http://en.cppreference.com/w/cpp/utility/hash
 * \sa http://www.boost.org/doc/libs/1_55_0/doc/html/hash/combine.html
 */
template <class K, int SIZE>
struct hash<Dune::XT::Common::FieldVector<K, SIZE>>
{
  using argument_type = Dune::XT::Common::FieldVector<K, SIZE>;
  using result_type = std::size_t;

  result_type operator()(argument_type const& s) const noexcept
  {
    std::size_t seed = 0;
    for (size_t ii = 0; ii < s.size(); ++ii)
      boost::hash_combine(seed, s[ii]);
    return seed;
  }
}; // struct hash<Dune::XT::Common::FieldVector<...>>

} // namespace std

#endif // DUNE_XT_COMMON_FVECTOR_26_HH


/* This file is part of libtrevisan, a modular implementation of
   Trevisan's randomness extraction construction.

   Copyright (C) 2011-2012, Wolfgang Mauerer <wm@linux-kernel.net>

   This program is free software; you can redistribute it and/or
   modify it under the terms of the GNU General Public License
   as published by the Free Software Foundation; either version 2
   of the License, or (at your option) any later version.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with libtrevisan. If not, see <http://www.gnu.org/licenses/>. */

// Generate a list of precomputed irreducible polynomials

#include <NTL/GF2X.h>
#include <NTL/GF2XFactoring.h>
#include <NTL/tools.h>
#include <cstring>
#include <string>
#include <iostream>
#include <sstream>
#include <algorithm>
#include <inttypes.h>

#include <stdlib.h>
#include <stdio.h>

#include "CumffaTypes.h"

NTL_CLIENT

unsigned int field_sizes[] = {8192, 16382};

void convertCharArrayToIntArray( unsigned char* s_value, ufixn* t_value, int num_chars ) {

  int offset = 0;

  if(num_chars == 0)
    return;

  int highest_block_idx = (num_chars-1)/8;
  double j=highest_block_idx+1-0.125;

  for(int i=num_chars-1; i>=0; --i) {
    t_value[(int)j] += ((ufixn)s_value[i] << offset);

    offset = (offset + 8) % 64;
    j-=0.125;
  }
}

void clearArray( ufixn* array, int size ) {
  for(int i=0; i<size; ++i) {
    array[i] = 0;
  }
}

void printbincharpad(unsigned char* ca, unsigned int n)
{
  for(int j=0; j<n; j++) {
    char c = ca[j];
      for (int i = 7; i >= 0; --i)
      {
          putchar( (c & (1 << i)) ? '1' : '0' );
      }
      putchar(' ');
  }
  putchar('\n');
}

void printbincharpad(int* ca, unsigned int n)
{
  for(int j=0; j<n; j++) {
    int c = ca[j];
      for (int i = 31; i >= 0; --i)
      {
          putchar( (c & (1 << i)) ? '1' : '0' );
      }
      putchar(' ');
  }
  putchar('\n');
}

void gen_irreps_ntl() {
  stringstream ss;
  cout << "// This file is auto-generated by gen_irreps.cc, don't modify" << endl;
  cout << "#include <NTL/GF2X.h>" << endl;
  cout << "#include <sstream>" << endl;
  cout << "#include <iostream>" << endl;
    cout << "#include <cstdlib>" << endl;
  cout << "NTL_CLIENT" << endl;
  cout << "void set_irrep(GF2X &P, unsigned n) {" << endl;
  cout << "stringstream ss;" << endl;
  cout << "switch (n) {" << endl;

  for( unsigned n = 0; n < sizeof(field_sizes)/sizeof(unsigned int); n++ )
  {
    GF2X P;
    BuildSparseIrred(P, field_sizes[n]);

    // This way of storage is extremely inefficient. But since
    // we're only talking about a few bytes on a big iron machine,
    // who cares...
    cout << "case " << field_sizes[n] << ":" << endl;
    cout << "ss << \"" << P  << "\";" << endl;
    cout << "ss >> P;" << endl;
    cout << "break;" << endl;
  }

  cout << "default:" << endl;
  cout << " cerr << \"Internal error: n out of bounds!\";" << endl;
  cout << " exit(-1);" << endl;
  cout << "}" << endl;
  cout << "}" << endl;
}

void gen_irreps_openssl() {
  stringstream ss;
  cout << "// This file is auto-generated by gen_irreps.cc, don't modify" << endl;
  cout << "#include <openssl/bn.h>" << endl;
  cout << "#include <iostream>" << endl;
    cout << "#include <cstdlib>" << endl;
    cout << "#include <vector>" << endl;
  cout << "using namespace std;" << endl;
  cout << "void set_irrep(vector<int> &p, unsigned n) {" << endl;
  cout << "switch (n) {" << endl;

  for( unsigned n = 0; n < sizeof(field_sizes)/sizeof(unsigned int); n++ )
  {
    GF2X P;
    BuildSparseIrred(P, field_sizes[n]);

    // Sure, there are much more efficient ways to detect the number
    // of set bits
    cout << "case " << field_sizes[n] << ": {" << endl;

    for (long i = deg(P); i >= 0; i--) {
      if (coeff(P, i) == 1)
        cout << "p.push_back("  << i << ");" << endl;
    }
    cout << "} break;" << endl;
  }

  cout << "default:" << endl;
  cout << " cerr << \"Internal error: n out of bounds!\";" << endl;
  cout << " exit(-1);" << endl;
  cout << "}" << endl;
  cout << "}" << endl;
}

void gen_irreps_cuda() {
  stringstream ss;
  cout << "// This file is auto-generated by gen_irreps.cc, don't modify" << endl;
  cout << "#include \"CumffaTypes.h\"" << endl;
  cout << "#include <iostream>" << endl;
    cout << "#include <cstdlib>" << endl;
    cout << "#include <vector>" << endl;
  cout << "using namespace std;" << endl;

  cout << "void set_irrep_cuda(vector<ufixn> &p, ufixn n) {" << endl;
  cout << "switch (n) {" << endl;

  for( unsigned n = 0; n < sizeof(field_sizes)/sizeof(unsigned int); n++ )
  {
    GF2X P;
    BuildSparseIrred(P, field_sizes[n]);

    // Sure, there are much more efficient ways to detect the number
    // of set bits
    cout << "case " << field_sizes[n] << ": {" << endl;

    long num_bytes = NumBytes(P);
    unsigned char bytes[num_bytes];
    int num_chunks = (num_bytes-1)/sizeof(ufixn)+1;
    ufixn bytes_as_ufixn[num_chunks];
    clearArray(bytes_as_ufixn, num_chunks);

    BytesFromGF2X(bytes, P, num_bytes);

    reverse(bytes, bytes+num_bytes);

    long offset = 0;
    long i = 0;

    convertCharArrayToIntArray(bytes, bytes_as_ufixn, num_bytes);

    for(int i=0; i<num_chunks; i++) {
      cout << "p.push_back("  << bytes_as_ufixn[i] << "ULL);" << endl;
    }

    cout << "} break;" << endl;
  }

  cout << "default:" << endl;
  cout << " cerr << \"Internal error: n out of bounds!\";" << endl;
  cout << " exit(-1);" << endl;
  cout << "}" << endl;
  cout << "}" << endl;
}

void gen_irreps_cuda_for_py() {
  stringstream ss;
  cout << "# This file is auto-generated by gen_irreps.cc, don't modify" << endl;
  cout << endl;

  cout << "def set_irrep( n ):" << endl;

  cout << endl;
  cout << "\tres = 0" << endl;
  cout << endl;

  for( unsigned n = 0; n < sizeof(field_sizes)/sizeof(unsigned int); n++ )
  {
    GF2X P;
    BuildSparseIrred(P, field_sizes[n]);

    // Sure, there are much more efficient ways to detect the number
    // of set bits
    if( field_sizes[n] == 1 )
      cout << "\tif n == " << field_sizes[n] << ":" << endl;
    else
      cout << "\telif n == " << field_sizes[n] << ":" << endl;

    long num_bytes = NumBytes(P);
    unsigned char bytes[num_bytes];

    BytesFromGF2X(bytes, P, num_bytes);

    reverse(bytes, bytes+num_bytes);

    long shift = 0;

    for( int i=num_bytes-1; i>=0; --i ) {
      cout << "\t\tres = res + (" << +bytes[i] << " << " << shift << ")" << endl;
      shift = shift + 8;
    }
  }

  cout << endl;
  cout << "\treturn res" << endl;
}

int main(int argc, char **argv) {
  if(strcmp(argv[1], "NTL") == 0) {
    gen_irreps_ntl();
  } else if(strcmp(argv[1], "CUDA") == 0) {
    gen_irreps_cuda();
  } else if(strcmp(argv[1], "PYTHON") == 0) {
    gen_irreps_cuda_for_py();
  } else {
    gen_irreps_openssl();
  }

  return 0;
}


/* ----------------------------------------------------------------------------
 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file dataset.cpp
 * @date Jan 22, 2010
 * @author Kai Ni, Luca Carlone, Frank Dellaert
 * @brief utility functions for loading datasets
 */

#include <gtsam/sam/BearingRangeFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/Values-inl.h>
#include <gtsam/linear/Sampler.h>
#include <gtsam/base/GenericValue.h>
#include <gtsam/base/Lie.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/types.h>
#include <gtsam/base/Value.h>
#include <gtsam/base/Vector.h>

#include <boost/assign/list_inserter.hpp>
#include <boost/filesystem/operations.hpp>
#include <boost/filesystem/path.hpp>

#include <cmath>
#include <fstream>
#include <iostream>
#include <stdexcept>

using namespace std;
namespace fs = boost::filesystem;
using namespace gtsam::symbol_shorthand;

#define LINESIZE 81920

namespace gtsam {

/* ************************************************************************* */
string findExampleDataFile(const string& name) {
  // Search source tree and installed location
  vector<string> rootsToSearch;

  // Constants below are defined by CMake, see gtsam/gtsam/CMakeLists.txt
  rootsToSearch.push_back(GTSAM_SOURCE_TREE_DATASET_DIR);
  rootsToSearch.push_back(GTSAM_INSTALLED_DATASET_DIR);

  // Search for filename as given, and with .graph and .txt extensions
  vector<string> namesToSearch;
  namesToSearch.push_back(name);
  namesToSearch.push_back(name + ".graph");
  namesToSearch.push_back(name + ".txt");
  namesToSearch.push_back(name + ".out");
  namesToSearch.push_back(name + ".xml");

  // Find first name that exists
  for(const fs::path& root: rootsToSearch) {
    for(const fs::path& name: namesToSearch) {
      if (fs::is_regular_file(root / name))
        return (root / name).string();
    }
  }

  // If we did not return already, then we did not find the file
  throw invalid_argument(
      "gtsam::findExampleDataFile could not find a matching file in\n"
      GTSAM_SOURCE_TREE_DATASET_DIR " or\n"
      GTSAM_INSTALLED_DATASET_DIR " named\n" + name + ", " + name
          + ".graph, or " + name + ".txt");
}

/* ************************************************************************* */
string createRewrittenFileName(const string& name) {
  // Search source tree and installed location
  if (!exists(fs::path(name))) {
    throw invalid_argument(
        "gtsam::createRewrittenFileName could not find a matching file in\n"
            + name);
  }

  fs::path p(name);
  fs::path newpath = fs::path(p.parent_path().string())
      / fs::path(p.stem().string() + "-rewritten.txt");

  return newpath.string();
}

/* ************************************************************************* */
GraphAndValues load2D(pair<string, SharedNoiseModel> dataset, int maxID,
    bool addNoise, bool smart, NoiseFormat noiseFormat,
    KernelFunctionType kernelFunctionType) {
  return load2D(dataset.first, dataset.second, maxID, addNoise, smart,
      noiseFormat, kernelFunctionType);
}

/* ************************************************************************* */
// Read noise parameters and interpret them according to flags
static SharedNoiseModel readNoiseModel(ifstream& is, bool smart,
    NoiseFormat noiseFormat, KernelFunctionType kernelFunctionType) {
  double v1, v2, v3, v4, v5, v6;
  is >> v1 >> v2 >> v3 >> v4 >> v5 >> v6;

  if (noiseFormat == NoiseFormatAUTO) {
    // Try to guess covariance matrix layout
    if (v1 != 0.0 && v2 == 0.0 && v3 != 0.0 && v4 != 0.0 && v5 == 0.0
        && v6 == 0.0) {
      // NoiseFormatGRAPH
      noiseFormat = NoiseFormatGRAPH;
    } else if (v1 != 0.0 && v2 == 0.0 && v3 == 0.0 && v4 != 0.0 && v5 == 0.0
        && v6 != 0.0) {
      // NoiseFormatCOV
      noiseFormat = NoiseFormatCOV;
    } else {
      throw std::invalid_argument(
          "load2D: unrecognized covariance matrix format in dataset file. Please specify the noise format.");
    }
  }

  // Read matrix and check that diagonal entries are non-zero
  Matrix M(3, 3);
  switch (noiseFormat) {
  case NoiseFormatG2O:
  case NoiseFormatCOV:
    // i.e., [ v1 v2 v3; v2' v4 v5; v3' v5' v6 ]
    if (v1 == 0.0 || v4 == 0.0 || v6 == 0.0)
      throw runtime_error(
          "load2D::readNoiseModel looks like this is not G2O matrix order");
    M << v1, v2, v3, v2, v4, v5, v3, v5, v6;
    break;
  case NoiseFormatTORO:
  case NoiseFormatGRAPH:
    // http://www.openslam.org/toro.html
    // inf_ff inf_fs inf_ss inf_rr inf_fr inf_sr
    // i.e., [ v1 v2 v5; v2' v3 v6; v5' v6' v4 ]
    if (v1 == 0.0 || v3 == 0.0 || v4 == 0.0)
      throw invalid_argument(
          "load2D::readNoiseModel looks like this is not TORO matrix order");
    M << v1, v2, v5, v2, v3, v6, v5, v6, v4;
    break;
  default:
    throw runtime_error("load2D: invalid noise format");
  }

  // Now, create a Gaussian noise model
  // The smart flag will try to detect a simpler model, e.g., unit
  SharedNoiseModel model;
  switch (noiseFormat) {
  case NoiseFormatG2O:
  case NoiseFormatTORO:
    // In both cases, what is stored in file is the information matrix
    model = noiseModel::Gaussian::Information(M, smart);
    break;
  case NoiseFormatGRAPH:
  case NoiseFormatCOV:
    // These cases expect covariance matrix
    model = noiseModel::Gaussian::Covariance(M, smart);
    break;
  default:
    throw invalid_argument("load2D: invalid noise format");
  }

  switch (kernelFunctionType) {
  case KernelFunctionTypeNONE:
    return model;
    break;
  case KernelFunctionTypeHUBER:
    return noiseModel::Robust::Create(
        noiseModel::mEstimator::Huber::Create(1.345), model);
    break;
  case KernelFunctionTypeTUKEY:
    return noiseModel::Robust::Create(
        noiseModel::mEstimator::Tukey::Create(4.6851), model);
    break;
  default:
    throw invalid_argument("load2D: invalid kernel function type");
  }
}

/* ************************************************************************* */
boost::optional<IndexedPose> parseVertex(istream& is, const string& tag) {
  if ((tag == "VERTEX2") || (tag == "VERTEX_SE2") || (tag == "VERTEX")) {
    Key id;
    double x, y, yaw;
    is >> id >> x >> y >> yaw;
    return IndexedPose(id, Pose2(x, y, yaw));
  } else {
    return boost::none;
  }
}

/* ************************************************************************* */
boost::optional<IndexedEdge> parseEdge(istream& is, const string& tag) {
  if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "EDGE_SE2")
      || (tag == "ODOMETRY")) {

    Key id1, id2;
    double x, y, yaw;
    is >> id1 >> id2 >> x >> y >> yaw;
    return IndexedEdge(pair<Key, Key>(id1, id2), Pose2(x, y, yaw));
  } else {
    return boost::none;
  }
}

/* ************************************************************************* */
GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID,
    bool addNoise, bool smart, NoiseFormat noiseFormat,
    KernelFunctionType kernelFunctionType) {

  ifstream is(filename.c_str());
  if (!is)
    throw invalid_argument("load2D: can not find file " + filename);

  Values::shared_ptr initial(new Values);
  NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph);

  string tag;

  // load the poses
  while (!is.eof()) {
    if (!(is >> tag))
      break;

    const auto indexed_pose = parseVertex(is, tag);
    if (indexed_pose) {
      Key id = indexed_pose->first;

      // optional filter
      if (maxID && id >= maxID)
        continue;

      initial->insert(id, indexed_pose->second);
    }
    is.ignore(LINESIZE, '\n');
  }
  is.clear(); /* clears the end-of-file and error flags */
  is.seekg(0, ios::beg);

  // If asked, create a sampler with random number generator
  Sampler sampler;
  if (addNoise) {
    noiseModel::Diagonal::shared_ptr noise;
    if (model)
      noise = std::dynamic_pointer_cast<noiseModel::Diagonal>(model);
    if (!noise)
      throw invalid_argument(
          "gtsam::load2D: invalid noise model for adding noise"
              "(current version assumes diagonal noise model)!");
    sampler = Sampler(noise);
  }

  // Parse the pose constraints
  Key id1, id2;
  bool haveLandmark = false;
  const bool useModelInFile = !model;
  while (!is.eof()) {
    if (!(is >> tag))
      break;

    auto between_pose = parseEdge(is, tag);
    if (between_pose) {
      std::tie(id1, id2) = between_pose->first;
      Pose2& l1Xl2 = between_pose->second;

      // read noise model
      SharedNoiseModel modelInFile = readNoiseModel(is, smart, noiseFormat,
          kernelFunctionType);

      // optional filter
      if (maxID && (id1 >= maxID || id2 >= maxID))
        continue;

      if (useModelInFile)
        model = modelInFile;

      if (addNoise)
        l1Xl2 = l1Xl2.retract(sampler.sample());

      // Insert vertices if pure odometry file
      if (!initial->exists(id1))
        initial->insert(id1, Pose2());
      if (!initial->exists(id2))
        initial->insert(id2, initial->at<Pose2>(id1) * l1Xl2);

      NonlinearFactor::shared_ptr factor(
          new BetweenFactor<Pose2>(id1, id2, l1Xl2, model));
      graph->push_back(factor);
    }
    // Parse measurements
    double bearing, range, bearing_std, range_std;

    // A bearing-range measurement
    if (tag == "BR") {
      is >> id1 >> id2 >> bearing >> range >> bearing_std >> range_std;
    }

    // A landmark measurement, TODO Frank says: don't know why is converted to bearing-range
    if (tag == "LANDMARK") {
      double lmx, lmy;
      double v1, v2, v3;

      is >> id1 >> id2 >> lmx >> lmy >> v1 >> v2 >> v3;

      // Convert x,y to bearing,range
      bearing = atan2(lmy, lmx);
      range = sqrt(lmx * lmx + lmy * lmy);

      // In our experience, the x-y covariance on landmark sightings is not very good, so assume
      // it describes the uncertainty at a range of 10m, and convert that to bearing/range uncertainty.
      if (std::abs(v1 - v3) < 1e-4) {
        bearing_std = sqrt(v1 / 10.0);
        range_std = sqrt(v1);
      } else {
        bearing_std = 1;
        range_std = 1;
        if (!haveLandmark) {
          cout
              << "Warning: load2D is a very simple dataset loader and is ignoring the\n"
                  "non-uniform covariance on LANDMARK measurements in this file."
              << endl;
          haveLandmark = true;
        }
      }
    }

    // Do some common stuff for bearing-range measurements
    if (tag == "LANDMARK" || tag == "BR") {

      // optional filter
      if (maxID && id1 >= maxID)
        continue;

      // Create noise model
      noiseModel::Diagonal::shared_ptr measurementNoise =
          noiseModel::Diagonal::Sigmas((Vector(2) << bearing_std, range_std).finished());

      // Add to graph
      *graph += BearingRangeFactor<Pose2, Point2>(id1, L(id2), bearing, range,
          measurementNoise);

      // Insert poses or points if they do not exist yet
      if (!initial->exists(id1))
        initial->insert(id1, Pose2());
      if (!initial->exists(L(id2))) {
        Pose2 pose = initial->at<Pose2>(id1);
        Point2 local(cos(bearing) * range, sin(bearing) * range);
        Point2 global = pose.transformFrom(local);
        initial->insert(L(id2), global);
      }
    }
    is.ignore(LINESIZE, '\n');
  }

  return make_pair(graph, initial);
}

/* ************************************************************************* */
GraphAndValues load2D_robust(const string& filename,
    const noiseModel::Base::shared_ptr& model, int maxID) {
  return load2D(filename, model, maxID);
}

/* ************************************************************************* */
void save2D(const NonlinearFactorGraph& graph, const Values& config,
    const noiseModel::Diagonal::shared_ptr model, const string& filename) {

  fstream stream(filename.c_str(), fstream::out);

  // save poses

  for(const Values::ConstKeyValuePair& key_value: config) {
    const Pose2& pose = key_value.value.cast<Pose2>();
    stream << "VERTEX2 " << key_value.key << " " << pose.x() << " " << pose.y()
        << " " << pose.theta() << endl;
  }

  // save edges
  Matrix R = model->R();
  Matrix RR = trans(R) * R; //prod(trans(R),R);
  for(std::shared_ptr<NonlinearFactor> factor_: graph) {
    std::shared_ptr<BetweenFactor<Pose2> > factor =
        std::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor_);
    if (!factor)
      continue;

    Pose2 pose = factor->measured().inverse();
    stream << "EDGE2 " << factor->key2() << " " << factor->key1() << " "
        << pose.x() << " " << pose.y() << " " << pose.theta() << " " << RR(0, 0)
        << " " << RR(0, 1) << " " << RR(1, 1) << " " << RR(2, 2) << " "
        << RR(0, 2) << " " << RR(1, 2) << endl;
  }

  stream.close();
}

/* ************************************************************************* */
GraphAndValues readG2o(const string& g2oFile, const bool is3D,
                       KernelFunctionType kernelFunctionType) {
  if (is3D) {
    return load3D(g2oFile);
  } else {
    // just call load2D
    int maxID = 0;
    bool addNoise = false;
    bool smart = true;
    return load2D(g2oFile, SharedNoiseModel(), maxID, addNoise, smart,
                  NoiseFormatG2O, kernelFunctionType);
  }
}

/* ************************************************************************* */
void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
    const string& filename) {
  fstream stream(filename.c_str(), fstream::out);

  // save 2D & 3D poses
  for (const auto& key_value : estimate) {
    auto p = dynamic_cast<const GenericValue<Pose2>*>(&key_value.value);
    if (!p) continue;
    const Pose2& pose = p->value();
    stream << "VERTEX_SE2 " << key_value.key << " " << pose.x() << " "
        << pose.y() << " " << pose.theta() << endl;
  }

  for(const auto& key_value: estimate) {
      auto p = dynamic_cast<const GenericValue<Pose3>*>(&key_value.value);
      if (!p) continue;
      const Pose3& pose = p->value();
      Point3 t = pose.translation();
      Rot3 R = pose.rotation();
      stream << "VERTEX_SE3:QUAT " << key_value.key << " " << t.x() << " "  << t.y() << " " << t.z()
        << " " << R.toQuaternion().x() << " " << R.toQuaternion().y() << " " << R.toQuaternion().z()
        << " " << R.toQuaternion().w() << endl;
  }

  // save edges (2D or 3D)
  for(const auto& factor_: graph) {
    std::shared_ptr<BetweenFactor<Pose2> > factor =
        std::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor_);
    if (factor){
      SharedNoiseModel model = factor->noiseModel();
      std::shared_ptr<noiseModel::Gaussian> gaussianModel =
          std::dynamic_pointer_cast<noiseModel::Gaussian>(model);
      if (!gaussianModel){
        model->print("model\n");
        throw invalid_argument("writeG2o: invalid noise model!");
      }
      Matrix Info = gaussianModel->R().transpose() * gaussianModel->R();
      Pose2 pose = factor->measured(); //.inverse();
      stream << "EDGE_SE2 " << factor->key1() << " " << factor->key2() << " "
          << pose.x() << " " << pose.y() << " " << pose.theta();
      for (int i = 0; i < 3; i++){
        for (int j = i; j < 3; j++){
          stream << " " << Info(i, j);
        }
      }
      stream << endl;
    }

    std::shared_ptr< BetweenFactor<Pose3> > factor3D =
        std::dynamic_pointer_cast< BetweenFactor<Pose3> >(factor_);

    if (factor3D){
      SharedNoiseModel model = factor3D->noiseModel();

      std::shared_ptr<noiseModel::Gaussian> gaussianModel =
          std::dynamic_pointer_cast<noiseModel::Gaussian>(model);
      if (!gaussianModel){
        model->print("model\n");
        throw invalid_argument("writeG2o: invalid noise model!");
      }
      Matrix Info = gaussianModel->R().transpose() * gaussianModel->R();
      Pose3 pose3D = factor3D->measured();
      Point3 p = pose3D.translation();
      Rot3 R = pose3D.rotation();

      stream << "EDGE_SE3:QUAT " << factor3D->key1() << " " << factor3D->key2() << " "
          << p.x() << " "  << p.y() << " " << p.z()  << " " << R.toQuaternion().x()
          << " " << R.toQuaternion().y() << " " << R.toQuaternion().z()  << " " << R.toQuaternion().w();

      Matrix InfoG2o = I_6x6;
      InfoG2o.block(0,0,3,3) = Info.block(3,3,3,3); // cov translation
      InfoG2o.block(3,3,3,3) = Info.block(0,0,3,3); // cov rotation
      InfoG2o.block(0,3,3,3) = Info.block(0,3,3,3); // off diagonal
      InfoG2o.block(3,0,3,3) = Info.block(3,0,3,3); // off diagonal

      for (int i = 0; i < 6; i++){
        for (int j = i; j < 6; j++){
          stream << " " << InfoG2o(i, j);
        }
      }
      stream << endl;
    }
  }
  stream.close();
}

/* ************************************************************************* */
std::map<Key, Pose3> parse3DPoses(const string& filename) {
  ifstream is(filename.c_str());
  if (!is)
    throw invalid_argument("parse3DPoses: can not find file " + filename);

  std::map<Key, Pose3> poses;
  while (!is.eof()) {
    char buf[LINESIZE];
    is.getline(buf, LINESIZE);
    istringstream ls(buf);
    string tag;
    ls >> tag;

    if (tag == "VERTEX3") {
      Key id;
      double x, y, z, roll, pitch, yaw;
      ls >> id >> x >> y >> z >> roll >> pitch >> yaw;
      poses.emplace(id, Pose3(Rot3::Ypr(yaw, pitch, roll), {x, y, z}));
    }
    if (tag == "VERTEX_SE3:QUAT") {
      Key id;
      double x, y, z, qx, qy, qz, qw;
      ls >> id >> x >> y >> z >> qx >> qy >> qz >> qw;
      poses.emplace(id, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z}));
    }
  }
  return poses;
}

/* ************************************************************************* */
BetweenFactorPose3s parse3DFactors(const string& filename) {
  ifstream is(filename.c_str());
  if (!is) throw invalid_argument("parse3DFactors: can not find file " + filename);

  std::vector<BetweenFactor<Pose3>::shared_ptr> factors;
  while (!is.eof()) {
    char buf[LINESIZE];
    is.getline(buf, LINESIZE);
    istringstream ls(buf);
    string tag;
    ls >> tag;

    if (tag == "EDGE3") {
      Key id1, id2;
      double x, y, z, roll, pitch, yaw;
      ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw;
      Matrix m(6, 6);
      for (size_t i = 0; i < 6; i++)
        for (size_t j = i; j < 6; j++) ls >> m(i, j);
      SharedNoiseModel model = noiseModel::Gaussian::Information(m);
      factors.emplace_back(new BetweenFactor<Pose3>(
          id1, id2, Pose3(Rot3::Ypr(yaw, pitch, roll), {x, y, z}), model));
    }
    if (tag == "EDGE_SE3:QUAT") {
      Key id1, id2;
      double x, y, z, qx, qy, qz, qw;
      ls >> id1 >> id2 >> x >> y >> z >> qx >> qy >> qz >> qw;
      Matrix m(6, 6);
      for (size_t i = 0; i < 6; i++) {
        for (size_t j = i; j < 6; j++) {
          double mij;
          ls >> mij;
          m(i, j) = mij;
          m(j, i) = mij;
        }
      }
      Matrix mgtsam(6, 6);

      mgtsam.block<3, 3>(0, 0) = m.block<3, 3>(3, 3);  // cov rotation
      mgtsam.block<3, 3>(3, 3) = m.block<3, 3>(0, 0);  // cov translation
      mgtsam.block<3, 3>(0, 3) = m.block<3, 3>(0, 3);  // off diagonal
      mgtsam.block<3, 3>(3, 0) = m.block<3, 3>(3, 0);  // off diagonal

      SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam);
      factors.emplace_back(new BetweenFactor<Pose3>(
          id1, id2, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z}), model));
    }
  }
  return factors;
}

/* ************************************************************************* */
GraphAndValues load3D(const string& filename) {
  const auto factors = parse3DFactors(filename);
  NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph);
  for (const auto& factor : factors) {
    graph->push_back(factor);
  }

  const auto poses = parse3DPoses(filename);
  Values::shared_ptr initial(new Values);
  for (const auto& key_pose : poses) {
    initial->insert(key_pose.first, key_pose.second);
  }

  return make_pair(graph, initial);
}

/* ************************************************************************* */
Rot3 openGLFixedRotation() { // this is due to different convention for cameras in gtsam and openGL
  /* R = [ 1   0   0
   *       0  -1   0
   *       0   0  -1]
   */
  Matrix3 R_mat = Matrix3::Zero(3, 3);
  R_mat(0, 0) = 1.0;
  R_mat(1, 1) = -1.0;
  R_mat(2, 2) = -1.0;
  return Rot3(R_mat);
}

/* ************************************************************************* */
Pose3 openGL2gtsam(const Rot3& R, double tx, double ty, double tz) {
  Rot3 R90 = openGLFixedRotation();
  Rot3 wRc = (R.inverse()).compose(R90);

  // Our camera-to-world translation wTc = -R'*t
  return Pose3(wRc, R.unrotate(Point3(-tx, -ty, -tz)));
}

/* ************************************************************************* */
Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz) {
  Rot3 R90 = openGLFixedRotation();
  Rot3 cRw_openGL = R90.compose(R.inverse());
  Point3 t_openGL = cRw_openGL.rotate(Point3(-tx, -ty, -tz));
  return Pose3(cRw_openGL, t_openGL);
}

/* ************************************************************************* */
Pose3 gtsam2openGL(const Pose3& PoseGTSAM) {
  return gtsam2openGL(PoseGTSAM.rotation(), PoseGTSAM.x(), PoseGTSAM.y(),
      PoseGTSAM.z());
}

/* ************************************************************************* */
bool readBundler(const string& filename, SfM_data &data) {
  // Load the data file
  ifstream is(filename.c_str(), ifstream::in);
  if (!is) {
    cout << "Error in readBundler: can not find the file!!" << endl;
    return false;
  }

  // Ignore the first line
  char aux[500];
  is.getline(aux, 500);

  // Get the number of camera poses and 3D points
  size_t nrPoses, nrPoints;
  is >> nrPoses >> nrPoints;

  // Get the information for the camera poses
  for (size_t i = 0; i < nrPoses; i++) {
    // Get the focal length and the radial distortion parameters
    float f, k1, k2;
    is >> f >> k1 >> k2;
    Cal3Bundler K(f, k1, k2);

    // Get the rotation matrix
    float r11, r12, r13;
    float r21, r22, r23;
    float r31, r32, r33;
    is >> r11 >> r12 >> r13 >> r21 >> r22 >> r23 >> r31 >> r32 >> r33;

    // Bundler-OpenGL rotation matrix
    Rot3 R(r11, r12, r13, r21, r22, r23, r31, r32, r33);

    // Check for all-zero R, in which case quit
    if (r11 == 0 && r12 == 0 && r13 == 0) {
      cout << "Error in readBundler: zero rotation matrix for pose " << i
          << endl;
      return false;
    }

    // Get the translation vector
    float tx, ty, tz;
    is >> tx >> ty >> tz;

    Pose3 pose = openGL2gtsam(R, tx, ty, tz);

    data.cameras.emplace_back(pose, K);
  }

  // Get the information for the 3D points
  data.tracks.reserve(nrPoints);
  for (size_t j = 0; j < nrPoints; j++) {
    SfM_Track track;

    // Get the 3D position
    float x, y, z;
    is >> x >> y >> z;
    track.p = Point3(x, y, z);

    // Get the color information
    float r, g, b;
    is >> r >> g >> b;
    track.r = r / 255.f;
    track.g = g / 255.f;
    track.b = b / 255.f;

    // Now get the visibility information
    size_t nvisible = 0;
    is >> nvisible;

    track.measurements.reserve(nvisible);
    track.siftIndices.reserve(nvisible);
    for (size_t k = 0; k < nvisible; k++) {
      size_t cam_idx = 0, point_idx = 0;
      float u, v;
      is >> cam_idx >> point_idx >> u >> v;
      track.measurements.emplace_back(cam_idx, Point2(u, -v));
      track.siftIndices.emplace_back(cam_idx, point_idx);
    }

    data.tracks.push_back(track);
  }

  is.close();
  return true;
}

/* ************************************************************************* */
bool readBAL(const string& filename, SfM_data &data) {
  // Load the data file
  ifstream is(filename.c_str(), ifstream::in);
  if (!is) {
    cout << "Error in readBAL: can not find the file!!" << endl;
    return false;
  }

  // Get the number of camera poses and 3D points
  size_t nrPoses, nrPoints, nrObservations;
  is >> nrPoses >> nrPoints >> nrObservations;

  data.tracks.resize(nrPoints);

  // Get the information for the observations
  for (size_t k = 0; k < nrObservations; k++) {
    size_t i = 0, j = 0;
    float u, v;
    is >> i >> j >> u >> v;
    data.tracks[j].measurements.emplace_back(i, Point2(u, -v));
  }

  // Get the information for the camera poses
  for (size_t i = 0; i < nrPoses; i++) {
    // Get the Rodrigues vector
    float wx, wy, wz;
    is >> wx >> wy >> wz;
    Rot3 R = Rot3::Rodrigues(wx, wy, wz); // BAL-OpenGL rotation matrix

    // Get the translation vector
    float tx, ty, tz;
    is >> tx >> ty >> tz;

    Pose3 pose = openGL2gtsam(R, tx, ty, tz);

    // Get the focal length and the radial distortion parameters
    float f, k1, k2;
    is >> f >> k1 >> k2;
    Cal3Bundler K(f, k1, k2);

    data.cameras.emplace_back(pose, K);
  }

  // Get the information for the 3D points
  for (size_t j = 0; j < nrPoints; j++) {
    // Get the 3D position
    float x, y, z;
    is >> x >> y >> z;
    SfM_Track& track = data.tracks[j];
    track.p = Point3(x, y, z);
    track.r = 0.4f;
    track.g = 0.4f;
    track.b = 0.4f;
  }

  is.close();
  return true;
}

/* ************************************************************************* */
bool writeBAL(const string& filename, SfM_data &data) {
  // Open the output file
  ofstream os;
  os.open(filename.c_str());
  os.precision(20);
  if (!os.is_open()) {
    cout << "Error in writeBAL: can not open the file!!" << endl;
    return false;
  }

  // Write the number of camera poses and 3D points
  size_t nrObservations = 0;
  for (size_t j = 0; j < data.number_tracks(); j++) {
    nrObservations += data.tracks[j].number_measurements();
  }

  // Write observations
  os << data.number_cameras() << " " << data.number_tracks() << " "
      << nrObservations << endl;
  os << endl;

  for (size_t j = 0; j < data.number_tracks(); j++) { // for each 3D point j
    const SfM_Track& track = data.tracks[j];

    for (size_t k = 0; k < track.number_measurements(); k++) { // for each observation of the 3D point j
      size_t i = track.measurements[k].first; // camera id
      double u0 = data.cameras[i].calibration().u0();
      double v0 = data.cameras[i].calibration().v0();

      if (u0 != 0 || v0 != 0) {
        cout
            << "writeBAL has not been tested for calibration with nonzero (u0,v0)"
            << endl;
      }

      double pixelBALx = track.measurements[k].second.x() - u0; // center of image is the origin
      double pixelBALy = -(track.measurements[k].second.y() - v0); // center of image is the origin
      Point2 pixelMeasurement(pixelBALx, pixelBALy);
      os << i /*camera id*/<< " " << j /*point id*/<< " "
          << pixelMeasurement.x() /*u of the pixel*/<< " "
          << pixelMeasurement.y() /*v of the pixel*/<< endl;
    }
  }
  os << endl;

  // Write cameras
  for (size_t i = 0; i < data.number_cameras(); i++) { // for each camera
    Pose3 poseGTSAM = data.cameras[i].pose();
    Cal3Bundler cameraCalibration = data.cameras[i].calibration();
    Pose3 poseOpenGL = gtsam2openGL(poseGTSAM);
    os << Rot3::Logmap(poseOpenGL.rotation()) << endl;
    os << poseOpenGL.translation().x() << endl;
    os << poseOpenGL.translation().y() << endl;
    os << poseOpenGL.translation().z() << endl;
    os << cameraCalibration.fx() << endl;
    os << cameraCalibration.k1() << endl;
    os << cameraCalibration.k2() << endl;
    os << endl;
  }

  // Write the points
  for (size_t j = 0; j < data.number_tracks(); j++) { // for each 3D point j
    Point3 point = data.tracks[j].p;
    os << point.x() << endl;
    os << point.y() << endl;
    os << point.z() << endl;
    os << endl;
  }

  os.close();
  return true;
}

bool writeBALfromValues(const string& filename, const SfM_data &data,
    Values& values) {
  using Camera = PinholeCamera<Cal3Bundler>;
  SfM_data dataValues = data;

  // Store poses or cameras in SfM_data
  size_t nrPoses = values.count<Pose3>();
  if (nrPoses == dataValues.number_cameras()) { // we only estimated camera poses
    for (size_t i = 0; i < dataValues.number_cameras(); i++) { // for each camera
      Key poseKey = symbol('x', i);
      Pose3 pose = values.at<Pose3>(poseKey);
      Cal3Bundler K = dataValues.cameras[i].calibration();
      Camera camera(pose, K);
      dataValues.cameras[i] = camera;
    }
  } else {
    size_t nrCameras = values.count<Camera>();
    if (nrCameras == dataValues.number_cameras()) { // we only estimated camera poses and calibration
      for (size_t i = 0; i < nrCameras; i++) { // for each camera
        Key cameraKey = i; // symbol('c',i);
        Camera camera = values.at<Camera>(cameraKey);
        dataValues.cameras[i] = camera;
      }
    } else {
      cout
          << "writeBALfromValues: different number of cameras in SfM_dataValues (#cameras "
          << dataValues.number_cameras() << ") and values (#cameras "
          << nrPoses << ", #poses " << nrCameras << ")!!"
          << endl;
      return false;
    }
  }

  // Store 3D points in SfM_data
  size_t nrPoints = values.count<Point3>(), nrTracks = dataValues.number_tracks();
  if (nrPoints != nrTracks) {
    cout
        << "writeBALfromValues: different number of points in SfM_dataValues (#points= "
        << nrTracks << ") and values (#points "
        << nrPoints << ")!!" << endl;
  }

  for (size_t j = 0; j < nrTracks; j++) { // for each point
    Key pointKey = P(j);
    if (values.exists(pointKey)) {
      Point3 point = values.at<Point3>(pointKey);
      dataValues.tracks[j].p = point;
    } else {
      dataValues.tracks[j].r = 1.0;
      dataValues.tracks[j].g = 0.0;
      dataValues.tracks[j].b = 0.0;
      dataValues.tracks[j].p = Point3(0,0,0);
    }
  }

  // Write SfM_data to file
  return writeBAL(filename, dataValues);
}

Values initialCamerasEstimate(const SfM_data& db) {
  Values initial;
  size_t i = 0; // NO POINTS:  j = 0;
  for(const SfM_Camera& camera: db.cameras)
    initial.insert(i++, camera);
  return initial;
}

Values initialCamerasAndPointsEstimate(const SfM_data& db) {
  Values initial;
  size_t i = 0, j = 0;
  for(const SfM_Camera& camera: db.cameras)
    initial.insert((i++), camera);
  for(const SfM_Track& track: db.tracks)
    initial.insert(P(j++), track.p);
  return initial;
}

} // \namespace gtsam


// Boost.Geometry - gis-projections (based on PROJ4)

// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2017, 2018.
// Modifications copyright (c) 2017-2018, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle.

// Use, modification and distribution is 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)

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 5.0.0

// Original copyright notice:

// 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 BOOST_GEOMETRY_PROJECTIONS_TMERC_HPP
#define BOOST_GEOMETRY_PROJECTIONS_TMERC_HPP

#include <boost/geometry/util/math.hpp>

#include <boost/geometry/srs/projections/impl/base_static.hpp>
#include <boost/geometry/srs/projections/impl/base_dynamic.hpp>
#include <boost/geometry/srs/projections/impl/projects.hpp>
#include <boost/geometry/srs/projections/impl/factory_entry.hpp>
#include <boost/geometry/srs/projections/impl/function_overloads.hpp>
#include <boost/geometry/srs/projections/impl/pj_mlfn.hpp>

namespace boost { namespace geometry
{

namespace srs { namespace par4
{
    struct tmerc {}; // Transverse Mercator

}} //namespace srs::par4

namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace tmerc
    {

            static const double epsilon10 = 1.e-10;

            template <typename T>
            inline T FC1() { return 1.; }
            template <typename T>
            inline T FC2() { return .5; }
            template <typename T>
            inline T FC3() { return .16666666666666666666666666666666666666; }
            template <typename T>
            inline T FC4() { return .08333333333333333333333333333333333333; }
            template <typename T>
            inline T FC5() { return .05; }
            template <typename T>
            inline T FC6() { return .03333333333333333333333333333333333333; }
            template <typename T>
            inline T FC7() { return .02380952380952380952380952380952380952; }
            template <typename T>
            inline T FC8() { return .01785714285714285714285714285714285714; }

            template <typename T>
            struct par_tmerc
            {
                T    esp;
                T    ml0;
                detail::en<T> en;
            };

            // template class, using CRTP to implement forward/inverse
            template <typename T, typename Parameters>
            struct base_tmerc_ellipsoid
                : public base_t_fi<base_tmerc_ellipsoid<T, Parameters>, T, Parameters>
            {
                par_tmerc<T> m_proj_parm;

                inline base_tmerc_ellipsoid(const Parameters& par)
                    : base_t_fi<base_tmerc_ellipsoid<T, Parameters>, T, Parameters>(*this, par)
                {}

                // FORWARD(e_forward)  ellipse
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(T& lp_lon, T& lp_lat, T& xy_x, T& xy_y) const
                {
                    static const T half_pi = detail::half_pi<T>();
                    static const T FC1 = tmerc::FC1<T>();
                    static const T FC2 = tmerc::FC2<T>();
                    static const T FC3 = tmerc::FC3<T>();
                    static const T FC4 = tmerc::FC4<T>();
                    static const T FC5 = tmerc::FC5<T>();
                    static const T FC6 = tmerc::FC6<T>();
                    static const T FC7 = tmerc::FC7<T>();
                    static const T FC8 = tmerc::FC8<T>();

                    T al, als, n, cosphi, sinphi, t;

                    /*
                     * Fail if our longitude is more than 90 degrees from the
                     * central meridian since the results are essentially garbage.
                     * Is error -20 really an appropriate return value?
                     *
                     *  http://trac.osgeo.org/proj/ticket/5
                     */
                    if( lp_lon < -half_pi || lp_lon > half_pi )
                    {
                        xy_x = HUGE_VAL;
                        xy_y = HUGE_VAL;
                        BOOST_THROW_EXCEPTION( projection_exception(error_lat_or_lon_exceed_limit) );
                        return;
                    }

                    sinphi = sin(lp_lat);
                    cosphi = cos(lp_lat);
                    t = fabs(cosphi) > 1e-10 ? sinphi/cosphi : 0.;
                    t *= t;
                    al = cosphi * lp_lon;
                    als = al * al;
                    al /= sqrt(1. - this->m_par.es * sinphi * sinphi);
                    n = this->m_proj_parm.esp * cosphi * cosphi;
                    xy_x = this->m_par.k0 * al * (FC1 +
                        FC3 * als * (1. - t + n +
                        FC5 * als * (5. + t * (t - 18.) + n * (14. - 58. * t)
                        + FC7 * als * (61. + t * ( t * (179. - t) - 479. ) )
                        )));
                    xy_y = this->m_par.k0 * (pj_mlfn(lp_lat, sinphi, cosphi, this->m_proj_parm.en) - this->m_proj_parm.ml0 +
                        sinphi * al * lp_lon * FC2 * ( 1. +
                        FC4 * als * (5. - t + n * (9. + 4. * n) +
                        FC6 * als * (61. + t * (t - 58.) + n * (270. - 330 * t)
                        + FC8 * als * (1385. + t * ( t * (543. - t) - 3111.) )
                        ))));
                }

                // INVERSE(e_inverse)  ellipsoid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(T& xy_x, T& xy_y, T& lp_lon, T& lp_lat) const
                {
                    static const T half_pi = detail::half_pi<T>();
                    static const T FC1 = tmerc::FC1<T>();
                    static const T FC2 = tmerc::FC2<T>();
                    static const T FC3 = tmerc::FC3<T>();
                    static const T FC4 = tmerc::FC4<T>();
                    static const T FC5 = tmerc::FC5<T>();
                    static const T FC6 = tmerc::FC6<T>();
                    static const T FC7 = tmerc::FC7<T>();
                    static const T FC8 = tmerc::FC8<T>();

                    T n, con, cosphi, d, ds, sinphi, t;

                    lp_lat = pj_inv_mlfn(this->m_proj_parm.ml0 + xy_y / this->m_par.k0, this->m_par.es, this->m_proj_parm.en);
                    if (fabs(lp_lat) >= half_pi) {
                        lp_lat = xy_y < 0. ? -half_pi : half_pi;
                        lp_lon = 0.;
                    } else {
                        sinphi = sin(lp_lat);
                        cosphi = cos(lp_lat);
                        t = fabs(cosphi) > 1e-10 ? sinphi/cosphi : 0.;
                        n = this->m_proj_parm.esp * cosphi * cosphi;
                        d = xy_x * sqrt(con = 1. - this->m_par.es * sinphi * sinphi) / this->m_par.k0;
                        con *= t;
                        t *= t;
                        ds = d * d;
                        lp_lat -= (con * ds / (1.-this->m_par.es)) * FC2 * (1. -
                            ds * FC4 * (5. + t * (3. - 9. *  n) + n * (1. - 4 * n) -
                            ds * FC6 * (61. + t * (90. - 252. * n +
                                45. * t) + 46. * n
                           - ds * FC8 * (1385. + t * (3633. + t * (4095. + 1574. * t)) )
                            )));
                        lp_lon = d*(FC1 -
                            ds*FC3*( 1. + 2.*t + n -
                            ds*FC5*(5. + t*(28. + 24.*t + 8.*n) + 6.*n
                           - ds * FC7 * (61. + t * (662. + t * (1320. + 720. * t)) )
                        ))) / cosphi;
                    }
                }

                static inline std::string get_name()
                {
                    return "tmerc_ellipsoid";
                }

            };

            // template class, using CRTP to implement forward/inverse
            template <typename T, typename Parameters>
            struct base_tmerc_spheroid
                : public base_t_fi<base_tmerc_spheroid<T, Parameters>, T, Parameters>
            {
                par_tmerc<T> m_proj_parm;

                inline base_tmerc_spheroid(const Parameters& par)
                    : base_t_fi<base_tmerc_spheroid<T, Parameters>, T, Parameters>(*this, par)
                {}

                // FORWARD(s_forward)  sphere
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(T& lp_lon, T& lp_lat, T& xy_x, T& xy_y) const
                {
                    static const T half_pi = detail::half_pi<T>();

                    T b, cosphi;

                    /*
                     * Fail if our longitude is more than 90 degrees from the
                     * central meridian since the results are essentially garbage.
                     * Is error -20 really an appropriate return value?
                     *
                     *  http://trac.osgeo.org/proj/ticket/5
                     */
                    if( lp_lon < -half_pi || lp_lon > half_pi )
                    {
                        xy_x = HUGE_VAL;
                        xy_y = HUGE_VAL;
                        BOOST_THROW_EXCEPTION( projection_exception(error_lat_or_lon_exceed_limit) );
                        return;
                    }

                    cosphi = cos(lp_lat);
                    b = cosphi * sin(lp_lon);
                    if (fabs(fabs(b) - 1.) <= epsilon10)
                        BOOST_THROW_EXCEPTION( projection_exception(error_tolerance_condition) );

                    xy_x = this->m_proj_parm.ml0 * log((1. + b) / (1. - b));
                    xy_y = cosphi * cos(lp_lon) / sqrt(1. - b * b);

                    b = fabs( xy_y );
                    if (b >= 1.) {
                        if ((b - 1.) > epsilon10)
                            BOOST_THROW_EXCEPTION( projection_exception(error_tolerance_condition) );
                        else xy_y = 0.;
                    } else
                        xy_y = acos(xy_y);

                    if (lp_lat < 0.)
                        xy_y = -xy_y;
                    xy_y = this->m_proj_parm.esp * (xy_y - this->m_par.phi0);
                }

                // INVERSE(s_inverse)  sphere
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(T& xy_x, T& xy_y, T& lp_lon, T& lp_lat) const
                {
                    T h, g;

                    h = exp(xy_x / this->m_proj_parm.esp);
                    g = .5 * (h - 1. / h);
                    h = cos(this->m_par.phi0 + xy_y / this->m_proj_parm.esp);
                    lp_lat = asin(sqrt((1. - h * h) / (1. + g * g)));

                    /* Make sure that phi is on the correct hemisphere when false northing is used */
                    if (xy_y < 0. && -lp_lat+this->m_par.phi0 < 0.0) lp_lat = -lp_lat;

                    lp_lon = (g != 0.0 || h != 0.0) ? atan2(g, h) : 0.;
                }

                static inline std::string get_name()
                {
                    return "tmerc_spheroid";
                }

            };

            template <typename Parameters, typename T>
            inline void setup(Parameters& par, par_tmerc<T>& proj_parm)
            {
                if (par.es != 0.0) {
                    proj_parm.en = pj_enfn<T>(par.es);
                    proj_parm.ml0 = pj_mlfn(par.phi0, sin(par.phi0), cos(par.phi0), proj_parm.en);
                    proj_parm.esp = par.es / (1. - par.es);
                } else {
                    proj_parm.esp = par.k0;
                    proj_parm.ml0 = .5 * proj_parm.esp;
                }
            }

    }} // namespace detail::tmerc
    #endif // doxygen

    /*!
        \brief Transverse Mercator projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Cylindrical
         - Spheroid
         - Ellipsoid
        \par Example
        \image html ex_tmerc.gif
    */
    template <typename T, typename Parameters>
    struct tmerc_ellipsoid : public detail::tmerc::base_tmerc_ellipsoid<T, Parameters>
    {
        inline tmerc_ellipsoid(const Parameters& par) : detail::tmerc::base_tmerc_ellipsoid<T, Parameters>(par)
        {
            detail::tmerc::setup(this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief Transverse Mercator projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Cylindrical
         - Spheroid
         - Ellipsoid
        \par Example
        \image html ex_tmerc.gif
    */
    template <typename T, typename Parameters>
    struct tmerc_spheroid : public detail::tmerc::base_tmerc_spheroid<T, Parameters>
    {
        inline tmerc_spheroid(const Parameters& par) : detail::tmerc::base_tmerc_spheroid<T, Parameters>(par)
        {
            detail::tmerc::setup(this->m_par, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Static projection
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::par4::tmerc, tmerc_spheroid, tmerc_ellipsoid)

        // Factory entry(s) - dynamic projection
        template <typename T, typename Parameters>
        class tmerc_entry : public detail::factory_entry<T, Parameters>
        {
            public :
                virtual base_v<T, Parameters>* create_new(const Parameters& par) const
                {
                    if (par.es)
                        return new base_v_fi<tmerc_ellipsoid<T, Parameters>, T, Parameters>(par);
                    else
                        return new base_v_fi<tmerc_spheroid<T, Parameters>, T, Parameters>(par);
                }
        };

        template <typename T, typename Parameters>
        inline void tmerc_init(detail::base_factory<T, Parameters>& factory)
        {
            factory.add_to_factory("tmerc", new tmerc_entry<T, Parameters>);
        }

    } // namespace detail
    #endif // doxygen

} // namespace projections

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_PROJECTIONS_TMERC_HPP


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// 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_UNITS_CODATA_PHYSICO_CHEMICAL_CONSTANTS_HPP
#define BOOST_UNITS_CODATA_PHYSICO_CHEMICAL_CONSTANTS_HPP

#include <boost/units/pow.hpp>
#include <boost/units/quantity.hpp>
#include <boost/units/static_constant.hpp>

#include <boost/units/systems/detail/constants.hpp>
#include <boost/units/systems/si/amount.hpp>
#include <boost/units/systems/si/area.hpp>
#include <boost/units/systems/si/electric_charge.hpp>
#include <boost/units/systems/si/energy.hpp>
#include <boost/units/systems/si/frequency.hpp>
#include <boost/units/systems/si/mass.hpp>
#include <boost/units/systems/si/power.hpp>
#include <boost/units/systems/si/solid_angle.hpp>
#include <boost/units/systems/si/temperature.hpp>

#include <boost/units/systems/si/codata/typedefs.hpp>

/// \file
/// CODATA recommended values of fundamental physico-chemical constants
/// CODATA 2014 values as of 2016/04/26

namespace boost {

namespace units {

namespace si {

namespace constants {

namespace codata {

// PHYSICO-CHEMICAL
/// Avogadro constant
BOOST_UNITS_PHYSICAL_CONSTANT(N_A,quantity<inverse_amount>,6.022140857e23/mole,7.4e15/mole);
/// atomic mass constant
BOOST_UNITS_PHYSICAL_CONSTANT(m_u,quantity<mass>,1.660539040e-27*kilograms,2.0e-35*kilograms);
/// Faraday constant
BOOST_UNITS_PHYSICAL_CONSTANT(F,quantity<electric_charge_over_amount>,96485.33289*coulombs/mole,5.9e-4*coulombs/mole);
/// molar gas constant
BOOST_UNITS_PHYSICAL_CONSTANT(R,quantity<energy_over_temperature_amount>,8.3144598*joules/kelvin/mole,4.8e-06*joules/kelvin/mole);
/// Boltzmann constant
BOOST_UNITS_PHYSICAL_CONSTANT(k_B,quantity<energy_over_temperature>,1.38064852e-23*joules/kelvin,7.9e-30*joules/kelvin);
/// Stefan-Boltzmann constant
BOOST_UNITS_PHYSICAL_CONSTANT(sigma_SB,quantity<power_over_area_temperature_4>,5.670367e-8*watts/square_meter/pow<4>(kelvin),1.3e-13*watts/square_meter/pow<4>(kelvin));
/// first radiation constant
BOOST_UNITS_PHYSICAL_CONSTANT(c_1,quantity<power_area>,3.741771790e-16*watt*square_meters,4.6e-24*watt*square_meters);
/// first radiation constant for spectral radiance
BOOST_UNITS_PHYSICAL_CONSTANT(c_1L,quantity<power_area_over_solid_angle>,1.191042953e-16*watt*square_meters/steradian,1.5e-24*watt*square_meters/steradian);
/// second radiation constant
BOOST_UNITS_PHYSICAL_CONSTANT(c_2,quantity<length_temperature>,1.43877736e-2*meter*kelvin,8.3e-9*meter*kelvin);
/// Wien displacement law constant : lambda_max T
BOOST_UNITS_PHYSICAL_CONSTANT(b,quantity<length_temperature>,2.8977729e-3*meter*kelvin,1.7e-9*meter*kelvin);
/// Wien displacement law constant : nu_max/T
BOOST_UNITS_PHYSICAL_CONSTANT(b_prime,quantity<frequency_over_temperature>,5.8789238e10*hertz/kelvin,3.4e4*hertz/kelvin);

} // namespace codata

} // namespace constants

} // namespace si

} // namespace units

} // namespace boost

#endif // BOOST_UNITS_CODATA_PHYSICO_CHEMICAL_CONSTANTS_HPP


#include "grid_tools.hpp"

#include <deal.II/base/exceptions.h>
#include <deal.II/grid/grid_tools.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/tria_iterator.h>
#include <deal.II/lac/sparsity_pattern.h>

#include <boost/lexical_cast.hpp>

extern "C" {
#include <metis.h>
}

#include <map>
#include <vector>

using namespace dealii;

//   Code taken from deal.II and adapted such that METIS takes
//   the dense boundary blocks into account when partitioning the
//   grid.

namespace boltzmann {

namespace {

/**
 * Exception
 */
DeclException1(ExcInvalidNumberOfPartitions,
               int,
               << "The number of partitions you gave is " << arg1
               << ", but must be greater than zero.");
/**
 * Exception
 */
DeclException1(ExcNonExistentSubdomain,
               int,
               << "The subdomain id " << arg1 << " has no cells associated with it.");
/**
 * Exception
 */
DeclException0(ExcTriangulationHasBeenRefined);

/**
 * Exception
 */
DeclException1(
    ExcScalingFactorNotPositive, double, << "The scaling factor must be positive, but is " << arg1);
/**
 * Exception
 */
template <int N>
DeclException1(ExcPointNotFoundInCoarseGrid,
               Point<N>,
               << "The point <" << arg1 << "> could not be found inside any of the "
               << "coarse grid cells.");
/**
 * Exception
 */
template <int N>
DeclException1(ExcPointNotFound,
               Point<N>,
               << "The point <" << arg1 << "> could not be found inside any of the "
               << "subcells of a coarse grid cell.");

/**
 * Exception
 */
DeclException1(ExcVertexNotUsed,
               unsigned int,
               << "The given vertex " << arg1 << " is not used in the given triangulation");

DeclException2(ExcInvalidArraySize,
               int,
               int,
               << "The array has size " << arg1 << " but should have size " << arg2);

struct face_connectivity_of_cells
{
  typedef std::map<std::pair<unsigned int, unsigned int>, unsigned int> indexmap_t;

  static void get(const dealii::Triangulation<2, 2>& triangulation,
                  SparsityPattern& cell_connectivity,
                  indexmap_t& indexmap)
  {
    // as built in this function, we only consider face neighbors, which leads
    // to a fixed number of entries per row (don't forget that each cell couples
    // with itself, and that neighbors can be refined)
    cell_connectivity.reinit(
        triangulation.n_active_cells(),
        triangulation.n_active_cells(),
        GeometryInfo<2>::faces_per_cell * GeometryInfo<2>::max_children_per_face + 1);

    // create a map pair<lvl,idx> -> SparsityPattern index
    // TODO: we are no longer using user_indices for this because we can get
    // pointer/index clashes when saving/restoring them. The following approach
    // works, but this map can get quite big. Not sure about more efficient solutions.
    // std::map< std::pair<unsigned int,unsigned int>, unsigned int >
    //   indexmap;
    unsigned int index = 0;
    for (typename dealii::internal::ActiveCellIterator<2, 2, Triangulation<2, 2> >::type
             cell = triangulation.begin_active();
         cell != triangulation.end();
         ++cell, ++index)
      indexmap[std::pair<unsigned int, unsigned int>(cell->level(), cell->index())] = index;

    // next loop over all cells and their neighbors to build the sparsity
    // pattern. note that it's a bit hard to enter all the connections when a
    // neighbor has children since we would need to find out which of its
    // children is adjacent to the current cell. this problem can be omitted if
    // we only do something if the neighbor has no children -- in that case it
    // is either on the same or a coarser level than we are. in return, we have
    // to add entries in both directions for both cells
    index = 0;
    for (typename dealii::internal::ActiveCellIterator<2, 2, Triangulation<2, 2> >::type
             cell = triangulation.begin_active();
         cell != triangulation.end();
         ++cell, ++index) {
      cell_connectivity.add(index, index);
      for (unsigned int f = 0; f < GeometryInfo<2>::faces_per_cell; ++f)
        if ((cell->at_boundary(f) == false) && (cell->neighbor(f)->has_children() == false)) {
          unsigned int other_index =
              indexmap
                  .find(std::pair<unsigned int, unsigned int>(cell->neighbor(f)->level(),
                                                              cell->neighbor(f)->index()))
                  ->second;
          cell_connectivity.add(index, other_index);
          cell_connectivity.add(other_index, index);
        }
    }

    // now compress the so-built connectivity pattern
    cell_connectivity.compress();
  }
};

// ----------------------------------------------------------------------
void partition(const SparsityPattern& sparsity_pattern,
               const unsigned int n_partitions,
               std::vector<unsigned int>& partition_indices,
               std::vector<idx_t> cell_weights = {})
{
  Assert(sparsity_pattern.n_rows() == sparsity_pattern.n_cols(), ExcNotQuadratic());
  Assert(sparsity_pattern.is_compressed(), SparsityPattern::ExcNotCompressed());

  Assert(n_partitions > 0, ExcInvalidNumberOfPartitions(n_partitions));
  Assert(partition_indices.size() == sparsity_pattern.n_rows(),
         ExcInvalidArraySize(partition_indices.size(), sparsity_pattern.n_rows()));

  // check for an easy return
  if (n_partitions == 1) {
    std::fill_n(partition_indices.begin(), partition_indices.size(), 0U);
    return;
  }

// Make sure that METIS is actually
// installed and detected
#ifndef DEAL_II_WITH_METIS
  AssertThrow(false, ExcMETISNotInstalled());
#else

  // generate the data structures for
  // METIS. Note that this is particularly
  // simple, since METIS wants exactly our
  // compressed row storage format. we only
  // have to set up a few auxiliary arrays
  idx_t n = static_cast<signed int>(sparsity_pattern.n_rows()),
        ncon = 1,                               // number of balancing constraints (should be >0)
      nparts = static_cast<int>(n_partitions),  // number of subdomains to create
      dummy;                                    // the numbers of edges cut by the
  // resulting partition

  // use default options for METIS
  idx_t options[METIS_NOPTIONS];
  METIS_SetDefaultOptions(options);

  // one more nuisance: we have to copy our
  // own data to arrays that store signed
  // integers :-(
  std::vector<idx_t> int_rowstart(1);
  int_rowstart.reserve(sparsity_pattern.n_rows() + 1);
  std::vector<idx_t> int_colnums;
  int_colnums.reserve(sparsity_pattern.n_nonzero_elements());
  for (SparsityPattern::size_type row = 0; row < sparsity_pattern.n_rows(); ++row) {
    for (SparsityPattern::iterator col = sparsity_pattern.begin(row);
         col < sparsity_pattern.end(row);
         ++col)
      int_colnums.push_back(col->column());
    int_rowstart.push_back(int_colnums.size());
  }

  std::vector<idx_t> int_partition_indices(sparsity_pattern.n_rows());

  // Make use of METIS' error code.
  int ierr;

  // Select which type of partitioning to
  // create

  // Use recursive if the number of
  // partitions is less than or equal to 8
  // if (n_partitions <= 8)
  //   ierr = METIS_PartGraphRecursive(&n, &ncon, &int_rowstart[0], &int_colnums[0],
  //                                   cell_weights.data(), NULL, NULL,
  //                                   &nparts,NULL,NULL,&options[0],
  //                                   &dummy,&int_partition_indices[0]);

  // // Otherwise use kway
  // else
  ierr = METIS_PartGraphKway(&n,
                             &ncon,
                             &int_rowstart[0],
                             &int_colnums[0],
                             cell_weights.data(),
                             NULL,
                             NULL,
                             &nparts,
                             NULL,
                             NULL,
                             &options[0],
                             &dummy,
                             &int_partition_indices[0]);
  // ierr = METIS_PartGraphRecursive(&n, &ncon, &int_rowstart[0], &int_colnums[0],
  //                                 cell_weights.data(), NULL, NULL,
  //                                 &nparts,NULL,NULL,&options[0],
  //                                 &dummy,&int_partition_indices[0]);

  // If metis returns normally, an
  // error code METIS_OK=1 is
  // returned from the above
  // functions (see metish.h)
  if (ierr != 1) {
    throw std::runtime_error("METIS-Error: " + boost::lexical_cast<std::string>(ierr));
  }
  //  AssertThrow (ierr == 1, ExcMETISError (ierr));

  // now copy back generated indices into the
  // output array
  std::copy(int_partition_indices.begin(), int_partition_indices.end(), partition_indices.begin());
#endif
}

}  // end namespace

void partition_triangulation(const unsigned int n_partitions,
                             dealii::Triangulation<2, 2>& triangulation,
                             const unsigned int cell_weight,
                             const unsigned int cell_weight_bc)
{
  Assert(n_partitions > 0, ExcInvalidNumberOfPartitions(n_partitions));

  // check for an easy return
  if (n_partitions == 1) {
    for (typename dealii::internal::ActiveCellIterator<2, 2, Triangulation<2, 2> >::type cell =
             triangulation.begin_active();
         cell != triangulation.end();
         ++cell)
      cell->set_subdomain_id(0);
    return;
  }

  // we decompose the domain by first generating the connection graph of all
  // cells with their neighbors, and then passing this graph off to METIS.
  // finally defer to the other function for partitioning and assigning
  // subdomain idsdealii::
  dealii::SparsityPattern cell_connectivity;
  typename face_connectivity_of_cells::indexmap_t cell_enumeration;
  face_connectivity_of_cells::get(triangulation, cell_connectivity, cell_enumeration);

  // check for an easy return
  if (n_partitions == 1) {
    for (typename dealii::internal::ActiveCellIterator<2, 2, Triangulation<2, 2> >::type cell =
             triangulation.begin_active();
         cell != triangulation.end();
         ++cell)
      cell->set_subdomain_id(0);
    return;
  }

  std::vector<idx_t> cell_weights(triangulation.n_active_cells());
  for (auto cell : triangulation.active_cell_iterators()) {
    int nfaces = dealii::GeometryInfo<2>::faces_per_cell;
    int count = 0;
    for (int i = 0; i < nfaces; ++i)
      if (cell->face(i)->at_boundary()) count++;

    unsigned int index =
        cell_enumeration.find(std::make_pair(cell->level(), cell->index()))->second;
    if (count == 0) cell_weights[index] = cell_weight;
    if (count > 0) cell_weights[index] = cell_weight_bc;
  }

  // partition this connection graph and get back a vector of indices, one per
  // degree of freedom (which is associated with a cell)
  std::vector<unsigned int> partition_indices(triangulation.n_active_cells());
  partition(cell_connectivity, n_partitions, partition_indices, cell_weights);

  // finally loop over all cells and set the subdomain ids
  unsigned int index = 0;
  for (typename dealii::internal::ActiveCellIterator<2, 2, Triangulation<2, 2> >::type
           cell = triangulation.begin_active();
       cell != triangulation.end();
       ++cell, ++index)
    cell->set_subdomain_id(partition_indices[index]);

  // (end)
}

}  // end namespace boltzmann