/*!
 * @file
 * Defines `boost::mpl11::detail::left_folds::bounded`.
 *
 *
 * @copyright Louis Dionne 2014
 * Distributed under the Boost Software License, Version 1.0.
 *         (See accompanying file LICENSE.md or copy at
 *             http://www.boost.org/LICENSE_1_0.txt)
 */

//////////////////////////////////////////////////////////////////////////
// GENERATED HEADER: DO NOT EDIT.
//////////////////////////////////////////////////////////////////////////

#ifndef BOOST_MPL11_DETAIL_LEFT_FOLDS_BOUNDED_HPP
#define BOOST_MPL11_DETAIL_LEFT_FOLDS_BOUNDED_HPP

#include <boost/mpl11/detail/std_size_t.hpp>
#include <boost/mpl11/fwd/iterable.hpp>

namespace boost { namespace mpl11 { namespace detail { namespace left_folds {
    <%
        def tail(n)
          (0...n).foldl("xs") { |xs, _| "tail<#{xs}>" }
        end

        def tails(n)
          (0...n).map(&method(:tail))
        end

        f = -> (state, xs) { "f<#{state}, head<#{xs}>>" }
        unroll = 5
    %>

    template <
        std_size_t n, template <typename ...> class f,
        typename state, typename xs>
    struct bounded_impl {
        using left = bounded_impl<
            (n - <%= unroll+1 %>)/2,
            f,
            <%= tails(unroll+1).foldl("state", &f) %>,
            <%= tail(unroll+1) %>
        >;
        using right = bounded_impl<
            (n - <%= unroll+1 %>) - (n - <%= unroll+1 %>)/2,
            f,
            typename left::result,
            typename left::rest
        >;

        using rest = typename right::rest;
        using result = typename right::result;
    };

    <% for n in 0..unroll %>
        template <template <typename ...> class f, typename state, typename xs>
        struct bounded_impl<<%= n %>, f, state, xs> {
            using rest = <%= tail n %>;
            using result = <%= tails(n).foldl("state", &f) %>;
        };
    <% end %>

    /*!
     * @ingroup details
     *
     * Left fold stopping after `n` steps.
     *
     * The metafunction is unlifted for performance.
     */
    template <
        std_size_t n,
        template <typename ...> class f,
        typename state,
        typename xs
    >
    using bounded = typename bounded_impl<n, f, state, xs>::result;
}}}} // end namespace boost::mpl11::detail::left_folds

#endif // !BOOST_MPL11_DETAIL_LEFT_FOLDS_BOUNDED_HPP


//////////////////////////////////////////////////////////////////////////////
// cross_validation::k_fold::initialize.hpp                                 //
//                                                                          //
//  (C) Copyright 2009 Erwann Rogard                                        //
//  Use, modification and distribution are subject to the                   //
//  Boost Software License, Version 1.0. (See accompanying file             //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)        //
//////////////////////////////////////////////////////////////////////////////
#ifndef  BOOST_STATISTICS_DETAIL_CROSS_VALIDATION_K_FOLD_INITILIAZE_HPP_ER_2009
#define  BOOST_STATISTICS_DETAIL_CROSS_VALIDATION_K_FOLD_INITILIAZE_HPP_ER_2009
#include <algorithm>
#include <boost/range.hpp>
#include <boost/typeof/typeof.hpp>
#include <boost/statistics/detail/cross_validation/k_fold/train_predict.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace cross_validation{
namespace k_fold{

    template<
        typename U,typename Ft,typename Fi,typename Fo,
        typename E,typename It,typename It1
    >
    It cross_validate(
        partition<U,Ft,Fi,Fo>& kfp,
        E estimator,
        It  i_p,    // predicted values
        It1 i_o     // true values
    ){
        kfp.initialize();
        while(kfp.index()<kfp.n_folds()){
            i_p = train_predict(
                kfp,
                estimator,
                i_p
            );

            BOOST_AUTO(
                r,
                kfp.output_range()
            );

            i_o = std::copy(
                boost::begin(r),
                boost::end(r),
                i_o
            );

            kfp.increment();
        }
        return i_p;
    }

}// k_fold
}// cross_validation
}// detail
}// statistics
}// boost

#endif


// ====================================================================
// This file is part of FlexibleSUSY.
//
// FlexibleSUSY 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 3 of the License,
// or (at your option) any later version.
//
// FlexibleSUSY 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 FlexibleSUSY.  If not, see
// <http://www.gnu.org/licenses/>.
// ====================================================================

#ifndef GSL_UTILS_H
#define GSL_UTILS_H

#include "gsl_vector.hpp"

#include <gsl/gsl_vector.h>
#include <Eigen/Core>
#include <cassert>

namespace flexiblesusy {

class GSL_vector;

/// Returns true if GSL_vector contains only finite elements, false otherwise
bool is_finite(const GSL_vector&);
/// Returns true if GSL vector contains only finite elements, false otherwise
bool is_finite(const gsl_vector*);
Eigen::ArrayXd to_eigen_array(const gsl_vector*);
Eigen::ArrayXd to_eigen_array(const GSL_vector&);
Eigen::VectorXd to_eigen_vector(const gsl_vector*);
Eigen::VectorXd to_eigen_vector(const GSL_vector&);
GSL_vector to_GSL_vector(const gsl_vector*);

template <typename Derived>
GSL_vector to_GSL_vector(const Eigen::DenseBase<Derived>& v)
{
   using Index_t = typename Derived::Index;
   GSL_vector v2(v.rows());

   for (Index_t i = 0; i < v.rows(); i++)
      v2[i] = v(i);

   return v2;
}

template <int Size>
Eigen::Matrix<double,Size,1> to_eigen_vector_fixed(const gsl_vector* v)
{
   assert(Size == v->size);

   using Result_t = Eigen::Matrix<double,Size,1>;
   using Index_t = typename Result_t::Index;
   Result_t result;

   for (Index_t i = 0; i < Size; i++)
      result(i) = gsl_vector_get(v, i);

   return result;
}

template <int Size>
Eigen::Matrix<double,Size,1> to_eigen_vector_fixed(const GSL_vector& v)
{
   assert(Size == v.size());

   using Result_t = Eigen::Matrix<double,Size,1>;
   using Index_t = typename Result_t::Index;
   Result_t result;

   for (Index_t i = 0; i < Size; i++)
      result(i) = v[i];

   return result;
}

/**
 * Copies values from an Eigen array/matrix to a GSL vector.
 *
 * @param src Eigen array/matrix
 * @param dst GSL vector
 */
template <typename Derived>
void copy(const Eigen::DenseBase<Derived>& src, gsl_vector* dst)
{
   using Index_t = typename Derived::Index;
   const auto dim = src.rows();

   assert(dst);
   assert(dim == dst->size);

   for (Index_t i = 0; i < dim; i++)
      gsl_vector_set(dst, i, src(i));
}

} // namespace flexiblesusy

#endif


#pragma once

#include <Eigen/Core>
#include <Eigen/Sparse>
#include <iostream>
#include <utility>
#include <vector>

#include "aux/filtered_range.hpp"
#include "enum/enum.hpp"
#include "spectral/basis/toolbox/spectral_basis.hpp"

namespace boltzmann {

// ----------------------------------------------------------------------
template <typename SPECTRAL_BASIS>
class RotateBasis
{
 private:
  typedef SPECTRAL_BASIS basis_t;
  typedef typename basis_t::index_t index_t;

 public:
  RotateBasis(const SPECTRAL_BASIS& basis)
      : basis_(basis)
  {
    this->init();
  }

  void init();

  /**
   * @brief rotate in counter clockwise direction
   *
   * @param out
   * @param in
   * @param phi angle
   * @param L number of repetitions
   */
  template <typename NUMERIC>
  void apply(NUMERIC* out, const NUMERIC* in, const double phi, const int L = 1) const;

 private:
  const basis_t& basis_;
  Eigen::SparseMatrix<double> R_;

  // internal variable
  mutable double theta_;

  typedef int a_freq;  // angular frequency
  std::unordered_map<a_freq, std::vector<std::pair<index_t, index_t> > > v_pairs_;

  // those elements who do not depend on the angular index `l` need just to be copied
  std::vector<index_t> v_copy_;

  int l_max;
};

// ----------------------------------------------------------------------
template <typename SPECTRAL_BASIS>
void
RotateBasis<SPECTRAL_BASIS>::init()
{
  int N = basis_.n_dofs();
  R_.resize(N, N);
  l_max = spectral::get_max_l(basis_);

  typedef typename basis_t::elem_t elem_t;
  typedef typename basis_t::index_t index_t;

  typedef typename boost::mpl::at_c<typename elem_t::types_t, 1>::type radial_elem_t;
  typedef typename boost::mpl::at_c<typename elem_t::types_t, 0>::type ang_elem_t;

  typename elem_t::Acc::template get<radial_elem_t> get_rad;
  typename elem_t::Acc::template get<ang_elem_t> get_ang;

  {
    // im cos(0*phi) elements do not change, append their
    // indices to the v_copy_ array.
    std::function<bool(const elem_t&)> pred = [&](const elem_t& e) {
      return (get_ang(e).get_id().l == 0);
    };
    auto range = filtered_range(basis_.begin(), basis_.end(), pred);

    auto begin = std::get<0>(range);
    auto end = std::get<1>(range);
    for (auto it = begin; it != end; ++it) {
      v_copy_.push_back(basis_.get_dof_index(it->get_id()));
    }
  }

  for (int l = 1; l <= l_max; ++l) {
    std::function<bool(const elem_t&)> pred = [&](const elem_t& e) {
      return (get_ang(e).get_id().l == l);
    };
    auto range = filtered_range(basis_.begin(), basis_.end(), pred);
    auto begin = std::get<0>(range);
    auto end = std::get<1>(range);

    for (auto it = begin; it != end; ++it) {
      auto ang_elem = get_ang(*it);
      auto rad_elem = get_rad(*it);

      typedef std::vector<std::pair<index_t, index_t> > vec_t;

      if (ang_elem.get_id().t == SIN) {
        index_t i_sin = basis_.get_dof_index(it->get_id());
        ang_elem_t cos_elem(COS, l);
        elem_t elem2(cos_elem, rad_elem);
        index_t i_cos = basis_.get_dof_index(elem2.get_id());

        auto search = v_pairs_.find(l);
        if (search == v_pairs_.end()) {
          v_pairs_[l] = vec_t({std::make_pair(i_cos, i_sin)});
        } else {
          v_pairs_[l].push_back(std::make_pair(i_cos, i_sin));
        }
      }
    }
  }
}

// ----------------------------------------------------------------------
template <typename SPECTRAL_BASIS>
template <typename NUMERIC>
void
RotateBasis<SPECTRAL_BASIS>::apply(NUMERIC* out,
                                   const NUMERIC* in,
                                   const double phi,
                                   const int L) const
{
  typedef NUMERIC numeric_t;
  const unsigned int N = basis_.n_dofs();

  Eigen::VectorXd vsin(l_max + 1);
  Eigen::VectorXd vcos(l_max + 1);

  for (int i = 0; i <= l_max; ++i) {
    vsin[i] = std::sin(-i * phi);
    vcos[i] = std::cos(i * phi);
  }

  //#pragma omp parallel for
  for (int ix = 0; ix < L; ++ix) {
    numeric_t* p_out = out + ix * N;
    const numeric_t* p_in = in + ix * N;

    // walk through v_copy_
    for (auto& i : v_copy_) {
      p_out[i] = p_in[i];
    }

    // apply the rotation
    for (auto it = v_pairs_.begin(); it != v_pairs_.end(); ++it) {
      int l = it->first;

      const double rcos = vcos[l];
      const double rsin = vsin[l];

      for (auto it_v = it->second.begin(); it_v != it->second.end(); ++it_v) {
        index_t i_cos = it_v->first;
        index_t i_sin = it_v->second;

        p_out[i_sin] = p_in[i_sin] * rcos - p_in[i_cos] * rsin;
        p_out[i_cos] = p_in[i_sin] * rsin + p_in[i_cos] * rcos;
      }
    }
  }
}

}  // end namespace


#include <boost/test/unit_test.hpp>

#include <boost/format.hpp>

#include <set>
#include <iostream>

using boost::format;
using namespace crave;

template <typename T>
bool check_unique(rand_vec<T>& v) {
  for (unsigned i = 0; i < v.size(); i++)
    for (unsigned j = 0; j < i; j++)
      if (v[i] == v[j]) return false;
  return true;
}

BOOST_FIXTURE_TEST_SUITE(Vector_Constraint_t, Context_Fixture)

struct Item : public rand_obj {
  Item() : i_(), v(this) {
    constraint(30 <= v().size() && v().size() <= 50);
    constraint(foreach (v(), v()[i_] == v()[i_ - 1] + v()[i_ - 2]));
    constraint(foreach (v(), if_then(i_ <= 1, v()[i_] == i_)));
  }

  placeholder i_;
  rand_vec<unsigned int> v;
};

BOOST_AUTO_TEST_CASE(fibo_test) {
  Item it;
  BOOST_REQUIRE(it.next());
  BOOST_REQUIRE_LE(30, it.v.size());
  BOOST_REQUIRE_LE(it.v.size(), 50);
  BOOST_REQUIRE_EQUAL(it.v[0], 0);
  BOOST_REQUIRE_EQUAL(it.v[1], 1);
  for (unsigned i = 2; i < it.v.size(); i++) BOOST_REQUIRE_EQUAL(it.v[i], it.v[i - 1] + it.v[i - 2]);
}

struct Item1 : public rand_obj {
  Item1() : v(this) {}

  __rand_vec<unsigned int> u;
  rand_vec<unsigned int> v;
};

BOOST_AUTO_TEST_CASE(free_vector_test) {
  Item1 it;
  BOOST_REQUIRE(it.next());
  BOOST_REQUIRE(it.u.size() == 0);
  BOOST_REQUIRE(it.v.size() > 0);
  for (unsigned i = 0; i < it.v.size(); i++) std::cout << it.v[i] << " ";
  std::cout << std::endl;
}

struct Item2 : public rand_obj {
  Item2() : idx(), v(this) { constraint(foreach (v(), 100 <= v()[idx] && v()[idx] <= 200)); }

  placeholder idx;
  rand_vec<unsigned int> v;
};

BOOST_AUTO_TEST_CASE(default_size_test) {
  Item2 it;
  BOOST_REQUIRE(it.next());
  BOOST_REQUIRE(it.v.size() == default_rand_vec_size());
  for (unsigned i = 0; i < it.v.size(); i++) {
    std::cout << it.v[i] << " ";
    BOOST_REQUIRE(100 <= it.v[i] && it.v[i] <= 200);
  }
  std::cout << std::endl;
}

struct Item3 : public rand_obj {
  Item3() : i(), v(this) {
    constraint(v().size() == 100);
    constraint(foreach (v(), v()[i] < 100));
    constraint(foreach (v(), v()[i] >= 0));
    constraint(unique(v()));
  }

  placeholder i;
  rand_vec<int> v;
};

BOOST_AUTO_TEST_CASE(unique_test_1) {
  Item3 it;
  BOOST_REQUIRE(it.next());
  BOOST_REQUIRE(it.v.size() == 100);
  for (unsigned i = 0; i < it.v.size(); i++) {
    BOOST_REQUIRE(0 <= it.v[i] && it.v[i] < 100);
    for (unsigned j = 0; j < i; j++) BOOST_REQUIRE(it.v[i] != it.v[j]);
  }
}

BOOST_AUTO_TEST_CASE(unique_test_2) {
  rand_vec<unsigned int> v(NULL);
  placeholder idx;
  Generator gen;
  gen(v().size() == 7);
  gen(foreach (v(), v()[idx] < 6));

  gen("unique", unique(v()));
  BOOST_REQUIRE(!gen.next());

  gen.disableConstraint("unique");
  BOOST_REQUIRE(gen.next());
  BOOST_REQUIRE(v.size() == 7);
  for (unsigned i = 0; i < v.size(); i++) {
    std::cout << v[i] << " ";
    BOOST_REQUIRE(v[i] < 6);
  }
  std::cout << std::endl;
}

BOOST_AUTO_TEST_CASE(soft_unique_test) {
  rand_vec<unsigned int> v(NULL);
  placeholder idx;
  Generator gen;
  gen(v().size() == 7);
  gen(foreach (v(), v()[idx] < 6));
  gen.soft(unique(v()));
  BOOST_REQUIRE(gen.next());
}

struct Item4 : public rand_obj {
  Item4() : _i(), v(this) {
    constraint(v().size() == 10);
    constraint("c1", foreach (v(), v()[_i] <= 100));
    constraint("c2", foreach (v(), v()[_i] > 100));
  }

  placeholder _i;
  rand_vec<unsigned int> v;
};

BOOST_AUTO_TEST_CASE(constraint_management_test) {
  Item4 it;

  BOOST_REQUIRE(!it.next());

  it.disable_constraint("c1");
  BOOST_REQUIRE(it.next());
  for (unsigned i = 0; i < it.v.size(); i++) BOOST_REQUIRE_GT(it.v[i], 100);

  it.enable_constraint("c1");
  it.disable_constraint("c2");
  BOOST_REQUIRE(it.next());
  for (unsigned i = 0; i < it.v.size(); i++) BOOST_REQUIRE(it.v[i] <= 100);

  it.enable_constraint("c2");
  BOOST_REQUIRE(!it.next());
}

struct Item5 : public rand_obj {
  Item5() : x(), v(this) {
    constraint(v().size() == 50);
    constraint(foreach (v(), if_then(x < 25, v()[x] == x)));
    constraint(foreach (v(), if_then(x == 25, v()[x] == 0)));
    constraint(foreach (v(), if_then(x > 25, v()[x] + x == 200)));
  }

  placeholder x;
  rand_vec<unsigned int> v;
};

BOOST_AUTO_TEST_CASE(index_constraint_test) {
  Item5 it;
  it.constraint.printDotGraph(std::cout);
  BOOST_REQUIRE(it.next());
  BOOST_REQUIRE(it.v.size() == 50);
  for (unsigned i = 0; i < it.v.size(); i++) {
    if (i < 25)
      BOOST_REQUIRE(it.v[i] == i);
    else if (i > 25)
      BOOST_REQUIRE(it.v[i] + i == 200);
    else
      BOOST_REQUIRE(it.v[i] == 0);
  }
}

BOOST_AUTO_TEST_CASE(soft_vec_constraint) {
  rand_vec<unsigned int> v(NULL);
  placeholder i;

  Generator gen;
  gen(v().size() == 10);
  gen(foreach (v(), v()[i] >= v()[i - 1]));
  gen.soft(foreach (v(), v()[i] < v()[i - 1]));
  BOOST_REQUIRE(gen.next());

  Generator gen1;
  gen1(v().size() == 4);
  gen1(foreach (v(), v()[i] >= v()[i - 1]));
  gen1(foreach (v(), v()[i] <= 1000));
  gen1.soft(foreach (v(), v()[i] <= v()[i - 1]));
  gen1.soft(foreach (v(), if_then(i == 0, v()[i] % 13 == 3)));
  BOOST_REQUIRE(gen1.next());
  for (int j = 0; j < 10; j++) {
    BOOST_REQUIRE_EQUAL(v.size(), 4);
    std::cout << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << std::endl;
    BOOST_REQUIRE_EQUAL(v[0], v[1]);
    BOOST_REQUIRE_EQUAL(v[1], v[2]);
    BOOST_REQUIRE_EQUAL(v[2], v[3]);
    BOOST_REQUIRE_EQUAL(v[0] % 13, 3);
    if (!gen1.next()) break;
  }
}

BOOST_AUTO_TEST_CASE(mixed_bv_width_1) {
  rand_vec<signed char> a(NULL);
  placeholder idx;
  Generator gen;
  gen(a().size() == 28);
  gen(foreach (a(), a()[idx] < (short)-100));
  gen(unique(a()));

  BOOST_REQUIRE(gen.next());
  for (unsigned i = 0; i < a.size(); i++) {
    BOOST_REQUIRE_LT(a[i], -100);
    std::cout << " " << (int)a[i];
  }
  std::cout << std::endl;
  BOOST_REQUIRE(check_unique(a));
}

BOOST_AUTO_TEST_CASE(mixed_bv_width_2) {
  rand_vec<short> a(NULL);
  placeholder p;
  Generator gen;
  gen(a().size() == 19);
  gen(foreach (a(), a()[p] < 10));
  gen(foreach (a(), a()[p] > -10));
  gen(unique(a()));

  BOOST_REQUIRE(gen.next());
  for (unsigned i = 0; i < a.size(); i++) BOOST_REQUIRE(-10 < a[i] && a[i] < 10);
  BOOST_REQUIRE(check_unique(a));
}

BOOST_AUTO_TEST_CASE(mixed_bv_width_3) {
  rand_vec<int> a(NULL);
  placeholder _i;
  Generator gen;
  gen(a().size() == 19);
  gen(foreach (a(), a()[_i] < (signed char)10));
  gen(foreach (a(), a()[_i] > (short)-10));
  gen(unique(a()));

  BOOST_REQUIRE(gen.next());
  for (unsigned i = 0; i < a.size(); i++) BOOST_REQUIRE(-10 < a[i] && a[i] < 10);
  BOOST_REQUIRE(check_unique(a));
}

BOOST_AUTO_TEST_CASE(mixed_bv_width_4) {
  rand_vec<short> a(NULL);
  placeholder _i;
  Generator gen;
  gen(a().size() == 10);
  gen(foreach (a(), -3 <= a()[_i] && a()[_i] <= 3));
  gen(foreach (a(), a()[_i] != 0));
  gen(foreach (a(), a()[_i] * a()[_i - 1] % 6 == 0));
  gen(foreach (a(), _i != 0 || a()[_i] % 2 == 0));

  BOOST_REQUIRE(gen.next());
  for (unsigned i = 0; i < a.size(); i++) {
    BOOST_REQUIRE(-3 <= a[i] && a[i] <= 3);
    BOOST_REQUIRE(a[i] != 0);
    if (i == 0)
      BOOST_REQUIRE(a[i] % 2 == 0);
    else
      BOOST_REQUIRE(a[i] * a[i - 1] % 6 == 0);
  }
}

BOOST_AUTO_TEST_CASE(bool_rand_vec) {
  rand_vec<bool> a(NULL);
  placeholder _i;
  Generator gen;
  gen(a().size() == 10);
  gen(foreach (a(), a()[_i] != a()[_i - 1]));

  BOOST_REQUIRE(gen.next());
  BOOST_REQUIRE(a.size() == 10);
  for (unsigned i = 1; i < a.size(); i++) BOOST_REQUIRE(a[i] != a[i - 1]);
}

struct Item6 : public rand_obj {
  Item6() : v(this) { constraint(v().size() == default_rand_vec_size() + 7); }
  rand_vec<unsigned int> v;
};

BOOST_AUTO_TEST_CASE(only_size_constrained_vect_test) {
  Item6 it;
  BOOST_REQUIRE(it.next());
  BOOST_REQUIRE_EQUAL(it.v.size(), default_rand_vec_size() + 7);
}

BOOST_AUTO_TEST_SUITE_END()  // Context

//  vim: ft=cpp:ts=2:sw=2:expandtab


/*
 * Copyright 2009-2020 The VOTCA Development Team (http://www.votca.org)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */
#define BOOST_TEST_MAIN

#define BOOST_TEST_MODULE eigenio_matrixmarket

// Third party includes
#include <boost/test/unit_test.hpp>

// Local VOTCA includes
#include "votca/tools/eigenio_matrixmarket.h"
#include "votca/tools/votca_tools_config.h"
#include <iostream>

using namespace votca::tools;

BOOST_AUTO_TEST_SUITE(eigenio_matrixmarket)

BOOST_AUTO_TEST_CASE(readvector_test) {

  Eigen::VectorXd ref = Eigen::VectorXd::Zero(4);
  ref << 1.0, 2.0, 3.0, 4.0;
  std::cout << std::string(TOOLS_TEST_DATA_FOLDER) << std::endl;
  Eigen::VectorXd readin =
      EigenIO_MatrixMarket::ReadVector(std::string(TOOLS_TEST_DATA_FOLDER) +
                                       "/eigenio_matrixmarket/eigen_vector.mm");

  bool check = ref.isApprox(readin, 1e-5);

  if (!check) {
    std::cout << "ref" << std::endl;
    std::cout << ref.transpose() << std::endl;
    std::cout << "result" << std::endl;
    std::cout << readin.transpose() << std::endl;
  }
  BOOST_CHECK(check);
}

BOOST_AUTO_TEST_CASE(writevector_test) {

  Eigen::VectorXd test = Eigen::VectorXd::Random(5);
  EigenIO_MatrixMarket::WriteVector("VectorRandom.mm", test);
  Eigen::VectorXd readin = EigenIO_MatrixMarket::ReadVector("VectorRandom.mm");
  bool check = test.isApprox(readin, 1e-5);

  if (!check) {
    std::cout << "ref" << std::endl;
    std::cout << test.transpose() << std::endl;
    std::cout << "result" << std::endl;
    std::cout << readin.transpose() << std::endl;
  }
  BOOST_CHECK(check);
}

BOOST_AUTO_TEST_CASE(readmatrix_test) {

  Eigen::MatrixXd ref = Eigen::MatrixXd::Zero(4, 3);
  ref << 1.0, 5.0, 9.0, 2.0, 6.0, 10.0, 3.0, 7.0, 11.0, 4.0, 8.0, 12.0;

  Eigen::MatrixXd readin =
      EigenIO_MatrixMarket::ReadMatrix(std::string(TOOLS_TEST_DATA_FOLDER) +
                                       "/eigenio_matrixmarket/eigen_matrix.mm");

  bool check = ref.isApprox(readin, 1e-5);

  if (!check) {
    std::cout << "ref" << std::endl;
    std::cout << ref << std::endl;
    std::cout << "result" << std::endl;
    std::cout << readin << std::endl;
  }
  BOOST_CHECK(check);
}

BOOST_AUTO_TEST_CASE(writematrix_test) {

  Eigen::MatrixXd test = Eigen::MatrixXd::Random(4, 3);
  EigenIO_MatrixMarket::WriteMatrix("MatrixRandom.mm", test);
  Eigen::MatrixXd readin = EigenIO_MatrixMarket::ReadMatrix("MatrixRandom.mm");
  bool check = test.isApprox(readin, 1e-5);

  if (!check) {
    std::cout << "ref" << std::endl;
    std::cout << test << std::endl;
    std::cout << "result" << std::endl;
    std::cout << readin << std::endl;
  }
  BOOST_CHECK(check);
}

BOOST_AUTO_TEST_SUITE_END()


//
// Copyright (c) 2018-2019 CNRS INRIA
//

#include "pinocchio/codegen/cppadcg.hpp"

#include "pinocchio/multibody/model.hpp"
#include "pinocchio/parsers/sample-models.hpp"

#include <iostream>

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

  BOOST_AUTO_TEST_CASE(test_simple_cppadcg)
  {
    using namespace CppAD;
    using namespace CppAD::cg;

    typedef CG<double> CGD;
    typedef AD<CGD> ADCG;

    /***************************************************************************
     *                               the model
     **************************************************************************/

    // independent variable vector
    CppAD::vector<ADCG> x(2);
    x[0] = 2.;
    x[1] = 3.;
    Independent(x);

    // dependent variable vector
    CppAD::vector<ADCG> y(1);

    // the model
    ADCG a = x[0] / 1. + x[1] * x[1];
    y[0] = a / 2;

    ADFun<CGD> fun(x, y); // the model tape

    /***************************************************************************
     *                        Generate the C source code
     **************************************************************************/

    /**
     * start the special steps for source code generation for a Jacobian
     */
    CodeHandler<double> handler;

    CppAD::vector<CGD> indVars(2);
    handler.makeVariables(indVars);

    CppAD::vector<CGD> jac = fun.SparseJacobian(indVars);

    LanguageC<double> langC("double");
    LangCDefaultVariableNameGenerator<double> nameGen;

    std::ostringstream code;
    handler.generateCode(code, langC, jac, nameGen);
    std::cout << code.str();
  }

  BOOST_AUTO_TEST_CASE(test_eigen_support)
  {
    typedef CppAD::cg::CG<double> CGD;
    typedef CppAD::AD<CGD> ADCG;

    typedef Eigen::Matrix<ADCG,Eigen::Dynamic,1> ADCGVector;

    ADCGVector vec_zero(ADCGVector::Zero(100));
    BOOST_CHECK(vec_zero.isZero());

    ADCGVector vec_ones(10);
    vec_ones.fill((ADCG)1);
    BOOST_CHECK(vec_ones.isOnes());
//    std::cout << (ADCG)1 << std::endl;

    ADCG value_one(1.);

    ADCG value_nan;
    value_nan = NAN;

    // nan
    ADCGVector vec_nan(10);
    vec_nan.fill((ADCG)NAN);
////    BOOST_CHECK(vec_ones.allFinite());
//    std::cout << vec_nan.transpose() << std::endl;
//
//    // abs
    ADCG val_minus_one(-1.);
    ADCG val_abs(abs(val_minus_one));
    ADCGVector vec_minus_one(10); vec_minus_one.fill(val_minus_one);
    vec_minus_one.array().abs();

    BOOST_CHECK(!vec_ones.isUnitary());

  }

  BOOST_AUTO_TEST_CASE(test_cast)
  {
    typedef CppAD::cg::CG<double> CGScalar;
    typedef CppAD::AD<double> ADScalar;
    typedef CppAD::AD<float> ADFloat;
    typedef pinocchio::ModelTpl<CGScalar> CGModel;

    pinocchio::SE3 M(pinocchio::SE3::Random());
    typedef pinocchio::SE3Tpl<CGScalar> CGSE3;

    CGSE3 cg_M = M.cast<CGScalar>();
    BOOST_CHECK(cg_M.cast<double>().isApprox(M));

    pinocchio::SE3::Vector3 axis(1.,1.,1.);
    axis.normalize();
    BOOST_CHECK(axis.isUnitary());

    pinocchio::JointModelPrismaticUnaligned jmodel_prismatic(axis);
    typedef pinocchio::JointModelPrismaticUnalignedTpl<CGScalar> CGJointModelPrismaticUnaligned;

    CGScalar cg_value; cg_value = -1.;
    ADScalar ad_value; ad_value = -1.;
    ADFloat ad_float; ad_float = -1.;
    abs(ad_value);
    abs(ad_float);
    abs(cg_value);

    CPPAD_TESTVECTOR(ADScalar) ad_x(3);
    CGJointModelPrismaticUnaligned cg_jmodel_prismatic(axis.cast<CGScalar>());

    pinocchio::Model model;
    pinocchio::buildModels::humanoidRandom(model);

    CGModel cg_model = model.cast<CGScalar>();

    {
      CppAD::AD<double> ad_value(-1.);
      abs(ad_value); // works perfectly

      CppAD::cg::CG<double> cg_value(-1.);
      abs(cg_value); // does not compile because abs<double>(const CppAD::cg::CG<double>&) is defined in namespace CppAD and not CppAD::cg

    }
  }

  BOOST_AUTO_TEST_CASE(test_dynamic_link)
  {
    using namespace CppAD;
    using namespace CppAD::cg;

    // use a special object for source code generation
    typedef CG<double> CGD;
    typedef AD<CGD> ADCG;

    typedef AD<double> ADScalar;

    /***************************************************************************
     *                               the model
     **************************************************************************/

    // independent variable vector
    std::vector<ADCG> x(2);
    Independent(x);

    // dependent variable vector
    std::vector<ADCG> y(1);

    // the model equation
    ADCG a = x[0] / 1. + x[1] * x[1];
    y[0] = a / 2;

    ADFun<CGD> fun(x, y);

    /***************************************************************************
     *                       Create the dynamic library
     *                  (generates and compiles source code)
     **************************************************************************/
    // generates source code
    ModelCSourceGen<double> cgen(fun, "model");
    cgen.setCreateJacobian(true);
    cgen.setCreateForwardOne(true);
    cgen.setCreateReverseOne(true);
    cgen.setCreateReverseTwo(true);
    ModelLibraryCSourceGen<double> libcgen(cgen);

    // compile source code
    DynamicModelLibraryProcessor<double> p(libcgen);

    GccCompiler<double> compiler;
    std::unique_ptr<DynamicLib<double>> dynamicLib = p.createDynamicLibrary(compiler);

    // save to files (not really required)
    SaveFilesModelLibraryProcessor<double> p2(libcgen);
    p2.saveSources();

    /***************************************************************************
     *                       Use the dynamic library
     **************************************************************************/

    std::unique_ptr<GenericModel<double>> model = dynamicLib->model("model");
    CPPAD_TESTVECTOR(double) xv(x.size()); xv[0] = 2.5; xv[1] = 3.5;
    CPPAD_TESTVECTOR(double) jac = model->Jacobian(xv);

    std::vector<ADScalar> x_ad(2);
    Independent(x_ad);

    // dependent variable vector
    std::vector<ADScalar> y_ad(1);

    // the model equation
    ADScalar a_ad = x_ad[0] / 1. + x_ad[1] * x_ad[1];
    y_ad[0] = a_ad / 2;

    ADFun<double> ad_fun(x_ad, y_ad);

    CPPAD_TESTVECTOR(double) jac_ref = ad_fun.Jacobian(xv);

    // print out the result
    std::cout << jac[0] << " " << jac[1] << std::endl;

    BOOST_CHECK(Eigen::Map<Eigen::Vector2d>(jac.data()).isApprox(Eigen::Map<Eigen::Vector2d>(jac_ref.data())));
  }

BOOST_AUTO_TEST_SUITE_END()


/****************************************************************************************
** Implementation of a variation of BASS Features, which has features to encode the
**  relative position between tiles.
**
** REMARKS: - This implementation is basically Erik Talvitie's implementation, presented
**            in the AAAI'15 LGCVG Workshop.
**
** Author: Marlos C. Machado
***************************************************************************************/

#ifndef BPRO_FEATURES_H
#define BPRO_FEATURES_H
#include "BPROFeatures.hpp"
#endif
#ifndef BASIC_FEATURES_H
#define BASIC_FEATURES_H
#include "BasicFeatures.hpp"
#endif

#include <set>
#include <assert.h>
using namespace std;
//#include <tuple>
//#include <boost/tuple/tuple.hpp> //TODO: I have to remove this to not have to depend on boost

BPROFeatures::BPROFeatures(Parameters *param){
    this->param = param;
    numColumns  = param->getNumColumns();
	numRows     = param->getNumRows();
	numColors   = param->getNumColors();

	if(this->param->getSubtractBackground()){
        this->background = new Background(param);
    }

	//To get the total number of features:
	//TODO: Fix this!
    numBasicFeatures = this->param->getNumColumns() * this->param->getNumRows() * this->param->getNumColors();
	numRelativeFeatures = (2 * this->param->getNumColumns() - 1) * (2 * this->param->getNumRows() - 1)
							* (1+this->param->getNumColors()) * this->param->getNumColors()/2;
    changed.clear();
    bproExistence.resize(2*numRows-1);
    for (int i=0;i<2*numRows-1;i++){
        bproExistence[i].resize(2*numColumns-1);
        for (int j=0;j<2*numColumns-1;j++){
            bproExistence[i][j]=true;
        }
    }
}

BPROFeatures::~BPROFeatures(){}

int BPROFeatures::getBasicFeaturesIndices(const ALEScreen &screen, int blockWidth, int blockHeight,
                                          vector<vector<tuple<int,int> > > &whichColors, vector<int>& features){
	int featureIndex = 0;
	// For each pixel block
	for (int by = 0; by < numRows; by++) {
		for (int bx = 0; bx < numColumns; bx++) {
			//vector<boost::tuple<int, int, int> > posAndColor;

			int xo = bx * blockWidth;
			int yo = by * blockHeight;
			vector<bool> hasColor(numColors, false);

			// Determine which colors are present
			for (int x = xo; x < xo + blockWidth; x++){
				for (int y = yo; y < yo + blockHeight; y++){
					unsigned char pixel = screen.get(y,x);

					if(!this->param->getSubtractBackground() || (this->background->getPixel(y, x) != pixel)){
						if(numColors == 8){ //SECAM, considering only 8 colors
							pixel = (pixel & 0xF) >> 1;
						}
						else if(numColors == 128){ //NTSC, considering 128 colors
							pixel = pixel >> 1;
						}

		  				hasColor[pixel] = true;
						//posAndColor.push_back(boost::make_tuple(x, y, pixel));
					}
				}
			}

			for(int c = 0; c < numColors; c++){
				if(hasColor[c]){
                    tuple<int,int> pos (by,bx);
					whichColors[c].push_back(pos);
					features.push_back(featureIndex);
				}
				featureIndex++;
			}
		}
	}
	return featureIndex;
}

void BPROFeatures::addRelativeFeaturesIndices(const ALEScreen &screen, int featureIndex,
	vector<vector<tuple<int,int> > > &whichColors, vector<int>& features){

	int numRowOffsets = 2*numRows - 1;
	int numColumnOffsets = 2*numColumns - 1;
	int numOffsets = numRowOffsets*numColumnOffsets;
	int numColorPairs = (1+numColors)*numColors/2;

    for (int c1=0;c1<numColors;c1++){
        for (int k=0;k<whichColors[c1].size();k++){
            for (int h=0;h<whichColors[c1].size();h++){
                int rowDelta = get<0>(whichColors[c1][k])-get<0>(whichColors[c1][h]);
                int columnDelta = get<1>(whichColors[c1][k])-get<1>(whichColors[c1][h]);
                bool newBproFeature = false;
                if (rowDelta>0){
                    newBproFeature = true;
                }else if (rowDelta==0 && columnDelta >=0){
                    newBproFeature = true;
                }
                rowDelta+=numRows-1;
                columnDelta+=numColumns-1;
                if (newBproFeature && bproExistence[rowDelta][columnDelta]){
                    tuple<int,int> pos (rowDelta,columnDelta);
                    changed.push_back(pos);
                    bproExistence[rowDelta][columnDelta]=false;
                    features.push_back(numBasicFeatures+(numColors+numColors-c1+1)*c1/2*numRowOffsets*numColumnOffsets+rowDelta*numColumnOffsets+columnDelta);

                }
            }
        }
        resetBproExistence(bproExistence,changed);

        for (int c2=c1+1;c2<numColors;c2++){
            if (whichColors[c1].size()>0 && whichColors[c2].size()>0){
                for (vector<tuple<int,int> >::iterator it1=whichColors[c1].begin();it1!=whichColors[c1].end();it1++){
                    for (vector<tuple<int,int> >::iterator it2=whichColors[c2].begin();it2!=whichColors[c2].end();it2++){
                        int rowDelta = get<0>(*it1)-get<0>(*it2)+numRows-1;
                        int columnDelta = get<1>(*it1)-get<1>(*it2)+numColumns-1;
                        if (bproExistence[rowDelta][columnDelta]){
                            tuple<int,int> pos(rowDelta,columnDelta);
                            changed.push_back(pos);
                            bproExistence[rowDelta][columnDelta]=false;
                            features.push_back(numBasicFeatures+(numColors+numColors-c1+1)*c1/2*numRowOffsets*numColumnOffsets+(c2-c1)*numRowOffsets*numColumnOffsets+rowDelta*numColumnOffsets+columnDelta);
                        }
                    }
                }
            }
            resetBproExistence(bproExistence,changed);
        }
    }
}

void BPROFeatures::getActiveFeaturesIndices(const ALEScreen &screen, const ALERAM &ram, vector<int>& features){
	int screenWidth = screen.width();
	int screenHeight = screen.height();
	int blockWidth = screenWidth / numColumns;
	int blockHeight = screenHeight / numRows;

	assert(features.size() == 0); //If the vector is not empty this can be a mess
    vector<vector<tuple<int,int> > > whichColors(numColors);

    //Before generating features we must check whether we can subtract the background:
    if(this->param->getSubtractBackground()){
        unsigned int sizeBackground = this->background->getWidth() * this->background->getHeight();
        assert(sizeBackground == screen.width()*screen.height());
    }

    //We first get the Basic features, keeping track of the next featureIndex vector:
    //We don't just use the Basic implementation because we need the whichColors information
	int featureIndex = getBasicFeaturesIndices(screen, blockWidth, blockHeight, whichColors, features);
	addRelativeFeaturesIndices(screen, featureIndex, whichColors, features);

	//Bias
	features.push_back(numBasicFeatures+numRelativeFeatures);
}

int BPROFeatures::getNumberOfFeatures(){
    return numBasicFeatures + numRelativeFeatures + 1;
}

void BPROFeatures::resetBproExistence(vector<vector<bool> >& bproExistence, vector<tuple<int,int> >& changed){
    for (vector<tuple<int,int> >::iterator it = changed.begin();it!=changed.end();it++){
        bproExistence[get<0>(*it)][get<1>(*it)]=true;
    }
    changed.clear();
}


// Copyright (C) 2013  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include "opaque_types.h"
#include <dlib/python.h>
#include <dlib/matrix.h>
#include <dlib/geometry.h>
#include <dlib/image_transforms.h>
#include <pybind11/stl_bind.h>
#include "indexing.h"

using namespace dlib;
using namespace std;

typedef matrix<double,0,1> cv;

void cv_set_size(cv& m, long s)
{
    m.set_size(s);
    m = 0;
}

double dotprod ( const cv& a, const cv& b)
{
    return dot(a,b);
}

string cv__str__(const cv& v)
{
    ostringstream sout;
    for (long i = 0; i < v.size(); ++i)
    {
        sout << v(i);
        if (i+1 < v.size())
            sout << "\n";
    }
    return sout.str();
}

string cv__repr__ (const cv& v)
{
    std::ostringstream sout;
    sout << "dlib.vector([";
    for (long i = 0; i < v.size(); ++i)
    {
        sout << v(i);
        if (i+1 < v.size())
            sout << ", ";
    }
    sout << "])";
    return sout.str();
}

std::shared_ptr<cv> cv_from_object(py::object obj)
{
    try {
        long nr = obj.cast<long>();
        auto temp = std::make_shared<cv>(nr);
        *temp = 0;
        return temp;
    } catch(py::cast_error&) {
        py::list li = obj.cast<py::list>();
        const long nr = len(obj);
        auto temp = std::make_shared<cv>(nr);
        for ( long r = 0; r < nr; ++r)
        {
            (*temp)(r) = li[r].cast<double>();
        }
        return temp;
    }
}

long cv__len__(cv& c)
{
    return c.size();
}

void cv__setitem__(cv& c, long p, double val)
{
    if (p < 0) {
        p = c.size() + p; // negative index
    }
    if (p > c.size()-1) {
        PyErr_SetString( PyExc_IndexError, "index out of range"
        );
        throw py::error_already_set();
    }
    c(p) = val;
}

double cv__getitem__(cv& m, long r)
{
    if (r < 0) {
        r = m.size() + r; // negative index
    }
    if (r > m.size()-1 || r < 0) {
        PyErr_SetString( PyExc_IndexError, "index out of range"
        );
        throw py::error_already_set();
    }
    return m(r);
}

cv cv__getitem2__(cv& m, py::slice r)
{
    size_t start, stop, step, slicelength;
    if (!r.compute(m.size(), &start, &stop, &step, &slicelength))
        throw py::error_already_set();

    cv temp(slicelength);

    for (size_t i = 0; i < slicelength; ++i) {
         temp(i) = m(start); start += step;
    }
    return temp;
}

py::tuple cv_get_matrix_size(cv& m)
{
    return py::make_tuple(m.nr(), m.nc());
}

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

string point_transform_projective__repr__ (const point_transform_projective& tform)
{
    std::ostringstream sout;
    sout << "point_transform_projective(\n" << csv << tform.get_m() << ")";
    return sout.str();
}

string point_transform_projective__str__(const point_transform_projective& tform)
{
    std::ostringstream sout;
    sout << "(" << csv << tform.get_m() << ")";
    return sout.str();
}

point_transform_projective init_point_transform_projective (
    const numpy_image<double>& m_
)
{
    const_image_view<numpy_image<double>> m(m_);
    DLIB_CASSERT(m.nr() == 3 && m.nc() == 3,
        "The matrix used to construct a point_transform_projective object must be 3x3.");

    return point_transform_projective(mat(m));
}

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

string point__repr__ (const point& p)
{
    std::ostringstream sout;
    sout << "point(" << p.x() << ", " << p.y() << ")";
    return sout.str();
}

string point__str__(const point& p)
{
    std::ostringstream sout;
    sout << "(" << p.x() << ", " << p.y() << ")";
    return sout.str();
}

string dpoint__repr__ (const dpoint& p)
{
    std::ostringstream sout;
    sout << "dpoint(" << p.x() << ", " << p.y() << ")";
    return sout.str();
}

string dpoint__str__(const dpoint& p)
{
    std::ostringstream sout;
    sout << "(" << p.x() << ", " << p.y() << ")";
    return sout.str();
}

long point_x(const point& p) { return p.x(); }
long point_y(const point& p) { return p.y(); }
double dpoint_x(const dpoint& p) { return p.x(); }
double dpoint_y(const dpoint& p) { return p.y(); }

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

template <typename T>
dlib::vector<T,2> numpy_to_dlib_vect (
    const py::array_t<T>& v
)
/*!
    ensures
        - converts a numpy array with 2 elements into a dlib::vector<T,2>
!*/
{
    DLIB_CASSERT(v.size() == 2, "You can only convert a numpy array to a dlib point or dpoint if it has just 2 elements.");
    DLIB_CASSERT(v.ndim() == 1 || v.ndim() == 2, "The input needs to be interpretable as a row or column vector.");
    dpoint temp;
    if (v.ndim() == 1)
    {
        temp.x() = v.at(0);
        temp.y() = v.at(1);
    }
    else if (v.shape(0) == 2)
    {
        temp.x() = v.at(0,0);
        temp.y() = v.at(1,0);
    }
    else
    {
        temp.x() = v.at(0,0);
        temp.y() = v.at(0,1);
    }
    return temp;
}

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

point_transform_projective py_find_projective_transform (
    const std::vector<dpoint>& from_points,
    const std::vector<dpoint>& to_points
)
{
    DLIB_CASSERT(from_points.size() == to_points.size(),
        "from_points and to_points must have the same number of points.");
    DLIB_CASSERT(from_points.size() >= 4,
        "You need at least 4 points to find a projective transform.");
    return find_projective_transform(from_points, to_points);
}

template <typename T>
point_transform_projective py_find_projective_transform2 (
    const numpy_image<T>& from_points_,
    const numpy_image<T>& to_points_
)
{
    const_image_view<numpy_image<T>> from_points(from_points_);
    const_image_view<numpy_image<T>> to_points(to_points_);

    DLIB_CASSERT(from_points.nc() == 2 && to_points.nc() == 2,
        "Both from_points and to_points must be arrays with 2 columns.");
    DLIB_CASSERT(from_points.nr() == to_points.nr(),
        "from_points and to_points must have the same number of rows.");
    DLIB_CASSERT(from_points.nr() >= 4,
        "You need at least 4 rows in the input matrices to find a projective transform.");

    std::vector<dpoint> from, to;
    for (long r = 0; r < from_points.nr(); ++r)
    {
        from.push_back(dpoint(from_points[r][0], from_points[r][1]));
        to.push_back(dpoint(to_points[r][0], to_points[r][1]));
    }

    return find_projective_transform(from, to);
}

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

void register_point_transform_projective(
    py::module& m
)
{

    py::class_<point_transform_projective>(m, "point_transform_projective",
        "This is an object that takes 2D points and applies a projective transformation to them.")
            .def(py::init<>(),
"ensures \n\
    - This object will perform the identity transform.  That is, given a point \n\
      as input it will return the same point as output.  Therefore, self.m == a 3x3 identity matrix."
        /*!
            ensures
                - This object will perform the identity transform.  That is, given a point
                  as input it will return the same point as output.  Therefore, self.m == a 3x3 identity matrix.
        !*/
                )
            .def(py::init<>(&init_point_transform_projective), py::arg("m"),
"ensures \n\
    - self.m == m"
                )
            .def("__repr__", &point_transform_projective__repr__)
            .def("__str__", &point_transform_projective__str__)
            .def("__call__", [](const point_transform_projective& tform, const dpoint& p){return tform(p);}, py::arg("p"),
"ensures \n\
    - Applies the projective transformation defined by this object's constructor \n\
      to p and returns the result.  To define this precisely: \n\
        - let p_h == the point p in homogeneous coordinates.  That is: \n\
            - p_h.x == p.x \n\
            - p_h.y == p.y \n\
            - p_h.z == 1  \n\
        - let x == m*p_h  \n\
        - Then this function returns the value x/x.z"
        /*!
            ensures
                - Applies the projective transformation defined by this object's constructor
                  to p and returns the result.  To define this precisely:
                    - let p_h == the point p in homogeneous coordinates.  That is:
                        - p_h.x == p.x
                        - p_h.y == p.y
                        - p_h.z == 1
                    - let x == m*p_h
                    - Then this function returns the value x/x.z
        !*/
                )
            .def_property_readonly("m", [](const point_transform_projective& tform){numpy_image<double> tmp; assign_image(tmp,tform.get_m()); return tmp;},
                "m is the 3x3 matrix that defines the projective transformation.")
            .def(py::pickle(&getstate<point_transform_projective>, &setstate<point_transform_projective>));

    m.def("inv", [](const point_transform_projective& tform){return inv(tform); }, py::arg("trans"),
"ensures \n\
    - If trans is an invertible transformation then this function returns a new \n\
      transformation that is the inverse of trans. "
    /*!
        ensures
            - If trans is an invertible transformation then this function returns a new
              transformation that is the inverse of trans.
    !*/
        );

    m.def("find_projective_transform", &py_find_projective_transform, py::arg("from_points"), py::arg("to_points"),
"requires \n\
    - len(from_points) == len(to_points) \n\
    - len(from_points) >= 4 \n\
ensures \n\
    - returns a point_transform_projective object, T, such that for all valid i: \n\
        length(T(from_points[i]) - to_points[i]) \n\
      is minimized as often as possible.  That is, this function finds the projective \n\
      transform that maps points in from_points to points in to_points.  If no \n\
      projective transform exists which performs this mapping exactly then the one \n\
      which minimizes the mean squared error is selected. "
    /*!
        requires
            - len(from_points) == len(to_points)
            - len(from_points) >= 4
        ensures
            - returns a point_transform_projective object, T, such that for all valid i:
                length(T(from_points[i]) - to_points[i])
              is minimized as often as possible.  That is, this function finds the projective
              transform that maps points in from_points to points in to_points.  If no
              projective transform exists which performs this mapping exactly then the one
              which minimizes the mean squared error is selected.
    !*/
        );

    const char* docs =
"requires \n\
    - from_points and to_points have two columns and the same number of rows. \n\
      Moreover, they have at least 4 rows. \n\
ensures \n\
    - returns a point_transform_projective object, T, such that for all valid i: \n\
        length(T(dpoint(from_points[i])) - dpoint(to_points[i])) \n\
      is minimized as often as possible.  That is, this function finds the projective \n\
      transform that maps points in from_points to points in to_points.  If no \n\
      projective transform exists which performs this mapping exactly then the one \n\
      which minimizes the mean squared error is selected. ";
    /*!
        requires
            - from_points and to_points have two columns and the same number of rows.
              Moreover, they have at least 4 rows.
        ensures
            - returns a point_transform_projective object, T, such that for all valid i:
                length(T(dpoint(from_points[i])) - dpoint(to_points[i]))
              is minimized as often as possible.  That is, this function finds the projective
              transform that maps points in from_points to points in to_points.  If no
              projective transform exists which performs this mapping exactly then the one
              which minimizes the mean squared error is selected.
    !*/
    m.def("find_projective_transform", &py_find_projective_transform2<float>, py::arg("from_points"), py::arg("to_points"), docs);
    m.def("find_projective_transform", &py_find_projective_transform2<double>, py::arg("from_points"), py::arg("to_points"), docs);

}

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

double py_polygon_area(
    const std::vector<dpoint>& pts
)
{
    return polygon_area(pts);
}

double py_polygon_area2(
    const py::list& pts
)
{
    std::vector<dpoint> temp(len(pts));
    for (size_t i = 0; i < temp.size(); ++i)
        temp[i] = pts[i].cast<dpoint>();

    return polygon_area(temp);
}

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

void bind_vector(py::module& m)
{
    {
    py::class_<cv, std::shared_ptr<cv>>(m, "vector", "This object represents the mathematical idea of a column vector.")
        .def(py::init())
        .def("set_size", &cv_set_size)
        .def("resize", &cv_set_size)
        .def(py::init(&cv_from_object))
        .def("__repr__", &cv__repr__)
        .def("__str__", &cv__str__)
        .def("__len__", &cv__len__)
        .def("__getitem__", &cv__getitem__)
        .def("__getitem__", &cv__getitem2__)
        .def("__setitem__", &cv__setitem__)
        .def_property_readonly("shape", &cv_get_matrix_size)
        .def(py::pickle(&getstate<cv>, &setstate<cv>));

    m.def("dot", &dotprod, "Compute the dot product between two dense column vectors.");
    }
    {
    typedef point type;
    py::class_<type>(m, "point", "This object represents a single point of integer coordinates that maps directly to a dlib::point.")
            .def(py::init<long,long>(), py::arg("x"), py::arg("y"))
            .def(py::init<dpoint>(), py::arg("p"))
            .def(py::init<>(&numpy_to_dlib_vect<long>), py::arg("v"))
            .def(py::init<>(&numpy_to_dlib_vect<float>), py::arg("v"))
            .def(py::init<>(&numpy_to_dlib_vect<double>), py::arg("v"))
            .def("__repr__", &point__repr__)
            .def("__str__", &point__str__)
            .def(py::self + py::self)
            .def(py::self - py::self)
            .def(py::self / double())
            .def(py::self * double())
            .def(double() * py::self)
            .def("normalize", &type::normalize, "Returns a unit normalized copy of this vector.")
            .def_property("x", &point_x, [](point& p, long x){p.x()=x;}, "The x-coordinate of the point.")
            .def_property("y", &point_y, [](point& p, long y){p.y()=y;}, "The y-coordinate of the point.")
            .def(py::pickle(&getstate<type>, &setstate<type>));
    }
    {
    typedef std::vector<point> type;
    py::bind_vector<type>(m, "points", "An array of point objects.")
        .def(py::init<size_t>(), py::arg("initial_size"))
        .def("clear", &type::clear)
        .def("resize", resize<type>)
        .def("extend", extend_vector_with_python_list<point>)
        .def(py::pickle(&getstate<type>, &setstate<type>));
    }

    {
    typedef dpoint type;
    py::class_<type>(m, "dpoint", "This object represents a single point of floating point coordinates that maps directly to a dlib::dpoint.")
            .def(py::init<double,double>(), py::arg("x"), py::arg("y"))
            .def(py::init<point>(), py::arg("p"))
            .def(py::init<>(&numpy_to_dlib_vect<long>), py::arg("v"))
            .def(py::init<>(&numpy_to_dlib_vect<float>), py::arg("v"))
            .def(py::init<>(&numpy_to_dlib_vect<double>), py::arg("v"))
            .def("__repr__", &dpoint__repr__)
            .def("__str__", &dpoint__str__)
            .def("normalize", &type::normalize, "Returns a unit normalized copy of this vector.")
            .def_property("x", &dpoint_x, [](dpoint& p, double x){p.x()=x;}, "The x-coordinate of the dpoint.")
            .def_property("y", &dpoint_y, [](dpoint& p, double y){p.y()=y;}, "The y-coordinate of the dpoint.")
            .def(py::self + py::self)
            .def(py::self - py::self)
            .def(py::self / double())
            .def(py::self * double())
            .def(double() * py::self)
            .def(py::pickle(&getstate<type>, &setstate<type>));
    }
    {
    typedef std::vector<dpoint> type;
    py::bind_vector<type>(m, "dpoints", "An array of dpoint objects.")
        .def(py::init<size_t>(), py::arg("initial_size"))
        .def("clear", &type::clear)
        .def("resize", resize<type>)
        .def("extend", extend_vector_with_python_list<dpoint>)
        .def(py::pickle(&getstate<type>, &setstate<type>));
    }

    m.def("length", [](const point& p){return length(p); },
        "returns the distance from p to the origin, i.e. the L2 norm of p.", py::arg("p"));
    m.def("length", [](const dpoint& p){return length(p); },
        "returns the distance from p to the origin, i.e. the L2 norm of p.", py::arg("p"));

    m.def("dot", [](const point& a, const point& b){return dot(a,b); },  "Returns the dot product of the points a and b.", py::arg("a"), py::arg("b"));
    m.def("dot", [](const dpoint& a, const dpoint& b){return dot(a,b); },  "Returns the dot product of the points a and b.", py::arg("a"), py::arg("b"));

    register_point_transform_projective(m);

    m.def("polygon_area", &py_polygon_area, py::arg("pts"));
    m.def("polygon_area", &py_polygon_area2, py::arg("pts"),
"ensures \n\
    - If you walk the points pts in order to make a closed polygon, what is its \n\
      area?  This function returns that area.  It uses the shoelace formula to \n\
      compute the result and so works for general non-self-intersecting polygons."
    /*!
        ensures
            - If you walk the points pts in order to make a closed polygon, what is its
              area?  This function returns that area.  It uses the shoelace formula to
              compute the result and so works for general non-self-intersecting polygons.
    !*/
        );

}


#ifndef ST_LAYER_HPP_
#define ST_LAYER_HPP_

#include <boost/shared_ptr.hpp>
#include <gflags/gflags.h>
#include <glog/logging.h>

#include <cmath>

#include "caffe/blob.hpp"
#include "caffe/common.hpp"
#include "caffe/layer.hpp"
#include "caffe/proto/caffe.pb.h"

namespace caffe {

template <typename Dtype>
class SpatialTransformerLayer : public Layer<Dtype> {

public:
	explicit SpatialTransformerLayer(const LayerParameter& param)
      : Layer<Dtype>(param) {
	      to_compute_dU_ = false;
	      global_debug = false;
	      pre_defined_count = 0;
      }
	virtual void LayerSetUp(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);
	virtual void Reshape(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);

	virtual inline const char* type() const { return "SpatialTransformer"; }
	virtual inline int ExactNumBottomBlobs() const { return 2; }
	virtual inline int ExactNumTopBlobs() const { return 1; }

protected:
	virtual void Forward_cpu(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);
	virtual void Forward_gpu(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);
	virtual void Backward_cpu(const vector<Blob<Dtype>*>& top,
      const vector<bool>& propagate_down, const vector<Blob<Dtype>*>& bottom);
	virtual void Backward_gpu(const vector<Blob<Dtype>*>& top,
      const vector<bool>& propagate_down, const vector<Blob<Dtype>*>& bottom);

private:
	inline Dtype abs(Dtype x) {
		if(x < 0) return -x; return x;
	}
	inline Dtype max(Dtype x, Dtype y) {
		if(x < y) return y; return x;
	}

	Dtype transform_forward_cpu(const Dtype* pic, Dtype px, Dtype py);
	void transform_backward_cpu(Dtype dV, const Dtype* U, const Dtype px,
			const Dtype py, Dtype* dU, Dtype& dpx, Dtype& dpy);

	string transform_type_;
	string sampler_type_;

	int output_H_;
	int output_W_;

	int N, C, H, W;

	bool global_debug;
	bool to_compute_dU_;

	Blob<Dtype> dTheta_tmp;	// used for back propagation part in GPU implementation
	Blob<Dtype> all_ones_2;	// used for back propagation part in GPU implementation

	Blob<Dtype> full_theta;	// used for storing data and diff for full six-dim theta
	Dtype pre_defined_theta[6];
	bool is_pre_defined_theta[6];
	int pre_defined_count;

	Blob<Dtype> output_grid;	// standard output coordinate system, [0, 1) by [0, 1).
	Blob<Dtype> input_grid;	// corresponding coordinate on input image after projection for each output pixel.
};

}  // namespace caffe

#endif  // CAFFE_COMMON_HPP_


//
// Copyright 2019 Mateusz Loskot <mateusz at loskot dot net>
// Copyright 2021 Pranam Lashkari <plashkari628@gmail.com>
//
// 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
//
#include <boost/gil.hpp>
#include <boost/gil/image_processing/kernel.hpp>
#include <boost/assert.hpp>

#include <initializer_list>
#include <type_traits>

namespace boost { namespace gil {

namespace test { namespace fixture {

template <typename T>
auto create_kernel(std::initializer_list<T> const& values)
    -> gil::kernel_1d<T>
{
    static_assert(std::is_arithmetic<T>::value,
        "kernel value type should be integral or floating-point type");
    BOOST_ASSERT_MSG((values.size() - 1) % 2 == 0, "expected odd number of kernel values");
    gil::kernel_1d<T> kernel(values.begin(), values.size(), (values.size() - 1) / 2);
    return kernel;
}

}}}} // namespace boost::gil::test::fixture


#include <sstream>

#include "catch.hpp"
#include "../libnfporb/libnfp.hpp"
#include "../Nester/Nester.hpp"
#include <boost/geometry/io/wkt/write.hpp>

using namespace nester;
using namespace libnfp;
using namespace std;

namespace NesterTests
{
	string str(point_t p) {
		stringstream s;
		s << std::setprecision(1) << std::fixed << bg::wkt(p);

		return s.str();
	}

	void writeSVG(point_t p, point_t r) {
		string name = Catch::getResultCapture().getCurrentTestName();
		std::ofstream svg(name+".svg");

		polygon_t x_axis{
			{ { -10.0, 0.0 },{ 10.0, 0.0 } },
			{ }
		};

		polygon_t y_axis{
			{ { 0.0, -10.0 },{ 0.0, 10.0 } }
		};

		boost::geometry::svg_mapper<point_t> mapper(svg, 400, 400);
		mapper.add(x_axis);
		mapper.add(y_axis);
		mapper.add(p);
		mapper.add(r);

		mapper.map(p, "fill-opacity:0.5;fill:rgb(153,204,0);stroke:rgb(153,204,0);stroke-width:2");
		mapper.map(r, "opacity:0.8;fill:none;stroke:rgb(255,128,0);stroke-width:4;");
		mapper.map(x_axis, "stroke:rgb(0,0,0);stroke-width:1");
		mapper.map(y_axis, "stroke:rgb(0,0,0);stroke-width:1");
	}

	TEST_CASE("make_transformation", "[transformation]") {
		point_t p(2.0, 3.0);
		point_t r;

		SECTION("shift up") {
			transformer_t transformer = makeTransformation(0.0, 0.0, 4.0);
			transformer.apply(p, r);
            REQUIRE(str(r) == "POINT(2.0 7.0)");
		}

		SECTION("shift right") {
			transformer_t transformer = makeTransformation( 0.0, 4.0, 0.0);
			transformer.apply(p, r);
            REQUIRE(str(r) == "POINT(6.0 3.0)");
		}

		SECTION("shift right and up") {
			transformer_t transformer = makeTransformation(0.0, 5.0, 7.0);
			transformer.apply(p, r);
            REQUIRE(str(r) == "POINT(7.0 10.0)");
		}

		SECTION("rotate 90deg CCW") {
			transformer_t transformer = makeTransformation(-90.0, 0.0, 0.0);
			transformer.apply(p, r);
            REQUIRE(str(r) == "POINT(-3.0 2.0)");
		}

        SECTION("rotate 90deg CCW then shift up") {
			transformer_t transformer = makeTransformation(-90.0, 0.0, 1.0);
			transformer.apply(p, r);
			REQUIRE(str(r) == "POINT(-3.0 3.0)");
		}

        SECTION("rotate 90deg CCW then shift right") {
			transformer_t transformer = makeTransformation(-90.0, 10.0, 0.0);
			transformer.apply(p, r);
            REQUIRE(str(r) == "POINT(7.0 2.0)");
		}

        SECTION("rotate 90deg CCW then shift to origo") {
			transformer_t transformer = makeTransformation(-90.0, 3.0, -2.0);
			transformer.apply(p, r);
            REQUIRE(str(r) == "POINT(0.0 0.0)");
		}

		SECTION("rotate 90deg CW") {
			transformer_t transformer = makeTransformation(90.0, 0.0, 0.0);
			transformer.apply(p, r);
			REQUIRE(str(r) == "POINT(3.0 -2.0)");
		}

		SECTION("rotate 90deg CW then shift to origo") {
			transformer_t transformer = makeTransformation(90.0, -3.0, 2.0);
			transformer.apply(p, r);
			REQUIRE(str(r) == "POINT(0.0 0.0)");
		}
	}
}


/*
 * Copyright 2018,
 * Mirabel Joseph
 *
 * CNRS/AIST
 *
 */

#ifndef SOT_CORE_VARIADICOP_HH
#define SOT_CORE_VARIADICOP_HH

/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */

/* Matrix */
#include <dynamic-graph/linear-algebra.h>

/* SOT */
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>
#include <sot/core/flags.hh>
#include <sot/core/matrix-geometry.hh>
#include <sot/core/pool.hh>

/* STD */
#include <string>

#include <boost/function.hpp>

namespace dynamicgraph {
namespace sot {

/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */

template <typename Tin, typename Tout, typename Time>
class VariadicAbstract : public Entity {
public: /* --- CONSTRUCTION --- */
  static std::string getTypeInName(void);
  static std::string getTypeOutName(void);

  VariadicAbstract(const std::string &name, const std::string &className)
      : Entity(name), SOUT(className + "(" + name + ")::output(" +
                           getTypeOutName() + ")::sout"),
        baseSigname(className + "(" + name + ")::input(" + getTypeInName() +
                    ")::") {
    signalRegistration(SOUT);
  }

  virtual ~VariadicAbstract(void) {
    for (std::size_t i = 0; i < signalsIN.size(); ++i) {
      _removeSignal(i);
    }
  };

public: /* --- SIGNAL --- */
  typedef SignalPtr<Tin, int> signal_t;
  SignalTimeDependent<Tout, int> SOUT;

  std::size_t addSignal() {
    std::ostringstream oss;
    oss << "sin" << signalsIN.size();
    return addSignal(oss.str());
  }

  std::size_t addSignal(const std::string &name) {
    signal_t *sig = new signal_t(NULL, baseSigname + name);
    try {
      _declareSignal(sig);
      signalsIN.push_back(sig);
      // names.push_back (name);
      return signalsIN.size() - 1;
    } catch (const ExceptionAbstract &) {
      delete sig;
      throw;
    }
  }

  void removeSignal() {
    assert(signalsIN.size() > 0);
    _removeSignal(signalsIN.size() - 1);
    // names.pop_back();
    signalsIN.pop_back();
  }

  void setSignalNumber(const int &n) {
    assert(n >= 0);
    const std::size_t oldSize = signalsIN.size();
    for (std::size_t i = n; i < oldSize; ++i)
      _removeSignal(i);
    signalsIN.resize(n, NULL);
    // names.resize(n);

    for (std::size_t i = oldSize; i < (std::size_t)n; ++i) {
      assert(signalsIN[i] == NULL);
      std::ostringstream oss;
      oss << baseSigname << "sin" << i;
      // names[i] = oss.str();
      // signal_t* s = new signal_t (NULL,names[i]);
      signal_t *s = new signal_t(NULL, oss.str());
      signalsIN[i] = s;
      _declareSignal(s);
    }
    updateSignalNumber(n);
  }

  int getSignalNumber() const { return (int)signalsIN.size(); }

  signal_t *getSignalIn(int i) {
    if (i < 0 || i >= (int)signalsIN.size())
      throw std::out_of_range("Wrong signal index");
    return signalsIN[i];
  }

protected:
  std::vector<signal_t *> signalsIN;
  // Use signal->shortName instead
  // std::vector< std::string > names;

  virtual void updateSignalNumber(int n) { (void)n; };

private:
  void _removeSignal(const std::size_t i) {
    // signalDeregistration(names[i]);
    signalDeregistration(signalsIN[i]->shortName());
    SOUT.removeDependency(*signalsIN[i]);
    delete signalsIN[i];
  }
  void _declareSignal(signal_t *s) {
    signalRegistration(*s);
    SOUT.addDependency(*s);
  }
  const std::string baseSigname;
};

template <typename Operator>
class VariadicOp : public VariadicAbstract<typename Operator::Tin,
                                           typename Operator::Tout, int> {
  typedef typename Operator::Tin Tin;
  typedef typename Operator::Tout Tout;
  typedef VariadicOp<Operator> Self;

public: /* --- CONSTRUCTION --- */
  Operator op;

  typedef VariadicAbstract<Tin, Tout, int> Base;

  // static std::string getTypeInName ( void ) { return Operator::nameTypeIn ();
  // } static std::string getTypeOutName( void ) { return
  // Operator::nameTypeOut(); }
  static const std::string CLASS_NAME;
  virtual const std::string &getClassName() const { return CLASS_NAME; }
  std::string getDocString() const { return op.getDocString(); }

  VariadicOp(const std::string &name) : Base(name, CLASS_NAME) {
    this->SOUT.setFunction(boost::bind(&Self::computeOperation, this, _1, _2));
    op.initialize(this, this->commandMap);
  }

  virtual ~VariadicOp(void){};

protected:
  Tout &computeOperation(Tout &res, int time) {
    std::vector<const Tin *> in(this->signalsIN.size());
    for (std::size_t i = 0; i < this->signalsIN.size(); ++i) {
      const Tin &x = this->signalsIN[i]->access(time);
      in[i] = &x;
    }
    op(in, res);
    return res;
  }

  inline void updateSignalNumber(int n) { op.updateSignalNumber(n); }
};
} // namespace sot
} // namespace dynamicgraph

#endif // #ifndef SOT_CORE_VARIADICOP_HH


//---------------------------------------------------------------------------//
//!
//! \file   Utility_GaussKronrodIntegrator.hpp
//! \author Luke Kersting
//! \brief  Gauss-Kronrod integrator
//!
//---------------------------------------------------------------------------//

#ifndef UTILITY_GAUSS_KRONROD_INTEGRATOR_HPP
#define UTILITY_GAUSS_KRONROD_INTEGRATOR_HPP

// std Includes
#include <queue>

// Boost Includes
#include <boost/numeric/odeint.hpp>

// FRENSIE Includes
#include "Utility_Vector.hpp"
#include "Utility_ArrayView.hpp"

namespace Utility{

template<typename T>
struct BinTraits
{
  T lower_limit;
  T upper_limit;
  T result;
  T error;

  bool operator <( const BinTraits<T>& bins ) const
  {
    return error < bins.error;
  }
};

template<typename T>
struct ExtrapolatedBinTraits
{
  T lower_limit;
  T upper_limit;
  T result;
  T error;
  int level;

  bool operator <( const ExtrapolatedBinTraits<T>& bins ) const
  {
    return error < bins.error;
  }
};

//! The Gauss-Kronrod integrator
template<typename T>
class GaussKronrodIntegrator
{

public:

typedef std::priority_queue<BinTraits<T>> BinQueue;
typedef std::vector<ExtrapolatedBinTraits<T>> BinArray;

  //! Constructor
  GaussKronrodIntegrator( const T relative_error_tol,
                          const T absolute_error_tol = 0.0,
                          const size_t subinterval_limit = 1000 );

  //! Destructor
  ~GaussKronrodIntegrator()
  { /* ... */ }

  //! Throw exception on dirty integration
  void throwExceptionOnDirtyIntegration();

  //! Check if an exception will be thrown on dirty integration
  bool isExceptionThrownOnDirtyIntegration() const;

  //! Warn on dirty integration (default)
  void warnOnDirtyIntegration();

  //! Use heuristic roundoff error estimator (default)
  void estimateRoundoff();

  //! Don't use heuristic roundoff error estimator
  void dontEstimateRoundoff();

  //! Set the realtive error tolerance
  void setRelativeErrorTolerance( const double relative_error_tol );

  //! Get the relative error tolerance
  double getRelativeErrorTolerance() const;

  //! Set the absolute error tolerance
  void setAbsoluteErrorTolerance( const double absolute_error_tol );

  //! Get the absolute error tolerance
  double getAbsoluteErrorTolerance() const;

/*
  //! Integrate the function
  template<typename Functor>
  void integrate( Functor& integrand,
		  T lower_limit,
		  T upper_limit,
		  T& result,
		  T& absolute_error,
		  size_t& number_of_function_eval ) const;
*/
  //! Integrate the function adaptively with BinQueue
  template<int Points, typename FunctorType = double, typename Functor>
  void integrateAdaptively( Functor& integrand,
			    T lower_limit,
			    T upper_limit,
			    T& result,
			    T& absolute_error ) const;

  //! Integrate the function adaptively with BinQueue
  template<int Points, typename FunctorType = double,
                       typename ParameterType = double, typename Functor>
  void integrateAdaptively( Functor& integrand,
                            ParameterType integrand_parameter,
			    T lower_limit,
			    T upper_limit,
			    T& result,
			    T& absolute_error ) const;
/*
  //! Integrate the function over a semi-infinite interval (+infinity)
  template<typename Functor>
  void integrateSemiInfiniteIntervalUpper( Functor& integrand,
					   T lower_limit,
					   T& result,
					   T& absolute_error ) const;

  //! Integrate the function over a semi-infinite interval (-infinity)
  template<typename Functor>
  void integrateSemiInfiniteIntervalLower( Functor& integrand,
					   T upper_limit,
					   T& result,
					   T& absolute_error ) const;

  //! Integrate the function over an infinite interval (-infinity,+infinity)
  template<typename Functor>
  void integrateInfiniteInterval( Functor& integrand,
				  T& result,
				  T& absolute_error ) const;

  //! Integrate a function with integrable singularities adaptively
  template<typename Functor>
  void integrateAdaptivelyWynnEpsilon( Functor& integrand,
				       T lower_limit,
				       T upper_limit,
				       T& result,
				       T& absolute_error ) const;
*/
  //! Integrate a function with known integrable singularities adaptively
  template<typename FunctorType = double, typename Functor>
  void integrateAdaptivelyWynnEpsilon(
			  Functor& integrand,
			  const Utility::ArrayView<T>& points_of_interest,
			  T& result,
			  T& absolute_error ) const;

  //! Integrate a function with known integrable singularities adaptively
  template<typename FunctorType = double, typename ParameterType = double, typename Functor>
  void integrateAdaptivelyWynnEpsilon(
			  Functor& integrand,
                          ParameterType integrand_parameter,
			  const Utility::ArrayView<T>& points_of_interest,
			  T& result,
			  T& absolute_error ) const;

  //! Integrate the function with point rule
  template<int Points, typename FunctorType = double, typename Functor>
  void integrateWithPointRule(
                Functor& integrand,
			    T lower_limit,
			    T upper_limit,
			    T& result,
			    T& absolute_error,
                T& result_abs,
                T& result_asc ) const;

  //! Integrate the function with point rule
  template<int Points, typename FunctorType = double,
                       typename ParameterType = double, typename Functor>
  void integrateWithPointRule(
                Functor& integrand,
                ParameterType integrand_parameter,
			    T lower_limit,
			    T upper_limit,
			    T& result,
			    T& absolute_error,
                T& result_abs,
                T& result_asc ) const;

protected:

  // Calculate the quadrature upper and lower integrand values at an abscissa
  template<typename FunctorType = double, typename Functor>
  void calculateQuadratureIntegrandValuesAtAbscissa(
    Functor& integrand,
    T abscissa,
    T half_length,
    T midpoint,
    T& integrand_value_lower,
    T& integrand_value_upper ) const;

  // Calculate the quadrature upper and lower integrand values at an abscissa
  template<typename FunctorType = double, typename ParameterType = double, typename Functor>
  void calculateQuadratureIntegrandValuesAtAbscissa(
    Functor& integrand,
    ParameterType functor_parameter,
    T abscissa,
    T half_length,
    T midpoint,
    T& integrand_value_lower,
    T& integrand_value_upper ) const;

  // Bisect and integrate the given bin interval
  template<int Points, typename FunctorType = double, typename Functor, typename Bin>
  void bisectAndIntegrateBinInterval(
    Functor& integrand,
    const Bin& bin,
    Bin& bin_1,
    Bin& bin_2,
    T& bin_1_asc,
    T& bin_2_asc ) const;

  // Bisect and integrate the given bin interval
  template<int Points, typename FunctorType = double, typename ParameterType = double, typename Functor, typename Bin>
  void bisectAndIntegrateBinInterval(
    Functor& integrand,
    ParameterType functor_parameter,
    const Bin& bin,
    Bin& bin_1,
    Bin& bin_2,
    T& bin_1_asc,
    T& bin_2_asc ) const;

  // Rescale absolute error from integration
  void rescaleAbsoluteError(
    T& absolute_error,
    T result_abs,
    T result_asc ) const;

  // Test if subinterval is too small
  template<int Points>
  bool subintervalTooSmall( T& lower_limit_1,
                                   T& lower_limit_2,
                                   T& upper_limit_2 ) const;

  // check the roundoff error
  void checkRoundoffError(
                       const BinTraits<T>& bin,
                       const BinTraits<T>& bin_1,
                       const BinTraits<T>& bin_2,
                       const T& bin_1_asc,
                       const T& bin_2_asc,
                       int& round_off_1,
                       int& round_off_2,
                       const int number_of_iterations ) const;

  // check the roundoff error
  void checkRoundoffError(
                       const ExtrapolatedBinTraits<T>& bin,
                       const ExtrapolatedBinTraits<T>& bin_1,
                       const ExtrapolatedBinTraits<T>& bin_2,
                       const T& bin_1_asc,
                       const T& bin_2_asc,
                       int& round_off_1,
                       int& round_off_2,
                       int& round_off_3,
                       const bool extrapolate,
                       const int number_of_iterations ) const;

  // Sort the bin order from highest to lowest error
  void sortBins(
        std::vector<int>& bin_order,
        BinArray& bin_array,
        const ExtrapolatedBinTraits<T>& bin_1,
        const ExtrapolatedBinTraits<T>& bin_2,
        const int& number_of_intervals,
        int& nr_max ) const;

  // get the Wynn Epsilon-Algorithm extrapolated value
  void getWynnEpsilonAlgorithmExtrapolation(
        std::vector<T>& bin_extrapolated_result,
        std::vector<T>& last_three_results,
        T& extrapolated_result,
        T& extrapolated_error,
        int& number_of_extrapolated_intervals,
        int& number_of_extrapolated_calls  ) const;

private:
  // The relative error tolerance
  T d_relative_error_tol;

  // The absolute error tolerance
  T d_absolute_error_tol;

  // The subinterval limit
  size_t d_subinterval_limit;

  // Throw exception on dirty integration
  bool d_throw_exceptions;

  // Estimate roundoff error and throw exceptions/warnings
  bool d_estimate_roundoff;

  // return epsilon numerical limit for type T
  T getLimitEpsilon() const;

  // return min numerical limit for type T
  T getLimitMin() const;

  // return max numerical limit for type T
  T getLimitMax() const;

  // return max of two variables of type T
  T getMax( T variable_1, T variable_2 ) const;
};

} // end Utility namespace

//---------------------------------------------------------------------------//
// Template Includes
//---------------------------------------------------------------------------//

#include "Utility_GaussKronrodIntegrator_def.hpp"

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

#endif // end UTILITY_GAUSS_KRONROD_INTEGRATOR_HPP

//---------------------------------------------------------------------------//
// end Utility_GaussKronrodIntegrator.hpp
//---------------------------------------------------------------------------//


// Copyright David Abrahams 2003-2004
// Copyright Aleksey Gurtovoy 2004
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// See http://www.boost.org/libs/mpl for documentation.

// $Source: /CVSROOT/boost/libs/mpl/test/pair_view.cpp,v $
// $Date: 2007/10/29 07:32:56 $
// $Revision: 1.1.1.1 $

#include <boost/mpl/pair_view.hpp>
#include <boost/mpl/vector/vector50_c.hpp>
#include <boost/mpl/range_c.hpp>
#include <boost/mpl/distance.hpp>
#include <boost/mpl/aux_/test.hpp>

MPL_TEST_CASE()
{
    typedef range_c<int,0,10> r;
    typedef vector10_c<int,9,8,7,6,5,4,3,2,1,10> v;

    typedef pair_view<r,v> view;
    typedef begin<view>::type first_;
    typedef end<view>::type last_;

    MPL_ASSERT(( is_same< first_::category, mpl::random_access_iterator_tag > ));

    MPL_ASSERT(( is_same< advance_c<first_,0>::type, first_ > ));
    MPL_ASSERT(( is_same< advance_c<last_,0>::type, last_ > ));
    MPL_ASSERT(( is_same< advance_c<first_,10>::type, last_ > ));
    MPL_ASSERT(( is_same< advance_c<last_,-10>::type, first_ > ));

    typedef advance_c<first_,5>::type iter;

    MPL_ASSERT(( is_same<
          deref<iter>::type
        , mpl::pair< integral_c<int,5>,integral_c<int,4> >
        > ));

}


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

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!

    @ingroup group-constant

    This constant is such that, for pairs of types (T, Tup)
    (namely (float,  double) and (double, long double)) the sum:

    abs(Tup(Pio_2lo<T>())+Tup(Pio_2<T>())-Pio_2\< Tup \>()) is  less than
    a few Eps<Tup>().

    This is used to improve accurracy when computing sums of the kind
    \f$\pi/2 + x\f$ with x small,  by replacing them by
    Pio_2 + (Pio_2lo + x)

    @par Semantic:

    For type T:

    @code
    T r = Pio_2lo<T>();
    @endcode

    is similar to:

    @code
    if T is double
      r = 6.123233995736766e-17
    else if T is float
      r = -4.3711388e-08
    @endcode

    @return a value of type T

**/
  template<typename T> T Pio_2lo();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant

      This constant is such that, for pairs of types (T, Tup)
      (namely (float,  double) and (double, long double)) the sum:

      Generate the  constant pio_2lo.

      @return The Pio_2lo constant for the proper type
    **/
    const boost::dispatch::functor<tag::pio_2lo_> pio_2lo = {};
  }
} }
#endif

#include <boost/simd/constant/definition/pio_2lo.hpp>
#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#endif


/**
 *  carAbst.cpp
 *
 *  The vehicle example from scots using abstraction-based control synthesis.
 *
 *  Created by Yinan Li on Feb. 18, 2020.
 *
 *  Hybrid Systems Group, University of Waterloo.
 */

#include <iostream>
#include <fstream>
#include <sstream>
#include <iomanip>
#include <cstdlib>
#include <boost/algorithm/string.hpp>

#include "src/abstraction.hpp"
#include "src/DBAparser.h"
#include "src/bsolver.hpp"

#include "src/hdf5io.h"

#include "car.hpp"

int main(int argc, char *argv[])
{
    /**
     * Default values
     **/
    std::string specfile;
    double eta[]{0.2, 0.2, 0.2}; /* partition precision */
    bool preprocess = false;

    /* Input arguments:
     * carAbst dbafile precision(e.g. 0.2 0.2 0.2) -preprocess
     */
    if (argc < 2 || argc > 6) {
	std::cout << "Improper number of arguments.\n";
	std::exit(1);
    }
    specfile = std::string(argv[1]);
    if (argc > 2 && argc < 5) {
	std::cout << "Input precision should be of 3-dim, e.g. 0.2 0.2 0.2.\n";
	std::exit(1);
    }
    if (argc >= 5) {

	for(int i = 2; i < 5; ++i)
	    eta[i-2] = std::atof(argv[i]);

    	if (argc > 5) {
    	    std::string param = std::string(argv[5]);
    	    if (param == "-preprocess")
    		preprocess = true;
    	    else {
    		std::cout << "Wrong argument for preprocessing.\n";
    		std::exit(1);
    	    }
    	}
    }
    std::cout << "Partition precision: " << eta[0] << ' '
	      << eta[1] << ' ' << eta[2] << '\n';

    clock_t tb, te;
    /* set the state space */
    const double theta = 3.5;
    double xlb[] = {0, 0, -theta};
    double xub[] = {10, 10, theta};
    /* set the control values */
    double ulb[] = {-1.0, -1.0};
    double uub[] = {1.0, 1.0};
    double mu[] = {0.3, 0.3};
    /* define the control system */
    rocs::DTCntlSys<carde> car("reach goal", h, carde::n, carde::m);
    car.init_workspace(xlb, xub);
    car.init_inputset(mu, ulb, uub);

    /**
     * Construct and save abstraction
     */
    // const double eta[] = {0.2, 0.2, 0.2}; /* set precision */
    rocs::abstraction<rocs::DTCntlSys<carde>> abst(&car);
    abst.init_state(eta, xlb, xub);
    std::cout << "The number of abstraction states: " << abst._x._nv << '\n';
    /* Assign the label of avoid area to -1 */
    rocs::UintSmall nAvoid = 4;
    double obs[4][4] = {
	{1.6, 5.7, 4.0, 5.0},
	{3.0, 5.0, 5.0, 8.0},
	{4.3, 5.7, 1.8, 4.0},
	{5.7, 8.5, 1.8, 2.5}
    };
    auto label_avoid = [&obs, &nAvoid, &abst, &eta](size_t i) {
    		     std::vector<double> x(abst._x._dim);
    		     abst._x.id_to_val(x, i);
    		     double c1= eta[0]/2.0+1e-10;
    		     double c2= eta[1]/2.0+1e-10;
    		     for(size_t i = 0; i < nAvoid; ++i) {
    			 if ((obs[i][0]-c1) <= x[0] && x[0] <= (obs[i][1]+c1) &&
    			     (obs[i][2]-c2) <= x[1] && x[1] <= (obs[i][3]+c2))
    			     return -1;
    		     }
    		     return 0;
    		 };
    abst.assign_labels(label_avoid);
    std::vector<size_t> obstacles;
    for (size_t i = 0; i < abst._x._nv; ++i) {
    	if (abst._labels[i] < 0)
    	    obstacles.push_back(i);
    }
    /* Compute abstraction */
    /* Robustness margins */
    double e1[] = {0,0,0};
    double e2[] = {0,0,0};
    tb = clock();
    abst.assign_transitions(e1, e2);
    // abst.assign_transitions();
    te = clock();
    float tabst = (float)(te - tb)/CLOCKS_PER_SEC;
    std::cout << "Time of computing abstraction: " << tabst << '\n';
    std::cout << "# of transitions: " << abst._ts._ntrans << '\n';

    /**
     * Read DBA from dba*.txt file
     */
    std::cout << "Reading the specification...\n";
    rocs::UintSmall nAP = 0, nNodes = 0, q0 = 0;
    std::vector<rocs::UintSmall> acc;
    std::vector<std::vector<rocs::UintSmall>> arrayM;
    if (!rocs::read_spec(specfile, nNodes, nAP, q0, arrayM, acc))
	std::exit(1);

    /*
     * Assign labels to states: has to be consistent with the dba file.
     */
    double goal[3][4] = {{1.0, 2.0, 0.5, 2.0},
			 {0.5, 2.5, 7.5, 8.5},
			 {7.1, 9.1, 4.6, 6.4}};
    rocs::UintSmall nGoal = 3;
    auto label_target = [&goal, &nGoal, &abst, &eta](size_t i) {
		      std::vector<double> x(abst._x._dim);
		      abst._x.id_to_val(x, i);
		      double c1= eta[0]/2.0; //+1e-10;
		      double c2= eta[1]/2.0; //+1e-10;
		      boost::dynamic_bitset<> label(nGoal, false); // n is the number of goals
		      for(rocs::UintSmall i = 0; i < nGoal; ++i) {
			  label[2-i] = (goal[i][0] <= (x[0]-c1) && (x[0]+c1) <= goal[i][1] &&
				      goal[i][2] <= (x[1]-c2) && (x[1]+c2) <= goal[i][3])
			      ? true: false;
		      }
		      return label.to_ulong();
		  };
    abst.assign_labels(label_target);
    std::cout << "Specification assignment is done.\n";

    /**
     * Solve a Buchi game on the product of NTS and DBA.
     */
    std::cout << "Start solving a Buchi game on the product of the abstraction and DBA...\n";
    rocs::BSolver solver;
    solver.construct_dba((int)nAP, (int)nNodes, (int)q0, acc, arrayM);
    tb = clock();
    solver.load_abstraction(abst);
    solver.generate_product(abst);
    solver.solve_buchigame_on_product();
    te = clock();
    float tsyn = (float)(te - tb)/CLOCKS_PER_SEC;
    std::cout << "Time of synthesizing controller: " << tsyn << '\n';

    /**
     * Display and save memoryless controllers.
     */
    std::vector<std::string> tokens;
    boost::split(tokens, specfile, boost::is_any_of("."));
    std::string datafile = "controller_" + tokens[0] + "_";
    for(int i = 0; i < 3; ++i) {
	std::stringstream ss;
	ss << std::setprecision(1);
	ss << eta[i];
	datafile += ss.str();
	if (i < 2)
	    datafile += "-";
    }
    datafile += ".h5";

    std::cout << "Writing the controller...\n";
    // solver.write_controller_to_txt(const_cast<char*>(datafile.c_str()));
    rocs::h5FileHandler ctlrWtr(datafile, H5F_ACC_TRUNC);
    ctlrWtr.write_problem_setting< rocs::DTCntlSys<carde> >(car);
    ctlrWtr.write_array<double>(eta, carde::n, "eta");
    ctlrWtr.write_2d_array<double>(abst._x._data, "xgrid");
    ctlrWtr.write_discrete_controller(&(solver._sol));
    std::cout << "Controller writing is done.\n";

    std::cout << "Total time of used (abstraction+synthesis): " << tabst+tsyn << '\n';

    return 0;
}


#include <trajopt_utils/macros.h>
TRAJOPT_IGNORE_WARNINGS_PUSH
#include <algorithm>
#include <Eigen/SparseCore>
#include <sstream>
TRAJOPT_IGNORE_WARNINGS_POP

#include <trajopt_sco/solver_utils.hpp>
#include <trajopt_utils/logging.hpp>

namespace sco
{
void exprToEigen(const AffExpr& expr, Eigen::SparseVector<double>& sparse_vector, const int& n_vars)
{
  sparse_vector.resize(n_vars);
  sparse_vector.reserve(static_cast<long int>(expr.size()));
  for (size_t i = 0; i < expr.size(); ++i)
  {
    int i_var_index = expr.vars[i].var_rep->index;
    if (i_var_index >= n_vars)
    {
      std::stringstream msg;
      msg << "Coefficient " << i << "has index " << i_var_index << " but n_vars is " << n_vars;
      throw std::runtime_error(msg.str());
    }
    if (expr.coeffs[i] != 0.)
      sparse_vector.coeffRef(i_var_index) += expr.coeffs[i];
  }
}

void exprToEigen(const QuadExpr& expr, Eigen::SparseMatrix<double>& sparse_matrix, Eigen::VectorXd& vector,
                 const int& n_vars, const bool& matrix_is_halved, const bool& force_diagonal)
{
  IntVec ind1 = vars2inds(expr.vars1);
  IntVec ind2 = vars2inds(expr.vars2);
  sparse_matrix.resize(n_vars, n_vars);
  sparse_matrix.reserve(static_cast<long int>(2 * expr.size()));

  Eigen::SparseVector<double> vector_sparse;
  exprToEigen(expr.affexpr, vector_sparse, n_vars);
  vector = vector_sparse;

  for (size_t i = 0; i < expr.coeffs.size(); ++i)
  {
    if (expr.coeffs[i] != 0.0)
    {
      if (ind1[i] == ind2[i])
        sparse_matrix.coeffRef(ind1[i], ind2[i]) += expr.coeffs[i];
      else
      {
        int c, r;
        if (ind1[i] < ind2[i])
        {
          r = ind1[i];
          c = ind2[i];
        }
        else
        {
          r = ind2[i];
          c = ind1[i];
        }
        sparse_matrix.coeffRef(r, c) += expr.coeffs[i];
      }
    }
  }

  auto sparse_matrix_T = Eigen::SparseMatrix<double>(sparse_matrix.transpose());
  sparse_matrix = sparse_matrix + sparse_matrix_T;

  if (!matrix_is_halved)
    sparse_matrix = 0.5 * sparse_matrix;

  if (force_diagonal)
    for (int k = 0; k < n_vars; ++k)
      sparse_matrix.coeffRef(k, k) += 0.0;
}

void exprToEigen(const AffExprVector& expr_vec, Eigen::SparseMatrix<double>& sparse_matrix, Eigen::VectorXd& vector,
                 const int& n_vars)
{
  vector.resize(static_cast<long int>(expr_vec.size()));
  vector.setZero();
  sparse_matrix.resize(static_cast<long int>(expr_vec.size()), n_vars);
  sparse_matrix.reserve(static_cast<long int>(expr_vec.size()) * n_vars);

  for (long int i = 0; i < static_cast<long int>(expr_vec.size()); ++i)
  {
    const AffExpr& expr = expr_vec[static_cast<size_t>(i)];
    Eigen::SparseVector<double> expr_vector;
    exprToEigen(expr, expr_vector, n_vars);
    for (Eigen::SparseVector<double>::InnerIterator it(expr_vector); it; ++it)
      sparse_matrix.coeffRef(i, it.index()) = it.value();
    vector[i] = -expr.constant;
  }
}

void tripletsToEigen(const IntVec& rows_i, const IntVec& cols_j, const DblVec& values_ij,
                     Eigen::SparseMatrix<double>& sparse_matrix)
{
  typedef Eigen::Triplet<double> T;
  std::vector<T, Eigen::aligned_allocator<T>> triplets;
  for (unsigned int i = 0; i < values_ij.size(); ++i)
    triplets.push_back(T(rows_i[i], cols_j[i], values_ij[i]));
  sparse_matrix.setFromTriplets(triplets.begin(), triplets.end());
}

void eigenToTriplets(const Eigen::SparseMatrix<double>& sparse_matrix, IntVec& rows_i, IntVec& cols_j,
                     DblVec& values_ij)
{
  auto& sm = sparse_matrix;
  rows_i.reserve(rows_i.size() + static_cast<size_t>(sm.nonZeros()));
  cols_j.reserve(cols_j.size() + static_cast<size_t>(sm.nonZeros()));
  values_ij.reserve(values_ij.size() + static_cast<size_t>(sm.nonZeros()));
  for (int k = 0; k < sm.outerSize(); ++k)
  {
    for (Eigen::SparseMatrix<double>::InnerIterator it(sm, k); it; ++it)
    {
      rows_i.push_back(static_cast<int>(it.row()));
      cols_j.push_back(static_cast<int>(it.col()));
      values_ij.push_back(it.value());
    }
  }
}
}


#include <elasty/cloth-sim-object.hpp>
#include <elasty/constraint.hpp>
#include <elasty/particle.hpp>
#include <iostream>
#include <Eigen/Geometry>
#include <tiny_obj_loader.h>

elasty::ClothSimObject::ClothSimObject(const std::string& obj_path,
                                       const double distance_stiffness,
                                       const double bending_stiffness,
                                       const Eigen::Affine3d& transform,
                                       const Strategy strategy)
{
    tinyobj::attrib_t attrib;
    std::vector<tinyobj::shape_t> shapes;
    std::vector<tinyobj::material_t> materials;

    std::string warn;
    std::string err;
    const bool return_value = tinyobj::LoadObj(&attrib, &shapes, &materials, &warn, &err, obj_path.c_str());

    if (!warn.empty()) { std::cerr << warn << std::endl; }
    if (!err.empty()) { std::cerr << err << std::endl; }
    if (!return_value) { throw std::runtime_error(""); }

    if (attrib.vertices.empty() || attrib.normals.empty()) { throw std::runtime_error(""); }

    const auto& shape = shapes[0];

    assert(shapes.size() == 1);
    assert(attrib.vertices.size() % 3 == 0);
    assert(shape.mesh.indices.size() % 3 == 0);

    m_triangle_list.resize(shape.mesh.indices.size() / 3, 3);
    for (unsigned int i = 0; i < shape.mesh.indices.size() / 3; ++ i)
    {
        m_triangle_list(i, 0) = shape.mesh.indices[i * 3 + 0].vertex_index;
        m_triangle_list(i, 1) = shape.mesh.indices[i * 3 + 1].vertex_index;
        m_triangle_list(i, 2) = shape.mesh.indices[i * 3 + 2].vertex_index;
    }

    std::map<unsigned int, std::shared_ptr<Particle>> map_from_obj_vertex_index_to_particle;
    for (unsigned int i = 0; i < attrib.vertices.size() / 3; ++ i)
    {
        const Eigen::Vector3d position
        {
            attrib.vertices[3 * i + 0],
            attrib.vertices[3 * i + 1],
            attrib.vertices[3 * i + 2]
        };

        const Eigen::Vector3d x = transform * position;
        const Eigen::Vector3d v = Eigen::Vector3d::Zero();
        const double m = 1.0 / double(attrib.vertices.size());

        auto particle = std::make_shared<elasty::Particle>(x, v, m);

        map_from_obj_vertex_index_to_particle[i] = particle;

        m_particles.push_back(particle);
    }

    for (unsigned int i = 0; i < shape.mesh.indices.size() / 3; ++ i)
    {
        const auto p_0 = map_from_obj_vertex_index_to_particle[shape.mesh.indices[i * 3 + 0].vertex_index];
        const auto p_1 = map_from_obj_vertex_index_to_particle[shape.mesh.indices[i * 3 + 1].vertex_index];
        const auto p_2 = map_from_obj_vertex_index_to_particle[shape.mesh.indices[i * 3 + 2].vertex_index];

        m_constraints.push_back(std::make_shared<elasty::DistanceConstraint>(p_0, p_1, distance_stiffness, (p_0->x - p_1->x).norm()));
        m_constraints.push_back(std::make_shared<elasty::DistanceConstraint>(p_0, p_2, distance_stiffness, (p_0->x - p_2->x).norm()));
        m_constraints.push_back(std::make_shared<elasty::DistanceConstraint>(p_1, p_2, distance_stiffness, (p_1->x - p_2->x).norm()));
    }

    using vertex_t = unsigned int;
    using triangle_t = unsigned int;
    using edge_t = std::pair<vertex_t, vertex_t>;
    std::map<edge_t, std::vector<triangle_t>> edges_and_triangles;
    for (unsigned int i = 0; i < shape.mesh.indices.size() / 3; ++ i)
    {
        const vertex_t index_0 = shape.mesh.indices[i * 3 + 0].vertex_index;
        const vertex_t index_1 = shape.mesh.indices[i * 3 + 1].vertex_index;
        const vertex_t index_2 = shape.mesh.indices[i * 3 + 2].vertex_index;

        const edge_t e_01 = std::make_pair(std::min(index_0, index_1), std::max(index_0, index_1));
        const edge_t e_02 = std::make_pair(std::min(index_0, index_2), std::max(index_0, index_2));
        const edge_t e_12 = std::make_pair(std::min(index_1, index_2), std::max(index_1, index_2));

        auto register_edge = [&](const edge_t& edge)
        {
            if (edges_and_triangles.find(edge) == edges_and_triangles.end())
            {
                edges_and_triangles[edge] =  { i };
            }
            else
            {
                edges_and_triangles[edge].push_back(i);
            }
        };

        register_edge(e_01);
        register_edge(e_02);
        register_edge(e_12);
    }

    for (const auto& key_value : edges_and_triangles)
    {
        const edge_t& edge = key_value.first;
        const std::vector<triangle_t>& triangles = key_value.second;

        assert(triangles.size() == 1 || triangles.size() == 2);

        // Boundary
        if (triangles.size() == 1) { continue; }

        auto obtain_another_vertex = [&](const triangle_t& triangle, const edge_t& edge)
        {
            const vertex_t vertex_0 = shape.mesh.indices[3 * triangle + 0].vertex_index;
            const vertex_t vertex_1 = shape.mesh.indices[3 * triangle + 1].vertex_index;
            const vertex_t vertex_2 = shape.mesh.indices[3 * triangle + 2].vertex_index;

            if (vertex_0 != edge.first && vertex_0 != edge.second)
            {
                return vertex_0;
            }
            else if (vertex_1 != edge.first && vertex_1 != edge.second)
            {
                return vertex_1;
            }
            else
            {
                assert(vertex_2 != edge.first && vertex_2 != edge.second);
                return vertex_2;
            }
        };

        const triangle_t another_vertex_0 = obtain_another_vertex(triangles[0], edge);
        const triangle_t another_vertex_1 = obtain_another_vertex(triangles[1], edge);

        switch (strategy)
        {
            case Strategy::Bending:
            {
                const auto p_0 = map_from_obj_vertex_index_to_particle[edge.first];
                const auto p_1 = map_from_obj_vertex_index_to_particle[edge.second];
                const auto p_2 = map_from_obj_vertex_index_to_particle[another_vertex_0];
                const auto p_3 = map_from_obj_vertex_index_to_particle[another_vertex_1];

                const Eigen::Vector3d& x_0 = p_0->x;
                const Eigen::Vector3d& x_1 = p_1->x;
                const Eigen::Vector3d& x_2 = p_2->x;
                const Eigen::Vector3d& x_3 = p_3->x;

                const Eigen::Vector3d p_10 = x_1 - x_0;
                const Eigen::Vector3d p_20 = x_2 - x_0;
                const Eigen::Vector3d p_30 = x_3 - x_0;

                const Eigen::Vector3d n_0 = p_10.cross(p_20).normalized();
                const Eigen::Vector3d n_1 = p_10.cross(p_30).normalized();

                assert(!n_0.hasNaN());
                assert(!n_1.hasNaN());

                // Typical value is 0.0 or pi
                const double dihedral_angle = std::acos(std::max(- 1.0, std::min(+ 1.0, n_0.dot(n_1))));

                assert(!std::isnan(dihedral_angle));

                m_constraints.push_back(std::make_shared<elasty::BendingConstraint>(p_0, p_1, p_2, p_3, bending_stiffness, dihedral_angle));

                break;
            }
            case Strategy::IsometricBending:
            {
                const auto p_0 = map_from_obj_vertex_index_to_particle[edge.first];
                const auto p_1 = map_from_obj_vertex_index_to_particle[edge.second];
                const auto p_2 = map_from_obj_vertex_index_to_particle[another_vertex_0];
                const auto p_3 = map_from_obj_vertex_index_to_particle[another_vertex_1];

                m_constraints.push_back(std::make_shared<elasty::IsometricBendingConstraint>(p_0, p_1, p_2, p_3, bending_stiffness));

                break;
            }
            case Strategy::Cross:
            {
                const auto p_2 = map_from_obj_vertex_index_to_particle[another_vertex_0];
                const auto p_3 = map_from_obj_vertex_index_to_particle[another_vertex_1];

                const Eigen::Vector3d& x_2 = p_2->x;
                const Eigen::Vector3d& x_3 = p_3->x;

                m_constraints.push_back(std::make_shared<elasty::DistanceConstraint>(p_2, p_3, bending_stiffness, (x_2 - x_3).norm()));

                break;
            }
        }
    }
}