#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Mesh_3/io_signature.h>
#include <CGAL/Mesh_triangulation_3.h>
#include <CGAL/Mesh_complex_3_in_triangulation_3.h>
#include <CGAL/IO/File_binary_mesh_3.h>

#include <boost/variant.hpp>

#include <string>

#include <CGAL/disable_warnings.h>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;

//
// Define two fake Mesh_domain, with index types
//

// One with all types equal to int.
struct MD_homogeneous_types {
  typedef CGAL::Tag_false Has_features;
  typedef int Subdomain_index;
  typedef int Surface_patch_index;
  typedef int Curve_index;
  typedef int Corner_index;
  typedef int Index;

  static Subdomain_index get_sub_domain_index_1() { return 1; }
  static Subdomain_index get_sub_domain_index_2() { return 2; }
  static Surface_patch_index get_surface_patch_index_1() { return 3; }
  static Surface_patch_index get_surface_patch_index_2() { return 4; }
  static Curve_index get_curve_index_1() { return 5; }
  static Curve_index get_curve_index_2() { return 6; }
  static Corner_index get_corner_index_1() { return 7; }
  static Corner_index get_corner_index_2() { return 8; }

  static std::string reference_format_string()
  {
    return "Triangulation_3(Weighted_point<Point_3>,Vb(Tvb_3+i+i),Cb(i+RTcb_3+(i)[4]))";
  }
};

// One with heterogeneous index types
struct MD_heterogeneous_types {
  typedef CGAL::Tag_true Has_features;
  enum Subdomain_index_enum { Z = 0, A, B, C, D};
  typedef Subdomain_index_enum Subdomain_index;
  typedef std::pair<int, int> Surface_patch_index;
  typedef int Curve_index;
  typedef double Corner_index;
  typedef boost::variant<Subdomain_index,
                         Surface_patch_index,
                         Curve_index,
                         Corner_index> Index;
  static Subdomain_index get_sub_domain_index_1() { return A; }
  static Subdomain_index get_sub_domain_index_2() { return B; }
  static Surface_patch_index get_surface_patch_index_1() { return std::make_pair(1, 2); }
  static Surface_patch_index get_surface_patch_index_2() { return std::make_pair(3, 4); }
  static Curve_index get_curve_index_1() { return 5; }
  static Curve_index get_curve_index_2() { return 6; }
  static Corner_index get_corner_index_1() { return 7.; }
  static Corner_index get_corner_index_2() { return 8.; }

  static std::string reference_format_string()
  {
    return "Triangulation_3(Weighted_point<Point_3>,Vb(Tvb_3+i+boost::variant<enum,std::pair<i,i>,i,d>),Cb(enum+RTcb_3+(std::pair<i,i>)[4]))";
  }
};

//
// Define I/O for MD_heterogeneous_types::Subdomain_index (then enum)
//

// First, new technique: specialization of Output_rep and Input_rep (from
// CGAL/IO/io.h). That works for CGAL::read and CGAL::write, and ease the
// overload of << and >>
namespace CGAL {

template <>
class Output_rep<MD_heterogeneous_types::Subdomain_index> {
  typedef MD_heterogeneous_types::Subdomain_index T;
  const T& t;
public:
  //! initialize with a const reference to \a t.
  Output_rep( const T& tt) : t(tt) {}
  //! perform the output, calls \c operator\<\< by default.
  std::ostream& operator()( std::ostream& out) const {
    if(IO::is_ascii(out)) {
      out << static_cast<int>(t);
    } else {
      CGAL::write(out, static_cast<int>(t));
    }
    return out;
  }
};

template <>
class Input_rep<MD_heterogeneous_types::Subdomain_index> {
  typedef MD_heterogeneous_types::Subdomain_index T;
  T& t;
public:
  //! initialize with a const reference to \a t.
  Input_rep( T& tt) : t(tt) {}
  //! perform the output, calls \c operator\<\< by default.
  std::istream& operator()( std::istream& in) const {
    int i;
    if(IO::is_ascii(in)) {
      in >> i;
    } else {
      CGAL::read(in, i);
    }
    t = static_cast<T>(i);
    return in;
  }
};
} // end namespace CGAL

// Second: operator<< and >>
namespace std {
std::ostream& operator<<(std::ostream& out,
                         MD_heterogeneous_types::Subdomain_index index) {
  return out << CGAL::IO::oformat(index);
}
std::istream& operator>>(std::istream& in,
                         MD_heterogeneous_types::Subdomain_index& index) {
  return in >> CGAL::IO::iformat(index);
}
} // end namespace std

// Second: the specialization of CGAL::Get_io_signature (in
// <CGAL/Mesh_3/io_signature.h>).
namespace CGAL {
template <>
struct Get_io_signature<MD_heterogeneous_types::Subdomain_index> {
  std::string operator()() const {
    return "enum";
  }
};

//
// Define I/O for MD_heterogeneous_types::Surface_patch_index (pair of int)
//

// First, new technique: specialization of Output_rep and Input_rep (from
// CGAL/IO/io.h). That works for CGAL::read and CGAL::write, and ease the
// overload of << and >>
template <>
class Output_rep<MD_heterogeneous_types::Surface_patch_index> {
  typedef MD_heterogeneous_types::Surface_patch_index T;
  const T& t;
public:
  //! initialize with a const reference to \a t.
  Output_rep( const T& tt) : t(tt) {}
  //! perform the output, calls \c operator\<\< by default.
  std::ostream& operator()( std::ostream& out) const {
    if(IO::is_ascii(out)) {
      out << t.first << " " << t.second;
    } else {
      CGAL::write(out, t.first);
      CGAL::write(out, t.second);
    }
    return out;
  }
};

template <>
class Input_rep<MD_heterogeneous_types::Surface_patch_index> {
  typedef MD_heterogeneous_types::Surface_patch_index T;
  T& t;
public:
  //! initialize with a const reference to \a t.
  Input_rep( T& tt) : t(tt) {}
  //! perform the output, calls \c operator\<\< by default.
  std::istream& operator()( std::istream& in) const {
    if(IO::is_ascii(in)) {
      in >> t.first >> t.second;
    } else {
      CGAL::read(in, t.first);
      CGAL::read(in, t.second);
    }
    return in;
  }
};
} // end namespace CGAL

// Second: operator<< and >>
namespace std {
std::ostream& operator<<(std::ostream& out,
                         MD_heterogeneous_types::Surface_patch_index index) {
  return out << CGAL::IO::oformat(index);
}
std::istream& operator>>(std::istream& in,
                         MD_heterogeneous_types::Surface_patch_index& index) {
  return in >> CGAL::IO::iformat(index);
}
} // end namespace std

//
// Class to test I/O of Mesh_complex_3_in_triangulation_3<Mesh_domain,K>
//
template <typename Mesh_domain>
struct Test_c3t3_io {
  typedef typename Mesh_domain::Subdomain_index Subdomain_index;
  typedef typename Mesh_domain::Surface_patch_index Surface_patch_index;
  typedef typename Mesh_domain::Curve_index Curve_index;
  typedef typename Mesh_domain::Corner_index Corner_index;
  typedef typename Mesh_domain::Index Index;

  typedef typename CGAL::Mesh_triangulation_3<Mesh_domain, K>::Type Tr;
  typedef CGAL::Mesh_complex_3_in_triangulation_3<
    Tr,
    Corner_index,
    Curve_index
    > C3t3;

  typedef typename Tr::Point Point;
  typedef typename Tr::Vertex_handle Vertex_handle;
  typedef typename Tr::Edge Edge;
  typedef typename Tr::Facet Facet;
  typedef typename Tr::Cell_handle Cell_handle;

  static bool check_equality(const C3t3& c1, const C3t3& c2) {
    const Tr& t1 = c1.triangulation();
    const Tr& t2 = c2.triangulation();
    if(t1.number_of_vertices() != t2.number_of_vertices()) {
      assert(false);
      return false;
    }
    if(t1.number_of_vertices() != t2.number_of_vertices()) {
      assert(false);
      return false;
    }
    if(c1.number_of_cells_in_complex() != c2.number_of_cells_in_complex()) {
      assert(false);
      return false;
    }
    if(c1.number_of_facets_in_complex() != c2.number_of_facets_in_complex()) {
      assert(false);
      return false;
    }
    for(typename Tr::Finite_vertices_iterator
          vit1 = t1.finite_vertices_begin(),
          vit2 = t2.finite_vertices_begin(),
          end1 = t1.finite_vertices_end();
        vit1 != end1; ++vit1, ++vit2)
    {
      if(!( vit1->in_dimension() == vit2->in_dimension() &&
            vit1->index() == vit2->index()) )
      {
        std::cerr << "Error: vertices are different:\n";
        std::cerr << *vit1 << "\n"
                  << *vit2 << std::endl;
        assert(false);
        return false;
      }
    }
#if 0
    // Note: The Triangulation_3 facets iterator order changes after a reload
    for(typename Tr::Finite_facets_iterator
          fit1 = t1.finite_facets_begin(),
          fit2 = t2.finite_facets_begin(),
          end1 = t1.finite_facets_end();
        fit1 != end1; ++fit1, ++fit2)
    {
      typename Tr::Cell_handle c1 = fit1->first;
      typename Tr::Cell_handle c2 = fit2->first;
      int i1 = fit1->second;
      int i2 = fit2->second;
      // CJ: this may cause an assertion because the 2 C3T3s may have
      // facets in different orders.
      // This is because the Finite_facets_iterator compares the
      // addresses of cells to ensure parsing unique facets. The
      // facets are stored in a Compact_container, which doesn't
      /// guarantee any order in memory (and even in the container itself)
      assert(i1 == i2);
      if( c1->surface_patch_index(i1) !=
          c2->surface_patch_index(i2) )
      {
        std::cerr << "Error: facets #" << i1
                  << "of the following cells are different:\n";
        std::cerr << *c1 << "\n"
                  << *c2 << std::endl;
        assert(false);
        return false;
      }
    }
#endif // not WIN32
    for(typename Tr::Finite_cells_iterator
          cit1 = t1.finite_cells_begin(),
          cit2 = t2.finite_cells_begin(),
          end1 = t1.finite_cells_end();
        cit1 != end1; ++cit1, ++cit2)
    {
      if(cit1->subdomain_index() != cit2->subdomain_index() )
      {
        std::cerr << "Error: cells are different:\n";
        std::cerr << *cit1 << "\n"
                  << *cit2 << std::endl;
        assert(false);
        return false;
      }
      for(int i = 0; i < 4; ++i) {
        if( cit1->surface_patch_index(i) !=
            cit2->surface_patch_index(i) )
        {
          std::cerr << "Error: cells are different:\n";
          std::cerr << *cit1 << "\n"
                    << *cit2 << "\n"
                    << "surface_patch_index(" << i << "):\n"
                    << cit1->surface_patch_index(i) << "\n"
                    << cit2->surface_patch_index(i) << "\n";
          assert(false);
          return false;
        }
      }
    }
    return true;
  }

  static bool test_io(const C3t3& c3t3, const char* prefix, bool binary) {
    std::string filename(prefix);
    std::cout << "test_io";
    std::ios_base::openmode mode = std::ios_base::in | std::ios_base::out;
    if(binary) {
      filename += ".binary";
      std::cout << " in binary mode";
      mode |= std::ios_base::binary;
    }
    filename += ".cgal";
    std::cout << std::endl;
    std::cout << "IO format: " << CGAL::Get_io_signature<C3t3>()() << std::endl;
    std::stringstream stream(mode);
    if(binary) {
      CGAL::IO::set_binary_mode(stream);
    }
    stream << c3t3;
    if(!binary) {
      std::cout << "Content of the stream:\n"
                << "*****begin*****\n"
                << stream.str()
                << "\n******end******" << std::endl;
    }
    stream.seekg(0);
    assert(stream);
    C3t3 c3t3_bis;
    stream >> c3t3_bis;
    std::cout << "Content of c3t3_bis:\n"
              << "*****begin*****\n"
              << c3t3_bis
              << "\n******end******" << std::endl;
    assert(stream);

    {
      std::ostringstream ss_c3t3, ss_c3t3_bis;
      ss_c3t3 << c3t3;
      ss_c3t3_bis << c3t3_bis;
      assert(ss_c3t3);
      assert(ss_c3t3_bis);
      assert(ss_c3t3.str() == ss_c3t3_bis.str());
      if(!check_equality(c3t3, c3t3_bis)) return false;
    }

    c3t3_bis.clear();
    {
      std::stringstream ss(std::ios_base::out |
                           ( binary ?
                             (std::ios_base::in | std::ios_base::binary)
                             : std::ios_base::in));
      CGAL::IO::save_binary_file(ss, c3t3, binary);
      assert(ss);
      CGAL::IO::load_binary_file(ss, c3t3_bis);
      assert(ss);
    }
    if(!check_equality(c3t3, c3t3_bis)) return false;

#ifndef CGAL_LITTLE_ENDIAN
    // skip binary I/O with the existing file for big endian
    return true;
#endif

    if (sizeof(void*) != 8) {
      // skip 32bits architectures as well
      return true;
    }

    {
      std::string filename(prefix);
      filename += "_new";
      if(binary) filename += ".binary";
      filename += ".cgal";
      std::ofstream output(filename.c_str(),
                           binary ? (std::ios_base::out | std::ios_base::binary)
                           : std::ios_base::out);
      CGAL::IO::save_binary_file(output, c3t3_bis, binary);
    }

    c3t3_bis.clear();
    {
      std::ifstream input(filename.c_str(),
                          binary ? (std::ios_base::in | std::ios_base::binary)
                          : std::ios_base::in);
      assert(input);
      CGAL::IO::load_binary_file(input, c3t3_bis);
      assert(input);
    }
    if(!check_equality(c3t3, c3t3_bis)) return false;

    return true;
  }

  bool operator()(const char* prefix) const {
    CGAL::Get_io_signature<C3t3> get_io_signature; CGAL_USE(get_io_signature);
    assert(get_io_signature() == Mesh_domain::reference_format_string());
    // Create a C3t3
    C3t3 c3t3;
    Tr& tr = c3t3.triangulation();
    Vertex_handle v1 = tr.insert(Point(10,11,12));
    Vertex_handle v2 = tr.insert(Point(11,13,10));
    Vertex_handle v3 = tr.insert(Point(7,4,6));
    Vertex_handle v4 = tr.insert(Point(5,2,14));
    Vertex_handle v5 = tr.insert(Point(1,2,3));
    Vertex_handle v6 = tr.insert(Point(3,9,13));

    Edge e1 = *(c3t3.triangulation().finite_edges_begin());
    Edge e2 = *(++c3t3.triangulation().finite_edges_begin());

    Facet f1 = *(c3t3.triangulation().finite_facets_begin());
    Facet f2 = *(++c3t3.triangulation().finite_facets_begin());

    Cell_handle c1 = c3t3.triangulation().finite_cells_begin();
    Cell_handle c2 = ++c3t3.triangulation().finite_cells_begin();

    c3t3.add_to_complex(c1, Mesh_domain::get_sub_domain_index_1());
    c3t3.add_to_complex(c2, Mesh_domain::get_sub_domain_index_2());
    c3t3.add_to_complex(f1, Mesh_domain::get_surface_patch_index_1());
    c3t3.add_to_complex(f2, Mesh_domain::get_surface_patch_index_2());
    c3t3.add_to_complex(e1, Mesh_domain::get_curve_index_1());
    c3t3.add_to_complex(e2, Mesh_domain::get_curve_index_2());
    c3t3.add_to_complex(v1, Mesh_domain::get_corner_index_1());
    c3t3.add_to_complex(v2, Mesh_domain::get_corner_index_2());

    // Fill indices in various faces (with different dimensions)
    v1->set_dimension(0);
    v1->set_index(Mesh_domain::get_corner_index_1());
    v2->set_dimension(0);
    v2->set_index(Mesh_domain::get_corner_index_2());
    v3->set_dimension(1);
    v3->set_index(Mesh_domain::get_curve_index_1());
    v4->set_dimension(1);
    v4->set_index(Mesh_domain::get_curve_index_2());
    v5->set_dimension(2);
    v5->set_index(Mesh_domain::get_surface_patch_index_1());
    v6->set_dimension(3);
    v6->set_index(Mesh_domain::get_sub_domain_index_1());

    // then test I/O
    return test_io(c3t3, prefix, false) && test_io(c3t3, prefix, true);
  }
};

int main()
{
  std::cout << "First test I/O when all indices are integers" << std::endl;
  bool ok = Test_c3t3_io<MD_homogeneous_types>()("data/c3t3_io-homo");
  if(!ok) {
    std::cerr << "Error\n";
    return -1;
  }
  std::cout << "Then test I/O when all indices are different types" << std::endl;
  ok = Test_c3t3_io<MD_heterogeneous_types>()("data/c3t3_io-hetero");
  if(!ok) {
    std::cerr << "Error\n";
    return -1;
  }
}


#define BOOST_TEST_MODULE "test_wca_potential"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <test/util/check_potential.hpp>
#include <mjolnir/forcefield/global/ParameterList.hpp>
#include <mjolnir/forcefield/global/WCAPotential.hpp>

BOOST_AUTO_TEST_CASE(WCA_double)
{
    using real_type = double;
    using potential_type = mjolnir::WCAPotential<real_type>;
    using parameter_type = potential_type::parameter_type;

    constexpr static std::size_t N = 10000;
    constexpr static real_type   h = 1e-6;
    constexpr static real_type tol = 1e-6;

    const real_type sigma   = 3.0;
    const real_type epsilon = 1.0;

    potential_type potential;

    const real_type x_min = 0.8 * sigma;
    const real_type x_max = potential.cutoff_ratio() * sigma - 2 * h;

    mjolnir::test::check_potential(potential, parameter_type{sigma, epsilon},
                                   x_min, x_max, tol, h, N);
}

BOOST_AUTO_TEST_CASE(WCA_float)
{
    using real_type = double;
    using potential_type = mjolnir::WCAPotential<real_type>;
    using parameter_type = potential_type::parameter_type;

    constexpr static std::size_t N = 1000;
    constexpr static real_type   h = 0.002f;
    constexpr static real_type tol = 0.005f;

    const real_type sigma   = 3.0f;
    const real_type epsilon = 1.0f;

    potential_type potential;

    const real_type x_min = 0.8f * sigma;
    const real_type x_max = potential.cutoff_ratio() * sigma - 2 * h;

    mjolnir::test::check_potential(potential, parameter_type{sigma, epsilon},
                                   x_min, x_max, tol, h, N);
}

BOOST_AUTO_TEST_CASE(WCA_cutoff_length)
{
    using real_type = double;
    using traits_type = mjolnir::SimulatorTraits<real_type, mjolnir::UnlimitedBoundary>;
    using potential_type = mjolnir::WCAPotential<real_type>;

    using parameter_list_type  = mjolnir::LorentzBerthelotRule<traits_type, potential_type>;
    using parameter_type       = parameter_list_type::parameter_type;
    using ignore_molecule_type = parameter_list_type::ignore_molecule_type;
    using ignore_group_type    = parameter_list_type::ignore_group_type;

    potential_type potential;

    // generate a parameter
    std::vector<std::pair<std::size_t, parameter_type>> parameters(10);
    for(std::size_t i=0; i<parameters.size(); ++i)
    {
        parameters.at(i).first          = i;
        parameters.at(i).second.epsilon = 1.0;
        parameters.at(i).second.sigma   = static_cast<real_type>(i) + 1.0;
    }
    parameter_list_type parameter_list(parameters, {}, ignore_molecule_type{"Nothing"}, ignore_group_type({}));

    // estimated maximum cutoff (from per-particle parameter)
    const auto max_cutoff = potential.max_cutoff(parameter_list.parameters().begin(), parameter_list.parameters().end());

    // secure maximum cutoff calculated from per-pair parameter
    // This requires O(N^2), so it will not calculated in usual cases
    real_type max_abs_cutoff = 0.0;
    for(std::size_t i=0; i<parameters.size(); ++i)
    {
        for(std::size_t j=0; j<parameters.size(); ++j)
        {
            max_abs_cutoff = std::max(max_abs_cutoff, potential.absolute_cutoff(parameter_list.prepare_params(i, j)));
        }
    }

    // max cutoff estimated from per-particle parameter should be the same or larger than the secure max cutoff
    BOOST_TEST(max_abs_cutoff <= max_cutoff);

    const real_type safety = 1.0 + std::numeric_limits<real_type>::epsilon();
    for(std::size_t i=0; i<parameters.size(); ++i)
    {
        for(std::size_t j=0; j<parameters.size(); ++j)
        {
            BOOST_TEST(potential.potential(max_abs_cutoff * safety, parameter_list.prepare_params(i, j)) == 0.0);
        }
    }
}


//   Copyright 2015 Patrick Putnam
//
//   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.
#ifndef QTL_HASH_CLASSIFIER_HPP_
#define QTL_HASH_CLASSIFIER_HPP_

#include "qtl_allele.h"

#include "clotho/utility/random_generator.hpp"
#include "clotho/classifiers/hash_classifier.hpp"
#include "clotho/classifiers/helpers/normalize.hpp"

#include <boost/random/poisson_distribution.hpp>

namespace clotho {
namespace classifiers {
namespace helpers {

// used in std::hash< clotho::classifiers::hash_classifier< Element, Bins>> specialization
inline double normalize_arg( qtl_allele const& q ) {
    return q.m_key;
}

}   // namespace helpers {
}   // namespace classifiers {
}   // namespace clotho {

namespace clotho {
namespace utility {

template < class URNG, unsigned int Bins >
class random_generator< URNG, clotho::classifiers::hash_classifier< qtl_allele, Bins > > {
public:
    typedef clotho::classifiers::hash_classifier< qtl_allele, Bins > result_type;

    typedef random_generator< URNG, result_type > self_type;

    typedef boost::random::uniform_01< double > position_dist_type;
    typedef boost::random::poisson_distribution< unsigned int, double > event_dist_type;

    random_generator( URNG & rng, boost::property_tree::ptree & config ) :
        m_rng( &rng )
        , m_lambda( DEFAULT_EVENT_RATE )
    {
        parseConfig( config );
    }

    result_type operator()() {
        result_type res;

        unsigned int N = m_event_dist( *m_rng );

        typename result_type::param_type::bin_type _sum = 0;
        unsigned int bin_idx = 0;
        double accum = 0.;
        qtl_allele::trait_weights coeff;

        while( N ) {
            accum += (log( m_pos_dist( *m_rng ) ) / (double) N);

            res.param()->class_bounds.push_back( qtl_allele( (1.0 - exp(accum)), DEFAULT_SELECTION, DEFAULT_DOMINANCE, DEFAULT_NEUTRAL, 0, coeff) );

            unsigned int bidx = m_hash( res.params()->class_bounds.back() );

            while( bin_idx < bidx ) {
                res.param()->bin_psum[ ++bin_idx ] = _sum;
            }

            ++_sum;
            --N;
        }

        while( bin_idx <= result_type::BINS ) {
            res.param()->bin_psum[ bin_idx++ ] = _sum;
        }

        return res;
    }

    virtual ~random_generator() {}
protected:
    URNG * m_rng;
    position_dist_type              m_pos_dist;
    event_dist_type                 m_event_dist;
    typename result_type::hash_type m_hash;
};

}   // namespace utility
}   // namespace clotho

#endif  // QTL_HASH_CLASSIFIER_HPP_


//  (C) Copyright Nick Thompson 2019.
//  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_MATH_INTERPOLATORS_CARDINAL_TRIGONOMETRIC_HPP
#define BOOST_MATH_INTERPOLATORS_CARDINAL_TRIGONOMETRIC_HPP
#include <memory>
#include <boost/math/interpolators/detail/cardinal_trigonometric_detail.hpp>

namespace boost { namespace math { namespace interpolators {

template<class RandomAccessContainer>
class cardinal_trigonometric
{
public:
    using Real = typename RandomAccessContainer::value_type;
    cardinal_trigonometric(RandomAccessContainer const & v, Real t0, Real h)
    {
        m_impl = std::make_shared<interpolators::detail::cardinal_trigonometric_detail<Real>>(v.data(), v.size(), t0, h);
    }

    Real operator()(Real t) const
    {
        return m_impl->operator()(t);
    }

    Real prime(Real t) const
    {
        return m_impl->prime(t);
    }

    Real double_prime(Real t) const
    {
        return m_impl->double_prime(t);
    }

    Real period() const
    {
        return m_impl->period();
    }

    Real integrate() const
    {
        return m_impl->integrate();
    }

    Real squared_l2() const
    {
        return m_impl->squared_l2();
    }

private:
    std::shared_ptr<interpolators::detail::cardinal_trigonometric_detail<Real>> m_impl;
};

}}}
#endif


#include "hashing.h"
#include <NTL/ZZ.h>
#include "encryption.h"

namespace CryptoHelpers
{
    void Hash(const std::vector<NTL::ZZ>& source, NTL::ZZ& hashValue)
    {
        hashValue = 0;

        for(const auto& chunk: source)
        {
            auto newKey = hashValue^chunk;
            auto currentHash = hashValue;
            Encrypt(currentHash, hashValue, &newKey);
            hashValue ^= currentHash;
        }
    }

    void Hash(const std::vector<unsigned long long>& source, unsigned long long& hashValue)
    {
        hashValue = 0;

        for (const auto& chunk : source)
        {
            auto newKey = hashValue^chunk;
            auto currentHash = hashValue;
            Encrypt(currentHash, hashValue, &newKey);

            hashValue ^= currentHash;
        }
    }
}


#pragma once

#include <iostream>
#include <chrono>
#include <memory>
#include <boost/optional.hpp>

#include <opencv2/core/mat.hpp>
#undef Success // For 'Success' conflict
#include <eigen3/Eigen/Dense>
#include <GL/gl.h>
#include <GLFW/glfw3.h>
//#undef Complex // For 'Complex' conflict
#include "phonebook.hpp"
#include "switchboard.hpp"

// Tell gldemo and timewarp_gl to use two texture handle for left and right eye
#define USE_ALT_EYE_FORMAT
#define NANO_SEC 1000000000.0

namespace ILLIXR {

	typedef std::chrono::time_point<std::chrono::system_clock> time_type;
	typedef unsigned long long ullong;

	// Data type that combines the IMU and camera data at a certain timestamp.
	// If there is only IMU data for a certain timestamp, img0 and img1 will be null
	// time is the current UNIX time where dataset_time is the time read from the csv
	typedef struct {
		time_type time;
		Eigen::Vector3f angular_v;
		Eigen::Vector3f linear_a;
		std::optional<cv::Mat*> img0;
		std::optional<cv::Mat*> img1;
		ullong dataset_time;
	} imu_cam_type;

	typedef struct {
		Eigen::Matrix<double,3,1> w_hat;
		Eigen::Matrix<double,3,1> a_hat;
		Eigen::Matrix<double,3,1> w_hat2;
		Eigen::Matrix<double,3,1> a_hat2;
		Eigen::Matrix<double,13,1> state_plus;
		time_type imu_time;
	} imu_raw_type;

	typedef struct {
	  int64_t time;
	  const unsigned char* rgb;
	  const unsigned short* depth;
	} rgb_depth_type;

	typedef struct {
		time_type sensor_time; // Recorded time of sensor data ingestion
		Eigen::Vector3f position;
		Eigen::Quaternionf orientation;
	} pose_type;

	typedef struct {
		pose_type pose;
		time_type predict_computed_time; // Time at which the prediction was computed
		time_type predict_target_time; // Time that prediction targeted.
	} fast_pose_type;

	typedef struct {
		int pixel[1];
	} camera_frame;

	// Single-texture format; arrayed by left/right eye
	// Single-texture format; arrayed by left/right eye
	struct rendered_frame {
		GLuint texture_handle;
		pose_type render_pose; // The pose used when rendering this frame.
		std::chrono::time_point<std::chrono::system_clock> sample_time;
	};

	// Using arrays as a swapchain
	// Array of left eyes, array of right eyes
	// This more closely matches the format used by Monado
	struct rendered_frame_alt {
		GLuint texture_handles[2]; // Does not change between swaps in swapchain
		GLuint swap_indices[2]; // Which element of the swapchain
		fast_pose_type render_pose; // The pose used when rendering this frame.
		std::chrono::time_point<std::chrono::system_clock> sample_time;
		std::chrono::time_point<std::chrono::system_clock> render_time;
	};

	typedef struct {
		int seq;
	} hologram_input;

	typedef struct {
		int dummy;
	} hologram_output;

	/* I use "accel" instead of "3-vector" as a datatype, because
	this checks that you meant to use an acceleration in a certain
	place. */
	struct accel { };

	// High-level HMD specification, timewarp plugin
	// may/will calculate additional HMD info based on these specifications
	struct hmd_physical_info {
		float   ipd;
		int		displayPixelsWide;
		int		displayPixelsHigh;
		float	chromaticAberration[4];
		float	K[11];
		int		visiblePixelsWide;
		int		visiblePixelsHigh;
		float	visibleMetersWide;
		float	visibleMetersHigh;
		float	lensSeparationInMeters;
		float	metersPerTanAngleAtCenter;
	};
}


#ifndef STAN_MATH_PRIM_SCAL_PROB_BERNOULLI_RNG_HPP
#define STAN_MATH_PRIM_SCAL_PROB_BERNOULLI_RNG_HPP

#include <boost/random/bernoulli_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <stan/math/prim/scal/err/check_consistent_sizes.hpp>
#include <stan/math/prim/scal/err/check_bounded.hpp>
#include <stan/math/prim/scal/err/check_finite.hpp>
#include <stan/math/prim/scal/err/check_not_nan.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/fun/inv_logit.hpp>
#include <stan/math/prim/scal/fun/log1m.hpp>
#include <stan/math/prim/scal/fun/value_of.hpp>
#include <stan/math/prim/scal/meta/include_summand.hpp>

namespace stan {
namespace math {

/**
 * Return pseudorandom Bernoulli draw with specified chance of success
 * using the specified random number generator.
 *
 * @tparam RNG type of random number generator
 * @param theta chance of success parameter
 * @param rng random number generator
 * @return Bernoulli random variate
 * @throw std::domain_error if probability parameter is invalid.
 */
template <class RNG>
inline int bernoulli_rng(double theta, RNG& rng) {
  using boost::bernoulli_distribution;
  using boost::variate_generator;

  static const char* function = "bernoulli_rng";

  check_finite(function, "Probability parameter", theta);
  check_bounded(function, "Probability parameter", theta, 0, 1);

  variate_generator<RNG&, bernoulli_distribution<> > bernoulli_rng(
      rng, bernoulli_distribution<>(theta));
  return bernoulli_rng();
}

}  // namespace math
}  // namespace stan
#endif


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_REDUCTION_FUNCTIONS_SIMD_SSE_SSSE3_SUM_HPP_INCLUDED
#define BOOST_SIMD_REDUCTION_FUNCTIONS_SIMD_SSE_SSSE3_SUM_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_SSSE3_SUPPORT
#include <boost/simd/reduction/functions/sum.hpp>
#include <boost/simd/include/constants/digits.hpp>
#include <boost/fusion/tuple.hpp>
#include <boost/dispatch/meta/strip.hpp>
#include <boost/simd/include/functions/simd/first.hpp>
#include <boost/simd/include/functions/simd/second.hpp>
#include <boost/fusion/include/fold.hpp>
/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is int16_
/////////////////////////////////////////////////////////////////////////////
namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::sum_, boost::simd::tag::ssse3_,
                      (A0),
                      ((simd_<ints16_<A0>,boost::simd::tag::sse_>))
                     )
  {
    typedef typename meta::scalar_of<A0>::type        result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      A0 tmp = _mm_hadd_epi16(a0, a0);
      A0 tmp1 = _mm_hadd_epi16(tmp, tmp);
      A0 tmp2 = _mm_hadd_epi16(tmp1, tmp1);
      return  first(tmp2);
    }
  };

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is double
/////////////////////////////////////////////////////////////////////////////

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::sum_, boost::simd::tag::ssse3_,
                      (A0),
                      ((simd_<double_<A0>,boost::simd::tag::sse_>))
                     )
  {
    typedef typename meta::scalar_of<A0>::type        result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      return first(simd::bitwise_cast<A0>(_mm_hadd_pd( a0, Zero<A0>())));
    }
  };

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is int64_
/////////////////////////////////////////////////////////////////////////////

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::sum_, boost::simd::tag::ssse3_,
                      (A0),
                      ((simd_<ints64_<A0>,boost::simd::tag::sse_>))
                     )
  {
    typedef typename meta::scalar_of<A0>::type        result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      return first(a0)+second(a0);
    }
  };

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is float
/////////////////////////////////////////////////////////////////////////////

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::sum_, boost::simd::tag::ssse3_,
                      (A0),
                      ((simd_<single_<A0>,boost::simd::tag::sse_>))
                     )
  {
    typedef typename meta::scalar_of<A0>::type        result_type;

    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      A0 tmp = _mm_hadd_ps(a0, Zero<A0>());
      return  first(simd::bitwise_cast<A0>(_mm_hadd_ps(tmp, Zero<A0>())));
    }
  };

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is int32_
/////////////////////////////////////////////////////////////////////////////

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::sum_, boost::simd::tag::ssse3_,
                      (A0),
                      ((simd_<ints32_<A0>,boost::simd::tag::sse_>))
                     )
  {
    typedef typename meta::scalar_of<A0>::type      result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      A0 tmp = _mm_hadd_epi32(a0, a0);
      A0 tmp1= _mm_hadd_epi32(tmp, tmp);
      return first(tmp1);
    }
  };
} } }
#endif
#endif


#include "libsnark/gadgetlib1/gadgets/basic_gadgets.hpp"
#include "libsnark/zk_proof_systems/ppzksnark/r1cs_ppzksnark/r1cs_ppzksnark.hpp"
#include "libsnark/common/default_types/r1cs_ppzksnark_pp.hpp"
#include "libsnark/common/utils.hpp"
#include <boost/optional.hpp>

using namespace libsnark;
using namespace std;

#include "gadget_neg.hpp"

template<typename ppzksnark_ppT>
r1cs_ppzksnark_keypair<ppzksnark_ppT> generate_keypair_neg(const int jw[2][32])
{
    typedef Fr<ppzksnark_ppT> FieldT;

    //根据预先定义的计算门和约束生成公共参数秘钥对
    //证明生成端若需要采用对应的公共秘钥生成证明数据成功，则必须使两端的数据符合预先定于的计算约束(R1+X=R2+R3)
    //如此，当验证端根据对应的验证秘钥验证证明数据为真时，验证者就能够相信对应的交易中是符合预定义的计算约束的，而不是生成假证明以通过检查
    protoboard<FieldT> pb;
    l_gadget_neg<FieldT> g(pb);
    g.generate_r1cs_constraints();
    const r1cs_constraint_system<FieldT> constraint_system = pb.get_constraint_system();

    cout << "Number of R1CS constraints: " << constraint_system.num_constraints() << endl;

    return r1cs_ppzksnark_generator<ppzksnark_ppT>(constraint_system);
}

template<typename ppzksnark_ppT>
boost::optional<r1cs_ppzksnark_proof<ppzksnark_ppT>> generate_proof_neg(r1cs_ppzksnark_proving_key<ppzksnark_ppT> proving_key,
                                                                   const std::vector<bit_vector> &hash_input_vec,
																	const std::vector<bit_vector> &hash_output_vec,
																	const std::vector<bit_vector> &input_vec,
																	const std::vector<bit_vector> &output_vec,
																	const bit_vector &x,
																   const int jw[2][32]
                                                                   )
{
    typedef Fr<ppzksnark_ppT> FieldT;

    protoboard<FieldT> pb;
    l_gadget_neg<FieldT> g(pb);
    g.generate_r1cs_constraints();
    g.generate_r1cs_witness(hash_input_vec, hash_output_vec, input_vec, output_vec, x);

    if (!pb.is_satisfied()) {
      std::cout << "System not satisfied!" << std::endl;
        return boost::none;
    }

    return r1cs_ppzksnark_prover<ppzksnark_ppT>(proving_key, pb.primary_input(), pb.auxiliary_input());
}

template<typename ppzksnark_ppT>
bool verify_proof(r1cs_ppzksnark_verification_key<ppzksnark_ppT> verification_key,
                  r1cs_ppzksnark_proof<ppzksnark_ppT> proof,
                  const std::vector<bit_vector> &hash_input_vec,
				  const std::vector<bit_vector> &hash_output_vec,
				  const bit_vector &x
                 )
{
    typedef Fr<ppzksnark_ppT> FieldT;

    const r1cs_primary_input<FieldT> input = l_input_map<FieldT>(hash_input_vec, hash_output_vec, x);

    std::cout << "**** After l_input_map *****" << std::endl;

    return r1cs_ppzksnark_verifier_strong_IC<ppzksnark_ppT>(verification_key, input, proof);

}


#ifndef OCV_CBIR_SPATIAL_VERIFIER_HPP
#define OCV_CBIR_SPATIAL_VERIFIER_HPP

#include "../arma/type_traits.hpp"
#include "features_indexer.hpp"

#include <armadillo>

#include <opencv2/calib3d.hpp>
#include <opencv2/core.hpp>
#include <opencv2/features2d.hpp>
#include <opencv2/imgproc.hpp>

#include <boost/container/small_vector.hpp>

#include <algorithm>
#include <vector>

/*!
 *  \addtogroup ocv
 *  @{
 */
namespace ocv{

/*!
 *  \addtogroup cbir
 *  @{
 */
namespace cbir{

template<typename CodeBook, typename FeaturesType,
         int BufSize = 20>
class spatial_verifier
{
public:
    using result_type =
    boost::container::small_vector<size_t, BufSize>;

    explicit
    spatial_verifier(features_indexer const &fi,
                     CodeBook const &code_book,
                     float ratio = 0.75,
                     size_t min_matches = 10,
                     size_t num_result = 16,
                     double reproj_thresh = 4.0) :
        code_book_(code_book),
        fi_(fi),
        min_matches_{min_matches},
        num_result_{num_result},
        ratio_{ratio},
        reproj_thresh_{reproj_thresh}
    {
        static_assert(std::is_arithmetic<FeaturesType>::value,
                      "FeaturesType should be arimetic");
        static_assert(armd::is_two_dim<CodeBook>::value,
                      "CodeBook should be arma::Mat or arma::SpMat");
    }

    size_t get_min_matches() const
    {
        return min_matches_;
    }

    size_t get_num_result() const
    {
        return num_result_;
    }

    double get_ratio() const
    {
        return ratio_;
    }

    double get_reproj_thresh() const
    {
        return reproj_thresh_;
    }

    template<typename T>
    result_type
    rerank(std::vector<cv::KeyPoint> const &query_kp,
           cv::Mat const &query_features,
           T &&search_results) const
    {
        using decay_type = typename std::decay<T>::type;
        using value_type = typename decay_type::value_type;
        using rerank_type =
        boost::container::small_vector<std::pair<size_t, value_type>, 16>;
        using rerank_vtype = typename rerank_type::value_type;

        static_assert(std::is_class<decay_type>::value,
                      "T should be class or struct");
        static_assert(std::is_copy_assignable<decay_type>::value ||
                      std::is_move_assignable<T>::value,
                      "T should be copy assignable");
        static_assert(std::is_integral<value_type>::value,
                      "The value_type of T should be intergral");

        decay_type sort_results = std::forward<T>(search_results);
        std::sort(std::begin(sort_results), std::end(sort_results));

        std::vector<cv::KeyPoint> kp;
        cv::Mat features;
        rerank_type rerank;
        rerank.reserve(sort_results.size());
        for(auto const val : sort_results){
            fi_.read_keypoints(kp, val);
            fi_.read_image_features(features, val);
            auto const match_pt_size =
                    match(query_kp, query_features,
                          kp, features);
            rerank.emplace_back(match_pt_size, val);
        }

        std::sort(std::begin(rerank), std::end(rerank),
                  [](rerank_vtype const &lhs, rerank_vtype const &rhs)
        {
            return lhs.first > rhs.first;
        });
        result_type result;
        result.reserve(rerank.size());
        for(auto const &val : rerank){
            result.emplace_back(val.second);
        }

        return result;
    }

    void set_min_matches(size_t value)
    {
        min_matches_ = value;
    }

    void set_num_result(size_t value)
    {
        num_result_ = value;
    }

    void set_ratio(double value)
    {
        ratio_ = value;
    }

    void set_reproj_thresh(double value)
    {
        reproj_thresh_ = value;
    }

private:
    size_t match(std::vector<cv::KeyPoint> const &query_kp,
                 cv::Mat const &query_features,
                 std::vector<cv::KeyPoint> const &target_kp,
                 cv::Mat const &target_features) const
    {
        cv::BFMatcher matcher(cv::NORM_L2);
        std::vector<std::vector<cv::DMatch>> nn_matches;
        matcher.knnMatch(query_features, target_features,
                         nn_matches, 2);

        std::vector<cv::Point2f> matched1, matched2;
        for(size_t i = 0; i < nn_matches.size(); i++){
            if(nn_matches[i].size() == 2){
                float const dist1 = nn_matches[i][0].distance;
                float const dist2 = nn_matches[i][1].distance;

                if(dist1 < ratio_ * dist2) {
                    cv::DMatch const &first = nn_matches[i][0];
                    matched1.push_back(query_kp[first.queryIdx].pt);
                    matched2.push_back(target_kp[first.trainIdx].pt);
                }
            }
        }

        if(matched1.size() >= min_matches_){
            auto const homo =
                    cv::findHomography(matched1, matched2,
                                       cv::RANSAC,
                                       reproj_thresh_);
            if(!homo.empty()){
                size_t sum = 0;
                for(size_t i = 0; i != matched1.size(); ++i){
                    cv::Mat col = cv::Mat::ones(3, 1, CV_64F);
                    col.at<double>(0) = matched1[i].x;
                    col.at<double>(1) = matched1[i].y;
                    col = homo * col;
                    col /= (col.at<double>(2) + 1e-10);
                    double const dist =
                            std::sqrt(std::pow(col.at<double>(0)
                                               - matched2[i].x, 2) +
                                      std::pow(col.at<double>(1) -
                                               matched2[i].y, 2));
                    if(dist < reproj_thresh_) {
                        ++sum;
                    }
                }
                return sum;
            }
        }

        return 0;
    }

    CodeBook const &code_book_;
    features_indexer const &fi_;
    size_t min_matches_;
    size_t num_result_;
    float ratio_;
    double reproj_thresh_;
};

} /*! @} End of Doxygen Groups*/

} /*! @} End of Doxygen Groups*/

#endif // OCV_CBIR_SPATIAL_VERIFIER_HPP


#pragma once

#include <ros/ros.h>
#include <common_utilities/rviz_visualization.hpp>
#include <roboy_communication_middleware/DarkRoom.h>
#include <atomic>
#include <mutex>
#include <Eigen/Dense>
#include <Eigen/Core>
#include "darkroom/Transform.hpp"

#define degreesToTicks(degrees) (degrees * 50.0 * 8333.0 / 180.0)

using namespace std;
using namespace Eigen;

class LighthouseSimulator:public rviz_visualization, DarkRoom::Transform{
public:
    /**
     * Constructor
     * @param id lighthouse id (currently only 0 or 1 is supported)
     * @param object_pose (initial object pose wrt world)
     */
    LighthouseSimulator(int id);
    ~LighthouseSimulator();

    void PublishSensorData();

    boost::shared_ptr<boost::thread> sensor_thread;
    atomic<bool> sensor_publishing;
    mutex mux;
private:
    ros::NodeHandlePtr nh;
    ros::Publisher sensors_pub;
    boost::shared_ptr<ros::AsyncSpinner> spinner;
    map<int, Vector4d> sensor_position;
    map<int, Vector2d> sensor_angle;
    int id;
    static int class_counter;
};


/* Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <string>
#include <thread>

#include <boost/test/unit_test.hpp>
#include <boost/make_shared.hpp>

#include "tudat/basics/testMacros.h"
#include "tudat/math/basic/linearAlgebra.h"
#include "tudat/astro/basic_astro/physicalConstants.h"
#include "tudat/astro/basic_astro/unitConversions.h"

#include "tudat/interface/spice/spiceInterface.h"
#include "tudat/math/integrators/rungeKuttaCoefficients.h"
#include "tudat/astro/basic_astro/accelerationModel.h"
#include "tudat/io/basicInputOutput.h"

#include "tudat/simulation/environment_setup/body.h"
#include "tudat/simulation/estimation_setup/variationalEquationsSolver.h"
#include "tudat/simulation/environment_setup/defaultBodies.h"
#include "tudat/simulation/environment_setup/createBodies.h"
#include "tudat/simulation/estimation_setup/createNumericalSimulator.h"
#include "tudat/simulation/estimation_setup/createEstimatableParameters.h"

namespace tudat
{

namespace unit_tests
{

//Using declarations.
using namespace tudat;
using namespace tudat::estimatable_parameters;
using namespace tudat::orbit_determination;
using namespace tudat::interpolators;
using namespace tudat::numerical_integrators;
using namespace tudat::spice_interface;
using namespace tudat::simulation_setup;
using namespace tudat::basic_astrodynamics;
using namespace tudat::orbital_element_conversions;
using namespace tudat::ephemerides;
using namespace tudat::propagators;
using namespace tudat::unit_conversions;

BOOST_AUTO_TEST_SUITE( test_hybrid_arc_variational_equation_calculation )

template< typename TimeType = double , typename StateScalarType  = double >
        std::pair< std::vector< Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic > >,
std::vector< Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > >
executeHybridArcMarsAndOrbiterSensitivitySimulation(
        const Eigen::Matrix< StateScalarType, 12, 1 > initialStateDifference = Eigen::Matrix< StateScalarType, 12, 1 >::Zero( ),
        const Eigen::VectorXd parameterPerturbation = Eigen::VectorXd::Zero( 2 ),
        const bool propagateVariationalEquations = 1,
        const bool patchMultiArcs = 0,
        const std::vector< Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > forcedMultiArcInitialStates =
        std::vector< Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > >( ),
        const double arcDuration = 0.5 * 86400.0,
        const double arcOverlap  = 5.0E3 )
{

    //Load spice kernels.
    spice_interface::loadStandardSpiceKernels( );

    std::vector< std::string > bodyNames;
    bodyNames.push_back( "Sun" );
    bodyNames.push_back( "Mars" );
    bodyNames.push_back( "Jupiter" );
    bodyNames.push_back( "Earth" );

    // Specify initial time
    double initialEphemerisTime = 1.0E7;
    double finalEphemerisTime = initialEphemerisTime + 2.0 * 86400.0;
    double maximumTimeStep = 3600.0;
    double buffer = 5.0 * maximumTimeStep;

    // Create bodies needed in simulation
    BodyListSettings bodySettings =
            getDefaultBodySettings( bodyNames, initialEphemerisTime - buffer, finalEphemerisTime + buffer );

    SystemOfBodies bodies = createSystemOfBodies( bodySettings );

    bodies.createEmptyBody( "Orbiter" );
    bodies.at( "Orbiter" )->setConstantBodyMass( 5.0E3 );
    bodies.at( "Orbiter" )->setEphemeris( std::make_shared< MultiArcEphemeris >(
                                            std::map< double, std::shared_ptr< Ephemeris > >( ),
                                            "Mars", "ECLIPJ2000" ) );
    bodies.processBodyFrameDefinitions( );

    double referenceAreaRadiation = 4.0;
    double radiationPressureCoefficient = 1.2;
    std::vector< std::string > occultingBodies;
    occultingBodies.push_back( "Earth" );
    std::shared_ptr< RadiationPressureInterfaceSettings > orbiterRadiationPressureSettings =
            std::make_shared< CannonBallRadiationPressureInterfaceSettings >(
                "Sun", referenceAreaRadiation, radiationPressureCoefficient, occultingBodies );

    // Create and set radiation pressure settings
    bodies.at( "Orbiter" )->setRadiationPressureInterface(
                "Sun", createRadiationPressureInterface(
                    orbiterRadiationPressureSettings, "Orbiter", bodies ) );

    // Set accelerations between bodies that are to be taken into account.
    SelectedAccelerationMap singleArcAccelerationMap;
    std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfMars;
    accelerationsOfMars[ "Earth" ].push_back( std::make_shared< AccelerationSettings >( point_mass_gravity ) );
    accelerationsOfMars[ "Sun" ].push_back( std::make_shared< AccelerationSettings >( point_mass_gravity ) );
    accelerationsOfMars[ "Jupiter" ].push_back( std::make_shared< AccelerationSettings >( point_mass_gravity ) );
    singleArcAccelerationMap[ "Mars" ] = accelerationsOfMars;

    std::vector< std::string > singleArcBodiesToIntegrate, singleArcCentralBodies;
    singleArcBodiesToIntegrate.push_back( "Mars" );
    singleArcCentralBodies.push_back( "SSB" );

    AccelerationMap singleArcAccelerationModelMap = createAccelerationModelsMap(
                bodies, singleArcAccelerationMap, singleArcBodiesToIntegrate, singleArcCentralBodies );
    Eigen::VectorXd singleArcInitialStates = getInitialStatesOfBodies(
                singleArcBodiesToIntegrate, singleArcCentralBodies, bodies, initialEphemerisTime );

    singleArcInitialStates += initialStateDifference.segment(
                0, singleArcInitialStates.rows( ) );

    std::shared_ptr< TranslationalStatePropagatorSettings< > > singleArcPropagatorSettings =
            std::make_shared< TranslationalStatePropagatorSettings< > >(
                singleArcCentralBodies, singleArcAccelerationModelMap, singleArcBodiesToIntegrate,
                singleArcInitialStates, finalEphemerisTime );

    SelectedAccelerationMap multiArcAccelerationMap;
    std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfOrbiter;
    accelerationsOfOrbiter[ "Mars" ].push_back( std::make_shared< SphericalHarmonicAccelerationSettings >( 2, 2 ) );
    accelerationsOfOrbiter[ "Sun" ].push_back( std::make_shared< AccelerationSettings >( point_mass_gravity ) );
    accelerationsOfOrbiter[ "Sun" ].push_back( std::make_shared< AccelerationSettings >( cannon_ball_radiation_pressure ) );
    accelerationsOfOrbiter[ "Jupiter" ].push_back( std::make_shared< AccelerationSettings >( point_mass_gravity ) );
    multiArcAccelerationMap[ "Orbiter" ] = accelerationsOfOrbiter;

    std::vector< std::string > multiArcBodiesToIntegrate, multiArcCentralBodies;
    multiArcBodiesToIntegrate.push_back( "Orbiter" );
    multiArcCentralBodies.push_back( "Mars" );

    AccelerationMap multiArcAccelerationModelMap = createAccelerationModelsMap(
                bodies, multiArcAccelerationMap, multiArcBodiesToIntegrate, multiArcCentralBodies );

    // Creater arc times
    std::vector< double > integrationArcStarts, integrationArcEnds;
    //    double integrationStartTime = initialEphemerisTime;
    //    double integrationEndTime = finalEphemerisTime - 1.0E4;
    //    double currentStartTime = integrationStartTime;
    //    double currentEndTime = integrationStartTime + arcDuration;
    //    do
    //    {
    //        integrationArcStarts.push_back( currentStartTime );
    //        integrationArcEnds.push_back( currentEndTime );

    //        currentStartTime = currentEndTime - arcOverlap;
    //        currentEndTime = currentStartTime + arcDuration;
    //    }
    double timeBetweenArcs = 86400.0;
    //    double arcDuration = 0.5E6;
    double currentStartTime = initialEphemerisTime;
    double currentEndTime = initialEphemerisTime + arcDuration;
    do
    {
        integrationArcStarts.push_back( currentStartTime );
        integrationArcEnds.push_back( currentEndTime );

        currentEndTime = currentStartTime + timeBetweenArcs + arcDuration;
        currentStartTime = currentStartTime + timeBetweenArcs;
    }
    while( currentEndTime < finalEphemerisTime );

    // Create list of multi-arc initial states
    unsigned int numberOfIntegrationArcs = integrationArcStarts.size( );
    std::vector< Eigen::VectorXd > multiArcSystemInitialStates;
    multiArcSystemInitialStates.resize( numberOfIntegrationArcs );

    // Define (quasi-arbitrary) arc initial states
    double marsGravitationalParameter =  bodies.at( "Mars" )->getGravityFieldModel( )->getGravitationalParameter( );

    if( forcedMultiArcInitialStates.size( ) == 0 )
    {
        for( unsigned int j = 0; j < numberOfIntegrationArcs; j++ )
        {
            Eigen::Vector6d orbiterInitialStateInKeplerianElements;
            orbiterInitialStateInKeplerianElements( semiMajorAxisIndex ) = 6000.0E3;
            orbiterInitialStateInKeplerianElements( eccentricityIndex ) = 0.05;
            orbiterInitialStateInKeplerianElements( inclinationIndex ) = convertDegreesToRadians( 85.3 );
            orbiterInitialStateInKeplerianElements( argumentOfPeriapsisIndex )
                    = convertDegreesToRadians( 235.7 - j );
            orbiterInitialStateInKeplerianElements( longitudeOfAscendingNodeIndex )
                    = convertDegreesToRadians( 23.4 + j );
            orbiterInitialStateInKeplerianElements( trueAnomalyIndex ) = convertDegreesToRadians( 139.87 + j * 10.0 );

            // Convert state from Keplerian elements to Cartesian elements.
            multiArcSystemInitialStates[ j ]  = convertKeplerianToCartesianElements(
                        orbiterInitialStateInKeplerianElements,
                        marsGravitationalParameter ) + initialStateDifference.segment(
                        singleArcInitialStates.rows( ), 6 );
        }
    }
    else
    {
        for( unsigned int j = 0; j < numberOfIntegrationArcs; j++ )
        {
            multiArcSystemInitialStates[ j ] = forcedMultiArcInitialStates.at( j ) + initialStateDifference.segment(
                        singleArcInitialStates.rows( ), 6 );
        }
    }

    // Create propagation settings for each arc
    std::vector< std::shared_ptr< SingleArcPropagatorSettings< double > > > arcPropagationSettingsList;
    for( unsigned int i = 0; i < numberOfIntegrationArcs; i++ )
    {
        arcPropagationSettingsList.push_back(
                    std::make_shared< TranslationalStatePropagatorSettings< double > >
                    ( multiArcCentralBodies, multiArcAccelerationModelMap, multiArcBodiesToIntegrate,
                      multiArcSystemInitialStates.at( i ), integrationArcEnds.at( i ) ) );
    }

    std::shared_ptr< MultiArcPropagatorSettings< > > multiArcPropagatorSettings =
            std::make_shared< MultiArcPropagatorSettings< > >( arcPropagationSettingsList, patchMultiArcs );

    std::shared_ptr< HybridArcPropagatorSettings< > > hybridArcPropagatorSettings =
            std::make_shared< HybridArcPropagatorSettings< > >(
                singleArcPropagatorSettings, multiArcPropagatorSettings );

    std::shared_ptr< IntegratorSettings< > > singleArcIntegratorSettings =
            std::make_shared< IntegratorSettings< > >
            ( rungeKutta4, initialEphemerisTime, 60.0 );

    std::shared_ptr< IntegratorSettings< > > multiArcIntegratorSettings =
            std::make_shared< IntegratorSettings< > >
            ( rungeKutta4, initialEphemerisTime, 45.0 );

    // Define parameters.
    std::vector< std::shared_ptr< EstimatableParameterSettings > > parameterNames;
    {
        parameterNames =
                getInitialStateParameterSettings< double >( hybridArcPropagatorSettings, bodies, integrationArcStarts );
        parameterNames.push_back( std::make_shared< EstimatableParameterSettings >( "Sun", gravitational_parameter ) );
        parameterNames.push_back( std::make_shared< EstimatableParameterSettings >( "Mars", gravitational_parameter ) );
    }

    // Create parameters
    std::shared_ptr< estimatable_parameters::EstimatableParameterSet< StateScalarType > > parametersToEstimate =
            createParametersToEstimate( parameterNames, bodies );

    // Perturb parameters.
    Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > parameterVector =
            parametersToEstimate->template getFullParameterValues< StateScalarType >( );

    parameterVector.block( parameterVector.rows( ) - 2, 0, 2, 1 ) += parameterPerturbation;
    parametersToEstimate->resetParameterValues( parameterVector );

    std::pair< std::vector< Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic > >,
            std::vector< Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > > results;
    {
        // Create dynamics simulator
        HybridArcVariationalEquationsSolver< StateScalarType, TimeType > variationalEquations =
                HybridArcVariationalEquationsSolver< StateScalarType, TimeType >(
                    bodies, singleArcIntegratorSettings, multiArcIntegratorSettings,
                    hybridArcPropagatorSettings, parametersToEstimate, integrationArcStarts );

        // Propagate requested equations.
        if( propagateVariationalEquations )
        {
            variationalEquations.integrateVariationalAndDynamicalEquations(
                        variationalEquations.getPropagatorSettings( )->getInitialStates( ), 1 );
        }
        else
        {
            variationalEquations.integrateDynamicalEquationsOfMotionOnly(
                        variationalEquations.getPropagatorSettings( )->getInitialStates( ) );
        }

        // Retrieve test data
        for( unsigned int arc = 0; arc < integrationArcEnds.size( ); arc++ )
        {
            double testEpoch = integrationArcEnds.at( arc ) - 2.0E4;
            Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > testStates =
                    Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >::Zero( 12 );
            testStates.block( 0, 0, 6, 1 ) = bodies.at( "Mars" )->getStateInBaseFrameFromEphemeris( testEpoch );

            testStates.block( 6, 0, 6, 1 ) = bodies.at( "Orbiter" )->getStateInBaseFrameFromEphemeris( testEpoch );/* -
                    testStates.block( 0, 0, 6, 1 );*/

            if( propagateVariationalEquations )
            {
                results.first.push_back( variationalEquations.getStateTransitionMatrixInterface( )->
                                         getCombinedStateTransitionAndSensitivityMatrix( testEpoch ) );
                results.second.push_back( hybridArcPropagatorSettings->getMultiArcPropagatorSettings( )->getInitialStateList( ).at( arc ) );

                Eigen::MatrixXd testMatrixDirect =
                        variationalEquations.getStateTransitionMatrixInterface( )->
                          getCombinedStateTransitionAndSensitivityMatrix( testEpoch );
                Eigen::MatrixXd testMatrixFull=
                        variationalEquations.getStateTransitionMatrixInterface( )->
                          getFullCombinedStateTransitionAndSensitivityMatrix( testEpoch );

                TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                            testMatrixDirect.block( 0, 0, 12, 6 ),
                            testMatrixFull.block( 0, 0, 12, 6 ),
                            std::numeric_limits< double >::epsilon( ) );

                TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                            testMatrixDirect.block( 0, 6, 12, 6 ),
                            testMatrixFull.block( 0, 6 * ( arc + 1 ), 12, 6 ),
                            std::numeric_limits< double >::epsilon( ) );

                TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                            testMatrixDirect.block( 0, 12, 12, 2 ),
                            testMatrixFull.block( 0, 18, 12, 2 ),
                            std::numeric_limits< double >::epsilon( ) );
            }
            else
            {
                results.second.push_back( testStates );
            }
        }
    }
    return results;
}

BOOST_AUTO_TEST_CASE( testMarsAndOrbiterHybridArcVariationalEquationCalculation )
{
    std::pair< std::vector< Eigen::MatrixXd >, std::vector< Eigen::VectorXd > > currentOutput;

    // Define variables for numerical differentiation
    Eigen::Matrix< double, 12, 1>  perturbedState;
    Eigen::Vector2d perturbedParameter;

    Eigen::Matrix< double, 12, 1> statePerturbation;
    Eigen::VectorXd parameterPerturbation;

    // Define parameter perturbation
    parameterPerturbation  = ( Eigen::VectorXd( 2 ) << 1.0E20, 1.0E10 ).finished( );
    statePerturbation = ( Eigen::Matrix< double, 12, 1>( )<<
                              1.0E10, 1.0E10, 1.0E10, 5.0E4, 5.0E4, 10.0E4,
                              10.0, 10.0, 10.0, 0.1, 0.1, 0.1 ).finished( );

    for( unsigned int patchArcs = 0; patchArcs < 1; patchArcs++ )
    {
        // Test for all requested propagator types.
        for( unsigned int k = 0; k < 1; k++ )
        {
            std::cout<<"Propagating state transition: "<<patchArcs<<" "<<" "<<k<<std::endl;
            // Compute state transition and sensitivity matrices
            currentOutput = executeHybridArcMarsAndOrbiterSensitivitySimulation < double, double >(
                        Eigen::Matrix< double, 12, 1 >::Zero( ), Eigen::VectorXd::Zero( 2 ), true, patchArcs );
            std::vector< Eigen::MatrixXd > stateTransitionAndSensitivityMatrixAtEpoch = currentOutput.first;

            std::vector< Eigen::VectorXd > nominalArcStartStates;
            if( patchArcs )
            {
                nominalArcStartStates = currentOutput.second;
            }

            std::vector< Eigen::MatrixXd > manualPartial;
            manualPartial.resize( stateTransitionAndSensitivityMatrixAtEpoch.size( ) );
            for( unsigned int arc = 0; arc < manualPartial.size( ); arc++ )
            {
                manualPartial[ arc ] = Eigen::MatrixXd::Zero( 12, 14 );
            }

            // Numerically compute state transition matrix
            for( unsigned int j = 0; j < 12; j++ )
            {
//                std::cout<<"Propagating perturbation "<<j<<std::endl;
                std::vector< Eigen::VectorXd > upPerturbedState, upPerturbedState2, downPerturbedState2, downPerturbedState;
                perturbedState.setZero( );
                perturbedState( j ) += statePerturbation( j );
                upPerturbedState = executeHybridArcMarsAndOrbiterSensitivitySimulation< double, double >(
                            perturbedState, Eigen::VectorXd::Zero( 2 ), false, false, nominalArcStartStates ).second;

                perturbedState.setZero( );
                perturbedState( j ) += 0.5 * statePerturbation( j );
                upPerturbedState2 = executeHybridArcMarsAndOrbiterSensitivitySimulation< double, double >(
                            perturbedState, Eigen::VectorXd::Zero( 2 ), false, false, nominalArcStartStates ).second;

                perturbedState.setZero( );
                perturbedState( j ) -= 0.5 * statePerturbation( j );
                downPerturbedState2 = executeHybridArcMarsAndOrbiterSensitivitySimulation< double, double >(
                            perturbedState, Eigen::VectorXd::Zero( 2 ), false, false, nominalArcStartStates ).second;

                perturbedState.setZero( );
                perturbedState( j ) -= statePerturbation( j );
                downPerturbedState = executeHybridArcMarsAndOrbiterSensitivitySimulation< double, double >(
                            perturbedState, Eigen::VectorXd::Zero( 2 ), false, false, nominalArcStartStates ).second;

                for( unsigned int arc = 0; arc < upPerturbedState.size( ); arc++ )
                {
                    manualPartial[ arc ].block( 0, j, 12, 1 ) =
                            ( -upPerturbedState[ arc ] + 8.0 * upPerturbedState2[ arc ] - 8.0 * downPerturbedState2[ arc ] + downPerturbedState[ arc ] ) /
                            ( 6.0 * statePerturbation( j ) );
                }
            }

            //Numerically compute sensitivity matrix
            for( unsigned int j = 0; j < 2; j ++ )
            {
                std::vector< Eigen::VectorXd > upPerturbedState, upPerturbedState2, downPerturbedState2, downPerturbedState;
                perturbedState.setZero( );
                Eigen::Vector2d upPerturbedParameter, downPerturbedParameter;

                perturbedParameter.setZero( );
                perturbedParameter( j ) += parameterPerturbation( j );
                upPerturbedState = executeHybridArcMarsAndOrbiterSensitivitySimulation< double, double >(
                            perturbedState, perturbedParameter, false, false, nominalArcStartStates).second;

                perturbedParameter.setZero( );
                perturbedParameter( j ) += 0.5 * parameterPerturbation( j );
                upPerturbedState2 = executeHybridArcMarsAndOrbiterSensitivitySimulation< double, double >(
                            perturbedState, perturbedParameter, false, false, nominalArcStartStates ).second;

                perturbedParameter.setZero( );
                perturbedParameter( j ) -= 0.5 * parameterPerturbation( j );
                downPerturbedState2 = executeHybridArcMarsAndOrbiterSensitivitySimulation< double, double >(
                            perturbedState, perturbedParameter, false, false, nominalArcStartStates ).second;

                perturbedParameter.setZero( );
                perturbedParameter( j ) -= parameterPerturbation( j );
                downPerturbedState = executeHybridArcMarsAndOrbiterSensitivitySimulation< double, double >(
                            perturbedState, perturbedParameter, false, false, nominalArcStartStates ).second;

                for( unsigned int arc = 0; arc < upPerturbedState.size( ); arc++ )
                {
                    manualPartial[ arc ].block( 0, j + 12, 12, 1 ) =
                            ( -upPerturbedState[ arc ] + 8.0 * upPerturbedState2[ arc ] - 8.0 * downPerturbedState2[ arc ] + downPerturbedState[ arc ] ) /
                            ( 6.0 * parameterPerturbation( j ) );
                }
            }

            for( unsigned int arc = 0; arc < manualPartial.size( ); arc++ )
            {
                TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                            stateTransitionAndSensitivityMatrixAtEpoch.at( arc ).block( 0, 0, 6, 6 ),
                            manualPartial.at( arc ).block( 0, 0, 6, 6 ), 5.0E-5 );
                TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                            stateTransitionAndSensitivityMatrixAtEpoch.at( arc ).block( 6, 6, 6, 6 ),
                            manualPartial.at( arc ).block( 6, 6, 6, 6 ), 5.0E-5 );

                double couplingTolerance;
                if( arc == 0 )
                {
                    couplingTolerance = 5.0E-1;
                }
                else if( arc == 1 || patchArcs )
                {
                    couplingTolerance = 5.0E-2;
                }
                else
                {
                    couplingTolerance = 1.0E-2;
                }

                // One component is, by chance, not computed to within relative precision (due to small numerical value),
                // next lines mitigate
                if( patchArcs == 0 )
                {
                    stateTransitionAndSensitivityMatrixAtEpoch[ arc ]( 7, 4 ) = 0.0;
                    manualPartial[ arc ]( 7, 4 ) = 0.0;
                }

                TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                            stateTransitionAndSensitivityMatrixAtEpoch.at( arc ).block( 6, 0, 6, 6 ),
                            manualPartial.at( arc ).block( 6, 0, 6, 6 ), couplingTolerance );
                TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                            stateTransitionAndSensitivityMatrixAtEpoch.at( arc ).block( 0, 12, 6, 1 ),
                            manualPartial.at( arc ).block( 0, 12, 6, 1 ), 5.0E-5 );
                TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                            stateTransitionAndSensitivityMatrixAtEpoch.at( arc ).block( 6, 12, 6, 1 ),
                            manualPartial.at( arc ).block( 6, 12, 6, 1 ), 5.0E-3 );
                TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                            stateTransitionAndSensitivityMatrixAtEpoch.at( arc ).block( 6, 13, 6, 1 ),
                            manualPartial.at( arc ).block( 6, 13, 6, 1 ), 5.0E-5 );

                //                    std::cout<<"Arc: "<<arc<<std::endl<<stateTransitionAndSensitivityMatrixAtEpoch.at( arc ).block( 6, 0, 6, 6 )<<std::endl<<std::endl<<
                //                               manualPartial.at( arc ).block( 6, 0, 6, 6 )<<std::endl<<std::endl<<
                //                               ( stateTransitionAndSensitivityMatrixAtEpoch.at( arc ) - manualPartial.at( arc ) ).block( 6, 0, 6, 6 ).cwiseQuotient(
                //                                manualPartial.at( arc ).block( 6, 0, 6, 6 ) )<<std::endl<<std::endl;
//                std::cout<<"Arc: "<<arc<<std::endl<<stateTransitionAndSensitivityMatrixAtEpoch.at( arc ).block( 0, 12, 12, 2 )<<std::endl<<std::endl<<
//                           manualPartial.at( arc ).block( 0, 12, 12, 2 )<<std::endl<<std::endl<<
//                           ( stateTransitionAndSensitivityMatrixAtEpoch.at( arc ) - manualPartial.at( arc ) ).block( 0, 12, 12, 2 ).cwiseQuotient(
//                               manualPartial.at( arc ).block( 0, 12, 12, 2 ) )<<std::endl<<std::endl;

            }

        }
    }
}

BOOST_AUTO_TEST_CASE( testVaryingCentralBodyHybridArcVariationalEquations )
{
    // Load spice kernels.
    spice_interface::loadStandardSpiceKernels( );

    // Create list of bodies to create.
    std::vector< std::string > bodyNames;
    bodyNames.push_back( "Jupiter" );
    bodyNames.push_back( "Io" );
    bodyNames.push_back( "Europa" );
    bodyNames.push_back( "Ganymede" );

    // Specify initial time
    double initialTime = 0.0;
    double finalTime = 4.0 * 86400.0;

    // Get body settings.
    BodyListSettings bodySettings =
            getDefaultBodySettings( bodyNames, initialTime - 3600.0, finalTime + 3600.0,
                                    "Jupiter", "ECLIPJ2000" );

    // Create bodies needed in simulation
    SystemOfBodies bodies = createSystemOfBodies( bodySettings );
    bodies.createEmptyBody( "Spacecraft" );
    bodies.at( "Spacecraft" )->setEphemeris( std::make_shared< MultiArcEphemeris >(
                                               std::map< double, std::shared_ptr< Ephemeris > >( ), "Jupiter", "ECLIPJ2000" ) );
    bodies.processBodyFrameDefinitions( );

    SelectedAccelerationMap singleArcAccelerationMap;
    std::vector< std::string > singleArcBodiesToPropagate = { "Io", "Europa", "Ganymede" };
    std::vector< std::string > singleArcCentralBodies = { "Jupiter", "Jupiter", "Jupiter" };

    for( unsigned int i = 0; i < singleArcBodiesToPropagate.size( ); i++ )
    {
        singleArcAccelerationMap[ singleArcBodiesToPropagate.at( i ) ][ "Jupiter" ].push_back(
                    std::make_shared< AccelerationSettings >( basic_astrodynamics::point_mass_gravity ) );

        for( unsigned int j = 0; j < singleArcBodiesToPropagate.size( ); j++ )
        {
            if( i != j )
            {
                singleArcAccelerationMap[ singleArcBodiesToPropagate.at( i ) ][ singleArcBodiesToPropagate.at( j ) ].push_back(
                            std::make_shared< AccelerationSettings >( basic_astrodynamics::point_mass_gravity ) );
            }
        }
    }

    basic_astrodynamics::AccelerationMap singleArcAccelerationModelMap = createAccelerationModelsMap(
                bodies, singleArcAccelerationMap, singleArcBodiesToPropagate, singleArcCentralBodies );

    Eigen::VectorXd singleArcInitialState = getInitialStatesOfBodies(
                singleArcBodiesToPropagate, singleArcCentralBodies, bodies, initialTime );

    std::shared_ptr< TranslationalStatePropagatorSettings< double > > singleArcPropagatorSettings =
            std::make_shared< TranslationalStatePropagatorSettings< double > >
            ( singleArcCentralBodies, singleArcAccelerationModelMap, singleArcBodiesToPropagate,singleArcInitialState, finalTime );

    std::vector< std::string > multiArcBodiesToPropagate =
    { "Spacecraft", "Spacecraft", "Spacecraft", "Spacecraft", "Spacecraft", "Spacecraft" };

    std::vector< std::string > multiArcCentralBodies =
    { "Io", "Ganymede", "Europa", "Ganymede", "Io", "Europa" };

    std::vector< double > arcStartTimes =
    { 3600.0, 3.0 * 3600, 5.0 * 3600.0, 7.0 * 3600.0, 9.0 * 3600.0, 11.0 * 3600.0 };
    std::map< std::string, std::vector< double > > arcStartTimesPerBody;

    for( unsigned int i = 0; i < arcStartTimes.size( ); i++ )
    {
        arcStartTimesPerBody[ multiArcCentralBodies.at( i ) ].push_back( arcStartTimes.at( i ) );
    }
    double arcDuration = 3600.0;

    std::vector< Eigen::VectorXd > multiArcSystemInitialStates;
    std::vector< std::shared_ptr< SingleArcPropagatorSettings< double > > > multiArcPropagationSettingsList;
    std::map< std::string, std::vector< std::shared_ptr< SingleArcPropagatorSettings< double > > > >
            multiArcPropagationSettingsListPerCentralBody;
    std::map< std::string, std::vector< int > > perBodyIndicesInFullPropagation;

    for( unsigned int i = 0; i < arcStartTimes.size( ); i++ )
    {
        Eigen::Vector6d spacecraftInitialStateInKeplerianElements;
        spacecraftInitialStateInKeplerianElements( semiMajorAxisIndex ) = 3500.0E3;
        spacecraftInitialStateInKeplerianElements( eccentricityIndex ) = 0.1;
        spacecraftInitialStateInKeplerianElements( inclinationIndex ) = unit_conversions::convertDegreesToRadians(
                    static_cast< double >( i * 30 ) );
        spacecraftInitialStateInKeplerianElements( argumentOfPeriapsisIndex ) = unit_conversions::convertDegreesToRadians(
                    static_cast< double >( i * 30 ) );
        spacecraftInitialStateInKeplerianElements( longitudeOfAscendingNodeIndex ) = unit_conversions::convertDegreesToRadians(
                    static_cast< double >( i * 30 ) );
        spacecraftInitialStateInKeplerianElements( trueAnomalyIndex ) = unit_conversions::convertDegreesToRadians(
                    static_cast< double >( i * 30 ) );
        double centralBodyGravitationalParameter = bodies.at( multiArcCentralBodies.at( i ) )->getGravityFieldModel( )->getGravitationalParameter( );
        multiArcSystemInitialStates.push_back( convertKeplerianToCartesianElements(
                                                   spacecraftInitialStateInKeplerianElements, centralBodyGravitationalParameter ) );

        SelectedAccelerationMap multiArcAccelerationMap;

        multiArcAccelerationMap[ "Spacecraft" ][ "Jupiter" ].push_back(
                    std::make_shared< AccelerationSettings >( basic_astrodynamics::point_mass_gravity ) );

        for( unsigned int i = 0; i < singleArcBodiesToPropagate.size( ); i++ )
        {
            multiArcAccelerationMap[ "Spacecraft" ][ singleArcBodiesToPropagate.at( i ) ].push_back(
                        std::make_shared< AccelerationSettings >( basic_astrodynamics::point_mass_gravity ) );
        }

        basic_astrodynamics::AccelerationMap multiArcAccelerationModelMap = createAccelerationModelsMap(
                    bodies, multiArcAccelerationMap, { multiArcBodiesToPropagate.at( i ) }, { multiArcCentralBodies.at( i ) } );

        multiArcPropagationSettingsList.push_back(
                    std::make_shared< TranslationalStatePropagatorSettings< double > >
                    ( std::vector< std::string >{ multiArcCentralBodies.at( i ) }, multiArcAccelerationModelMap,
                      std::vector< std::string >{ multiArcBodiesToPropagate.at( i ) },
                      multiArcSystemInitialStates.at( i ), arcStartTimes.at( i ) + arcDuration ) );
        multiArcPropagationSettingsListPerCentralBody[
                multiArcCentralBodies.at( i ) ].push_back(
                    std::make_shared< TranslationalStatePropagatorSettings< double > >
                    ( std::vector< std::string >{ multiArcCentralBodies.at( i ) }, multiArcAccelerationModelMap,
                      std::vector< std::string >{ multiArcBodiesToPropagate.at( i ) },
                      multiArcSystemInitialStates.at( i ), arcStartTimes.at( i ) + arcDuration ) );
        perBodyIndicesInFullPropagation[ multiArcCentralBodies.at( i ) ].push_back( i );

    }
    std::shared_ptr< MultiArcPropagatorSettings< > > multiArcPropagationSettings =
            std::make_shared< MultiArcPropagatorSettings< > >( multiArcPropagationSettingsList );

    std::shared_ptr< HybridArcPropagatorSettings< > > hybridArcPropagatorSettings =
            std::make_shared< HybridArcPropagatorSettings< > >( singleArcPropagatorSettings, multiArcPropagationSettings );

    std::vector< std::shared_ptr< EstimatableParameterSettings > > parameterNames;
    parameterNames = getInitialHybridArcParameterSettings< >( hybridArcPropagatorSettings, bodies, arcStartTimes );

    for( unsigned int i = 0; i < singleArcBodiesToPropagate.size( ); i++ )
    {
        parameterNames.push_back( std::make_shared< EstimatableParameterSettings >(
                                      singleArcBodiesToPropagate.at( i ), gravitational_parameter ) );
    }

    std::shared_ptr< estimatable_parameters::EstimatableParameterSet< double > > parametersToEstimate =
            createParametersToEstimate< double >( parameterNames, bodies, hybridArcPropagatorSettings );
    printEstimatableParameterEntries( parametersToEstimate );

    std::shared_ptr< IntegratorSettings< > > singleArcIntegratorSettings =
            std::make_shared< IntegratorSettings< > >
            ( rungeKutta4, initialTime, 60.0 );

    std::shared_ptr< IntegratorSettings< > > multiArcIntegratorSettings =
            std::make_shared< IntegratorSettings< > >
            ( rungeKutta4, TUDAT_NAN, 15.0 );

    // Create dynamics simulator
    HybridArcVariationalEquationsSolver< > variationalEquations =
            HybridArcVariationalEquationsSolver< >(
                bodies, singleArcIntegratorSettings, multiArcIntegratorSettings,
                hybridArcPropagatorSettings, parametersToEstimate, arcStartTimes, true, false, true );

    std::vector< std::vector< std::map< double, Eigen::MatrixXd > > > fullMultiArcVariationalSolution =
            variationalEquations.getMultiArcSolver( )->getNumericalVariationalEquationsSolution( );
    std::vector< std::map< double, Eigen::VectorXd > > fullMultiArcStateSolution =
            variationalEquations.getMultiArcSolver( )->getDynamicsSimulator( )->getEquationsOfMotionNumericalSolution( );

    for( unsigned int i = 0; i < singleArcBodiesToPropagate.size( ); i++ )
    {
        std::shared_ptr< MultiArcPropagatorSettings< > > multiArcPerBodyPropagationSettings =
                std::make_shared< MultiArcPropagatorSettings< > >( multiArcPropagationSettingsListPerCentralBody.at(
                                                                       singleArcBodiesToPropagate.at( i ) ) );
        std::shared_ptr< HybridArcPropagatorSettings< > > hybridArcPerBodyPropagatorSettings =
                std::make_shared< HybridArcPropagatorSettings< > >(
                    singleArcPropagatorSettings, multiArcPerBodyPropagationSettings );

        std::vector< std::shared_ptr< EstimatableParameterSettings > > parameterNamesPerBody;

        parameterNamesPerBody = getInitialHybridArcParameterSettings< >(
                    hybridArcPerBodyPropagatorSettings, bodies, arcStartTimesPerBody.at( singleArcBodiesToPropagate.at( i ) ) );

        for( unsigned int j = 0; j < singleArcBodiesToPropagate.size( ); j++ )
        {
            parameterNamesPerBody.push_back( std::make_shared< EstimatableParameterSettings >(
                                                 singleArcBodiesToPropagate.at( j ), gravitational_parameter ) );
        }

        std::shared_ptr< estimatable_parameters::EstimatableParameterSet< double > > parametersToEstimatePerBody =
                createParametersToEstimate< double >( parameterNamesPerBody, bodies, hybridArcPerBodyPropagatorSettings );
        printEstimatableParameterEntries( parametersToEstimatePerBody );

        HybridArcVariationalEquationsSolver< > perCentralBodyVariationalEquations =
                HybridArcVariationalEquationsSolver< >(
                    bodies, singleArcIntegratorSettings, multiArcIntegratorSettings,
                    hybridArcPerBodyPropagatorSettings, parametersToEstimatePerBody, arcStartTimesPerBody.at(
                        singleArcBodiesToPropagate.at( i ) ), true, false, true );

        std::vector< std::vector< std::map< double, Eigen::MatrixXd > > > perBodyMultiArcVariationalSolution =
                perCentralBodyVariationalEquations.getMultiArcSolver( )->getNumericalVariationalEquationsSolution( );
        std::vector< std::map< double, Eigen::VectorXd > > perBodyMultiArcStateSolution =
                perCentralBodyVariationalEquations.getMultiArcSolver( )->getDynamicsSimulator( )->getEquationsOfMotionNumericalSolution( );

        for( unsigned int j = 0; j < perBodyIndicesInFullPropagation.at( singleArcBodiesToPropagate.at( i ) ).size( ); j++ )
        {
            for( unsigned int k = 0; k < 2; k++ )
            {
                std::map< double, Eigen::MatrixXd > fullMultiArcMatrixHistory = fullMultiArcVariationalSolution.at(
                            perBodyIndicesInFullPropagation.at( singleArcBodiesToPropagate.at( i ) ).at( j ) ).at( k );
                std::map< double, Eigen::MatrixXd > perBodyMultiMatrixHistory = perBodyMultiArcVariationalSolution.at( j ).at( k );

                auto fullIterator = fullMultiArcMatrixHistory.begin( );
                auto perBodyIterator = perBodyMultiMatrixHistory.begin( );

                BOOST_CHECK_EQUAL( fullMultiArcMatrixHistory.size( ), perBodyMultiMatrixHistory.size( ) );

                for( unsigned int i = 0; i < fullMultiArcMatrixHistory.size( ); i++ )
                {
                    BOOST_CHECK_CLOSE_FRACTION( fullIterator->first, perBodyIterator->first, std::numeric_limits< double >::epsilon( ) );
                    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( fullIterator->second, perBodyIterator->second, std::numeric_limits< double >::epsilon( ) );

                    fullIterator++;
                    perBodyIterator++;
                }
            }
        }
    }
}

BOOST_AUTO_TEST_SUITE_END( )

}

}


// Copyright (c) 2008-2016 Emil Dotchevski and Reverge Studios, Inc.

// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef UUID_A61EC088D31511DFA59D2B03E0D72085
#define UUID_A61EC088D31511DFA59D2B03E0D72085

#include <boost/qvm/vec_mat_operations2.hpp>
#include <boost/qvm/vec_mat_operations3.hpp>
#include <boost/qvm/vec_mat_operations4.hpp>

namespace boost {
namespace qvm {
////////////////////////////////////////////////

namespace qvm_detail {
template <int M, int N> struct mul_mv_defined {
  static bool const value = false;
};
} // namespace qvm_detail

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS typename lazy_enable_if_c<
    is_mat<A>::value && is_vec<B>::value &&
        mat_traits<A>::cols == vec_traits<B>::dim &&
        !qvm_detail::mul_mv_defined<mat_traits<A>::rows,
                                    mat_traits<A>::cols>::value,
    deduce_vec2<A, B, mat_traits<A>::rows>>::type
operator*(A const &a, B const &b) {
  typedef typename deduce_vec2<A, B, mat_traits<A>::rows>::type R;
  R r;
  for (int i = 0; i < mat_traits<A>::rows; ++i) {
    typedef typename vec_traits<R>::scalar_type Tr;
    Tr x(scalar_traits<Tr>::value(0));
    for (int j = 0; j < mat_traits<A>::cols; ++j)
      x += mat_traits<A>::read_element_idx(i, j, a) *
           vec_traits<B>::read_element_idx(j, b);
    vec_traits<R>::write_element_idx(i, r) = x;
  }
  return r;
}

namespace qvm_detail {
template <int M, int N> struct mul_vm_defined {
  static bool const value = false;
};
} // namespace qvm_detail

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS typename lazy_enable_if_c<
    is_vec<A>::value && is_mat<B>::value &&
        vec_traits<A>::dim == mat_traits<B>::rows &&
        !qvm_detail::mul_vm_defined<mat_traits<B>::rows,
                                    mat_traits<B>::cols>::value,
    deduce_vec2<A, B, mat_traits<B>::cols>>::type
operator*(A const &a, B const &b) {
  typedef typename deduce_vec2<A, B, mat_traits<B>::cols>::type R;
  R r;
  for (int i = 0; i < mat_traits<B>::cols; ++i) {
    typedef typename vec_traits<R>::scalar_type Tr;
    Tr x(scalar_traits<Tr>::value(0));
    for (int j = 0; j < mat_traits<B>::rows; ++j)
      x += vec_traits<A>::read_element_idx(j, a) *
           mat_traits<B>::read_element_idx(j, i, b);
    vec_traits<R>::write_element_idx(i, r) = x;
  }
  return r;
}

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

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS
    typename lazy_enable_if_c<mat_traits<A>::rows == 4 &&
                                  mat_traits<A>::cols == 4 &&
                                  vec_traits<B>::dim == 3,
                              deduce_vec2<A, B, 3>>::type
    transform_point(A const &a, B const &b) {
  typedef typename mat_traits<A>::scalar_type Ta;
  typedef typename vec_traits<B>::scalar_type Tb;
  Ta const a00 = mat_traits<A>::template read_element<0, 0>(a);
  Ta const a01 = mat_traits<A>::template read_element<0, 1>(a);
  Ta const a02 = mat_traits<A>::template read_element<0, 2>(a);
  Ta const a03 = mat_traits<A>::template read_element<0, 3>(a);
  Ta const a10 = mat_traits<A>::template read_element<1, 0>(a);
  Ta const a11 = mat_traits<A>::template read_element<1, 1>(a);
  Ta const a12 = mat_traits<A>::template read_element<1, 2>(a);
  Ta const a13 = mat_traits<A>::template read_element<1, 3>(a);
  Ta const a20 = mat_traits<A>::template read_element<2, 0>(a);
  Ta const a21 = mat_traits<A>::template read_element<2, 1>(a);
  Ta const a22 = mat_traits<A>::template read_element<2, 2>(a);
  Ta const a23 = mat_traits<A>::template read_element<2, 3>(a);
  Tb const b0 = vec_traits<B>::template read_element<0>(b);
  Tb const b1 = vec_traits<B>::template read_element<1>(b);
  Tb const b2 = vec_traits<B>::template read_element<2>(b);
  typedef typename deduce_vec2<A, B, 3>::type R;
  BOOST_QVM_STATIC_ASSERT(vec_traits<R>::dim == 3);
  R r;
  vec_traits<R>::template write_element<0>(r) =
      a00 * b0 + a01 * b1 + a02 * b2 + a03;
  vec_traits<R>::template write_element<1>(r) =
      a10 * b0 + a11 * b1 + a12 * b2 + a13;
  vec_traits<R>::template write_element<2>(r) =
      a20 * b0 + a21 * b1 + a22 * b2 + a23;
  return r;
}

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS
    typename lazy_enable_if_c<mat_traits<A>::rows == 4 &&
                                  mat_traits<A>::cols == 4 &&
                                  vec_traits<B>::dim == 3,
                              deduce_vec2<A, B, 3>>::type
    transform_vector(A const &a, B const &b) {
  typedef typename mat_traits<A>::scalar_type Ta;
  typedef typename vec_traits<B>::scalar_type Tb;
  Ta const a00 = mat_traits<A>::template read_element<0, 0>(a);
  Ta const a01 = mat_traits<A>::template read_element<0, 1>(a);
  Ta const a02 = mat_traits<A>::template read_element<0, 2>(a);
  Ta const a10 = mat_traits<A>::template read_element<1, 0>(a);
  Ta const a11 = mat_traits<A>::template read_element<1, 1>(a);
  Ta const a12 = mat_traits<A>::template read_element<1, 2>(a);
  Ta const a20 = mat_traits<A>::template read_element<2, 0>(a);
  Ta const a21 = mat_traits<A>::template read_element<2, 1>(a);
  Ta const a22 = mat_traits<A>::template read_element<2, 2>(a);
  Tb const b0 = vec_traits<B>::template read_element<0>(b);
  Tb const b1 = vec_traits<B>::template read_element<1>(b);
  Tb const b2 = vec_traits<B>::template read_element<2>(b);
  typedef typename deduce_vec2<A, B, 3>::type R;
  BOOST_QVM_STATIC_ASSERT(vec_traits<R>::dim == 3);
  R r;
  vec_traits<R>::template write_element<0>(r) = a00 * b0 + a01 * b1 + a02 * b2;
  vec_traits<R>::template write_element<1>(r) = a10 * b0 + a11 * b1 + a12 * b2;
  vec_traits<R>::template write_element<2>(r) = a20 * b0 + a21 * b1 + a22 * b2;
  return r;
}

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

namespace sfinae {
using ::boost::qvm::operator*;
using ::boost::qvm::transform_point;
using ::boost::qvm::transform_vector;
} // namespace sfinae

////////////////////////////////////////////////
} // namespace qvm
} // namespace boost

#endif


/**
 * Copyright (c) 2017 Melown Technologies SE
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * *  Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>

#if BOOST_VERSION < 105600
#  include <boost/geometry/multi/geometries/multi_polygon.hpp>
#else
#  include <boost/geometry/geometries/multi_polygon.hpp>
#endif

#include "dbglog/dbglog.hpp"

#include "nonconvexclip.hpp"
#include "triangulate.hpp"

namespace bg = boost::geometry;

namespace geometry {

typedef bg::model::d2::point_xy<double> Point;
typedef bg::model::polygon<Point, false, false> Polygon; // ccw, unclosed
typedef bg::model::multi_polygon<Polygon> MultiPolygon;
typedef Polygon::ring_type Ring;

namespace {

template<typename List>
std::vector<Point> bgPoints(const List &list)
{
    std::vector<Point> result;
    result.reserve(list.size() + 1);
    for (const auto &p : list) {
        result.emplace_back(p(0), p(1));
    }
    return result;
}

inline math::Points2d ringPoints(const Ring &ring)
{
    math::Points2d result;
    result.reserve(ring.size());
    for (const auto &p : ring) {
        result.emplace_back(p.x(), p.y());
    }
    return result;
}

inline double checkCcw(const math::Point2 &a, const math::Point2 &b
                       , const math::Point2 &c)
{
    return math::crossProduct(math::Point2(math::normalize(b - a))
                              , math::Point2(math::normalize(c - a)));
}

inline double area(const math::Triangle2d &t) {
    return 0.5 * std::abs(math::crossProduct( math::Point2(t[1] - t[0])
                                            , math::Point2(t[2] - t[0])));
}

} // namespace

math::Triangles3d clipTriangleNonconvex(const math::Triangle3d &tri_,
                                        const math::MultiPolygon &clipRegion)
{
    math::Triangle3d tri(tri_);

    // tri -> tri2
    math::Triangle2d tri2;
    for (int i = 0; i < 3; i++) {
        tri2[i](0) = tri[i](0);
        tri2[i](1) = tri[i](1);
    }

    bool flip = false;
    double ccw(checkCcw(tri2[0], tri2[1], tri2[2]));

    // convert clipRegion to boost MultiPolygon
    MultiPolygon clipMultiPoly;
    for (const auto &pts : clipRegion) {
        Polygon part;
        bg::assign_points(part, bgPoints(pts));
        clipMultiPoly.push_back(part);
    }

    if (std::abs(ccw) < 1e-4)
    {
        // TODO: handle exactly vertical triangles properly
        if (bg::within(Point{tri[0](0), tri[0](1)}, clipMultiPoly) &&
            bg::within(Point{tri[1](0), tri[1](1)}, clipMultiPoly) &&
            bg::within(Point{tri[2](0), tri[2](1)}, clipMultiPoly))
        {
            LOG(debug)
                << "Including near vertical triangle, all vertices lie inside.";
            return {tri};
        } else {
            LOG(debug) << "Excluding near vertical triangle, "
                          "not all vertices lie inside.";
            return {};
        }
    }

    // ensure counter-clockwise orientation for clipping
    if (ccw < 0.0) {
        std::swap(tri[1], tri[2]);
        std::swap(tri2[1], tri2[2]);
        flip = true;
    }

    // convert input to 2D polygons
    Polygon trianglePoly;
    bg::assign_points(trianglePoly, bgPoints(tri));

    // calculate intersection
    std::deque<Polygon> isect;
    bg::intersection(trianglePoly, clipMultiPoly, isect);

    // triangulate
    math::MultiPolygon isect2;
    isect2.reserve(isect.size());
    for (const auto &poly : isect) {
        isect2.push_back(ringPoints(poly.outer()));
        for (const auto &ring : poly.inners()) {
            isect2.push_back(ringPoints(ring));
        }
    }

    math::Triangles2d tris2(generalPolyTriangulate(isect2));

    // work around boost errorneously returning whole polygon as intersection
    // check area of input triangle against area of the result, should be same
    // or less. Definitelly it should not be significantly bigger.
    double interArea(0.0);
    for (const auto &t2 : tris2) {
        interArea += area(t2);
    }
    if (interArea > (area(tri2) * 1.1) ) { // 1.1 for numerical stability
        LOG(warn1) << "Throwing away spurious intersection (ratio of areas: "
                   << interArea / area(tri2) << ").";
        return {};
    }

    // restore Z coords
    math::Triangles3d tris3;
    tris3.reserve(tris2.size());
    for (const auto &t2 : tris2)
    {
        math::Triangle3d t3;
        for (int i = 0; i < 3; i++)
        {
            math::Point3 l(math::barycentricCoords(t2[i], tri2));
            t3[i](0) = t2[i](0);
            t3[i](1) = t2[i](1);
            t3[i](2) = l(0)*tri[0](2) +
                       l(1)*tri[1](2) +
                       l(2)*tri[2](2);
        }
        if (flip) {
            std::swap(t3[1], t3[2]);
        }
        tris3.push_back(t3);
    }

    return tris3;
}

math::Point3 barycentric3D(const math::Point3 &p, const math::Point3 &a,
                           const math::Point3 &b, const math::Point3 &c)
{
    typedef math::Point3 P3;
    double abp = norm_2(math::crossProduct(P3(b - a), P3(p - a)));
    double bcp = norm_2(math::crossProduct(P3(c - b), P3(p - b)));
    double cap = norm_2(math::crossProduct(P3(a - c), P3(p - c)));
    double nor = 1.0 / (abp + bcp + cap);
    return {bcp*nor, cap*nor, abp*nor};
}

std::tuple<math::Triangles3d, math::Triangles2d>
    clipTexturedTriangleNonconvex(const math::Triangle3d &tri,
                                  const math::Triangle2d &uv,
                                  const math::MultiPolygon &clipRegion)
{
    std::tuple<math::Triangles3d, math::Triangles2d> result;
    auto &tris3(std::get<0>(result));
    auto &uvs(std::get<1>(result));

    // clip the geometry first
    tris3 = clipTriangleNonconvex(tri, clipRegion);

    // if one same triangle -> copy texcoorsds
    // this clearly solves degen cases.
    if ((tris3.size() == 1) && (tris3[0] == tri)) {
        uvs.push_back(uv);
        return result;
    }

    // interpolate UV coords
    uvs.reserve(tris3.size());
    for (const auto &t3 : tris3)
    {
        math::Triangle2d t2;
        for (int i = 0; i < 3; i++)
        {
            math::Point3 l(barycentric3D(t3[i], tri[0], tri[1], tri[2]));
            for (int j = 0; j < 2; j++) {
                t2[i](j) = l(0)*uv[0](j) + l(1)*uv[1](j) + l(2)*uv[2](j);
            }
        }
        uvs.push_back(t2);
    }

    return result;
}

} // namespace geometry