/*
 *
 * Copyright (c) Toon Knapen & Kresimir Fresl 2003
 *
 * Permission to copy, modify, use and distribute this software
 * for any non-commercial or commercial purpose is granted provided
 * that this license appear on all copies of the software source code.
 *
 * Authors assume no responsibility whatsoever for its use and makes
 * no guarantees about its quality, correctness or reliability.
 *
 * KF acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_GESV_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_GESV_HPP

#include <boost/numeric/bindings/traits/type_traits.hpp>
#include <boost/numeric/bindings/traits/traits.hpp>
#include <boost/numeric/bindings/lapack/lapack.h>
#include <boost/numeric/bindings/traits/detail/array.hpp>

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
#  include <boost/static_assert.hpp>
#  include <boost/type_traits/is_same.hpp>
#endif

namespace boost { namespace numeric { namespace bindings {

  namespace lapack {

    ///////////////////////////////////////////////////////////////////
    //
    // general system of linear equations A * X = B
    //
    ///////////////////////////////////////////////////////////////////

    /*
     * gesv() computes the solution to a system of linear equations
     * A * X = B, where A is an N-by-N matrix and X and B are N-by-NRHS
     * matrices.
     *
     * The LU decomposition with partial pivoting and row interchanges
     * is used to factor A as A = P * L * U, where P is a permutation
     * matrix, L is unit lower triangular, and U is upper triangular.
     * The factored form of A is then used to solve the system of
     * equations A * X = B.
     */

    namespace detail {

      inline
      void gesv (int const n, int const nrhs,
                 float* a, int const lda, int* ipiv,
                 float* b, int const ldb, int* info)
      {
        LAPACK_SGESV (&n, &nrhs, a, &lda, ipiv, b, &ldb, info);
      }

      inline
      void gesv (int const n, int const nrhs,
                 double* a, int const lda, int* ipiv,
                 double* b, int const ldb, int* info)
      {
        LAPACK_DGESV (&n, &nrhs, a, &lda, ipiv, b, &ldb, info);
      }

      inline
      void gesv (int const n, int const nrhs,
                 traits::complex_f* a, int const lda, int* ipiv,
                 traits::complex_f* b, int const ldb, int* info)
      {
        LAPACK_CGESV (&n, &nrhs,
                      traits::complex_ptr (a), &lda, ipiv,
                      traits::complex_ptr (b), &ldb, info);
      }

      inline
      void gesv (int const n, int const nrhs,
                 traits::complex_d* a, int const lda, int* ipiv,
                 traits::complex_d* b, int const ldb, int* info)
      {
        LAPACK_ZGESV (&n, &nrhs,
                      traits::complex_ptr (a), &lda, ipiv,
                      traits::complex_ptr (b), &ldb, info);
      }

    }

    template <typename MatrA, typename MatrB, typename IVec>
    inline
    int gesv (MatrA& a, IVec& ipiv, MatrB& b) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));
#endif

      int const n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));
      assert (n == traits::matrix_size1 (b));
      assert (n == traits::vector_size (ipiv));

      int info;
      detail::gesv (n, traits::matrix_size2 (b),
                    traits::matrix_storage (a),
                    traits::leading_dimension (a),
                    traits::vector_storage (ipiv),
                    traits::matrix_storage (b),
                    traits::leading_dimension (b),
                    &info);
      return info;
    }

    template <typename MatrA, typename MatrB>
    inline
    int gesv (MatrA& a, MatrB& b) {
      // with 'internal' pivot vector

      // gesv() errors:
      //   if (info == 0), successful
      //   if (info < 0), the -info argument had an illegal value
      //   -- we will use -101 if allocation fails
      //   if (info > 0), U(i-1,i-1) is exactly zero
      int info = -101;
      traits::detail::array<int> ipiv (traits::matrix_size1 (a));
      if (ipiv.valid())
        info = gesv (a, ipiv, b);
      return info;
    }

    /*
     * getrf() computes an LU factorization of a general M-by-N matrix A
     * using partial pivoting with row interchanges. The factorization
     * has the form A = P * L * U, where P is a permutation matrix,
     * L is lower triangular with unit diagonal elements (lower
     * trapezoidal if M > N), and U is upper triangular (upper
     * trapezoidal if M < N).
     */

    namespace detail {

      inline
      void getrf (int const n, int const m,
                  float* a, int const lda, int* ipiv, int* info)
      {
        LAPACK_SGETRF (&n, &m, a, &lda, ipiv, info);
      }

      inline
      void getrf (int const n, int const m,
                  double* a, int const lda, int* ipiv, int* info)
      {
        LAPACK_DGETRF (&n, &m, a, &lda, ipiv, info);
      }

      inline
      void getrf (int const n, int const m,
                  traits::complex_f* a, int const
                  lda, int* ipiv, int* info)
      {
        LAPACK_CGETRF (&n, &m, traits::complex_ptr (a), &lda, ipiv, info);
      }

      inline
      void getrf (int const n, int const m,
                  traits::complex_d* a, int const lda,
                  int* ipiv, int* info)
      {
        LAPACK_ZGETRF (&n, &m, traits::complex_ptr (a), &lda, ipiv, info);
      }

    }

    template <typename MatrA, typename IVec>
    inline
    int getrf (MatrA& a, IVec& ipiv) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::matrix_structure,
        traits::general_t
      >::value));
#endif

      int const n = traits::matrix_size1 (a);
      int const m = traits::matrix_size2 (a);
      assert (traits::vector_size (ipiv) == (m < n ? m : n));

      int info;
      detail::getrf (n, m,
                     traits::matrix_storage (a),
                     traits::leading_dimension (a),
                     traits::vector_storage (ipiv),
                     &info);
      return info;
    }

    /*
     * getrs() solves a system of linear equations A * X = B
     * or A^T * X = B with a general N-by-N matrix A using
     * the LU factorization computed by getrf().
     */

    namespace detail {

      inline
      void getrs (char const trans, int const n, int const nrhs,
                  float const* a, int const lda, int const* ipiv,
                  float* b, int const ldb, int* info)
      {
        LAPACK_SGETRS (&trans, &n, &nrhs, a, &lda, ipiv, b, &ldb, info);
      }

      inline
      void getrs (char const trans, int const n, int const nrhs,
                  double const* a, int const lda, int const* ipiv,
                  double* b, int const ldb, int* info)
      {
        LAPACK_DGETRS (&trans, &n, &nrhs, a, &lda, ipiv, b, &ldb, info);
      }

      inline
      void getrs (char const trans, int const n, int const nrhs,
                  traits::complex_f const* a, int const lda,
                  int const* ipiv,
                  traits::complex_f* b, int const ldb, int* info)
      {
        LAPACK_CGETRS (&trans, &n, &nrhs,
                       traits::complex_ptr (a), &lda, ipiv,
                       traits::complex_ptr (b), &ldb, info);
      }

      inline
      void getrs (char const trans, int const n, int const nrhs,
                  traits::complex_d const* a, int const lda,
                  int const* ipiv,
                  traits::complex_d* b, int const ldb, int* info)
      {
        LAPACK_ZGETRS (&trans, &n, &nrhs,
                       traits::complex_ptr (a), &lda, ipiv,
                       traits::complex_ptr (b), &ldb, info);
      }

    }

    template <typename MatrA, typename MatrB, typename IVec>
    inline
    int getrs (char const trans, MatrA const& a, IVec const& ipiv, MatrB& b)
    {
      assert (trans == 'N' || trans == 'T' || trans == 'C');

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));
#endif

      int const n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));
      assert (n == traits::matrix_size1 (b));
      assert (n == traits::vector_size (ipiv));

      int info;
      detail::getrs (trans, n, traits::matrix_size2 (b),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                     traits::matrix_storage (a),
#else
                     traits::matrix_storage_const (a),
#endif
                     traits::leading_dimension (a),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                     traits::vector_storage (ipiv),
#else
                     traits::vector_storage_const (ipiv),
#endif
                     traits::matrix_storage (b),
                     traits::leading_dimension (b),
                     &info);
      return info;
    }

    template <typename MatrA, typename MatrB, typename IVec>
    inline
    int getrs (MatrA const& a, IVec const& ipiv, MatrB& b) {
      char const no_transpose = 'N';
      return getrs (no_transpose, a, ipiv, b);
    }

    // TO DO: getri()

  }

}}}

#endif


// Copyright 2021 Yu-Kai Lin. All rights reserved.
// Use of this source code is governed by a BSD-style
// license that can be found in the LICENSE file.

#pragma once

#include <Eigen/Dense>

#include <vector>

/**
 * @brief Namespace for the KCP library.
 *
 */
namespace kcp {

/**
 * @brief Data structure for point correspondences.
 *
 */
struct Correspondences {
  /**
   * @brief Correspondences in terms of position, where the first and the second
   * parts correspond to the source and the target clouds respectively.
   *
   */
  std::pair<Eigen::Matrix3Xd, Eigen::Matrix3Xd> points;

  /**
   * @brief Correspondences in terms of point indices, where the first and the
   * second parts correspond to the source and the target clouds respectively.
   *
   */
  std::pair<std::vector<int>, std::vector<int>> indices;
};

};  // namespace kcp


// -*- mode: C++ -*-
/**
   Dietrich Bollmann, Kamakura, 2015/01/01

   DMatrix-test.h

   Unit tests for class: DMatrix

   Copyright (c) 2015 Dietrich Bollmann

   This software may be modified and distributed under the terms
   of the MIT license.  See the LICENSE file for details.
*/

#define BOOST_TEST_DYN_LINK
#ifdef STAND_ALONE
#   define BOOST_TEST_MODULE Main
#endif
#include <boost/test/unit_test.hpp>

#include "DMatrix.h"

namespace nsl {

BOOST_AUTO_TEST_SUITE(TestSuite_DMatrix)

// Test equality operator
bool test_equality(const std::vector<std::vector<double> > &left,
		   const std::vector<std::vector<double> > &right)
{
  DMatrix vLeft(left);
  DMatrix vRight(right);

  return (vLeft == vRight);
}

BOOST_AUTO_TEST_CASE(Test_DMatrix_equality_operator)
{
  // equal
  BOOST_REQUIRE(   test_equality( {},          {}          ) );
  BOOST_REQUIRE(   test_equality( {{}},        {{}}        ) );
  BOOST_REQUIRE(   test_equality( {{1}},       {{1}}       ) );

  BOOST_REQUIRE(   test_equality( {{1, 2}},    {{1, 2}}    ) );
  BOOST_REQUIRE(   test_equality( {{1}, {2}},  {{1}, {2}}  ) );

  BOOST_REQUIRE(   test_equality( {{1, 2, 3}}, {{1, 2, 3}}         ) );
  BOOST_REQUIRE(   test_equality( {{1}, {2}, {3}}, {{1}, {2}, {3}} ) );

  BOOST_REQUIRE(   test_equality( {{1, 2, 3}, {4, 5, 6}, {7, 8, 9}},
				  {{1, 2, 3}, {4, 5, 6}, {7, 8, 9}}  ) );

  // different dimensions
  BOOST_REQUIRE( ! test_equality( {{}},        {{1}}       ) );
  BOOST_REQUIRE( ! test_equality( {{1}},       {{}}        ) );
  BOOST_REQUIRE( ! test_equality( {{1}},       {{1, 2}}    ) );
  BOOST_REQUIRE( ! test_equality( {{1, 2}},    {{1}}       ) );
  BOOST_REQUIRE( ! test_equality( {{1, 2}},    {{1}, {2}}  ) );
  BOOST_REQUIRE( ! test_equality( {{1}, {2}},  {{1, 2}}    ) );

  // different elements
  BOOST_REQUIRE( ! test_equality( {{1, 2, 3}, {4, 5, 6}, {7, 8, 9}},
				  {{0, 2, 3}, {4, 5, 6}, {7, 8, 9}}  ) );
  BOOST_REQUIRE( ! test_equality( {{1, 2, 3}, {4, 5, 6}, {7, 8, 9}},
				  {{1, 2, 3}, {4, 0, 6}, {7, 8, 9}}  ) );
  BOOST_REQUIRE( ! test_equality( {{1, 2, 3}, {4, 5, 6}, {7, 8, 9}},
				  {{1, 2, 3}, {4, 5, 6}, {7, 8, 0}}  ) );
}

// Test matrix vector multiplication
bool test_mvmult(const std::vector<std::vector<double> > &m,
		 const std::vector<double> &v,
		 const std::vector<double> &r)
{
  DMatrix m2(m);
  DVector v2(v);
  DVector r2(r);

  return (m2 * v2 == r2);
}

// Test matrix vector multiplication
bool test_mvmult2(const std::vector<std::vector<double> > &m,
		  const std::vector<double> &v,
		  const std::vector<double> &r)
{
  DMatrix m2(m);
  DVector v2(v);
  DVector r2(r);

  double ed = euclideanDistance((m2 * v2), r2);

  // DEBUG
  //| std::cout << "euclidian distance: " << ed << std::endl;

  return (ed < 1e-10);
}

BOOST_AUTO_TEST_CASE(Test_DVector_multiplication_operator)
{
  BOOST_REQUIRE( test_mvmult(                                {},         {},            {} ) );
  BOOST_REQUIRE( test_mvmult(                             {{2}},        {3},           {6} ) );
  BOOST_REQUIRE( test_mvmult(                          {{1, 2}},     {3, 4},          {11} ) );
  BOOST_REQUIRE( test_mvmult(                        {{1}, {2}},        {3},        {3, 6} ) );
  BOOST_REQUIRE( test_mvmult(                  {{1, 2}, {3, 4}},     {5, 6},      {17, 39} ) );
  BOOST_REQUIRE( test_mvmult( {{1, 2, 3}, {4, 5, 6}, {7, 8, 9}},  {1, 2, 3},  {14, 32, 50} ) );

  BOOST_REQUIRE( test_mvmult2( {{ 1000,     0, -1000,     0},
 	                        {    0,  3000,     0, -3000},
			        {-1000,     0,  3000, -2000},
			        {    0, -3000, -2000,  5000}},
                               { 0, 0, 10.0/11.0, 15.0/11.0 },
                               { -10000.0/11.0, -45000.0/11.0, 0, 55000.0/11.0 } ) );
}

BOOST_AUTO_TEST_SUITE_END()

} // namespace nsl

/* fin */


#ifndef SHASTA_SHORTEST_PATH_BOUNDED_DISTANCE_HPP
#define SHASTA_SHORTEST_PATH_BOUNDED_DISTANCE_HPP

// #include <boost/graph/adjacency_list.hpp>
// #include <boost/graph/iteration_macros.hpp>
#include <boost/multi_index_container.hpp>
#include <boost/multi_index/ordered_index.hpp>
#include <boost/multi_index/identity.hpp>
#include <boost/multi_index/member.hpp>

#include <algorithm>
#include <limits>
#include <map>
#include <set>
#include "vector.hpp"

namespace shasta {
    template<class Graph, class DistanceType>
        DistanceType shortestPathBoundedDistance(
            const Graph&,
            typename Graph::vertex_descriptor source,
            typename Graph::vertex_descriptor destination,
            DistanceType maxPathLength,
            const std::map<typename Graph::edge_descriptor, DistanceType>& edgeLength,
            vector<typename Graph::edge_descriptor>& path
        );

    void testShortestPathBoundedDistance();
}

// Compute a shortest path between two given vertices
// of a directed graph, subject to a maximum path length.
// If there is no such path, an empty path is computed.
// The graph must be a directed graph described
// as a boost::graph::adjacency_list.
// A map with edge lengths must be given.
// Edge lengths must be non-negative.
// All data structures used in this function are of size
// proportional to size of the portion of the graph within a distance maxPathlength
// from the source vertex.
// It may be possible to implement this using boost::dijkstra_shortest_paths
// with a suitable visitor.

// See pseudocode here
// https://en.wikipedia.org/wiki/Dijkstra's_algorithm#Pseudocode

// The code mostly follows that pseudocode with a few differences:
// - Instead of maintaining Q, with maintain its complement notQ.
//   This way all data structures are of size proportional
//   to the size of the graph neighborhood visited.
// - The prev map stores the edge, not the vertex, that led to the
//   shortest path for each vertex encountered.
// - Distances larger than maxPathLength are never stored.
// - Returns the length of the path found.
template<class Graph, class DistanceType> DistanceType
    shasta::shortestPathBoundedDistance(
        const Graph& graph,
        typename Graph::vertex_descriptor source,
        typename Graph::vertex_descriptor destination,
        DistanceType maxPathLength,
        const std::map<typename Graph::edge_descriptor, DistanceType>& edgeLength,
        vector<typename Graph::edge_descriptor>& path
    )
{
    using vertex_descriptor = typename Graph::vertex_descriptor;
    using edge_descriptor = typename Graph::edge_descriptor;

    // The Boost MultiIndex library is used to implement the
    // updatable priority queue required by the Dijkstra algorithm.
    using boost::multi_index::indexed_by;
    using boost::multi_index::multi_index_container;
    using boost::multi_index::ordered_unique;
    using boost::multi_index::ordered_non_unique;
    using boost::multi_index::member;
    using boost::multi_index::tag;

    // The complement of the Q vertex set.
    std::set<vertex_descriptor> notQ;

    // The prev map contain for each vertex the edge
    // that gave the minimum distance for that vertex.
    std::map<vertex_descriptor, edge_descriptor> prev;

    // The vertices and their distances, accessible both by vertex descriptor
    // and by distance.
    using VertexInfo = pair<vertex_descriptor, DistanceType>;
    using VertexContainer = multi_index_container< VertexInfo, indexed_by<
        ordered_unique<     member<VertexInfo, vertex_descriptor, &VertexInfo::first > >,
        ordered_non_unique< member<VertexInfo, DistanceType     , &VertexInfo::second> > > >;
    VertexContainer vertexContainer;

    // Vertex indexes by vertex descriptor and by distance.
    // That use of the template keyword is weird but necessary.
    auto& verticesByDescriptor = vertexContainer.template get<0>();
    auto& verticesByDistance = vertexContainer.template get<1>();

    // Insert the source vertex.
    vertexContainer.insert(VertexInfo(source, 0));

    // Main loop.
    while(not vertexContainer.empty()) {

        // Find the vertex non in notQ with minimum distance.
        vertex_descriptor u = Graph::null_vertex();
        DistanceType uDistance = std::numeric_limits<DistanceType>::max();
        for(const auto& uInfo: verticesByDistance) {
            if(notQ.find(uInfo.first) != notQ.end()) {
                continue;
            }
            u = uInfo.first;
            uDistance = uInfo.second;
            break;
        }
        if(u == Graph::null_vertex()) {
            break;
        }

         // Instead of removing u from Q (see the pseudocode reference above),
         // add u to not notQ.
         notQ.insert(u);

         // If done, construct the path back to the source.
         if(u == destination) {
             path.clear();
             DistanceType pathLength = DistanceType(0ULL);
             while(u != source) {
                 const edge_descriptor e = prev[u];
                 path.push_back(e);
                 auto ite = edgeLength.find(e);
                 SHASTA_ASSERT(ite != edgeLength.end());
                 pathLength += ite->second;;
                 u = boost::source(e, graph);
             }
             std::reverse(path.begin(), path.end());
             return pathLength;
         }

         // Loop over children of u that are not in notQ.
         BGL_FORALL_OUTEDGES_T(u, e, graph, Graph) {
             const vertex_descriptor v = target(e, graph);
             if(notQ.find(v) != notQ.end()) {
                 // It is notQ. Skip.
                 continue;
             }

             // Compute the tentative new distance of v.
             auto ite = edgeLength.find(e);
             SHASTA_ASSERT(ite != edgeLength.end());
             const DistanceType alt = uDistance + ite->second;
             if(alt > maxPathLength) {
                 continue;
             }

             // If we found a lower distance, update it.
             auto itv = verticesByDescriptor.find(v);
             if(itv == verticesByDescriptor.end()) {
                 vertexContainer.insert(VertexInfo(v, alt));
                 prev[v] = e;
             } else {
                 const DistanceType vDistance = itv->second;
                 if(alt < vDistance) {
                     verticesByDescriptor.replace(itv, VertexInfo(v, alt));
                     prev[v] = e;
                 }
             }
         }

    }

    path.clear();
    return DistanceType(0ULL);
}

inline void shasta::testShortestPathBoundedDistance()
{
    using namespace boost;
    using Graph = adjacency_list<listS, listS, bidirectionalS, uint64_t>;
    using vertex_descriptor = Graph::vertex_descriptor;
    using edge_descriptor = Graph::edge_descriptor;
    using DistanceType = uint64_t;

    Graph graph;

    // Create the vertices.
    vector<vertex_descriptor> v;
    const uint64_t n = 4;
    for(uint64_t i=0; i<n; i++) {
        v.push_back(add_vertex(i, graph));
    }

    // Edges and their lengths.
    vector< pair< pair<uint64_t, uint64_t>, DistanceType> > edgeTable = {
        {{0, 1}, 1},
        {{0, 2}, 1},
        {{1, 3}, 10},
        {{2, 3}, 4},
        {{0, 3}, 1000},
    };

    // Create the edges.
    vector<edge_descriptor> edges;
    std::map<edge_descriptor, DistanceType> edgeLength;
    for(const auto& p: edgeTable) {
        edge_descriptor e;
        tie(e, ignore) = add_edge(v[p.first.first], v[p.first.second], graph);
        edgeLength.insert(make_pair(e, p.second));
    }

    // Compute the shortest path.
    const uint64_t maxPathLength = 5;
    vector<edge_descriptor> path;
    const uint64_t pathLength = shortestPathBoundedDistance(graph, v[0], v[3], maxPathLength, edgeLength, path);
    cout << "Found a path with " << path.size() << " edges of length " << pathLength << endl;\
    for(const edge_descriptor e: path) {
        const vertex_descriptor v0 = source(e, graph);
        const vertex_descriptor v1 = target(e, graph);
        cout << graph[v0] << " " << graph[v1] << endl;
    }

}

#endif


/**
 * @file print_helper.hpp
 * @author francois.hamonic@gmail.com
 * @brief
 * @version 0.1
 * @date 2021-07-19
 */
#ifndef PRINT_HELPER_HPP
#define PRINT_HELPER_HPP

#include <math.h>
#include <filesystem>

#include <boost/range/adaptors.hpp>
#include <boost/range/algorithm.hpp>

#include <fmt/os.h>

#include "landscape/decored_landscape.hpp"
#include "landscape/mutable_landscape.hpp"

#include "Eigen/Dense"
#include "algorithms/multiplicative_dijkstra.hpp"
#include "lemon/dijkstra.h"

#include "fast-cpp-csv-parser/csv.h"
#include "solvers/concept/solver.hpp"

#include "lemon/dim2.h"
#include "lemon/graph_to_eps.h"

#include "indices/eca.hpp"

#include "helper.hpp"

namespace Helper {
/**
 * @brief Finds the maximum node scaling for which no nodes overlaps.
 *
 * Finds the maximum scaling coeficient of nodes radius for which their is no
 * overlaps in their graphical representation. Runs in $O(n^2)$, where $n$ is
 * the number of nodes.
 *
 * @tparam GR
 * @tparam QM
 * @tparam DM
 * @tparam CM
 * @param landscape
 * @return double
 */
template <typename LS>
std::pair<double, double> findNodeScale(const LS & landscape) {
    using Graph = typename LS::Graph;
    const Graph & graph = landscape.getNetwork();
    const typename LS::CoordsMap & coordsMap = landscape.getCoordsMap();
    const typename LS::QualityMap & qualityMap = landscape.getQualityMap();

    if(lemon::countNodes(graph) < 2) return std::make_pair(1, 0);

    auto dist = [&](typename Graph::Node u, typename Graph::Node v) {
        const double dx = coordsMap[u].x - coordsMap[v].x;
        const double dy = coordsMap[u].y - coordsMap[v].y;
        return std::sqrt(dx * dx + dy * dy);
    };
    auto radius = [&](typename Graph::Node u) {
        return std::sqrt(qualityMap[u] / (2 * M_PI));
    };
    double r_max = std::numeric_limits<double>::max();
    double min_dist = 0.0;
    for(typename Graph::NodeIt u(graph); u != lemon::INVALID; ++u) {
        for(typename Graph::NodeIt v(graph); v != lemon::INVALID; ++v) {
            if(u == v) continue;
            const double r = dist(u, v) / (radius(u) + radius(v));
            if(r < r_max) {
                r_max = r;
                min_dist = dist(u, v);
            }
        }
    }
    return std::make_pair(r_max, min_dist);
}

template <typename LS>
void printLandscape(const LS & landscape, std::filesystem::path path) {
    const MutableLandscape::Graph & graph = landscape.getNetwork();
    std::string name = path.stem();

    auto radius = [&](double area) { return std::sqrt(area / (2 * M_PI)); };
    const auto & [r_max, min_dist] = findNodeScale(landscape);
    const double a_max = r_max * 2 * radius(minNonZeroQuality(landscape));

    const bool directed = false;

    double node_scale = 0.7;
    double text_scale =
        0.75 / (1 + static_cast<int>(std::log10(lemon::countNodes(graph))));
    double arrow_scale = (1 - node_scale) * 2 / 3;
    double arc_width = node_scale * a_max / 16;

    MutableLandscape::Graph::NodeMap<std::string> node_idsMap(graph, "\7");
    MutableLandscape::Graph::NodeMap<lemon::Color> node_colorsMap(graph,
                                                                  lemon::WHITE);
    MutableLandscape::Graph::NodeMap<double> node_sizesMap(graph,
                                                           arc_width * 0.9);
    for(MutableLandscape::NodeIt v(graph); v != lemon::INVALID; ++v) {
        if(landscape.getQuality(v) == 0) continue;
        node_idsMap[v] = std::to_string(graph.id(v));
        node_sizesMap[v] = radius(landscape.getQuality(v));
    }

    MutableLandscape::Graph::ArcMap<lemon::Color> arcs_colorsMap(graph,
                                                                 lemon::BLACK);
    MutableLandscape::Graph::ArcMap<double> arc_widths(graph, arc_width);

    return lemon::graphToEps(graph, "output/" + name)
        .title(name)
        .coords(landscape.getCoordsMap())
        .autoNodeScale(false)
        .absoluteNodeSizes(true)
        .nodeSizes(node_sizesMap)
        .nodeScale(node_scale * r_max)
        .autoArcWidthScale(false)
        .absoluteArcWidths(true)
        .arcWidths(arc_widths)
        .arcWidthScale(1)
        .drawArrows(directed)
        .arrowLength(arrow_scale * min_dist)
        .arrowWidth(arc_width * 2)
        .nodeTexts(node_idsMap)
        .nodeTextSize(text_scale * a_max)
        .nodeColors(node_colorsMap)
        .arcColors(arcs_colorsMap)
        .enableParallel(directed)
        .parArcDist(2 * arrow_scale * min_dist)
        .border(20)
        .run();
}

template <typename LS>
void printInstance(const LS & landscape, const RestorationPlan<LS> & plan,
                   std::filesystem::path path) {
    using Graph = typename LS::Graph;

    const Graph & graph = landscape.getNetwork();
    std::string name = path.stem();

    auto radius = [&](double area) { return std::sqrt(area / (2 * M_PI)); };
    const auto & [r_max, min_dist] = findNodeScale(landscape);
    const double a_max = r_max * 2 * radius(minNonZeroQuality(landscape));

    const bool directed = false;

    double node_scale = 0.7;
    double text_scale =
        0.75 / (1 + static_cast<int>(std::log10(lemon::countNodes(graph))));
    double arrow_scale = (1 - node_scale) * 2 / 3;
    double arc_width = node_scale * a_max / 16;

    typename Graph::template NodeMap<std::string> node_idsMap(graph, "");
    typename Graph::template NodeMap<lemon::Color> node_colorsMap(graph,
                                                                  lemon::BLACK);
    typename Graph::template NodeMap<double> node_sizesMap(graph,
                                                           arc_width * 0.9);
    for(typename Graph::NodeIt v(graph); v != lemon::INVALID; ++v) {
        if(landscape.getQuality(v) == 0) continue;
        node_idsMap[v] = std::to_string(graph.id(v));
        node_colorsMap[v] = lemon::WHITE;
        node_sizesMap[v] = radius(landscape.getQuality(v));
    }

    typename Graph::template ArcMap<lemon::Color> arcs_colorsMap(graph,
                                                                 lemon::BLACK);
    typename Graph::template ArcMap<double> arc_widths(graph, arc_width);

    for(typename Graph::NodeIt u(graph); u != lemon::INVALID; ++u)
        node_colorsMap[u] = plan[u].empty() ? lemon::BLACK : lemon::RED;

    for(typename Graph::ArcIt a(graph); a != lemon::INVALID; ++a)
        arcs_colorsMap[a] = plan[a].empty() ? lemon::BLACK : lemon::RED;

    return lemon::graphToEps(graph, path)
        .title(name)
        .coords(landscape.getCoordsMap())
        .autoNodeScale(false)
        .absoluteNodeSizes(true)
        .nodeSizes(node_sizesMap)
        .nodeScale(node_scale * r_max)
        .autoArcWidthScale(false)
        .absoluteArcWidths(true)
        .arcWidths(arc_widths)
        .arcWidthScale(1)
        .drawArrows(directed)
        .arrowLength(arrow_scale * min_dist)
        .arrowWidth(arc_width * 2)
        .nodeTexts(node_idsMap)
        .nodeTextSize(text_scale * a_max)
        .nodeColors(node_colorsMap)
        .arcColors(arcs_colorsMap)
        .enableParallel(directed)
        .parArcDist(2 * arrow_scale * min_dist)
        .border(2)
        .run();
}

void printSolution(const MutableLandscape & landscape,
                   const RestorationPlan<MutableLandscape> & plan,
                   std::string name, concepts::Solver & solver, double B,
                   const Solution & solution);

// need to include the binary search tree for y-h , y+h search
std::pair<MutableLandscape::Node, MutableLandscape::Node> neerestNodes(
    const MutableLandscape & landscape);

template <typename Graph>
class GraphToGraphviz {
public:
    using NodePosMap =
        typename Graph::template NodeMap<lemon::dim2::Point<double>>;
    using NodeSizeMap = typename Graph::template NodeMap<double>;
    using NodeColorMap = typename Graph::template NodeMap<int>;
    using ArcSizeMap = typename Graph::template ArcMap<double>;
    using ArcColorMap = typename Graph::template ArcMap<int>;

    using Node = typename Graph::Node;
    using NodeIt = typename Graph::NodeIt;
    using Arc = typename Graph::Arc;
    using ArcIt = typename Graph::ArcIt;

private:
    const Graph & _graph;
    std::filesystem::path _path;
    NodePosMap _nodePos;
    NodeSizeMap _nodeSizes;
    NodeColorMap _nodeColors;
    ArcSizeMap _arcSizes;
    ArcColorMap _arcColors;

    double _pageWidth;
    double _pageHeight;

    double _node_size_scale;
    double _arc_size_scale;

public:
    GraphToGraphviz(const Graph & g, const std::filesystem::path & p)
        : _graph(g)
        , _path(p)
        , _nodePos(g)
        , _nodeSizes(g, 1)
        , _nodeColors(g, 0xffffff)
        , _arcSizes(g, 1)
        , _arcColors(g, 0x000000)
        , _pageWidth(8)
        , _pageHeight(11)
        , _node_size_scale(1)
        , _arc_size_scale(1) {}

    template <typename PM>
    GraphToGraphviz<Graph> & nodePos(const PM & posMap) {
        for(NodeIt u(_graph); u != lemon::INVALID; ++u) _nodePos[u] = posMap[u];
        return *this;
    }
    template <typename SM>
    GraphToGraphviz<Graph> & nodeSizes(const SM & sizeMap) {
        for(NodeIt u(_graph); u != lemon::INVALID; ++u)
            _nodeSizes[u] = sizeMap[u];
        return *this;
    }
    GraphToGraphviz<Graph> & nodeScale(const double scale) {
        _node_size_scale = scale;
        return *this;
    }
    template <typename CM>
    GraphToGraphviz<Graph> & nodeColors(const CM & colorMap) {
        for(NodeIt u(_graph); u != lemon::INVALID; ++u)
            _nodeColors[u] = colorMap[u];
        return *this;
    }
    template <typename AM>
    GraphToGraphviz<Graph> & arcSizes(const AM & sizeMap) {
        for(ArcIt a(_graph); a != lemon::INVALID; ++a)
            _arcSizes[a] = sizeMap[a];
        return *this;
    }
    GraphToGraphviz<Graph> & arcScale(const double scale) {
        _arc_size_scale = scale;
        return *this;
    }
    template <typename CM>
    GraphToGraphviz<Graph> & arcColors(const CM & colorMap) {
        for(ArcIt a(_graph); a != lemon::INVALID; ++a)
            _arcColors[a] = colorMap[a];
        return *this;
    }
    GraphToGraphviz<Graph> & pageSize(double width, double height) {
        _pageWidth = width;
        _pageHeight = height;
        return *this;
    }

    void run() const {
        double min_x, max_x, min_y, max_y;
        min_x = max_x = _nodePos[_graph.nodeFromId(0)].x;
        min_y = max_y = _nodePos[_graph.nodeFromId(0)].y;
        for(NodeIt u(_graph); u != lemon::INVALID; ++u) {
            min_x = std::min(min_x, _nodePos[u].x);
            max_x = std::max(max_x, _nodePos[u].x);
            min_y = std::min(min_y, _nodePos[u].y);
            max_y = std::max(max_y, _nodePos[u].y);
        }
        const double scale = std::min(_pageWidth / (max_x - min_x),
                                      _pageHeight / (max_y - min_y));
        auto scale_x = [&](double x) { return scale * (x - min_x); };
        auto scale_y = [&](double y) { return scale * (y - min_y); };
        auto scale_size = [&](double s) {
            return _node_size_scale * scale * s;
        };

        std::vector<Node> colorSortedNodes;
        for(NodeIt u(_graph); u != lemon::INVALID; ++u)
            colorSortedNodes.push_back(u);
        std::sort(colorSortedNodes.begin(), colorSortedNodes.end(),
                  [&](Node & a, Node & b) {
                      return _nodeColors[a] < _nodeColors[b];
                  });

        std::vector<Arc> colorSortedArcs;
        for(ArcIt a(_graph); a != lemon::INVALID; ++a)
            colorSortedArcs.push_back(a);
        std::sort(
            colorSortedArcs.begin(), colorSortedArcs.end(),
            [&](Arc & a, Arc & b) { return _arcColors[a] < _arcColors[b]; });

        auto dot_file = fmt::output_file(_path.generic_string());
        dot_file.print("digraph {{size=\"{},{}\";\n", _pageWidth, _pageHeight);
        dot_file.print(
            "graph [pad=\"0.2,0.1\" bgcolor=transparent overlap=scale]\n");
        dot_file.print("node [style=filled shape=\"circle\"]\n");
        dot_file.print("edge [style=filled]\n");

        int prev_color = -1;
        for(Node u : colorSortedNodes) {
            if(_nodeColors[u] != prev_color) {
                dot_file.print("node [fillcolor=\"#{:06x}\"]\n",
                               _nodeColors[u]);
                prev_color = _nodeColors[u];
            }
            dot_file.print("{} [width=\"{}\" pos=\"{},{}!\"]\n", _graph.id(u),
                           scale_size(std::sqrt(_nodeSizes[u])), scale_x(_nodePos[u].x),
                           scale_y(_nodePos[u].y));
        }

        prev_color = -1;
        for(Arc a : colorSortedArcs) {
            if(_arcColors[a] != prev_color) {
                dot_file.print("edge [color=\"#{:06x}\"]\n", _arcColors[a]);
                prev_color = _arcColors[a];
            }
            dot_file.print(
                "{} -> {} [penwidth=\"{}\"]\n", _graph.id(_graph.source(a)),
                _graph.id(_graph.target(a)), _arc_size_scale * _arcSizes[a]);
        }

        dot_file.print("}}");
    }
};

template <typename LS>
void printLandscapeGraphviz(const LS & landscape, std::filesystem::path path) {
    using Graph = typename LS::Graph;
    using Node = typename LS::Graph::Node;
    using NodeIt = typename LS::Graph::NodeIt;
    using Arc = typename LS::Graph::Arc;
    using ArcIt = typename LS::Graph::ArcIt;
    using NodeColorMap = typename Graph::template NodeMap<int>;
    using ArcColorMap = typename Graph::template ArcMap<int>;

    const Graph & graph = landscape.getNetwork();

    NodeColorMap nodeColors(graph);
    for(NodeIt u(graph); u != lemon::INVALID; ++u)
        nodeColors[u] = landscape.getQuality(u) > 0 ? 0x50e050 : 0x101010;

    GraphToGraphviz<Graph>(graph, path)
        .nodePos(landscape.getCoordsMap())
        .nodeSizes(landscape.getQualityMap())
        .nodeColors(nodeColors)
        .run();
};

template <typename LS>
void printInstanceGraphviz(const LS & landscape,
                           const RestorationPlan<LS> & plan,
                           std::filesystem::path path) {
    using Graph = typename LS::Graph;
    using Node = typename LS::Graph::Node;
    using NodeIt = typename LS::Graph::NodeIt;
    using Arc = typename LS::Graph::Arc;
    using ArcIt = typename LS::Graph::ArcIt;
    using NodeColorMap = typename Graph::template NodeMap<int>;
    using ArcColorMap = typename Graph::template ArcMap<int>;

    const Graph & graph = landscape.getNetwork();

    NodeColorMap nodeColors(graph);
    for(NodeIt u(graph); u != lemon::INVALID; ++u)
        nodeColors[u] = plan.contains(u) ? 0xe05050 : 0x50e050;

    ArcColorMap arcColors(graph);
    for(ArcIt a(graph); a != lemon::INVALID; ++a) {
        if(!plan.contains(a)) {
            arcColors[a] = 0x101010;
            continue;
        }
        arcColors[a] = 0xe05050;
        Node u = graph.source(a);
        Node v = graph.target(a);
        nodeColors[u] = (nodeColors[u] & 0xffff00) | 0xe00000;
        nodeColors[v] = (nodeColors[v] & 0xffff00) | 0xe00000;
    }
    for(NodeIt u(graph); u != lemon::INVALID; ++u)
        nodeColors[u] = landscape.getQuality(u) > 0 ? nodeColors[u] : 0x101010;

    GraphToGraphviz<Graph>(graph, path)
        .nodePos(landscape.getCoordsMap())
        .nodeSizes(landscape.getQualityMap())
        .nodeColors(nodeColors)
        .arcScale(3)
        .arcColors(arcColors)
        .run();
};

template <typename LS>
void printSolutionGraphviz(const LS & landscape,
                           const RestorationPlan<LS> & plan,
                           const Solution & solution,
                           std::filesystem::path path) {
    using Graph = typename LS::Graph;
    using Node = typename LS::Graph::Node;
    using NodeIt = typename LS::Graph::NodeIt;
    using Arc = typename LS::Graph::Arc;
    using ArcIt = typename LS::Graph::ArcIt;
    using NodeColorMap = typename Graph::template NodeMap<int>;
    using ArcColorMap = typename Graph::template ArcMap<int>;
    using ArcSizesMap = typename Graph::template ArcMap<double>;

    const Graph & graph = landscape.getNetwork();

    NodeColorMap nodeColors(graph);
    for(NodeIt u(graph); u != lemon::INVALID; ++u)
        nodeColors[u] =
            plan.contains(u)
                ? 0xe05050
                : (landscape.getQuality(u) > 0 ? 0x50e050 : 0x101010);

    ArcColorMap arcColors(graph);
    for(ArcIt a(graph); a != lemon::INVALID; ++a) {
        arcColors[a] = plan.contains(a) ? 0xe05050 : 0x101010;
        if(plan.contains(a)) {
            Node u = graph.source(a);
            Node v = graph.target(a);
            nodeColors[u] = (nodeColors[u] & 0x00ffff) | 0xe00000;
            nodeColors[v] = (nodeColors[v] & 0x00ffff) | 0xe00000;
        }
    }

    ArcSizesMap arcSizes(graph, 1);
    const auto & arcCentrality = *Helper::corridorCentralityMap(landscape);
    double max_centrality = 0;
    for(ArcIt a(graph); a != lemon::INVALID; ++a)
        max_centrality = std::max(max_centrality, arcCentrality[a]);

    const double pow = std::log(10) / std::log(max_centrality);

    for(ArcIt a(graph); a != lemon::INVALID; ++a) {
        if(arcCentrality[a] == 0) continue;
        arcSizes[a] = std::pow(arcCentrality[a], pow);
        if(arcSizes[a] > 0) arcColors[a] = 0x5050e0;
        if(Node u = graph.source(a); landscape.getQuality(u) == 0)
            nodeColors[u] = (nodeColors[u] & 0xffff00) | 0x0000e0;
        if(Node v = graph.target(a); landscape.getQuality(v) == 0)
            nodeColors[v] = (nodeColors[v] & 0xffff00) | 0x0000e0;
    }

    GraphToGraphviz<Graph>(graph, path)
        .nodePos(landscape.getCoordsMap())
        .nodeSizes(landscape.getQualityMap())
        .nodeColors(nodeColors)
        .arcSizes(arcSizes)
        .arcColors(arcColors)
        .run();
};

}  // namespace Helper

#endif  // PRINT_HELPER_HPP


/**
 * Test Suite for array_arithmetic namespace methods
 * command for running tests:
 * ./array_arithmetic_test --log_level=test_suite
 * http://www.alittlemadness.com/2009/03/31/c-unit-testing-with-boosttest/
 * http://www.ibm.com/developerworks/aix/library/au-ctools1_boost/
 * http://www.boost.org/doc/libs/1_34_1/libs/test/doc/components/test_tools/reference/index.html
 *
 * Created by Marshall Farrier on 6/28/2012.
 * Copyright (c) 2012 Marshall Farrier
 * license http://opensource.org/licenses/gpl-license.php GNU Public License
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>
#include <iostream>
#include <stdexcept>

#include "array_arithmetic.h"

struct ArraySize {
	int small;
	int large;
	unsigned char* small_array;
	unsigned char* large_array;
	ArraySize() {
		small = 4;
		large = 8;
		small_array = new unsigned char[small];
		large_array = new unsigned char[large];
	}
	~ArraySize() {
		delete[] small_array;
		delete[] large_array;
	}
};

BOOST_FIXTURE_TEST_SUITE(conversion, ArraySize)
	BOOST_AUTO_TEST_CASE(one) {
		const unsigned int ONE = 1;
		array_arithmetic::convert::from_int(ONE, small_array, small);
		array_arithmetic::convert::from_int(ONE, large_array, large);
		BOOST_CHECK_EQUAL(array_arithmetic::convert::to_int(small_array, small), ONE);
		BOOST_CHECK_EQUAL(array_arithmetic::convert::to_int(large_array, large), ONE);
	}

	BOOST_AUTO_TEST_CASE(one_billion) {
		const unsigned int ONE_BILLION = 1000000000;
		array_arithmetic::convert::from_int(ONE_BILLION, small_array, small);
		array_arithmetic::convert::from_int(ONE_BILLION, large_array, large);
		BOOST_CHECK_EQUAL(array_arithmetic::convert::to_int(small_array, small), ONE_BILLION);
		BOOST_CHECK_EQUAL(array_arithmetic::convert::to_int(large_array, large), ONE_BILLION);
	}
BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(conversion_helpers)
	BOOST_AUTO_TEST_CASE(to_digit_array) {
		unsigned char* digits1 = array_arithmetic::convert::helpers::to_digit_array("123");
		BOOST_CHECK_EQUAL(digits1[0], static_cast<unsigned char>(1));
		BOOST_CHECK_EQUAL(digits1[1], static_cast<unsigned char>(2));
		BOOST_CHECK_EQUAL(digits1[2], static_cast<unsigned char>(3));
		delete[] digits1;
		BOOST_CHECK_THROW(array_arithmetic::convert::helpers::to_digit_array("12."), std::invalid_argument);
	}

	BOOST_AUTO_TEST_CASE(times_radix) {
		unsigned char digits[] = {3, 3};
		BOOST_CHECK_EQUAL(array_arithmetic::convert::helpers::times_radix(digits, 2), 84);
		BOOST_CHECK_EQUAL(digits[0], 4);
		BOOST_CHECK_EQUAL(digits[1], 8);
	}
	BOOST_AUTO_TEST_CASE(times_ten) {
		unsigned char digits[] = {100, 100};
		BOOST_CHECK_EQUAL(array_arithmetic::convert::helpers::times_ten(digits, 2), 3);
		BOOST_CHECK_EQUAL(digits[0], 235);
		BOOST_CHECK_EQUAL(digits[1], 232);
	}
BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(helpers)
	BOOST_AUTO_TEST_CASE(copy_into) {
		unsigned char source_arr[] = {0, 1, 2};
		unsigned char target_arr[5];
		array_arithmetic::helpers::copy_into(source_arr, target_arr, 3, 5);
		BOOST_CHECK_EQUAL(target_arr[1], 1);
		BOOST_CHECK_EQUAL(target_arr[3], 0);
	}
	BOOST_AUTO_TEST_CASE(shift_right) {
		unsigned char arr[] = {1, 2, 3, 4, 5};
		array_arithmetic::helpers::shift_right(arr, 2, 5);
		BOOST_CHECK_EQUAL(arr[0], 0);
		BOOST_CHECK_EQUAL(arr[1], 0);
		BOOST_CHECK_EQUAL(arr[2], 1);
		BOOST_CHECK_EQUAL(arr[4], 3);
	}
	BOOST_AUTO_TEST_CASE(less_or_equal) {
		unsigned char arr1[] = {1, 1};
		unsigned char arr2[] = {1, 2};
		unsigned char arr3[] = {2, 1};
		// case of equality
		BOOST_CHECK(array_arithmetic::helpers::less_or_equal(arr1, arr1, 2));
		// inequality
		BOOST_CHECK(array_arithmetic::helpers::less_or_equal(arr1, arr2, 2));
		BOOST_CHECK(array_arithmetic::helpers::less_or_equal(arr1, arr3, 2));
		// false case
		BOOST_CHECK(!array_arithmetic::helpers::less_or_equal(arr2, arr1, 2));
	}
	BOOST_AUTO_TEST_CASE(is_zero) {
		unsigned char arr1[] = {0, 0};
		unsigned char arr2[] = {0, 0, 0, 0, 1};
		unsigned char arr3[] = {1, 0};
		BOOST_CHECK(array_arithmetic::helpers::is_zero(arr1, 2));
		BOOST_CHECK(!array_arithmetic::helpers::is_zero(arr2, 5));
		BOOST_CHECK(!array_arithmetic::helpers::is_zero(arr3, 2));
	}
	BOOST_AUTO_TEST_CASE(set_to_zero) {
		unsigned char arr[] = {1, 129, 255};
		array_arithmetic::helpers::set_to_zero(arr, 3);
		BOOST_CHECK_EQUAL(arr[0], 0);
		BOOST_CHECK_EQUAL(arr[1], 0);
		BOOST_CHECK_EQUAL(arr[2], 0);
	}
BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(division_helpers)
	BOOST_AUTO_TEST_CASE(align_dividend) {
		unsigned char arr1[] = {129, 0};
		unsigned char arr2[] = {0, 129};
		int result = array_arithmetic::helpers::division::align_dividend(arr1, arr2, 2);
		// verify proper return value
		BOOST_CHECK_EQUAL(result, -9);
		// verify that arr1 was properly shifted
		BOOST_CHECK_EQUAL(arr1[0], 0);
		BOOST_CHECK_EQUAL(arr1[1], 64);
		// verify that arr2 is unchanged
		BOOST_CHECK_EQUAL(arr2[0], 0);
		BOOST_CHECK_EQUAL(arr2[1], 129);
	}
	BOOST_AUTO_TEST_CASE(align_divisor) {
		unsigned char arr1[] = {129, 0};
		unsigned char arr2[] = {0, 129};
		// verify proper return value
		BOOST_CHECK_EQUAL(array_arithmetic::helpers::division::align_divisor(arr2, arr1, 2), 8);
		// verify that arr1 was properly shifted
		BOOST_CHECK_EQUAL(arr1[0], 0);
		BOOST_CHECK_EQUAL(arr1[1], 129);
		// verify that arr2 is unchanged
		BOOST_CHECK_EQUAL(arr2[0], 0);
		BOOST_CHECK_EQUAL(arr2[1], 129);
	}
	BOOST_AUTO_TEST_CASE(align) {
		unsigned char arr1[] = {129, 0};
		unsigned char arr2[] = {0, 129};
		// verify proper return value
		BOOST_CHECK_EQUAL(array_arithmetic::helpers::division::align(arr1, arr2, 2), -8);
		// arr1 properly shifted
		BOOST_CHECK_EQUAL(arr1[0], 0);
		BOOST_CHECK_EQUAL(arr1[1], 64);
		// arr2 properly shifted
		BOOST_CHECK_EQUAL(arr2[0], 0);
		BOOST_CHECK_EQUAL(arr2[1], 64);
	}
	BOOST_AUTO_TEST_CASE(readjust) {
		unsigned char arr1[] = {129, 0};
		unsigned char arr2[] = {0, 129};
		unsigned char zero_arr[] = {0, 0};
		unsigned char arr3[] = {129, 129};
		// verify proper return value
		BOOST_CHECK_EQUAL(array_arithmetic::helpers::division::readjust(arr2, arr1, 2), 8);
		// arr1 properly shifted
		BOOST_CHECK_EQUAL(arr1[0], 0);
		BOOST_CHECK_EQUAL(arr1[1], 129);
		// arr2 unchanged
		BOOST_CHECK_EQUAL(arr2[0], 0);
		BOOST_CHECK_EQUAL(arr2[1], 129);
		// check edge case when dividend is 0
		BOOST_CHECK_EQUAL(array_arithmetic::helpers::division::readjust(zero_arr, arr3, 2), 16);
		BOOST_CHECK_EQUAL(arr3[0], 0);
		BOOST_CHECK_EQUAL(arr3[1], 0);
	}
BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(bits)
	BOOST_AUTO_TEST_CASE(get_mask) {
		BOOST_CHECK_EQUAL(array_arithmetic::bits::get_mask(0), 0);
		BOOST_CHECK_EQUAL(array_arithmetic::bits::get_mask(2), 3);
		BOOST_CHECK_EQUAL(array_arithmetic::bits::get_mask(8), 255);
		BOOST_CHECK_EQUAL(array_arithmetic::bits::get_mask(7), 127);
	}
	BOOST_AUTO_TEST_CASE(shift_right) {
		unsigned char arr[] = {129, 129, 129};
		array_arithmetic::bits::shift_right(arr, 1, 3);
		BOOST_CHECK_EQUAL(arr[0], 64);
		BOOST_CHECK_EQUAL(arr[1], 192);
		BOOST_CHECK_EQUAL(arr[2], 192);
		// reset array to original values
		for (std::size_t i = 0; i < 3; ++i) { arr[i] = 129; }
		array_arithmetic::bits::shift_right(arr, 15, 3);
		BOOST_CHECK_EQUAL(arr[0], 0);
		BOOST_CHECK_EQUAL(arr[1], 1);
		BOOST_CHECK_EQUAL(arr[2], 3);
	}
	BOOST_AUTO_TEST_CASE(set) {
		unsigned char arr[] = {0, 0};
		// test setting first bit at each index
		array_arithmetic::bits::set(arr, 0);
		BOOST_CHECK_EQUAL(arr[0], 128);
		array_arithmetic::bits::set(arr, 8);
		BOOST_CHECK_EQUAL(arr[1], 128);
		// reset arr
		for (std::size_t i = 0; i < 2; ++i) { arr[i] = 0; }
		// test setting last bit at each index
		array_arithmetic::bits::set(arr, 7);
		BOOST_CHECK_EQUAL(arr[0], 1);
		array_arithmetic::bits::set(arr, 15);
		BOOST_CHECK_EQUAL(arr[1], 1);
		// verify that there is no change to bits that are already set
		arr[0] = 255;
		array_arithmetic::bits::set(arr, 3);
		BOOST_CHECK_EQUAL(arr[0], 255);
	}
BOOST_AUTO_TEST_SUITE_END()

struct MathTestCases {
	unsigned int one;
	unsigned int two;
	unsigned int three;
	unsigned int x1;
	unsigned int x2;
	unsigned int one_billion;
	unsigned int two_billion;
	unsigned int three_billion;
	std::size_t small_arr_len;
	std::size_t bigger_arr_len;	// to be used for a array
	unsigned char* small_arr1;
	unsigned char* small_arr2;
	unsigned char* small_arr3;
	unsigned char* smaller_arr;
	unsigned char* small_result_arr;
	unsigned char* bigger_arr1;
	unsigned char* bigger_arr2;
	unsigned char* bigger_arr3;
	unsigned char* big_arr;
	unsigned char* big_sum_arr;
	unsigned char* big_product_arr;
	MathTestCases() {
		one = 1;
		two = 2;
		three = 3;
		x1 = 128;
		x2 = 256;
		one_billion = 1000000000;
		two_billion = 2000000000;
		three_billion = 3000000000;
		small_arr_len = 2;
		bigger_arr_len = 4;
		small_arr1 = new unsigned char[small_arr_len];
		small_arr2 = new unsigned char[small_arr_len];
		small_arr3 = new unsigned char[small_arr_len];
		smaller_arr = new unsigned char[small_arr_len - 1];
		small_result_arr = new unsigned char[small_arr_len + 1];
		bigger_arr1 = new unsigned char[bigger_arr_len];
		bigger_arr2 = new unsigned char[bigger_arr_len];
		bigger_arr3 = new unsigned char[bigger_arr_len];
		big_arr = new unsigned char[bigger_arr_len - 2];
		big_sum_arr = new unsigned char[bigger_arr_len + 1];
		big_product_arr = new unsigned char[2 * bigger_arr_len - 1];
	}
	~MathTestCases() {
		delete[] small_arr1;
		delete[] small_arr2;
		delete[] small_arr3;
		delete[] smaller_arr;
		delete[] small_result_arr;
		delete[] bigger_arr1;
		delete[] bigger_arr2;
		delete[] bigger_arr3;
		delete[] big_arr;
		delete[] big_sum_arr;
		delete[] big_product_arr;
	}
};
BOOST_FIXTURE_TEST_SUITE(addition, MathTestCases)
	// uniform array length addition
	BOOST_AUTO_TEST_CASE(uniform_length) {
		// small array size
		array_arithmetic::convert::from_int(one, small_arr1, small_arr_len);
		array_arithmetic::convert::from_int(two, small_arr2, small_arr_len);
		array_arithmetic::add(small_arr1, small_arr2, small_arr3, small_arr_len);
		BOOST_CHECK_EQUAL(array_arithmetic::convert::to_int(small_arr3, small_arr_len), three);
		array_arithmetic::convert::from_int(x1, small_arr1, small_arr_len);
		array_arithmetic::add(small_arr1, small_arr1, small_arr3, small_arr_len);
		BOOST_CHECK_EQUAL(array_arithmetic::convert::to_int(small_arr3, small_arr_len), x2);
		// large array size
		array_arithmetic::convert::from_int(one_billion, bigger_arr1, bigger_arr_len);
		array_arithmetic::convert::from_int(two_billion, bigger_arr2, bigger_arr_len);
		array_arithmetic::add(bigger_arr1, bigger_arr2, bigger_arr3, bigger_arr_len);
		BOOST_CHECK_EQUAL(array_arithmetic::convert::to_int(bigger_arr3, bigger_arr_len),
				three_billion);
	}
	// adding arrays of different sizes
	BOOST_AUTO_TEST_CASE(different_lengths) {
		// inputs of equal size
		array_arithmetic::convert::from_int(one_billion, bigger_arr1, bigger_arr_len);
		array_arithmetic::convert::from_int(two_billion, bigger_arr2, bigger_arr_len);
		array_arithmetic::add(bigger_arr1, bigger_arr2, big_sum_arr, bigger_arr_len, bigger_arr_len);
		BOOST_CHECK_EQUAL(array_arithmetic::convert::to_int(big_sum_arr, bigger_arr_len + 1),
				three_billion);
		// different input sizes: small
		array_arithmetic::convert::from_int(one, small_arr1, small_arr_len);
		array_arithmetic::convert::from_int(two, smaller_arr, small_arr_len - 1);
		array_arithmetic::add(small_arr1, smaller_arr, small_result_arr, small_arr_len, small_arr_len - 1);
		BOOST_CHECK_EQUAL(array_arithmetic::convert::to_int(small_result_arr, small_arr_len + 1), three);
		// different input sizes: large
		array_arithmetic::convert::from_int(one_billion, bigger_arr1, bigger_arr_len);
		array_arithmetic::convert::from_int(255, big_arr, bigger_arr_len - 2);
		array_arithmetic::add(bigger_arr1, big_arr, big_sum_arr, bigger_arr_len, bigger_arr_len - 2);
		BOOST_CHECK_EQUAL(array_arithmetic::convert::to_int(big_sum_arr, bigger_arr_len + 1),
				static_cast<unsigned int>(1000000255));
	}
BOOST_AUTO_TEST_SUITE_END()

BOOST_FIXTURE_TEST_SUITE(subtraction, MathTestCases)
	// uniform array length
	BOOST_AUTO_TEST_CASE(uniform_length) {
		array_arithmetic::convert::from_int(three_billion, bigger_arr1, bigger_arr_len);
		array_arithmetic::convert::from_int(two_billion, bigger_arr2, bigger_arr_len);
		array_arithmetic::subtract(bigger_arr1, bigger_arr2, bigger_arr3, bigger_arr_len);
		BOOST_CHECK_EQUAL(array_arithmetic::convert::to_int(bigger_arr3, bigger_arr_len),
			one_billion);
	}
	// different array lengths
	BOOST_AUTO_TEST_CASE(different_lengths) {
		array_arithmetic::convert::from_int(1000000255, bigger_arr1, bigger_arr_len);
		array_arithmetic::convert::from_int(255, big_arr, bigger_arr_len - 2);
		array_arithmetic::subtract(bigger_arr1, big_arr, bigger_arr2, bigger_arr_len, bigger_arr_len - 2);
		BOOST_CHECK_EQUAL(array_arithmetic::convert::to_int(bigger_arr2, bigger_arr_len), one_billion);
	}
BOOST_AUTO_TEST_SUITE_END()

BOOST_FIXTURE_TEST_SUITE(multiplication, MathTestCases)
	BOOST_AUTO_TEST_CASE(school_multiply) {
		array_arithmetic::convert::from_int(5000000, bigger_arr1, bigger_arr_len);
		array_arithmetic::convert::from_int(400, big_arr, bigger_arr_len - 2);
		array_arithmetic::school_multiply(bigger_arr1, big_arr, big_product_arr, bigger_arr_len, bigger_arr_len - 2);
		BOOST_CHECK_EQUAL(array_arithmetic::convert::to_int(big_product_arr, bigger_arr_len * 2 - 1),
				two_billion);
	}
BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(division)
	BOOST_AUTO_TEST_CASE(divide_by_zero) {
		unsigned char divisor[] = {0, 0, 0};
		unsigned char dividend[] = {1, 1, 1};
		unsigned char result[] = {129, 129, 129};
		BOOST_CHECK_THROW(array_arithmetic::divide(dividend, divisor, result, 3, 1), std::invalid_argument);
	}
	BOOST_AUTO_TEST_CASE(dividend_is_zero) {
		unsigned char dividend[] = {0, 0};
		unsigned char divisor[] = {0, 1};
		unsigned char result[] = {129, 13};
		array_arithmetic::divide(dividend, divisor, result, 2, 1);
		BOOST_CHECK_EQUAL(result[0], 0);
		BOOST_CHECK_EQUAL(result[1], 0);
	}
	BOOST_AUTO_TEST_CASE(result_too_big) {
		unsigned char dividend[] = {128, 0};
		unsigned char divisor[] = {0, 128};	// represents 0.5
		unsigned char result[] = {255, 255};
		BOOST_CHECK_THROW(array_arithmetic::divide(dividend, divisor, result, 2, 1), std::overflow_error);
	}
	BOOST_AUTO_TEST_CASE(exact_result) {
		// this also tests that we exit the loop properly when the long division subtraction
		// gives a 0 result
		unsigned char dividend[] = {1, 0, 0};
		unsigned char divisor[] = {2, 0, 0};	// result should represent 0.5
		unsigned char result[] = {3, 4, 5};
		array_arithmetic::divide(dividend, divisor, result, 3, 1);
		BOOST_CHECK_EQUAL(result[0], 0);
		BOOST_CHECK_EQUAL(result[1], 128);
		BOOST_CHECK_EQUAL(result[2], 0);
	}
	BOOST_AUTO_TEST_CASE(approximate_result) {
		unsigned char dividend[] = {1, 0, 0, 0};
		unsigned char divisor[] = {0, 3, 0, 0};
		unsigned char result[] = {0, 0, 0, 0};
		// result will represent 256 / 3 = 85.3333
		array_arithmetic::divide(dividend, divisor, result, 4, 1);
		BOOST_CHECK_EQUAL(result[0], 85);
		// 85 infinitely repeating is the representation of 1 / 3 in base 256
		BOOST_CHECK_EQUAL(result[1], 85);
		BOOST_CHECK_EQUAL(result[2], 85);
		BOOST_CHECK_EQUAL(result[3], 85);
	}
BOOST_AUTO_TEST_SUITE_END()


/*
 * Copyright (C) 2012-2015 Open Source Robotics Foundation
 *
 * 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.
 *
*/
#include <boost/bind.hpp>
#include <gazebo/gazebo.hh>
#include <ignition/math.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <stdio.h>

namespace gazebo
{
  class TrajectoryGuided : public ModelPlugin
  {
    public: void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/)
    {
      // Store the pointer to the model
      this->model = _parent;

        // create the animation
        gazebo::common::PoseAnimationPtr anim(
              // name the animation "test",
              // make it last 10 seconds,
              // and set it on a repeat loop
              new gazebo::common::PoseAnimation("test", 32.0, true));

        gazebo::common::PoseKeyFrame *key;

       // Trajectory goes through 12 points and takes 32s in between them

       // set starting location of the model
        key = anim->CreateKeyFrame(0);
        key->Translation(ignition::math::Vector3d(0, -20, 0)); // midway
        key->Rotation(ignition::math::Quaterniond(0, 0, 0));

        // set waypoint location after 2 seconds
        key = anim->CreateKeyFrame(2.0);
        key->Translation(ignition::math::Vector3d(30, -20, 0)); // in
        key->Rotation(ignition::math::Quaterniond(0, 0, 0));

        key = anim->CreateKeyFrame(6.0);
        key->Translation(ignition::math::Vector3d(40, -10, 0));
        key->Rotation(ignition::math::Quaterniond(0, 0, 1.5707)); // out

        key = anim->CreateKeyFrame(8.0);
        key->Translation(ignition::math::Vector3d(40, 0, 0));
        key->Rotation(ignition::math::Quaterniond(0, 0, 1.5707)); // midway

        key = anim->CreateKeyFrame(10.0);
        key->Translation(ignition::math::Vector3d(40, 10, 0));
        key->Rotation(ignition::math::Quaterniond(0, 0, 1.5707)); // in

        key = anim->CreateKeyFrame(14.0);
        key->Translation(ignition::math::Vector3d(30, 20, 0));
        key->Rotation(ignition::math::Quaterniond(0, 0, 3.14159)); // out

        key = anim->CreateKeyFrame(16.0);
        key->Translation(ignition::math::Vector3d(0, 20, 0));
        key->Rotation(ignition::math::Quaterniond(0, 0, 3.14159)); // midway

        key = anim->CreateKeyFrame(18.0);
        key->Translation(ignition::math::Vector3d(-30, 20, 0));
        key->Rotation(ignition::math::Quaterniond(0, 0, 3.14159)); // in

        key = anim->CreateKeyFrame(22.0);
        key->Translation(ignition::math::Vector3d(-40, 10, 0));
        key->Rotation(ignition::math::Quaterniond(0, 0, 4.71239)); // out

        key = anim->CreateKeyFrame(24.0);
        key->Translation(ignition::math::Vector3d(-40, 0, 0));
        key->Rotation(ignition::math::Quaterniond(0, 0, 4.71239)); // midway

        key = anim->CreateKeyFrame(26.0);
        key->Translation(ignition::math::Vector3d(-40, -10, 0));
        key->Rotation(ignition::math::Quaterniond(0, 0, 4.71239)); // in

        key = anim->CreateKeyFrame(30.0);
        key->Translation(ignition::math::Vector3d(-30, -20, 0));
        key->Rotation(ignition::math::Quaterniond(0, 0, 6.28319)); // out

        // set final location equal to starting location
        key = anim->CreateKeyFrame(32.0);
        key->Translation(ignition::math::Vector3d(0, -20, 0));
        key->Rotation(ignition::math::Quaterniond(0, 0, 6.28319)); // midway

        // set the animation
        _parent->SetAnimation(anim);
    }

    // Pointer to the model
    private: physics::ModelPtr model;

    // Pointer to the update event connection
    private: event::ConnectionPtr updateConnection;
  };

  // Register this plugin with the simulator
  GZ_REGISTER_MODEL_PLUGIN(TrajectoryGuided)
}


// Copyright (c) 2021 OUXT Polaris
//
// 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.

/**
 * @file hungarian_solver.h
 * @author Masaya Kataoka (ms.kataoka@gmail.com)
 * @brief Definition of the Hungarian Solver Class
 * @version 0.1
 * @date 2019-08-11
 *
 * @copyright Copyright (c) 2019
 *
 */

// Headers in Eigen
#define EIGEN_MPL2_ONLY
#include <Eigen/Core>

// Headers in ROS
#include <rclcpp/rclcpp.hpp>

// Headers in STL
#include <float.h>

#include <random>

// Headers in Google Test
#include <gtest/gtest.h>

// Headers in Boost
#include <boost/optional.hpp>

namespace hungarian_solver
{
/**
     * @brief Hungarian Solver Class
     */
class Solver
{
public:
  Solver();
  ~Solver();
  /**
         * @brief solve hungarian algorithum by using padding matrix
         */
  boost::optional<std::vector<std::pair<int, int> > > solve(Eigen::MatrixXd cost_matrix);
  /**
         * @brief solve hungarian algorithum by using cost matrix
         */
  boost::optional<std::vector<std::pair<int, int> > > solve(
    Eigen::MatrixXd cost_matrix, double cost_of_non_assignment);

private:
  /**
         * @brief update cost matrix by using delete line index
         */
  Eigen::MatrixXd updateCostMatrix(
    Eigen::MatrixXd mat, std::vector<int> delete_rows_index, std::vector<int> delete_cols_index);
  /**
         * @brief subtract col values by minimam value of col
         */
  Eigen::MatrixXd subtractColMinima(Eigen::MatrixXd mat);
  /**
         * @brief subtract row values by minimam value of row
         */
  Eigen::MatrixXd subtractRowMinima(Eigen::MatrixXd mat);
  /**
         * @brief get non zero col flags
         */
  std::vector<bool> getNonZeroColFlags(Eigen::MatrixXd mat);
  /**
         * @brief get initial cost matrix
         */
  Eigen::MatrixXd getInitialCostMatrix(Eigen::MatrixXd cost_matrix);
  /**
         * @brief get padded cost matrix
         */
  Eigen::MatrixXd getPaddCostMatrix(Eigen::MatrixXd cost_matrix, double cost_of_non_assignment);
  /**
         * @brief get assinment of cost matrix
         */
  boost::optional<std::vector<std::pair<int, int> > > getAssignment(Eigen::MatrixXd cost_matrix);
  /**
         * @brief get index with value zero
         */
  std::vector<std::pair<int, int> > getZeroIndex(Eigen::MatrixXd mat);
  /**
         * @brief get delete lines index
         */
  std::pair<std::vector<int>, std::vector<int> > getDeleteLinesIndex(Eigen::MatrixXd mat);
  // macros for Rostest
  friend class SolverTestSuite;
  /**
         * @brief FRIENT TEST macro for getInitialCostMatrix
         * @sa getInitialCostMatrix
         */
  FRIEND_TEST(SolverTestSuite, getInitialCostMatrixTestCase1);
  /**
         * @brief FRIENT TEST macro for getInitialCostMatrix
         * @sa getInitialCostMatrix
         */
  FRIEND_TEST(SolverTestSuite, getInitialCostMatrixTestCase2);
  /**
         * @brief FRIENT TEST macro for getNonZeroColFlags
         * @sa getNonZeroColFlags
         */
  FRIEND_TEST(SolverTestSuite, getNonZeroColFlagsTestCase1);
  /**
         * @brief FRIENT TEST macro for getNonZeroColFlags
         * @sa getNonZeroColFlags
         */
  FRIEND_TEST(SolverTestSuite, getNonZeroColFlagsTestCase2);
  /**
         * @brief FRIENT TEST macro for getPaddCostMatrix
         * @sa getPaddCostMatrix
         */
  FRIEND_TEST(SolverTestSuite, getPaddCostMatrixTestCase1);
  /**
         * @brief FRIENT TEST macro for subtractRowMinima
         * @sa subtractRowMinima
         */
  FRIEND_TEST(SolverTestSuite, subtractRowMinimaTestCase1);
  /**
         * @brief FRIENT TEST macro for subtractColMinima
         * @sa subtractColMinima
         */
  FRIEND_TEST(SolverTestSuite, subtractColMinimaTestCase1);
  /**
         * @brief FRIENT TEST macro for getAssignment
         * @sa getAssignment
         */
  FRIEND_TEST(SolverTestSuite, getAssignmentTestCase1);
  /**
         * @brief FRIENT TEST macro for getAssignment
         * @sa getAssignment
         */
  FRIEND_TEST(SolverTestSuite, getAssignmentTestCase2);
  /**
         * @brief FRIENT TEST macro for getZeroIndex
         * @sa getZeroIndex
         */
  FRIEND_TEST(SolverTestSuite, getZeroIndexTestCase1);
  /**
         * @brief FRIENT TEST macro for getZeroIndex
         * @sa getZeroIndex
         */
  FRIEND_TEST(SolverTestSuite, getZeroIndexTestCase2);
  /**
         * @brief FRIENT TEST macro for getDeleteLinesIndex
         * @sa getDeleteLinesIndex
         */
  FRIEND_TEST(SolverTestSuite, getDeleteLinesIndexTestCase1);
  /**
         * @brief FRIENT TEST macro for updateCostMatrix
         * @sa updateCostMatrix
         */
  FRIEND_TEST(SolverTestSuite, updateCostMatrixTestCase1);
};
}  // namespace hungarian_solver


#include <slam3d/core/Solver.hpp>

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

using namespace slam3d;

void test_optimization(Solver* solver)
{
	Transform pose(Eigen::Translation<double, 3>(0,0,0));

	TransformWithCovariance tf_1_2(Transform(Eigen::Translation<double, 3>(1,0,0)), Covariance<6>::Identity());
	TransformWithCovariance tf_2_3(Transform(Eigen::Translation<double, 3>(0,1,0)), Covariance<6>::Identity());
	TransformWithCovariance tf_3_1(Transform(Eigen::Translation<double, 3>(-0.8, -0.7, 0.1)), Covariance<6>::Identity());
	TransformWithCovariance tf_3_4(Transform(Eigen::Translation<double, 3>(0, 1, 0)), Covariance<6>::Identity());

	solver->addVertex(1, pose);
	solver->addVertex(2, pose);
	solver->addVertex(3, pose);
	BOOST_CHECK_THROW(solver->addVertex(3, pose), Solver::DuplicateVertex);

	SE3Constraint::Ptr c_1_2(new SE3Constraint("DummySensor", tf_1_2));
	SE3Constraint::Ptr c_2_3(new SE3Constraint("DummySensor", tf_2_3));
	SE3Constraint::Ptr c_3_1(new SE3Constraint("DummySensor", tf_3_1));
	SE3Constraint::Ptr c_3_4(new SE3Constraint("DummySensor", tf_3_4));

	solver->addEdge(1,2,c_1_2);
	solver->addEdge(2,3,c_2_3);
	solver->addEdge(3,1,c_3_1);

	BOOST_CHECK_THROW(solver->addEdge(1,4,c_1_2), Solver::BadEdge);
	BOOST_CHECK_THROW(solver->setFixed(4), Solver::UnknownVertex);

	BOOST_CHECK_NO_THROW(solver->saveGraph("graph_original.g2o"));
	BOOST_CHECK_NO_THROW(solver->setFixed(1));

	solver->compute();
	IdPoseVector corr = solver->getCorrections();
	BOOST_CHECK_EQUAL(corr.size(), 3);

	solver->addVertex(4, pose);
	solver->addEdge(3,4,c_3_4);

	solver->compute();
	corr = solver->getCorrections();
	BOOST_CHECK_EQUAL(corr.size(), 4);

	BOOST_CHECK_NO_THROW(solver->saveGraph("graph_optimized.g2o"));
}


//---------------------------------------------------------------------------//
// Copyright (c) 2013-2014 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE TestIsSorted
#include <boost/test/unit_test.hpp>

#include <boost/compute/system.hpp>
#include <boost/compute/algorithm/iota.hpp>
#include <boost/compute/algorithm/is_sorted.hpp>
#include <boost/compute/algorithm/sort.hpp>
#include <boost/compute/algorithm/reverse.hpp>
#include <boost/compute/container/vector.hpp>

#include "context_setup.hpp"

namespace compute = boost::compute;

BOOST_AUTO_TEST_CASE(is_sorted_int)
{
    compute::vector<int> vec(context);
    BOOST_CHECK(compute::is_sorted(vec.begin(), vec.end(), queue));

    vec.push_back(1, queue);
    BOOST_CHECK(compute::is_sorted(vec.begin(), vec.end(), queue));

    vec.push_back(2, queue);
    BOOST_CHECK(compute::is_sorted(vec.begin(), vec.end(), queue));

    vec.push_back(0, queue);
    BOOST_CHECK(compute::is_sorted(vec.begin(), vec.end(), queue) == false);

    vec.push_back(-2, queue);
    BOOST_CHECK(compute::is_sorted(vec.begin(), vec.end(), queue) == false);

    compute::sort(vec.begin(), vec.end(), queue);
    BOOST_CHECK(compute::is_sorted(vec.begin(), vec.end(), queue));
}

BOOST_AUTO_TEST_CASE(is_sorted_ones)
{
    compute::vector<int> vec(2048, context);
    compute::fill(vec.begin(), vec.end(), 1, queue);
    BOOST_CHECK(compute::is_sorted(vec.begin(), vec.end(), queue));
}

BOOST_AUTO_TEST_CASE(is_sorted_iota)
{
    // create vector with values from 1..1000
    compute::vector<int> vec(1000, context);
    compute::iota(vec.begin(), vec.end(), 1, queue);
    BOOST_CHECK(compute::is_sorted(vec.begin(), vec.end(), queue));

    // reverse the range
    compute::reverse(vec.begin(), vec.end(), queue);
    BOOST_CHECK(compute::is_sorted(vec.begin(), vec.end(), queue) == false);
    BOOST_CHECK(compute::is_sorted(vec.begin(), vec.end(), compute::greater<int>(), queue));

    // reverse it back
    compute::reverse(vec.begin(), vec.end(), queue);
    BOOST_CHECK(compute::is_sorted(vec.begin(), vec.end(), queue));
    BOOST_CHECK(compute::is_sorted(vec.begin(), vec.end(), compute::greater<int>(), queue) == false);
}

BOOST_AUTO_TEST_SUITE_END()


/*
 * Copyright (c) 2013-2015 Masahide Kashiwagi (kashi@waseda.jp)
 */

#ifndef ODESCALE_HPP
#define ODESCALE_HPP

#include <boost/numeric/ublas/vector.hpp>

/*
 *  from "Iri, Fujino: Suuchi Keisan no Joushiki, pp.95-96"
 */

namespace kv {

namespace ub = boost::numeric::ublas;

template <class F> class Odescale {
	public:
	F f;
	Odescale(F f) : f(f) {}
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x, T t){
		ub::vector<T> y, tx, ty;
		T g0;
		int s = x.size();
		int i;

		y.resize(s);
		tx.resize(s-1);

		for (i=0; i<s-1; i++) {
			tx(i) = x(i);
		}

		ty = f(tx, x(s-1));

		g0 = 1.;
		for (i=0; i<s-1; i++) {
			g0 += ty(i) * ty(i);
		}
		g0 = 1. / sqrt(g0);

		for (i=0; i<s-1; i++) {
			y(i) = ty(i) * g0;
		}
		y(s-1) = g0;

		return y;
	}
};

} // namespace kv

#endif // ODESCALE_HPP


#pragma once

// system includes --------------------------------------------------
#include <Eigen/Core>
#include <Eigen/Dense>
#include <cmath>
#include <stdexcept>
#include <vector>
// own includes -----------------------------------------------------
#include "aux/message.hpp"
#include "aux/timer.hpp"
#include "shift_hermite.hpp"

namespace boltzmann {

template <typename BASIS, typename NUMERIC = double>
class ShiftHermite2D
{
 public:
  typedef NUMERIC numeric_t;
  typedef BASIS basis_t;
  typedef std::vector<numeric_t> std_vec_t;

 public:
  /**
   *
   *
   * @param basis
   * @param N      max. polynomial degree+1 in one variable
   */
  ShiftHermite2D(const basis_t& basis);

  void init();

  void shift(numeric_t* c, numeric_t x, numeric_t y);

  // debug
  const HShiftMatrix<numeric_t>& get_sx() { return sx_op_; }
  const HShiftMatrix<numeric_t>& get_sy() { return sy_op_; }

 private:
  typedef typename BASIS::elem_t elem_t;
  typedef typename boost::mpl::at_c<typename elem_t::types_t, 0>::type hx_t;
  typedef typename boost::mpl::at_c<typename elem_t::types_t, 1>::type hy_t;
  typedef Eigen::Matrix<numeric_t, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> matrix_t;

 private:
  const basis_t& basis_;
  /// buffers
  matrix_t buf_in;
  matrix_t buf_out;
  /// permutation vector
  std::vector<unsigned int> perm_;
  bool is_initialized_;

  typename elem_t::Acc::template get<hx_t> get_hx;
  typename elem_t::Acc::template get<hy_t> get_hy;

  HShiftMatrix<numeric_t> sx_op_;
  HShiftMatrix<numeric_t> sy_op_;
};

// ----------------------------------------------------------------------
template <typename BASIS, typename NUMERIC>
ShiftHermite2D<BASIS, NUMERIC>::ShiftHermite2D(const basis_t& basis)
    : basis_(basis)
    ,

    perm_(basis.n_dofs())
    , is_initialized_(false)
{ /* empty */
}

// ----------------------------------------------------------------------
template <typename BASIS, typename NUMERIC>
void
ShiftHermite2D<BASIS, NUMERIC>::init()
{
  // max degree
  unsigned int max_kx =
      get_hx(*std::max_element(basis_.begin(),
                               basis_.end(),
                               [&](const elem_t& e1, const elem_t& e2) {
                                 return get_hx(e1).get_id().k < get_hx(e2).get_id().k;
                               }))
          .get_id()
          .k;
  unsigned int max_ky =
      get_hy(*std::max_element(basis_.begin(),
                               basis_.end(),
                               [&](const elem_t& e1, const elem_t& e2) {
                                 return get_hy(e1).get_id().k < get_hy(e2).get_id().k;
                               }))
          .get_id()
          .k;
  buf_in.resize(max_kx + 1, max_ky + 1);
  buf_out.resize(max_kx + 1, max_ky + 1);

  buf_in.fill(0);
  buf_out.fill(0);

  unsigned int stride = buf_in.cols();

  // build permutation vector
  unsigned int i = 0;
  for (auto elem = basis_.begin(); elem < basis_.end(); ++elem, ++i) {
    unsigned int kx = get_hx(*elem).get_id().k;
    unsigned int ky = get_hy(*elem).get_id().k;
    perm_[i] = kx * stride + ky;
  }

  Timer timer;
  // initialize shift matrices
  timer.restart();
  sx_op_.init(max_kx);
  sy_op_.init(max_ky);
  print_timer(timer.stop(), "initialize shift matrices");
  is_initialized_ = true;
}

// ----------------------------------------------------------------------
template <typename BASIS, typename NUMERIC>
void
ShiftHermite2D<BASIS, NUMERIC>::shift(numeric_t* c, numeric_t x, numeric_t y)
{
  assert(is_initialized_);

  // permute
  numeric_t* in = buf_in.data();

  for (unsigned int i = 0; i < basis_.n_dofs(); ++i) {
    in[perm_[i]] = c[i];
  }

  // apply shift matrices
  sx_op_.setx(x);
  sy_op_.setx(y);

  auto& Sx = sx_op_.get();
  auto& Sy = sy_op_.get();

  buf_out = Sx * buf_in * Sy.transpose();

  numeric_t* out = buf_out.data();

  // invert permutation and overwrite c
  for (unsigned int i = 0; i < basis_.n_dofs(); ++i) {
    c[i] = out[perm_[i]];
  }
}

}  // end namespace boltzmann


/*
// Copyright 2018 University of Liege
//
// 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.
//
// Authors:
// - Adrien Crovato
*/

//// Surface pressure writing
// Write panel pressure surface in external .dat and .pos files
//
// I/O:
// - outPath: path to write file
// - sRef: reference surface of the full wing
// - Minf: freestream Mach number
// - alpha: freestream angle of attack
// - bPan: (network of) body panels
// - cL: lift coefficient
// - cD: drag coefficient

#include <iostream>
#include <iomanip>
#include <fstream>
#include <Eigen/Dense>
#include "write_sp.h"

#define  PI 3.14159

using namespace std;
using namespace Eigen;

void write_sp(string outPath, double sRef, double alpha, double Minf, Network &bPan, double cL, double cD) {

    //// Write to .dat file
    cout << "Writing surface pressure file in 'sp.dat'... " << flush;
    string outPathDat = outPath + "sp.dat";
    int idx = 0;
    ofstream sp;
    sp.open(outPathDat);

    // General information (header)
    sp << "Surface pressure file" << endl << endl;
    sp.width(25); sp << left << "Angle of attack: "; sp.width(20); sp << left << alpha*180/PI << endl;
    sp.width(25); sp << left << "Mach: "; sp.width(20); sp << left << Minf << endl;
    sp.width(25); sp << left << "Panels: "; sp.width(20); sp << left << bPan.nP << endl;
    sp.width(25); sp << left << "Full reference surface: "; sp.width(20); sp << left << sRef << endl;
    sp.width(25); sp << left << "Lift coefficient: "; sp.width(20); sp << left << cL << endl;
    sp.width(25); sp << left << "Drag coefficient: "; sp.width(20); sp << left << cD << endl;
    sp.width(25); sp << left << "Lift to drag ratio: "; sp.width(20); sp << left << cL/cD << endl << endl;

    // Panel pressure coefficient
    for (int j = 0; j < bPan.nS_; ++j) {
        // Labeling
        sp << "*** Stat" << j << " - y = "<< bPan.CG(idx + 1,1) << " ***" << endl;
        sp.width(15); sp << right << "x";
        sp.width(15); sp << right << "z";
        sp.width(15); sp << right << "cp" << endl;
        for (int i = 0; i < bPan.nC_; ++i) {
            idx = i + j * bPan.nC_;
            sp.width(15); sp << right << bPan.CG(idx,0);
            sp.width(15); sp << right << bPan.CG(idx,2);
            sp.width(15); sp << right << bPan.cP(idx) << endl;
        }
        sp << endl;
    }

    // Close file
    sp.close();
    cout << "Done!" << endl;

    //// Write to gmsh .pos file
    cout << "Writing surface pressure file in 'Cp.pos'... " << flush;
    string outPathPos = outPath + "Cp.pos";
    idx = 0;
    sp.open(outPathPos);

    // Gmsh header
    sp << "View \"Cp\" {" << endl;

    // Panel pressure coefficient
    for (int j = 0; j < bPan.nS_; ++j) {
        for (int i = 0; i < bPan.nC_; ++i) {
            sp << "SQ(";
            for (int k = 0; k < 3; ++k)
                sp << bPan.v0(idx,k) << ",";
            for (int k = 0; k < 3; ++k)
                sp << bPan.v1(idx,k) << ",";
            for (int k = 0; k < 3; ++k)
                sp << bPan.v2(idx,k) << ",";
            for (int k = 0; k < 2; ++k)
                sp << bPan.v3(idx,k) << ",";
            sp << bPan.v3(idx,2) << "){";
            for (int k = 0; k < 3; ++k)
                sp << bPan.cP(idx) << ",";
            sp << bPan.cP(idx) << "};" << endl;
            idx++;
        }
    }

    // Gmsh footer
    sp << "};" << endl << endl;

    // Close file
    sp.close();
    cout << "Done!" << endl;
}


#include <scitbx/math/weighted_covariance.h>

#include <boost/python/class.hpp>
#include <boost/python/return_arg.hpp>

namespace scitbx { namespace math { namespace boost_python {

  template <typename FloatType>
  struct weighted_covariance_wrapper
  {
    typedef weighted_covariance<FloatType> wt;
    typedef af::const_ref<FloatType> const &const_ref_t;

    #define ADD_PROPERTY(name) add_property(#name, &wt::name)

    static void wrap(char const *name) {
      using namespace boost::python;
      return_self<> rs;
      class_<wt>(name, no_init)
        .def(init<>())
        .def(init<const_ref_t, const_ref_t, const_ref_t>
             ((arg("x"), arg("y"), arg("weights"))))
        .def("accumulate", &wt::accumulate,
             (arg("x"), arg("y"), arg("weight")), rs)
        .ADD_PROPERTY(mean_x)
        .ADD_PROPERTY(mean_y)
        .ADD_PROPERTY(variance_x)
        .ADD_PROPERTY(variance_y)
        .ADD_PROPERTY(covariance_xy)
        .ADD_PROPERTY(correlation)
        ;
    }
  };

  template <typename FloatType>
  struct multivariate_moments_wrapper
  {
    typedef multivariate_moments<FloatType> wt;
    typedef af::const_ref<FloatType> const &const_ref_t;

    static void wrap(char const *name) {
      using namespace boost::python;
      class_<wt>(name,no_init)
        .def(init<>())
        .def(init<const_ref_t>
              ((arg("weights"))))
        .def("update", &wt::update,
               (arg("data")) )
        .def("mean", &wt::mean)
        .def("variance", &wt::variance)
        .def("vcv_upper_triangle_packed", &wt::vcv_upper_triangle_packed)
        .def("vcv_raw_upper_triangle_packed", &wt::vcv_raw_upper_triangle_packed)
        ;
    }

  };

  void wrap_weighted_covariance() {
    weighted_covariance_wrapper<double>::wrap("weighted_covariance");
    multivariate_moments_wrapper<double>::wrap("multivariate_moments");
  }

}}}


#pragma once
#ifndef CANNON_RAY_CAMERA_H
#define CANNON_RAY_CAMERA_H

/*!
 * \file cannon/ray/camera.hpp
 * \brief File containing Camera class definition.
 */

#include <Eigen/Dense>

#include <cannon/math/random_double.hpp>
#include <cannon/utils/class_forward.hpp>

using namespace Eigen;

using namespace cannon::math;

namespace cannon {
  namespace ray {

    CANNON_CLASS_FORWARD(Ray);

    /*!
     * \brief Struct representing the sampled random values necessary for
     * generating a camera ray.
     */
    struct CameraSample {
      Vector2d p_film;
      Vector2d p_lens;
      double time;
    };

    /*!
     * \brief Class representing a camera generating rays into a scene.
     */
    class Camera {
      public:

        /*!
         * Constructor taking look point, lookat point, camera up vector, field
         * of view (in radians) and aspect ratio.
         */
        Camera(const Vector3d& look_from, const Vector3d& look_at, const
            Vector3d& vup, double vfov, double aspect_ratio, double aperture,
            double focus_dist, double time_0 = 0.0, double time_1 = 1.0) {
          auto h = std::tan(vfov / 2);
          auto viewport_height = 2.0 * h;
          auto viewport_width = aspect_ratio * viewport_height;

          w_ = (look_from - look_at).normalized();
          u_ = vup.cross(w_).normalized();
          v_ = w_.cross(u_);

          origin_ = look_from;
          horizontal_ = focus_dist * viewport_width * u_;
          vertical_ = focus_dist * viewport_height * v_;
          lower_left_corner_ = origin_ - horizontal_/2 - vertical_/2 - focus_dist * w_;

          lens_radius_ = aperture / 2;

          time_0_ = time_0;
          time_1_ = time_1;
        }

        /*!
         * Get a ray corresponding to a particular direction into the scene.
         *
         * \param s Horizontal component of ray direction along view.
         * \param t Vertical component of ray direction along view.
         *
         * \returns The sampled ray.
         */
        Ray get_ray(double s, double t) const;

        /*!
         * Get a ray for a particular pixel using the input camera sample.
         *
         * \param s Horizontal component of ray direction along view.
         * \param t Vertical component of ray direction along view.
         * \param sample CameraSample to use for random samples
         *
         * \returns The sampled ray.
         */
        Ray get_ray(double s, double t, const CameraSample& sample) const;

      private:
        Vector3d origin_; //!< Camera origin
        Vector3d lower_left_corner_; //!< Lower-left corner of camera view plane
        Vector3d horizontal_; //!< Horizontal extent of camera view plane
        Vector3d vertical_; //!< Vertical extent of camera view plane
        Vector3d u_, v_, w_; //!< Basis vectors for camera
        double lens_radius_; //!< Simulated thin lens radius
        double time_0_, time_1_; //!< Shutter open and close time
    };

  } // namespace ray
} // namespace cannon

#endif /* ifndef CANNON_RAY_CAMERA_H */


/*
 * num_gen.cpp: Number generation utility. Generates a list of integers within a
 * given range (min to max).
 *
 * Version:     1.0.0
 * License:     MIT License (see LICENSE.txt for more details)
 * Author:      Joshua Morrison (GitHub: MrM21632)
 * Last Edited: 7/4/2018, 4:00pm
 */

#include <fstream>
#include <cstdio>
#include <cstdint>
#include <cstdlib>
#include <cstring>
#include <ctime>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/random_device.hpp>
#include <boost/random/uniform_int_distribution.hpp>

int main(int argc, char **argv) {
    if (argc != 4) {
        std::printf("Usage: num_gen total min max\n");
        std::printf("Generates a list of numbers in the range [min, max].\n\n");
        std::printf("total\t\tNumber of numbers to generate\n");
        std::printf("min\t\tMinimum bound for number generation\n");
        std::printf("max\t\tMaximum bound for number generation\n\n");
        std::printf("NOTE: Bounds must be within (-2^63, 2^63).\n");

        return EXIT_FAILURE;
    }

    char *end;
    uint64_t total = std::strtoull(argv[1], &end, 10);
    int64_t min = std::strtoll(argv[2], &end, 10);
    int64_t max = std::strtoll(argv[3], &end, 10);
    std::ofstream numbers_file("numbers.txt");

    boost::random::random_device rd;
    boost::random::mt19937 mt(rd());
    boost::random::uniform_int_distribution<int64_t> dist(min, max);

    for (uint64_t i = 0; i < total; ++i)
        numbers_file << dist(mt) << "\n";

    numbers_file.close();
    std::printf("Number generation complete.\n");
}


#pragma once

#include <cassert>
#include <iostream>
#include <algorithm>
#include <numeric>

#include <boost/archive/binary_iarchive.hpp>
#include <boost/archive/binary_oarchive.hpp>
#include <boost/serialization/vector.hpp>

#include <common/error.hpp>

#include <tensor/index.hpp>
#include <tensor/tensor.hpp>

namespace Construction {
	namespace Tensor {

		class IsNoPermutationException : public Exception {
		public:
			IsNoPermutationException(const std::string& error) : Exception(error) { }
			IsNoPermutationException() : Exception("The given combination is no permutation") {}
		};

		class BinaryPermutation {
		public:
			BinaryPermutation() = default;

			BinaryPermutation(unsigned a, unsigned b) : a(a), b(b) { }
			BinaryPermutation(const BinaryPermutation& other)
				: a(other.a), b(other.b) { }
		public:
			bool operator==(const BinaryPermutation& other) const {
				return a == other.a && b == other.b;
			}

			bool operator!=(const BinaryPermutation& other) const {
				return a != other.a || b != other.b;
			}
		public:
			Indices operator()(const Indices& indices) const {
				assert(a > 0 && b > 0 && indices.Size() >= a && indices.Size() >= b);

				Indices result;

				for (unsigned i=0; i < indices.Size(); ++i) {
					if (i == a-1) result.Insert(indices[b-1]);
					else if (i == b-1) result.Insert(indices[a-1]);
					else result.Insert(indices[i]);
				}

				return result;
			}

			std::vector<int> operator()(std::vector<int> list) const {
				assert(a > 0 && b > 0 && list.size() >= a && list.size() >= b);

				std::vector<int> result;

				for (unsigned i=0; i < list.size(); ++i) {
					if (i == a-1) result.push_back(list[b-1]);
					else if (i == b-1) result.push_back(list[a-1]);
					else result.push_back(list[i]);
				}

				return result;
			}

			template<typename... Args>
			std::vector<int> operator()(Args... args) const {
				return (*this)({ args... });
			}

			/*Tensor operator()(const Tensor& tensor) const {
				return Tensor
			}*/
		public:
			friend std::ostream& operator<<(std::ostream& os, const BinaryPermutation& permutation) {
				os << "(" << permutation.a << " <-> " << permutation.b << ")";
				return os;
			}
		private:
			friend class boost::serialization::access;

			template<class Archive>
			void serialize(Archive& ar, const unsigned int version) {
				ar & a;
				ar & b;
			}
		private:
			unsigned a;
			unsigned b;
		};

		/**
			\class Permutation

			Allows arbitrary permutation of an index combination
		 */
		class Permutation {
		public:
			Permutation() = default;

			Permutation(unsigned a, unsigned b) {
				this->permute.emplace_back(BinaryPermutation(a,b));
			}

			Permutation(const BinaryPermutation& permute) {
				this->permute.push_back(permute);
			}

			Permutation(std::vector<BinaryPermutation> permute)
				: permute(permute) { }

			// Copy constructor
			Permutation(const Permutation& other)
				: permute(other.permute) { }
		public:
			/*Permutation& operator=(std::vector<BinaryPermutation> permute) {
				this->permute = permute;
				return *this;
			}*/

			Permutation& operator=(const Permutation& other) {
				permute = other.permute;
				return *this;
			}

			Permutation& operator=(Permutation&& other) {
				permute = std::move(other.permute);
				return *this;
			}
		public:
			void Insert(const BinaryPermutation& p) {
				permute.push_back(p);
			}

			void Insert(unsigned a, unsigned b) {
				permute.emplace_back(BinaryPermutation(a,b));
			}
 		public:
			Indices operator()(const Indices& indices) const {
				Indices result = indices;
				for (auto& p : permute) {
					result = p(result);
				}
				return result;
			}

			std::vector<int> operator()(const std::vector<int>& indices) const {
				std::vector<int> result = indices;
				for (auto& p : permute) {
					result = p(result);
				}
				return result;
			}

			template<typename... Args>
			std::vector<unsigned> operator()(Args... args) const {
				return (*this)({args...});
			}
		public:
			bool IsEven() const {
				return permute.size() % 2 == 0;
			}

			bool IsOdd() const {
				return permute.size() % 2 != 0;
			}

			int Sign() const {
				return IsOdd() ? -1 : 1;
			}
		public:
			/**
				\brief A cyclic permutation of the indices

				{abcd} => {bcda}
			 */
			static Indices Cyclic(const Indices& indices) {
				Permutation p;
				for (int i=1; i<indices.Size(); i++) {
					p.Insert(i, i+1);
				}
				return p(indices);
			}

			static std::vector<int> Cyclic(const std::vector<int>& indices) {
				Permutation p;
				for (int i=1; i<indices.size(); i++) {
					p.Insert(i, i+1);
				}
				return p(indices);
			}

			template<typename... Args>
			static std::vector<int> Cyclic(Args... args) {
				return Cyclic({ args... });
			}

			/**
				Returns a permutation to get from an index combination to another

			 */
			static Permutation From(const Indices& indices, const Indices& to) {
				if (!indices.IsPermutationOf(to)) {
					throw IsNoPermutationException();
				}

				// Clone the vector
				auto vec = indices;

				int pos = 0;
				Permutation result;

				for (int i=0; i<indices.Size(); ++i) {
					auto current = vec[pos];
					auto id = std::find(to.begin(), to.end(), current) - to.begin();

					if (id == pos) {
						pos++;
						continue;
					}

					result.Insert(pos + 1, id + 1);

					// Do the switch
					std::iter_swap(vec.begin() + pos, vec.begin() + id);
				}

				return result;
			}

			static Permutation From(const std::vector<int>& indices, const std::vector<int>& to) {
				if (!Indices::IsPermutationOf(indices, to)) {
					throw IsNoPermutationException();
				}

				// Clone the vector
				auto vec = indices;

				int pos = 0;
				Permutation result;

				for (int i=0; i<indices.size(); ++i) {
					auto current = vec[pos];
					auto id = std::find(to.begin(), to.end(), current) - to.begin();

					if (id == pos) {
						pos++;
						continue;
					}

					result.Insert(pos + 1, id + 1);

					// Do the switch
					std::iter_swap(vec.begin() + pos, vec.begin() + id);
				}

				return result;
			}

			static Permutation From(const std::vector<unsigned>& indices, const std::vector<unsigned>& to) {
				if (!Indices::IsPermutationOf(indices, to)) {
					throw IsNoPermutationException();
				}

				// Clone the vector
				auto vec = indices;

				int pos = 0;
				Permutation result;

				for (int i=0; i<indices.size(); ++i) {
					auto current = vec[pos];
					auto id = std::find(to.begin(), to.end(), current) - to.begin();

					if (id == pos) {
						pos++;
						continue;
					}

					result.Insert(pos + 1, id + 1);

					// Do the switch
					std::iter_swap(vec.begin() + pos, vec.begin() + id);
				}

				return result;
			}

            static Permutation From(Indices indices, std::map<Index, Index> transformation) {
                Permutation result;

                for (unsigned i=0; i<indices.Size(); ++i) {
                    auto it = transformation.find(indices[i]);
                    if (it == transformation.end()) throw IsNoPermutationException();

                    unsigned pos=0;
                    bool found=false;
                    for (unsigned j=0; j<indices.Size(); ++j) {
                        if (it->second == indices[j]) { pos = j; found=true; break; }
                    }

                    if (!found) throw IsNoPermutationException();

                    result.Insert({ i+1, pos+1 });
                }

                return result;
            }

            inline static Indices Shuffle(Indices indices, std::map<Index, Index> transformation) {
                return From(indices, transformation)(indices);
            }
		public:
			friend std::ostream& operator<<(std::ostream& os, const Permutation& permutation) {
				os << "[";
				for (int i=0; i<permutation.permute.size(); i++) {
					os << permutation.permute[i];
					if (i !=permutation.permute.size()-1) os << ", ";
				}
				os << "]";
				return os;
			}
		private:
			friend class boost::serialization::access;

			template<class Archive>
			void serialize(Archive& ar, const unsigned int version) {
				ar & permute;
			}
		private:
			std::vector<BinaryPermutation> permute;
		};

	}
}


/*
 * Copyright 2014-2019, CNRS
 * Copyright 2018-2019, INRIA
 */

#ifndef __eigenpy_angle_axis_hpp__
#define __eigenpy_angle_axis_hpp__

#include "eigenpy/fwd.hpp"

#include <boost/python.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>

namespace eigenpy
{

  namespace bp = boost::python;

  template<typename AngleAxis> class AngleAxisVisitor;

  template<typename Scalar>
  struct call< Eigen::AngleAxis<Scalar> >
  {
    typedef Eigen::AngleAxis<Scalar> AngleAxis;
    static inline void expose()
    {
      AngleAxisVisitor<AngleAxis>::expose();
    }

    static inline bool isApprox(const AngleAxis & self, const AngleAxis & other,
                                const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
    {
      return self.isApprox(other,prec);
    }
  };

  BOOST_PYTHON_FUNCTION_OVERLOADS(isApproxAngleAxis_overload,call<Eigen::AngleAxisd>::isApprox,2,3)

  template<typename AngleAxis>
  class AngleAxisVisitor
  : public bp::def_visitor< AngleAxisVisitor<AngleAxis> >
  {

    typedef typename AngleAxis::Scalar Scalar;
    typedef typename AngleAxis::Vector3 Vector3;
    typedef typename AngleAxis::Matrix3 Matrix3;

    typedef typename Eigen::Quaternion<Scalar,0> Quaternion;

  public:

    template<class PyClass>
    void visit(PyClass& cl) const
    {
      cl
      .def(bp::init<>("Default constructor"))
      .def(bp::init<Scalar,Vector3>
           ((bp::arg("angle"),bp::arg("axis")),
            "Initialize from angle and axis."))
      .def(bp::init<Matrix3>
           ((bp::arg("rotationMatrix")),
            "Initialize from a rotation matrix"))
      .def(bp::init<Quaternion>(bp::arg("quaternion"),"Initialize from a quaternion."))
      .def(bp::init<AngleAxis>(bp::arg("copy"),"Copy constructor."))

      /* --- Properties --- */
      .add_property("axis",
                    bp::make_function((const Vector3 & (AngleAxis::*)()const)&AngleAxis::axis,
                                      bp::return_value_policy<bp::copy_const_reference>()),
                    &AngleAxisVisitor::setAxis,"The rotation axis.")
      .add_property("angle",
                    (Scalar (AngleAxis::*)()const)&AngleAxis::angle,
                    &AngleAxisVisitor::setAngle,"The rotation angle.")

      /* --- Methods --- */
      .def("inverse",&AngleAxis::inverse,"Return the inverse rotation.")
      .def("fromRotationMatrix",&AngleAxis::template fromRotationMatrix<Matrix3>,
           bp::arg("Sets *this from a 3x3 rotation matrix."),bp::return_self<>())
      .def("toRotationMatrix",&AngleAxis::toRotationMatrix,"Constructs and returns an equivalent 3x3 rotation matrix.")
      .def("matrix",&AngleAxis::matrix,"Returns an equivalent rotation matrix.")

      .def("isApprox",
           &call<AngleAxis>::isApprox,
           isApproxAngleAxis_overload(bp::args("other","prec"),
                                      "Returns true if *this is approximately equal to other, within the precision determined by prec."))

      /* --- Operators --- */
      .def(bp::self * bp::other<Vector3>())
      .def(bp::self * bp::other<Quaternion>())
      .def(bp::self * bp::self)
      .def("__eq__",&AngleAxisVisitor::__eq__)
      .def("__ne__",&AngleAxisVisitor::__ne__)

      .def("__str__",&print)
      .def("__repr__",&print)
      ;
    }

  private:

    static void setAxis(AngleAxis & self, const Vector3 & axis)
    { self.axis() = axis; }

    static void setAngle( AngleAxis & self, const Scalar & angle)
    { self.angle() = angle; }

    static bool __eq__(const AngleAxis & u, const AngleAxis & v)
    { return u.axis() == v.axis() && v.angle() == u.angle(); }

    static bool __ne__(const AngleAxis & u, const AngleAxis & v)
    { return !__eq__(u,v); }

    static std::string print(const AngleAxis & self)
    {
      std::stringstream ss;
      ss << "angle: " << self.angle() << std::endl;
      ss << "axis: " << self.axis().transpose() << std::endl;

      return ss.str();
    }

  public:

    static void expose()
    {
      bp::class_<AngleAxis>("AngleAxis",
                            "AngleAxis representation of rotations.\n\n",
                            bp::no_init)
      .def(AngleAxisVisitor<AngleAxis>());
    }

  };

} // namespace eigenpy

#endif //ifndef __eigenpy_angle_axis_hpp__