/**
 * @author Andre Anjos <andre.anjos@idiap.ch>
 * @date Fri 10 Jan 2014 14:26:25 CET
 *
 * @brief Python bindings for bob::learn::activation
 *
 * Copyright (C) 2011-2014 Idiap Research Institute, Martigny, Switzerland
 */

#define BOB_LEARN_ACTIVATION_MODULE
#include <bob.blitz/cppapi.h>
#include <bob.blitz/cleanup.h>
#include <bob.extension/defines.h>
#include <bob.learn.activation/api.h>
#include <bob.io.base/api.h>
#include <bob.learn.activation/Activation.h>
#include <boost/bind.hpp>
#include <boost/function.hpp>
#include <structmember.h>

/*******************************************
 * Implementation of Activation base class *
 *******************************************/

PyDoc_STRVAR(s_activation_str, BOB_EXT_MODULE_PREFIX ".Activation");

PyDoc_STRVAR(s_activation_doc,
"Base class for activation functors.\n\
\n\
.. warning::\n\
\n\
   You cannot instantiate an object of this type directly, you must\n\
   use it through one of the inherited types.\n\
\n\
.. warning::\n\
\n\
   You cannot create classes in Python that derive from this one and\n\
   expect them to work fine with the C++ code, as no hook is\n\
   implemented as of this time to allow for this. You must create\n\
   a class that inherits from the C++\n\
   :cpp:type:`bob::learn::activation::Activation` in C++ and then bind it to\n\
   Python like we have done for the classes available in these\n\
   bindings.\n\
\n\
");

static PyObject* PyBobLearnActivation_new
(PyTypeObject* type, PyObject*, PyObject*) {

  /* Allocates the python object itself */
  PyBobLearnActivationObject* self =
    (PyBobLearnActivationObject*)type->tp_alloc(type, 0);

  self->cxx.reset();

  return reinterpret_cast<PyObject*>(self);

}

PyObject* PyBobLearnActivation_NewFromActivation
(boost::shared_ptr<bob::learn::activation::Activation> a) {

  PyBobLearnActivationObject* retval = (PyBobLearnActivationObject*)PyBobLearnActivation_new(&PyBobLearnActivation_Type, 0, 0);

  retval->cxx = a;

  return Py_BuildValue("N", retval);

}

static void PyBobLearnActivation_delete (PyBobLearnActivationObject* self) {

  self->cxx.reset();
  Py_TYPE(self)->tp_free((PyObject*)self);

}

static int PyBobLearnActivation_init(PyBobLearnActivationObject* self,
    PyObject*, PyObject*) {

  PyErr_Format(PyExc_NotImplementedError, "cannot initialize object of base type `%s' - use one of the inherited classes", Py_TYPE(self)->tp_name);
  return -1;

}

/**
 * Maps all elements of arr through function() into retval
 */
static int apply(boost::function<double (double)> function,
    PyBlitzArrayObject* z, PyBlitzArrayObject* res) {

  if (z->ndim == 1) {
    auto z_ = PyBlitzArrayCxx_AsBlitz<double,1>(z);
    auto res_ = PyBlitzArrayCxx_AsBlitz<double,1>(res);
    for (int k=0; k<z_->extent(0); ++k)
      res_->operator()(k) = function(z_->operator()(k));
    return 1;
  }

  else if (z->ndim == 2) {
    auto z_ = PyBlitzArrayCxx_AsBlitz<double,2>(z);
    auto res_ = PyBlitzArrayCxx_AsBlitz<double,2>(res);
    for (int k=0; k<z_->extent(0); ++k)
      for (int l=0; l<z_->extent(1); ++l)
        res_->operator()(k,l) = function(z_->operator()(k,l));
    return 1;
  }

  else if (z->ndim == 3) {
    auto z_ = PyBlitzArrayCxx_AsBlitz<double,3>(z);
    auto res_ = PyBlitzArrayCxx_AsBlitz<double,3>(res);
    for (int k=0; k<z_->extent(0); ++k)
      for (int l=0; l<z_->extent(1); ++l)
        for (int m=0; m<z_->extent(2); ++m)
          res_->operator()(k,l,m) = function(z_->operator()(k,l,m));
    return 1;
  }

  else if (z->ndim == 4) {
    auto z_ = PyBlitzArrayCxx_AsBlitz<double,4>(z);
    auto res_ = PyBlitzArrayCxx_AsBlitz<double,4>(res);
    for (int k=0; k<z_->extent(0); ++k)
      for (int l=0; l<z_->extent(1); ++l)
        for (int m=0; m<z_->extent(2); ++m)
          for (int n=0; n<z_->extent(3); ++n)
            res_->operator()(k,l,m,n) = function(z_->operator()(k,l,m,n));
    return 1;
  }

  return 0;

}

static PyObject* PyBobLearnActivation_call1(PyBobLearnActivationObject* self,
    double (bob::learn::activation::Activation::*method) (double) const,
    PyObject* args, PyObject* kwds) {

  /* Parses input arguments in a single shot */
  static const char* const_kwlist[] = {"z", 0};
  static char** kwlist = const_cast<char**>(const_kwlist);

  PyObject* z = 0;

  if (!PyArg_ParseTupleAndKeywords(args, kwds, "O", kwlist, &z)) return 0;

  //note: object z is borrowed

  if (PyBlitzArray_Check(z) || PyArray_Check(z)) {

    PyBlitzArrayObject* z_converted = 0;
    if (!PyBlitzArray_Converter(z, &z_converted)) return 0;
    auto z_converted_ = make_safe(z_converted);

    if (z_converted->type_num != NPY_FLOAT64) {
      PyErr_Format(PyExc_TypeError, "`%s' function only supports 64-bit float arrays for input array `z'", Py_TYPE(self)->tp_name);
      return 0;
    }

    if (z_converted->ndim < 1 || z_converted->ndim > 4) {
      PyErr_Format(PyExc_TypeError, "`%s' function only accepts 1, 2, 3 or 4-dimensional arrays (not %" PY_FORMAT_SIZE_T "dD arrays)", Py_TYPE(self)->tp_name, z_converted->ndim);
      return 0;
    }

    // creates output array
    PyObject* res = PyBlitzArray_SimpleNew(NPY_FLOAT64, z_converted->ndim,
        z_converted->shape);
    auto res_ = make_safe(res);

    // processes the data
    int ok = apply(boost::bind(method, self->cxx, _1),
        z_converted, reinterpret_cast<PyBlitzArrayObject*>(res));

    if (!ok) {
      PyErr_Format(PyExc_RuntimeError, "unexpected error occurred applying `%s' to input array (DEBUG ME)", Py_TYPE(self)->tp_name);
      return 0;
    }

    return PyBlitzArray_NUMPY_WRAP(Py_BuildValue("O", res));

  }

  else if (PyBob_NumberCheck(z)) {

    PyObject* z_float = PyNumber_Float(z);
    auto z_float_ = make_safe(z_float);
    auto bound_method = boost::bind(method, self->cxx, _1);
    double res_c = bound_method(PyFloat_AsDouble(z_float));
    return PyFloat_FromDouble(res_c);

  }

  PyErr_Format(PyExc_TypeError, "`%s' is not capable to process input objects of type `%s'", Py_TYPE(self)->tp_name, Py_TYPE(z)->tp_name);
  return 0;

}

static PyObject* PyBobLearnActivation_call2(PyBobLearnActivationObject* self,
    double (bob::learn::activation::Activation::*method) (double) const,
    PyObject* args, PyObject* kwds) {

  /* Parses input arguments in a single shot */
  static const char* const_kwlist[] = {"z", "res", 0};
  static char** kwlist = const_cast<char**>(const_kwlist);

  PyBlitzArrayObject* z = 0;
  PyBlitzArrayObject* res = 0;

  if (!PyArg_ParseTupleAndKeywords(args, kwds, "O&O&", kwlist,
        &PyBlitzArray_Converter, &z,
        &PyBlitzArray_OutputConverter, &res
        )) return 0;

  //protects acquired resources through this scope
  auto z_ = make_safe(z);
  auto res_ = make_safe(res);

  if (z->type_num != NPY_FLOAT64) {
    PyErr_Format(PyExc_TypeError, "`%s' function only supports 64-bit float arrays for input array `z'", Py_TYPE(self)->tp_name);
    return 0;
  }

  if (res->type_num != NPY_FLOAT64) {
    PyErr_Format(PyExc_TypeError, "`%s' function only supports 64-bit float arrays for output array `res'", Py_TYPE(self)->tp_name);
    return 0;
  }

  if (z->ndim < 1 || z->ndim > 4) {
    PyErr_Format(PyExc_TypeError, "`%s' function only accepts 1, 2, 3 or 4-dimensional arrays (not %" PY_FORMAT_SIZE_T "dD arrays)", Py_TYPE(self)->tp_name, z->ndim);
    return 0;
  }

  if (z->ndim != res->ndim) {
    PyErr_Format(PyExc_RuntimeError, "Input and output arrays should have matching number of dimensions, but input array `z' has %" PY_FORMAT_SIZE_T "d dimensions while output array `res' has %" PY_FORMAT_SIZE_T "d dimensions", z->ndim, res->ndim);
    return 0;
  }

  for (Py_ssize_t i=0; i<z->ndim; ++i) {

    if (z->shape[i] != res->shape[i]) {
      PyErr_Format(PyExc_RuntimeError, "Input and output arrays should have matching sizes, but dimension %" PY_FORMAT_SIZE_T "d of input array `z' has %" PY_FORMAT_SIZE_T "d positions while output array `res' has %" PY_FORMAT_SIZE_T "d positions", i, z->shape[i], res->shape[i]);
      return 0;
    }

  }

  //at this point all checks are done, we can proceed into calling C++
  int ok = apply(boost::bind(method, self->cxx, _1), z, res);

  if (!ok) {
    PyErr_Format(PyExc_RuntimeError, "unexpected error occurred applying C++ `%s' to input array (DEBUG ME)", Py_TYPE(self)->tp_name);
    return 0;
  }

  return PyBlitzArray_NUMPY_WRAP(Py_BuildValue("O", res));

}

PyDoc_STRVAR(s_call_str, "f");
PyDoc_STRVAR(s_call_doc,
"o.f(z, [res]) -> array | scalar\n\
\n\
Computes the activated value, given an input array or scalar\n\
``z``, placing results in ``res`` (and returning it).\n\
\n\
If ``z`` is an array, then you can pass another array in ``res``\n\
to store the results and, in this case, we won't allocate a new\n\
one for that purpose. This can be a speed-up in certain scenarios.\n\
Note this does not work for scalars as it makes little sense to\n\
avoid scalar allocation at this level.\n\
\n\
If you decide to pass an array in ``res``, note this array should\n\
have the exact same dimensions as the input array ``z``. It is an\n\
error otherwise.\n\
\n\
.. note::\n\
\n\
   This method only accepts 64-bit float arrays as input or\n\
   output.\n\
\n\
");

static PyObject* PyBobLearnActivation_call(PyBobLearnActivationObject* self,
  PyObject* args, PyObject* kwds) {

  Py_ssize_t nargs = (args?PyTuple_Size(args):0) + (kwds?PyDict_Size(kwds):0);

  switch (nargs) {

    case 1:
      return PyBobLearnActivation_call1
        (self, &bob::learn::activation::Activation::f, args, kwds);
      break;

    case 2:
      return PyBobLearnActivation_call2
        (self, &bob::learn::activation::Activation::f, args, kwds);
      break;

    default:

      PyErr_Format(PyExc_RuntimeError, "number of arguments mismatch - %s requires 1 or 2 arguments, but you provided %" PY_FORMAT_SIZE_T "d (see help)", s_call_str, nargs);

  }

  return 0;

}

PyDoc_STRVAR(s_f_prime_str, "f_prime");
PyDoc_STRVAR(s_f_prime_doc,
"o.f_prime(z, [res]) -> array | scalar\n\
\n\
Computes the derivative of the activated value, given an input\n\
array or scalar ``z``, placing results in ``res`` (and returning\n\
it).\n\
\n\
If ``z`` is an array, then you can pass another array in ``res``\n\
to store the results and, in this case, we won't allocate a new\n\
one for that purpose. This can be a speed-up in certain scenarios.\n\
Note this does not work for scalars as it makes little sense to\n\
avoid scalar allocation at this level.\n\
\n\
If you decide to pass an array in ``res``, note this array should\n\
have the exact same dimensions as the input array ``z``. It is an\n\
error otherwise.\n\
\n\
.. note::\n\
\n\
   This method only accepts 64-bit float arrays as input or\n\
   output.\n\
\n\
");

static PyObject* PyBobLearnActivation_f_prime(PyBobLearnActivationObject* self,
  PyObject* args, PyObject* kwds) {

  Py_ssize_t nargs = (args?PyTuple_Size(args):0) + (kwds?PyDict_Size(kwds):0);

  switch (nargs) {

    case 1:
      return PyBobLearnActivation_call1
        (self, &bob::learn::activation::Activation::f_prime, args, kwds);
      break;

    case 2:
      return PyBobLearnActivation_call2
        (self, &bob::learn::activation::Activation::f_prime, args, kwds);
      break;

    default:

      PyErr_Format(PyExc_RuntimeError, "number of arguments mismatch - %s requires 1 or 2 arguments, but you provided %" PY_FORMAT_SIZE_T "d (see help)", s_call_str, nargs);

  }

  return 0;

}

PyDoc_STRVAR(s_f_prime_from_f_str, "f_prime_from_f");
PyDoc_STRVAR(s_f_prime_from_f_doc,
"o.f_prime_from_f(a, [res]) -> array | scalar\n\
\n\
Computes the derivative of the activated value, given the\n\
derivative value ``a``, placing results in ``res`` (and returning\n\
it).\n\
\n\
If ``a`` is an array, then you can pass another array in ``res``\n\
to store the results and, in this case, we won't allocate a new\n\
one for that purpose. This can be a speed-up in certain scenarios.\n\
Note this does not work for scalars as it makes little sense to\n\
avoid scalar allocation at this level.\n\
\n\
If you decide to pass an array in ``res``, note this array should\n\
have the exact same dimensions as the input array ``a``. It is an\n\
error otherwise.\n\
\n\
.. note::\n\
\n\
   This method only accepts 64-bit float arrays as input or\n\
   output.\n\
\n\
");

static PyObject* PyBobLearnActivation_f_prime_from_f
(PyBobLearnActivationObject* self, PyObject* args, PyObject* kwds) {

  Py_ssize_t nargs = (args?PyTuple_Size(args):0) + (kwds?PyDict_Size(kwds):0);

  switch (nargs) {

    case 1:
      return PyBobLearnActivation_call1
        (self, &bob::learn::activation::Activation::f_prime_from_f, args, kwds);
      break;

    case 2:
      return PyBobLearnActivation_call2
        (self, &bob::learn::activation::Activation::f_prime_from_f, args, kwds);
      break;

    default:

      PyErr_Format(PyExc_RuntimeError, "number of arguments mismatch - %s requires 1 or 2 arguments, but you provided %" PY_FORMAT_SIZE_T "d (see help)", s_call_str, nargs);

  }

  return 0;

}

PyDoc_STRVAR(s_unique_id_str, "unique_identifier");
PyDoc_STRVAR(s_unique_id_doc,
"o.unique_identifier() -> str\n\
\n\
Returns a unique (string) identifier, used by this class\n\
in connection with the Activation registry.\n\
\n\
");

static PyObject* PyBobLearnActivation_UniqueIdentifier
(PyBobLearnActivationObject* self) {
  return Py_BuildValue("s", self->cxx->unique_identifier().c_str());
}

PyDoc_STRVAR(s_load_str, "load");
PyDoc_STRVAR(s_load_doc,
"o.load(f) -> None\n\
\n\
Loads itself from a :py:class:`bob.io.base.HDF5File`\n\
\n\
");

static PyObject* PyBobLearnActivation_Load(PyBobLearnActivationObject* self,
    PyObject* f) {

  if (!PyBobIoHDF5File_Check(f)) {
    PyErr_Format(PyExc_TypeError, "`%s' cannot load itself from `%s', only from an HDF5 file", Py_TYPE(self)->tp_name, Py_TYPE(f)->tp_name);
    return 0;
  }

  auto h5f = reinterpret_cast<PyBobIoHDF5FileObject*>(f);

  try {
    self->cxx->load(*h5f->f);
  }
  catch (std::exception& e) {
    PyErr_SetString(PyExc_RuntimeError, e.what());
    return 0;
  }
  catch (...) {
    PyErr_Format(PyExc_RuntimeError, "cannot read data from file `%s' (at group `%s'): unknown exception caught", h5f->f->filename().c_str(),
        h5f->f->cwd().c_str());
    return 0;
  }

  Py_RETURN_NONE;
}

PyDoc_STRVAR(s_save_str, "save");
PyDoc_STRVAR(s_save_doc,
"o.save(f) -> None\n\
\n\
Saves itself to a :py:class:`bob.io.base.HDF5File`\n\
\n\
");

static PyObject* PyBobLearnActivation_Save
(PyBobLearnActivationObject* self, PyObject* f) {

  if (!PyBobIoHDF5File_Check(f)) {
    PyErr_Format(PyExc_TypeError, "`%s' cannot write itself to `%s', only to an HDF5 file", Py_TYPE(self)->tp_name, Py_TYPE(f)->tp_name);
    return 0;
  }

  auto h5f = reinterpret_cast<PyBobIoHDF5FileObject*>(f);

  try {
    self->cxx->save(*h5f->f);
  }
  catch (std::exception& e) {
    PyErr_SetString(PyExc_RuntimeError, e.what());
    return 0;
  }
  catch (...) {
    PyErr_Format(PyExc_RuntimeError, "cannot write data to file `%s' (at group `%s'): unknown exception caught", h5f->f->filename().c_str(),
        h5f->f->cwd().c_str());
    return 0;
  }

  Py_RETURN_NONE;
}

static PyMethodDef PyBobLearnActivation_methods[] = {
  {
    s_call_str,
    (PyCFunction)PyBobLearnActivation_call,
    METH_VARARGS|METH_KEYWORDS,
    s_call_doc
  },
  {
    s_f_prime_str,
    (PyCFunction)PyBobLearnActivation_f_prime,
    METH_VARARGS|METH_KEYWORDS,
    s_f_prime_doc
  },
  {
    s_f_prime_from_f_str,
    (PyCFunction)PyBobLearnActivation_f_prime_from_f,
    METH_VARARGS|METH_KEYWORDS,
    s_f_prime_from_f_doc
  },
  {
    s_unique_id_str,
    (PyCFunction)PyBobLearnActivation_UniqueIdentifier,
    METH_NOARGS,
    s_unique_id_doc
  },
  {
    s_load_str,
    (PyCFunction)PyBobLearnActivation_Load,
    METH_O,
    s_load_doc
  },
  {
    s_save_str,
    (PyCFunction)PyBobLearnActivation_Save,
    METH_O,
    s_save_doc
  },
  {0} /* Sentinel */
};

int PyBobLearnActivation_Check(PyObject* o) {
  return PyObject_IsInstance(o, reinterpret_cast<PyObject*>(&PyBobLearnActivation_Type));
}

static PyObject* PyBobLearnActivation_RichCompare (PyBobLearnActivationObject* self, PyObject* other, int op) {

  if (!PyBobLearnActivation_Check(other)) {
    PyErr_Format(PyExc_TypeError, "cannot compare `%s' with `%s'",
        Py_TYPE(self)->tp_name, Py_TYPE(other)->tp_name);
    return 0;
  }

  auto other_ = reinterpret_cast<PyBobLearnActivationObject*>(other);

  switch (op) {
    case Py_EQ:
      if (self->cxx->str() == other_->cxx->str()) Py_RETURN_TRUE;
      Py_RETURN_FALSE;
      break;
    case Py_NE:
      if (self->cxx->str() != other_->cxx->str()) Py_RETURN_TRUE;
      Py_RETURN_FALSE;
      break;
    default:
      Py_INCREF(Py_NotImplemented);
      return Py_NotImplemented;
  }

}

static PyObject* PyBobLearnActivation_Str (PyBobLearnActivationObject* self) {
  return Py_BuildValue("s", self->cxx->str().c_str());
}

PyTypeObject PyBobLearnActivation_Type = {
    PyVarObject_HEAD_INIT(0, 0)
    s_activation_str,                               /* tp_name */
    sizeof(PyBobLearnActivationObject),             /* tp_basicsize */
    0,                                              /* tp_itemsize */
    (destructor)PyBobLearnActivation_delete,        /* tp_dealloc */
    0,                                              /* tp_print */
    0,                                              /* tp_getattr */
    0,                                              /* tp_setattr */
    0,                                              /* tp_compare */
    0,                                              /* tp_repr */
    0,                                              /* tp_as_number */
    0,                                              /* tp_as_sequence */
    0,                                              /* tp_as_mapping */
    0,                                              /* tp_hash */
    (ternaryfunc)PyBobLearnActivation_call,         /* tp_call */
    (reprfunc)PyBobLearnActivation_Str,             /* tp_str */
    0,                                              /* tp_getattro */
    0,                                              /* tp_setattro */
    0,                                              /* tp_as_buffer */
    Py_TPFLAGS_DEFAULT | Py_TPFLAGS_BASETYPE,       /* tp_flags */
    s_activation_doc,                               /* tp_doc */
    0,                                              /* tp_traverse */
    0,                                              /* tp_clear */
    (richcmpfunc)PyBobLearnActivation_RichCompare,  /* tp_richcompare */
    0,                                              /* tp_weaklistoffset */
    0,                                              /* tp_iter */
    0,                                              /* tp_iternext */
    PyBobLearnActivation_methods,                   /* tp_methods */
    0,                                              /* tp_members */
    0,                                              /* tp_getset */
    0,                                              /* tp_base */
    0,                                              /* tp_dict */
    0,                                              /* tp_descr_get */
    0,                                              /* tp_descr_set */
    0,                                              /* tp_dictoffset */
    (initproc)PyBobLearnActivation_init,            /* tp_init */
    0,                                              /* tp_alloc */
    PyBobLearnActivation_new,                       /* tp_new */
};


/********************************************************************************
  Copyright 2017 Frank Imeson and Stephen L. Smith

  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.
*********************************************************************************/

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

// guard
#ifndef SMT_H
#define SMT_H

/********************************************************************************
 * INCLUDE
 ********************************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <time.h>
#include <ctime>
#include <string>
#include <iostream>
#include <sstream>
#include <vector>
#include <algorithm>
#include <sys/time.h>
#include <boost/format.hpp>
#include <boost/algorithm/string.hpp>

#include "tsp.hpp"
#include "formula.hpp"
#include "minisat/core/Solver.h"

using namespace std;
using namespace Minisat;

/********************************************************************************
 * Defs
 ********************************************************************************/

/********************************************************************************
 * Prototypes
 ********************************************************************************/

/************************************************************//**
 * @brief	                    Overide class
 * @version						v0.01b
 ****************************************************************/
class Theory {
//    friend class TSP_Theory;
//    friend class Edge_TSP_Theory;
//    friend class Metric_TSP_Theory;
//    friend class MST_Theory;
//    friend class Cardinality_EQ_Theory;
//    private:
//        bool conflict, conflict_checked;

    public:
        virtual void reset () {};
        virtual void print () = 0;
        virtual void minisat_trail_push_cb (
            const VMap<lbool> &assigns,
            const vec<Lit>& trail,
            vec<Lit>& propagate_list)
        {};
        virtual void minisat_trail_shrink_cb (
            const VMap<lbool> &assigns,
            const vec<Lit> &trail,
            int amount)
        {};
        virtual bool minisat_check_conflict_cb (
            const VMap<lbool> &assigns,
            const vec<Lit> &trail,
            vec<Lit> &conflict_list)
        {};
};

/************************************************************//**
 * @brief	                    Overide class
 * @version						v0.01b
 ****************************************************************/
class TSP_Theory : public Theory {
    public:
        int cb_interval;
        int tsp_cost_budget, subgraph_cost_budget;
        double theory_time;
        vector<int> soln_tour;
        virtual void print () {printf("TSP_Theory\n");};
};

/************************************************************//**
 * @brief
 * @version						v0.01b
 ****************************************************************/
class Edge_TSP_Theory : public TSP_Theory {
    private:
        TSP *graph;
        int edge_costs;
        vector<int> soln_eids;
        bool conflict, conflict_checked;

    public:
        Edge_TSP_Theory (TSP *graph, int verbose_level=-1);
        virtual void print () {printf("Edge_TSP_Theory\n");};
        virtual void reset ();

        virtual void minisat_trail_push_cb (
            const VMap<lbool> &assigns,
            const vec<Lit>& trail,
            vec<Lit>& propagate_list);

        virtual void minisat_trail_shrink_cb (
            const VMap<lbool> &assigns,
            const vec<Lit> &trail,
            int amount);

        virtual bool minisat_check_conflict_cb (
            const VMap<lbool> &assigns,
            const vec<Lit> &trail,
            vec<Lit> &conflict_list);
};

/************************************************************//**
 * @brief
 * @version						v0.01b
 ****************************************************************/
class Metric_TSP_Theory : public TSP_Theory {
    private:
        TSP *graph;
        vector<int> soln_vids;
        bool conflict, conflict_checked;

    public:
        Metric_TSP_Theory (TSP *graph, int verbose_level=-1);
        virtual void print () {printf("Metric_TSP_Theory\n");};
        virtual void reset ();

        virtual void minisat_trail_push_cb (
            const VMap<lbool> &assigns,
            const vec<Lit>& trail,
            vec<Lit>& propagate_list);

        virtual void minisat_trail_shrink_cb (
            const VMap<lbool> &assigns,
            const vec<Lit> &trail,
            int amount);

        virtual bool minisat_check_conflict_cb (
            const VMap<lbool> &assigns,
            const vec<Lit> &trail,
            vec<Lit> &conflict_list);
};

/************************************************************//**
 * @brief
 * @version						v0.01b
 ****************************************************************/
class MST_Theory : public TSP_Theory {
    private:
        TSP *graph;
        MST *mst;
        vector<int> soln_vids;
        bool mst_valid;
        bool conflict, conflict_checked;

    public:
        int tsp_cost_budget, subgraph_cost_budget;
        MST_Theory (TSP *graph, int verbose_level=-1);
        virtual void print () {printf("MST_Theory\n");};

        virtual void reset ();

        virtual void minisat_trail_push_cb (
            const VMap<lbool> &assigns,
            const vec<Lit>& trail,
            vec<Lit>& propagate_list);

        virtual void minisat_trail_shrink_cb (
            const VMap<lbool> &assigns,
            const vec<Lit> &trail,
            int amount);

        virtual bool minisat_check_conflict_cb (
            const VMap<lbool> &assigns,
            const vec<Lit> &trail,
            vec<Lit> &conflict_list);
};

/************************************************************//**
 * @brief
 * @version						v0.01b
 ****************************************************************/
class Cardinality_EQ_Theory : public Theory {
    private:
        vector<Lit> lit_set;
        vector<bool> var_in_set, var_lit_sign;
        int cardinality;
        bool conflict, conflict_checked;

    public:
        Cardinality_EQ_Theory (vector<Lit> lit_set, int cardinality, int verbose_level=-1);
        virtual void print () {printf("Cardinality_EQ_Theory\n");};

        virtual void minisat_trail_push_cb (
            const VMap<lbool> &assigns,
            const vec<Lit>& trail,
            vec<Lit>& propagate_list);

        virtual void minisat_trail_shrink_cb (
            const VMap<lbool> &assigns,
            const vec<Lit> &trail,
            int amount);

        virtual bool minisat_check_conflict_cb (
            const VMap<lbool> &assigns,
            const vec<Lit> &trail,
            vec<Lit> &conflict_list);
};

#endif


#include <boost/hana/symmetric_difference.hpp>


// Copyright 2013-2020 Lawrence Livermore National Security, LLC and other
// HavoqGT Project Developers. See the top-level LICENSE file for details.
//
// SPDX-License-Identifier: MIT

#ifndef HAVOQGT_UPPER_TRIANGLE_EDGE_GENERATOR_INCLUDED
#define HAVOQGT_UPPER_TRIANGLE_EDGE_GENERATOR_INCLUDED

#include <boost/random.hpp>
#include <havoqgt/detail/hash.hpp>

#include <utility>
#include <stdint.h>

namespace havoqgt {

class upper_triangle_edge_generator {
 public:
  // typedef uint64_t                      vertex_descriptor;
  typedef std::pair<uint64_t, uint64_t> edge_type;

  ///
  /// InputIterator class for upper_triangle_edge_generator

  class input_iterator_type : public std::iterator<std::input_iterator_tag,
      edge_type, ptrdiff_t, const edge_type*, const edge_type&> {
   private:
    void get_next() {
      if (m_count == m_num_edges) {
        m_current = std::make_pair(0, 0);
        assert(false);
        return;
      }

      if (m_undirected && m_make_undirected) {
        std::swap(m_current.first, m_current.second);
        m_make_undirected = false;
      } else {
        m_current = generate_edge();
        m_make_undirected = true;
        ++m_count;
      }
    };  // get_next

   public:
    input_iterator_type(uint64_t max_vertex, uint64_t num_edges,
          bool undirected)
      : m_max_vertex(max_vertex)
      , m_num_edges(num_edges)
      , m_undirected(undirected)
      , m_count(num_edges) {
      m_current = std::make_pair(0, 0);
    }

    input_iterator_type(uint64_t max_vertex, uint64_t num_edges,
          bool undirected, uint64_t i, uint64_t j)
      : m_max_vertex(max_vertex)
      , m_num_edges(num_edges)
      , m_undirected(undirected)
      , m_i(i)
      , m_j(j) {
      // Initilize the first current value
      get_next();
    }

    const edge_type& operator*() const { return m_current; }
    // const uint64_t* operator->() const { return &(operator*()); }
    input_iterator_type& operator++() {
      get_next();
      return *this;
    }

    input_iterator_type operator++(int) {
      input_iterator_type __tmp = *this;
      get_next();
      return __tmp;
    }

    edge_type *operator->() {
      return &m_current;
    }

    bool is_equal(const input_iterator_type& _x) const {
      return m_count == (_x.m_count);
    }

    // Return true if x and y are both end or not end, or x and y are the same.
    friend bool
    operator==(const input_iterator_type& x, const input_iterator_type& y)
    { return x.is_equal(y); }

    // Return false if x and y are both end or not end, or x and y are the same.
    friend bool
    operator!=(const input_iterator_type& x, const input_iterator_type& y)
    { return !x.is_equal(y); }

   private:
    input_iterator_type();

    edge_type generate_edge() {
      if (m_j < m_max_vertex) {
        m_j++;
      } else {
        m_i++;
        m_j = m_i;
      }
      assert(m_i <= m_max_vertex);
      assert(m_j <= m_max_vertex);
      return std::make_pair(m_j + 1, m_i + 1);
    }  // generate_edge

    const uint64_t m_max_vertex, m_num_edges;
    const bool m_undirected;
    bool m_make_undirected {false};
    uint64_t m_count {0}, m_i, m_j;
    edge_type m_current;
  };  // class input_iterator_type

  uint64_t find_max_vertex(uint64_t num_edges) {
    // (1/2)(x)(x+1) <= num_edges.
    uint64_t x = 0, y = 0;
    num_edges *= 2;
    while (y < num_edges) {
      x++;
      y = x * (x + 1);
    }

    uint64_t count = 0;
    for (uint64_t i = 0; i < x; i++) {
      for (uint64_t j = i; j < x; j++) {
        count++;
      }
    }
    assert(count * 2 >= num_edges);

    return x + 1; // +1 because we skip 0 vertex
  };

  upper_triangle_edge_generator(uint64_t total_edges, int rank, int size,
        bool undirected)
      : m_total_edges(total_edges)
      , m_undirected(undirected)
      , m_max_vertex(find_max_vertex(total_edges)) {
    // Determine the max_vertex id
    m_lower_edge_id = (total_edges / size) * rank;
    m_upper_edge_id = (total_edges / size) * (rank + 1);
    m_upper_edge_id = std::min(total_edges, m_upper_edge_id);

    uint64_t start_count = (total_edges / size) * rank;

    uint64_t count = 0;
    for (m_i = 0; count < start_count && m_i < m_max_vertex; m_i++) {
      for (m_j = m_i; count < start_count && m_j < m_max_vertex; m_j++) {
        count++;
      }
    }
    sanity_check();
  }

  void sanity_check() {
    auto itr1 = begin();
    auto itr2 = begin();
    auto itr_end = end();
    while (itr1 != itr_end) {
      assert(itr1 == itr2);
      assert(*itr1 == *itr2);
      itr1++;
      itr2++;
    }

  }

  /// Returns the begin of the input iterator
  input_iterator_type begin() {
    const uint64_t num_edges = m_upper_edge_id - m_lower_edge_id;
    return input_iterator_type(m_max_vertex, num_edges, m_undirected, m_i, m_j);
  }

  /// Returns the end of the input iterator
  input_iterator_type end() {
    const uint64_t num_edges = m_upper_edge_id - m_lower_edge_id;
    return input_iterator_type(m_max_vertex, num_edges, m_undirected);
  }

  uint64_t max_vertex_id() {
    return m_max_vertex;
  }

  size_t size() {
    const uint64_t num_edges = m_upper_edge_id - m_lower_edge_id;
    return num_edges;
  }

  uint64_t m_upper_edge_id, m_lower_edge_id;
  const uint64_t m_total_edges;
  const bool m_undirected;

  const uint64_t m_max_vertex;
  uint64_t m_i, m_j;

};

}  // name space havoqgt

#endif  // HAVOQGT_UPPER_TRIANGLE_EDGE_GENERATOR_INCLUDED


// MIT License
//
// Copyright (c) 2021 Oliver Schick
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#include <mpo19/utility.hpp>

#include <vector>
#include <list>
#include <map>
#include <random>
#include <functional>
#include <string>
#include <numeric>
#include <algorithm>

#define BOOST_TEST_MODULE circuits_test
#include <boost/test/included/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/data/monomorphic.hpp>

namespace bdata = boost::unit_test::data;
constexpr int m = 10000;

BOOST_DATA_TEST_CASE(utility_log_test
                     , bdata::random(1, m) ^ bdata::xrange(10000)
                     , s1, idx)
{
    (void) idx;
    static_assert(ceil_log2(0) == 0, "");
    static_assert(ceil_log2(1) == 0, "");
    static_assert(floor_log2(0) == 0, "");
    static_assert(floor_log2(1) == 0, "");
    uint64_t clog = ceil_log2(s1), flog = floor_log2(s1);
    BOOST_TEST( (1u << flog) <= uint64_t(s1) );
    BOOST_TEST( (1u << clog) >= uint64_t(s1) );
    if(clog == flog) BOOST_TEST( (1u << flog) == uint64_t(s1) );
    else BOOST_TEST( (1u << clog) > uint64_t(s1) );

}

BOOST_DATA_TEST_CASE(nothrow_swap_test
                     , bdata::random(0, m) ^ bdata::random(0, m)
                     ^ bdata::xrange(10000)
                     , s1, s2, idx)
{
    (void) idx;
    auto s1_copy = s1;
    auto s2_copy = s2;

    static_assert(noexcept(nothrow_swap), "nothrow_swap throws an exception");
    nothrow_swap(s1_copy, s2_copy);
    BOOST_TEST(s1_copy == s2);
    BOOST_TEST(s2_copy == s1);

}

BOOST_DATA_TEST_CASE(utility_words_to_hold_bitlen_test
                     , bdata::random(1, m) ^ bdata::xrange(10000)
                     , s1, idx)
{
    (void) idx;

    BOOST_TEST(bytes_to_hold_bitlen(s1) == words_to_hold_bitlen(s1, CHAR_BIT));
    BOOST_TEST(bytes_to_hold_bitlen(s1 * CHAR_BIT) == s1);
    BOOST_TEST(words_to_hold_bitlen(s1 * 16, 16) == s1);
    BOOST_TEST(words_to_hold_bitlen(s1 * 32, 16) == 2*s1);
    BOOST_TEST(words_to_hold_bitlen(s1 * 16, 32) == s1/2 + s1%2);

}

BOOST_DATA_TEST_CASE(utility_code_len_test
                     , bdata::random(1, std::numeric_limits<int>::max()) ^ bdata::xrange(10000)
                     , len, idx)
{
    (void) idx;
    char buf[encode_len_buffer_size];
    size_t num_chars = encode_len(buf, len);
    size_t dec_len = decode_len([&, i = 0u](unsigned char* c, size_t len) mutable{
        assert(i + len <= encode_len_buffer_size);
        std::copy(buf + i, buf + i + len, c);
        i += len;
    });

    BOOST_TEST(dec_len == len);
    BOOST_TEST(num_chars == words_to_hold_bitlen(ceil_log2(len), CHAR_BIT - 1));
}

BOOST_DATA_TEST_CASE(utility_test
                     , bdata::random(0, m) ^ bdata::xrange(10)
                     , s1, idx)
{
    (void) idx;

    static_assert(is_range<std::vector<int>>(), "");
    static_assert(is_range<std::list<int>>(), "");
    static_assert(is_range<std::map<int, double>>(), "");
    static_assert(!is_range<int>(), "");
    static_assert(!is_range<std::map<int, double>::iterator>(), "");

    static_assert(is_serializable<int>(), "");
    static_assert(!is_serializable<std::vector<int>>(), "");

    static_assert(is_deserializable<int>(), "");
    static_assert(!is_deserializable<std::vector<int>>(), "");

    auto f = [](int, double) {};
    auto any_arg = [](auto) {};
    auto any = [](auto...) {};

    static_assert(is_invocable<decltype(f)(int, double)>::value, "");
    static_assert(!is_invocable<decltype(f)(int, double, float)>::value, "");
    static_assert(is_invocable<decltype(any_arg)(int)>::value, "");
    static_assert(is_invocable<decltype(any_arg)(double)>::value, "");
    static_assert(!is_invocable<decltype(any_arg)()>::value, "");
    static_assert(is_invocable<decltype(any)(int, double, char, float)>::value, "");
    static_assert(is_invocable<decltype(any)(double, double, std::vector<int>)>::value, "");
    static_assert(is_invocable<decltype(any)()>::value, "");

    std::vector<int> v(100 + idx), v_copy;
    std::vector<std::vector<int>> vv(10 + idx), vv_copy;

    std::mt19937 gen(s1 + idx);
    std::uniform_int_distribution<int> d{0, m};

    std::generate(v.begin(), v.end(), [&] {return d(gen);});

    for(auto& vec : vv) {
        vec.resize(10 + idx);
        std::generate(vec.begin(), vec.end(), [&] {return d(gen);});
    }

    v_copy = v;
    vv_copy = vv;

    multi_for_each(v, [&](auto& el) {
        el += s1;
    });

    std::transform(v_copy.begin(), v_copy.end(), v_copy.begin(), [&](auto const& el) {
        return el + s1;
    });

    BOOST_TEST(std::equal(v.begin(), v.end(), v_copy.begin()));

    multi_for_each(vv, vv_copy, [](auto& a, auto& b) {
        a += 2*b;
    });

    for(size_t i = 0; i != vv.size(); ++i) {
        for(size_t j = 0; j != vv[i].size(); ++j) {
            BOOST_TEST(vv[i][j]/3 == vv_copy[i][j]);
        }
    }

    BOOST_TEST(tree_accumulate(v, std::plus<> {}) == std::accumulate(v.begin(), v.end(), 0, std::plus<> {}));
    auto res1 = tree_accumulate(v, std::plus<> {}, [](auto&& a, size_t idx) {
        return 2*a + idx;
    });

    int sum = 0;
    for(size_t i = 0; i != v.size(); ++i) {
        sum += 2*v[i] + i;
    }

    BOOST_TEST(res1 == sum);
}


/*-----------------------------------------------------------------------------+
Copyright (c) 2008-2010: Joachim Faulhaber
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/
#ifndef BOOST_ICL_INTERVAL_SET_ALGO_HPP_JOFA_081005
#define BOOST_ICL_INTERVAL_SET_ALGO_HPP_JOFA_081005

#include <boost/next_prior.hpp>
#include <boost/icl/detail/notate.hpp>
#include <boost/icl/detail/relation_state.hpp>
#include <boost/icl/type_traits/identity_element.hpp>
#include <boost/icl/type_traits/is_map.hpp>
#include <boost/icl/type_traits/is_total.hpp>
#include <boost/icl/type_traits/is_combinable.hpp>
#include <boost/icl/concept/set_value.hpp>
#include <boost/icl/concept/map_value.hpp>
#include <boost/icl/interval_combining_style.hpp>
#include <boost/icl/detail/element_comparer.hpp>
#include <boost/icl/detail/interval_subset_comparer.hpp>
#include <boost/icl/detail/associated_value.hpp>

namespace boost{namespace icl
{

namespace Interval_Set
{

//------------------------------------------------------------------------------
// Lexicographical comparison on ranges of two interval container
//------------------------------------------------------------------------------

template<class LeftT, class RightT>
bool is_element_equal(const LeftT& left, const RightT& right)
{
    return subset_compare
            (
                left, right,
                left.begin(), left.end(),
                right.begin(), right.end()
            ) == inclusion::equal;
}

template<class LeftT, class RightT>
bool is_element_less(const LeftT& left, const RightT& right)
{
    return element_compare
            (
                left, right,
                left.begin(), left.end(),
                right.begin(), right.end()
            )  == comparison::less;
}

template<class LeftT, class RightT>
bool is_element_greater(const LeftT& left, const RightT& right)
{
    return element_compare
            (
                left, right,
                left.begin(), left.end(),
                right.begin(), right.end()
            )  == comparison::greater;
}

//------------------------------------------------------------------------------
// Subset/superset compare on ranges of two interval container
//------------------------------------------------------------------------------

template<class IntervalContainerT>
bool is_joinable(const IntervalContainerT& container,
                 typename IntervalContainerT::const_iterator first,
                 typename IntervalContainerT::const_iterator past)
{
    if(first == container.end())
        return true;

    typename IntervalContainerT::const_iterator it_ = first, next_ = first;
    ++next_;

    while(next_ != container.end() && it_ != past)
        if(!icl::touches(key_value<IntervalContainerT>(it_++),
                         key_value<IntervalContainerT>(next_++)))
            return false;

    return true;
}

template<class LeftT, class RightT>
bool is_inclusion_equal(const LeftT& left, const RightT& right)
{
    return subset_compare
            (
                left, right,
                left.begin(), left.end(),
                right.begin(), right.end()
            ) == inclusion::equal;
}

template<class LeftT, class RightT>
typename enable_if<mpl::and_<is_concept_combinable<is_interval_set, is_interval_map, LeftT, RightT>,
                             is_total<RightT> >,
                   bool>::type
within(const LeftT&, const RightT&)
{
    return true;
}

template<class LeftT, class RightT>
typename enable_if<mpl::and_<is_concept_combinable<is_interval_set, is_interval_map, LeftT, RightT>,
                             mpl::not_<is_total<RightT> > >,
                   bool>::type
within(const LeftT& sub, const RightT& super)
{
    int result =
        subset_compare
        (
            sub, super,
            sub.begin(), sub.end(),
            super.begin(), super.end()
        );
    return result == inclusion::subset || result == inclusion::equal;
}

template<class LeftT, class RightT>
typename enable_if<is_concept_combinable<is_interval_map, is_interval_map, LeftT, RightT>,
                   bool>::type
within(const LeftT& sub, const RightT& super)
{
    int result =
        subset_compare
        (
            sub, super,
            sub.begin(), sub.end(),
            super.begin(), super.end()
        );
    return result == inclusion::subset || result == inclusion::equal;
}

template<class LeftT, class RightT>
typename enable_if<is_concept_combinable<is_interval_set, is_interval_set, LeftT, RightT>,
                   bool>::type
within(const LeftT& sub, const RightT& super)
{
    int result =
        subset_compare
        (
            sub, super,
            sub.begin(), sub.end(),
            super.begin(), super.end()
        );
    return result == inclusion::subset || result == inclusion::equal;
}

template<class LeftT, class RightT>
typename enable_if<mpl::and_<is_concept_combinable<is_interval_map, is_interval_set, LeftT, RightT>,
                             is_total<LeftT> >,
                   bool>::type
contains(const LeftT&, const RightT&)
{
    return true;
}

template<class LeftT, class RightT>
typename enable_if<mpl::and_<is_concept_combinable<is_interval_map, is_interval_set, LeftT, RightT>,
                             mpl::not_<is_total<LeftT> > >,
                   bool>::type
contains(const LeftT& super, const RightT& sub)
{
    int result =
        subset_compare
        (
            super, sub,
            super.begin(), super.end(),
            sub.begin(), sub.end()
        );
    return result == inclusion::superset || result == inclusion::equal;
}

template<class LeftT, class RightT>
typename enable_if<is_concept_combinable<is_interval_set, is_interval_set, LeftT, RightT>,
                   bool>::type
contains(const LeftT& super, const RightT& sub)
{
    int result =
        subset_compare
        (
            super, sub,
            super.begin(), super.end(),
            sub.begin(), sub.end()
        );
    return result == inclusion::superset || result == inclusion::equal;
}

template<class IntervalContainerT>
bool is_dense(const IntervalContainerT& container,
              typename IntervalContainerT::const_iterator first,
              typename IntervalContainerT::const_iterator past)
{
    if(first == container.end())
        return true;

    typename IntervalContainerT::const_iterator it_ = first, next_ = first;
    ++next_;

    while(next_ != container.end() && it_ != past)
        if(!icl::touches(key_value<IntervalContainerT>(it_++),
                         key_value<IntervalContainerT>(next_++)))
            return false;

    return true;
}

} // namespace Interval_Set

namespace segmental
{

template<class Type>
inline bool joinable(const Type& _Type, typename Type::iterator& some, typename Type::iterator& next)
{
    // assert: next != end && some++ == next
    return touches(key_value<Type>(some), key_value<Type>(next))
        && co_equal(some, next, &_Type, &_Type);
}

template<class Type>
inline void join_nodes(Type& object, typename Type::iterator& left_,
                                     typename Type::iterator& right_)
{
    typedef typename Type::interval_type interval_type;
    interval_type right_interval = key_value<Type>(right_);
    object.erase(right_);
    const_cast<interval_type&>(key_value<Type>(left_))
        = hull(key_value<Type>(left_), right_interval);
}

template<class Type>
inline typename Type::iterator
    join_on_left(Type& object, typename Type::iterator& left_,
                               typename Type::iterator& right_)
{
    //CL typedef typename Type::interval_type interval_type;
    // both left and right are in the set and they are neighbours
    BOOST_ASSERT(exclusive_less(key_value<Type>(left_), key_value<Type>(right_)));
    BOOST_ASSERT(joinable(object, left_, right_));

    join_nodes(object, left_, right_);
    return left_;
}

template<class Type>
inline typename Type::iterator
    join_on_right(Type& object, typename Type::iterator& left_,
                                typename Type::iterator& right_)
{
    //CL typedef typename Type::interval_type interval_type;
    // both left and right are in the map and they are neighbours
    BOOST_ASSERT(exclusive_less(key_value<Type>(left_), key_value<Type>(right_)));
    BOOST_ASSERT(joinable(object, left_, right_));

    join_nodes(object, left_, right_);
    right_ = left_;
    return right_;
}

template<class Type>
typename Type::iterator join_left(Type& object, typename Type::iterator& it_)
{
    typedef typename Type::iterator iterator;

    if(it_ == object.begin())
        return it_;

    // there is a predecessor
    iterator pred_ = it_;
    if(joinable(object, --pred_, it_))
        return join_on_right(object, pred_, it_);

    return it_;
}

template<class Type>
typename Type::iterator join_right(Type& object, typename Type::iterator& it_)
{
    typedef typename Type::iterator iterator;

    if(it_ == object.end())
        return it_;

    // there is a successor
    iterator succ_ = it_;

    if(++succ_ != object.end() && joinable(object, it_, succ_))
        return join_on_left(object, it_, succ_);

    return it_;
}

template<class Type>
typename Type::iterator join_neighbours(Type& object, typename Type::iterator& it_)
{
           join_left (object, it_);
    return join_right(object, it_);
}

template<class Type>
inline typename Type::iterator
    join_under(Type& object, const typename Type::value_type& addend)
{
    //ASSERT: There is at least one interval in object that overlaps with addend
    typedef typename Type::iterator      iterator;
    typedef typename Type::interval_type interval_type;
    typedef typename Type::value_type    value_type;

    std::pair<iterator,iterator> overlap = object.equal_range(addend);
    iterator first_ = overlap.first,
             end_   = overlap.second,
             last_  = end_; --last_;

    iterator second_= first_; ++second_;

    interval_type left_resid  = right_subtract(key_value<Type>(first_), addend);
    interval_type right_resid =  left_subtract(key_value<Type>(last_) , addend);

    object.erase(second_, end_);

    const_cast<value_type&>(key_value<Type>(first_))
        = hull(hull(left_resid, addend), right_resid);
    return first_;
}

template<class Type>
inline typename Type::iterator
    join_under(Type& object, const typename Type::value_type& addend,
                                   typename Type::iterator last_)
{
    //ASSERT: There is at least one interval in object that overlaps with addend
    typedef typename Type::iterator      iterator;
    typedef typename Type::interval_type interval_type;
    typedef typename Type::value_type    value_type;

    iterator first_ = object.lower_bound(addend);
    //BOOST_ASSERT(next(last_) == this->_set.upper_bound(inter_val));
    iterator second_= boost::next(first_), end_ = boost::next(last_);

    interval_type left_resid  = right_subtract(key_value<Type>(first_), addend);
    interval_type right_resid =  left_subtract(key_value<Type>(last_) , addend);

    object.erase(second_, end_);

    const_cast<value_type&>(key_value<Type>(first_))
        = hull(hull(left_resid, addend), right_resid);
    return first_;
}

} // namespace segmental

namespace Interval_Set
{
using namespace segmental;

template<class Type, int combining_style>
struct on_style;

template<class Type>
struct on_style<Type, interval_combine::joining>
{
    typedef          on_style            type;
    typedef typename Type::interval_type interval_type;
    typedef typename Type::iterator      iterator;

    inline static iterator handle_inserted(Type& object, iterator inserted_)
    { return join_neighbours(object, inserted_); }

    inline static iterator add_over
        (Type& object, const interval_type& addend, iterator last_)
    {
        iterator joined_ = join_under(object, addend, last_);
        return join_neighbours(object, joined_);
    }

    inline static iterator add_over
        (Type& object, const interval_type& addend)
    {
        iterator joined_ = join_under(object, addend);
        return join_neighbours(object, joined_);
    }
};

template<class Type>
struct on_style<Type, interval_combine::separating>
{
    typedef          on_style            type;
    typedef typename Type::interval_type interval_type;
    typedef typename Type::iterator      iterator;

    inline static iterator handle_inserted(Type&, iterator inserted_)
    { return inserted_; }

    inline static iterator add_over
        (Type& object, const interval_type& addend, iterator last_)
    {
        return join_under(object, addend, last_);
    }

    inline static iterator add_over
        (Type& object, const interval_type& addend)
    {
        return join_under(object, addend);
    }
};

template<class Type>
struct on_style<Type, interval_combine::splitting>
{
    typedef          on_style            type;
    typedef typename Type::interval_type interval_type;
    typedef typename Type::iterator      iterator;

    inline static iterator handle_inserted(Type&, iterator inserted_)
    { return inserted_; }

    inline static iterator add_over
        (Type& object, const interval_type& addend, iterator last_)
    {
        iterator first_ = object.lower_bound(addend);
        //BOOST_ASSERT(next(last_) == this->_set.upper_bound(inter_val));

        iterator it_ = first_;
        interval_type rest_interval = addend;

        add_front(object, rest_interval, it_);
        add_main (object, rest_interval, it_, last_);
        add_rear (object, rest_interval, it_);
        return it_;
    }

    inline static iterator add_over
        (Type& object, const interval_type& addend)
    {
        std::pair<iterator,iterator> overlap = object.equal_range(addend);
        iterator first_ = overlap.first,
                 end_   = overlap.second,
                 last_  = end_; --last_;

        iterator it_ = first_;
        interval_type rest_interval = addend;

        add_front(object, rest_interval, it_);
        add_main (object, rest_interval, it_, last_);
        add_rear (object, rest_interval, it_);

        return it_;
    }
};

template<class Type>
void add_front(Type& object, const typename Type::interval_type& inter_val,
                                   typename Type::iterator&      first_)
{
    typedef typename Type::interval_type interval_type;
    typedef typename Type::iterator      iterator;
    // If the collision sequence has a left residual 'left_resid' it will
    // be split, to provide a standardized start of algorithms:
    // The addend interval 'inter_val' covers the beginning of the collision sequence.

    // only for the first there can be a left_resid: a part of *first_ left of inter_val
    interval_type left_resid = right_subtract(key_value<Type>(first_), inter_val);

    if(!icl::is_empty(left_resid))
    {   //            [------------ . . .
        // [left_resid---first_ --- . . .
        iterator prior_ = cyclic_prior(object, first_);
        const_cast<interval_type&>(key_value<Type>(first_))
            = left_subtract(key_value<Type>(first_), left_resid);
        //NOTE: Only splitting
        object._insert(prior_, icl::make_value<Type>(left_resid, co_value<Type>(first_)));
    }

    //POST:
    // [----- inter_val ---- . . .
    // ...[-- first_ --...
}

template<class Type>
void add_segment(Type& object, const typename Type::interval_type& inter_val,
                                     typename Type::iterator&      it_      )
{
    typedef typename Type::interval_type interval_type;
    interval_type lead_gap = right_subtract(inter_val, *it_);
    if(!icl::is_empty(lead_gap))
        //           [lead_gap--- . . .
        // [prior_)           [-- it_ ...
        object._insert(prior(it_), lead_gap);

    // . . . --------- . . . addend interval
    //      [-- it_ --)      has a common part with the first overval
    ++it_;
}

template<class Type>
void add_main(Type& object, typename Type::interval_type& rest_interval,
                            typename Type::iterator&      it_,
                      const typename Type::iterator&      last_)
{
    typedef typename Type::interval_type interval_type;
    interval_type cur_interval;
    while(it_ != last_)
    {
        cur_interval = *it_ ;
        add_segment(object, rest_interval, it_);
        // shrink interval
        rest_interval = left_subtract(rest_interval, cur_interval);
    }
}

template<class Type>
void add_rear(Type& object, const typename Type::interval_type& inter_val,
                                  typename Type::iterator&      it_      )
{
    typedef typename Type::interval_type interval_type;
    typedef typename Type::iterator      iterator;

    iterator prior_ = cyclic_prior(object, it_);
    interval_type cur_itv = *it_;

    interval_type lead_gap = right_subtract(inter_val, cur_itv);
    if(!icl::is_empty(lead_gap))
        //          [lead_gap--- . . .
        // [prior_)          [-- it_ ...
        object._insert(prior_, lead_gap);

    interval_type end_gap = left_subtract(inter_val, cur_itv);
    if(!icl::is_empty(end_gap))
        // [---------------end_gap)
        //      [-- it_ --)
        it_ = object._insert(it_, end_gap);
    else
    {
        // only for the last there can be a right_resid: a part of *it_ right of addend
        interval_type right_resid = left_subtract(cur_itv, inter_val);

        if(!icl::is_empty(right_resid))
        {
            // [--------------)
            //      [-- it_ --right_resid)
            const_cast<interval_type&>(*it_) = right_subtract(*it_, right_resid);
            it_ = object._insert(it_, right_resid);
        }
    }
}

//==============================================================================
//= Addition
//==============================================================================
template<class Type>
typename Type::iterator
    add(Type& object, const typename Type::value_type& addend)
{
    //CL typedef typename Type::interval_type interval_type;
    typedef typename Type::iterator      iterator;
    typedef typename on_style<Type, Type::fineness>::type on_style_;

    if(icl::is_empty(addend))
        return object.end();

    std::pair<iterator,bool> insertion = object._insert(addend);

    if(insertion.second)
        return on_style_::handle_inserted(object, insertion.first);
    else
        return on_style_::add_over(object, addend, insertion.first);
}

template<class Type>
typename Type::iterator
    add(Type& object, typename Type::iterator    prior_,
                const typename Type::value_type& addend)
{
    //CL typedef typename Type::interval_type interval_type;
    typedef typename Type::iterator      iterator;
    typedef typename on_style<Type, Type::fineness>::type on_style_;

    if(icl::is_empty(addend))
        return prior_;

    iterator insertion = object._insert(prior_, addend);

    if(*insertion == addend)
        return on_style_::handle_inserted(object, insertion);
    else
        return on_style_::add_over(object, addend);
}

//==============================================================================
//= Subtraction
//==============================================================================
template<class Type>
void subtract(Type& object, const typename Type::value_type& minuend)
{
    typedef typename Type::iterator      iterator;
    typedef typename Type::interval_type interval_type;
    //CL typedef typename Type::value_type    value_type;

    if(icl::is_empty(minuend)) return;

    std::pair<iterator, iterator> exterior = object.equal_range(minuend);
    if(exterior.first == exterior.second) return;

    iterator first_ = exterior.first;
    iterator end_   = exterior.second;
    iterator last_  = end_; --last_;

    interval_type leftResid = right_subtract(*first_, minuend);
    interval_type rightResid;
    if(first_ != end_  )
        rightResid = left_subtract(*last_ , minuend);

    object.erase(first_, end_);

    if(!icl::is_empty(leftResid))
        object._insert(leftResid);

    if(!icl::is_empty(rightResid))
        object._insert(rightResid);
}

} // namespace Interval_Set

}} // namespace icl boost

#endif


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

#include <boost/test/unit_test.hpp>
#include "StaticEnvironment.h"
#include <vector>
#include <array>

#include <shiva\distribution.h>

BOOST_AUTO_TEST_SUITE(DistributionTest)

BOOST_AUTO_TEST_CASE(EqualDistribution)
{
	shiva::environment& env = StaticEnvironment::mpi_environment;
	shiva::communicator comm;
	const int n = 11;
	std::vector<int> data((size_t)n);

	auto dist = shiva::make_equal_distribution(comm, n);

	auto& sizes = std::get<0>(dist);
	auto& displacements = std::get<1>(dist);

	std::array<int, 3> sizes_correct = {4, 4, 3};
	std::array<int, 3> displacements_correct = {0, 4, 8};

	BOOST_CHECK_EQUAL_COLLECTIONS(std::begin(sizes), std::end(sizes), std::begin(sizes_correct), std::end(sizes_correct));
	BOOST_CHECK_EQUAL_COLLECTIONS(std::begin(displacements), std::end(displacements), std::begin(displacements_correct), std::end(displacements_correct));
}

BOOST_AUTO_TEST_SUITE_END()


#include "mantella_bits/samplesSelection.hpp"

// C++ standard library
#include <cassert>
#include <iterator>
#include <limits>
#include <stdexcept>
#include <utility>

// Armadillo
#include <armadillo>

// Mantella
#include "mantella_bits/assert.hpp"

namespace mant {
  std::unordered_map<arma::vec, double, Hash, IsEqual> elitists(
      const std::unordered_map<arma::vec, double, Hash, IsEqual>& samples,
      const arma::uword numberOfSamplesToSelect) {
    assert(samples.size() >= numberOfSamplesToSelect && "elitists: The number of given samples must be greater than or equal to the number of samples to select.");
    assert(isDimensionallyConsistent(samples) && "elitists: The samples must be dimensionally consistent.");

    if (samples.size() == 0) {
      return {};
    }

    if (samples.size() - 1 > std::numeric_limits<std::unordered_map<arma::vec, double, Hash, IsEqual>::difference_type>::max()) {
      throw std::range_error("elitists: The number of samples must be less than or equal to the largest representable iterator difference type.");
    }

    // Generates a list of all objective values
    arma::rowvec objectiveValues(samples.size());
    arma::uword n = 0;
    for (const auto& sample : samples) {
      objectiveValues(n++) = sample.second;
    }

    // Sorts the list of objective value and returns the samples with the ´numberOfSamplesToSelect` lowest objective values.
    std::unordered_map<arma::vec, double, Hash, IsEqual> selectedSamples;
    for (const auto& i : arma::uvec(arma::uvec(arma::sort_index(objectiveValues)).head(numberOfSamplesToSelect))) {
      // **Note:** C++ guarantees under C++11 23.2.5/8 and C++11 23.2.5/14 that the iteration order does not change as long as the container is not resized. This is very important, as we use the iteration order of the range-based for loop, that filled `objectiveValues`, to select the sample.
      const auto& selectedSample = std::next(std::begin(samples), static_cast<decltype(selectedSamples)::difference_type>(i));
      selectedSamples.insert({selectedSample->first, selectedSample->second});
    }

    return selectedSamples;
  }
}


#include <boost/intrusive/treap_set.hpp>


#ifndef GRAPH_TEST_HPP
#define GRAPH_TEST_HPP

// Louis Jenkins & Benjamin Valpey
#include <cstdint>
#include <random>
#include <chrono>
#include <algorithm>
#include <array>
#include "RGraph.hpp"
#include "Recoverable.hpp"
#include <omp.h>
#include <boost/histogram.hpp>
#include <boost/format.hpp>
#include <unistd.h>
#include <cmath>

static void print_stats(int numV, int numE, double averageDegree, int *vertexDegrees, int vertexDegreesLength) {
    int maxDegree = 0;
    for (auto i = 0; i < vertexDegreesLength; i++) {
        maxDegree = max(maxDegree, vertexDegrees[i]);
    }
    std::cout << "|V|=" << numV << ",|E|=" << numE << ",average degree = " << averageDegree << ",maximum degree = " << maxDegree << std::endl;
    using namespace boost::histogram;

    auto h = make_histogram(axis::regular<>(maxDegree, 0, maxDegree));
    for (auto i = 0; i < vertexDegreesLength; i++) {
        h(vertexDegrees[i]);
    }
    for (auto&& x : indexed(h)) {
        if (*x == 0) continue;
      std::cout << boost::format("bin %i: %i\n")
        % x.index() % *x;
    }
}

class GraphTest : public Test {
    public:
        RGraph *g;
        uint64_t total_ops;
        uint64_t *thd_ops;
        int max_verts;
        int addEdgeProb;
        int remEdgeProb;
        int addVerProb;
        int remVerProb;
        int desiredAvgDegree;
        int vertexLoad;
        int edge_op;
        std::atomic<int> workingThreads;
        std::atomic<int> threadsDone;
        padded<std::array<int,4>>* operations;

        GraphTest(uint64_t numOps, int max_verts, int desiredAvgDegree, int vertexLoad, int edge_op) :
            total_ops(numOps), max_verts(max_verts), desiredAvgDegree(desiredAvgDegree), vertexLoad(vertexLoad), edge_op(edge_op) {
        }

        void init(GlobalTestConfig *gtc) {
            uint64_t new_ops = total_ops / gtc->task_num;
            operations = new padded<std::array<int,4>>[gtc->task_num];
            thd_ops = new uint64_t[gtc->task_num];
            for (int i = 0; i<gtc->task_num; i++) {
                thd_ops[i] = new_ops;
            }
            if (new_ops * gtc->task_num != total_ops) {
                thd_ops[0] += (total_ops - new_ops * gtc->task_num);
            }

            Rideable* ptr = gtc->allocRideable();
            g = dynamic_cast<RGraph*>(ptr);
            if(!g){
                errexit("GraphTest must be run on RGraph type object.");
            }

            /* set interval to inf so this won't be killed by timeout */
            gtc->interval = numeric_limits<double>::max();
            auto stats = g->grab_stats();
            if(gtc->verbose) std::apply(print_stats, stats);
            addEdgeProb = std::max(1,(int)edge_op*desiredAvgDegree*100/(max_verts*vertexLoad));
            remEdgeProb = std::max(1,edge_op-addEdgeProb);
            addVerProb = (10000-edge_op)/2;
            remVerProb = 10000-edge_op-addVerProb;
            // Printing out real ratio of operations
            if(gtc->verbose) std::cout<<"AddEdge:RemoveEdge:AddVertex:RemoveVertex="<<addEdgeProb<<":"<<remEdgeProb<<":"<<addVerProb<<":"<<remVerProb<<std::endl;
            workingThreads = gtc->task_num;
            threadsDone = 0;
        }

        int execute(GlobalTestConfig *gtc, LocalTestConfig *ltc) {
            int tid = ltc->tid;
            if (tid == 0) if(gtc->verbose) std::cout << "Starting test now..." << std::endl;
            std::mt19937_64 gen_p(ltc->seed);
            std::mt19937_64 gen_v(ltc->seed + 1);
            std::uniform_int_distribution<> dist(0,9999);
            std::uniform_int_distribution<> distv(0,max_verts-1);
            for (size_t i = 0; i < thd_ops[tid]; i++) {
            	int rng = dist(gen_p);
                if (rng < addEdgeProb) {
                    // std::cout << "rng(" << rng << ") is add_edge <= " << addEdgeProb << std::endl;
                    if(g->add_edge(distv(gen_v), distv(gen_v), -1))
                        operations[tid].ui[0]++;
                } else if (rng < addEdgeProb + remEdgeProb) {
                    // std::cout << "rng(" << rng << ") is remove_any_edge <= " << addEdgeProb + remEdgeProb << std::endl;
                    if(g->remove_edge(distv(gen_v), distv(gen_v)))
                        operations[tid].ui[1]++;
                } else if (rng < addEdgeProb + remEdgeProb + addVerProb) {
                    // std::cout << "rng(" << rng << ") is has_edge <= " << addEdgeProb + remEdgeProb + addVerProb << std::endl;
                    if(g->add_vertex(distv(gen_v)))
                        operations[tid].ui[2]++;
                } else {
                    // std::cout << "rng(" << rng << ") is remove_vertex...";
                    if(g->remove_vertex(distv(gen_v)))
                        operations[tid].ui[3]++;
                }
            }
            return thd_ops[ltc->tid];
        }

        void cleanup(GlobalTestConfig *gtc) {
            auto stats = g->grab_stats();
            if(gtc->verbose) std::apply(print_stats, stats);
            size_t total=0,add_edge=0,rem_edge=0,add_ver=0,rem_ver=0;
            for(int i=0;i<gtc->task_num;i++){
                total += (operations[i].ui[0] + operations[i].ui[1] + operations[i].ui[2] + operations[i].ui[3]);
                add_edge += operations[i].ui[0];
                rem_edge += operations[i].ui[1];
                add_ver += operations[i].ui[2];
                rem_ver += operations[i].ui[3];
            }
            delete operations;
            double add_edge_prop = add_edge*100 / (double) total;
            double rem_edge_prop = rem_edge*100 / (double) total;
            double add_ver_prop = add_ver*100 / (double) total;
            double rem_ver_prop = rem_ver*100 / (double) total;
            // Printing out ratio of successful operations
            if(gtc->verbose) std::cout << "add_edge = " << add_edge << " (" << add_edge_prop << "%)" << std::endl
                << ", remove_edge = " << rem_edge << " (" << rem_edge_prop << "%)" << std::endl
                << ", add_vertex = " << add_ver << " (" << add_ver_prop << "%)" << std::endl
                << ", remove_vertex = " << rem_ver << " (" << rem_ver_prop << "%)" << std::endl;
                delete g;
        }

        void parInit(GlobalTestConfig *gtc, LocalTestConfig *ltc) {
            g->init_thread(gtc, ltc);
        }
};
#endif


//          Copyright Carl Philipp Reh 2009 - 2016.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#ifndef FCPPT_MPL_IMPLICATION_HPP_INCLUDED
#define FCPPT_MPL_IMPLICATION_HPP_INCLUDED

#include <fcppt/preprocessor/disable_gcc_warning.hpp>
#include <fcppt/preprocessor/pop_warning.hpp>
#include <fcppt/preprocessor/push_warning.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/mpl/not.hpp>
#include <boost/mpl/or.hpp>
#include <fcppt/config/external_end.hpp>

namespace fcppt
{
namespace mpl
{

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

/**
\brief The Implication function: \a Left -> \a Right

\ingroup fcpptmpl

Calculates the implication of \a Left and \a Right. Equal to (not \a Left) or
\a Right)

\snippet mpl/various.cpp mpl_implication

\tparam Left A boolean expression

\tparam Right A boolean expression
*/
template<
	typename Left,
	typename Right
>
struct implication
:
boost::mpl::or_<
	boost::mpl::not_<
		Left
	>,
	Right
>
{
};

FCPPT_PP_POP_WARNING

}
}

#endif


// Copyright (C) 2021 Christian Brommer, Control of Networked Systems, University of Klagenfurt, Austria.
//
// All rights reserved.
//
// This software is licensed under the terms of the BSD-2-Clause-License with
// no commercial use allowed, the full terms of which are made available
// in the LICENSE file. No license in patents is granted.
//
// You can contact the author at <christian.brommer@ieee.org>

#include "mars_wrapper_position.h"
#include "mars_msg_conv.h"

#include <mars/core_logic.h>
#include <mars/core_state.h>
#include <mars/sensors/imu/imu_measurement_type.h>
#include <mars/sensors/imu/imu_sensor_class.h>
#include <mars/sensors/position/position_measurement_type.h>
#include <mars/type_definitions/buffer_data_type.h>
#include <mars/type_definitions/buffer_entry_type.h>
#include <mars_msg_conv.h>
#include <nav_msgs/Odometry.h>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <Eigen/Dense>
#include <iostream>
#include <string>

using namespace mars;

MarsWrapperPosition::MarsWrapperPosition(ros::NodeHandle nh)
  : reconfigure_cb_(boost::bind(&MarsWrapperPosition::configCallback, this, _1, _2))
  , p_wi_init_(0, 0, 0)
  , q_wi_init_(Eigen::Quaterniond::Identity())
{
  reconfigure_srv_.setCallback(reconfigure_cb_);

  initialization_service_ = nh.advertiseService("init_service", &MarsWrapperPosition::initServiceCallback, this);

  // Read parameter
  publish_on_propagation_ = nh.param<bool>("pub_on_prop", publish_on_propagation_);
  use_ros_time_now_ = nh.param<bool>("use_ros_time_now", use_ros_time_now_);
  verbose_output_ = nh.param<bool>("verbose", verbose_output_);
  verbose_ooo_ = nh.param<bool>("verbose_out_of_order", verbose_ooo_);
  discard_ooo_prop_meas_ = nh.param<bool>("discard_ooo_prop_meas", discard_ooo_prop_meas_);

  pub_cb_buffer_size_ = uint32_t(nh.param<int>("pub_cb_buffer_size", int(pub_cb_buffer_size_)));
  sub_imu_cb_buffer_size_ = uint32_t(nh.param<int>("sub_imu_cb_buffer_size", int(sub_imu_cb_buffer_size_)));
  sub_sensor_cb_buffer_size_ = uint32_t(nh.param<int>("sub_sensor_cb_buffer_size", int(sub_sensor_cb_buffer_size_)));

  std::cout << "Setup framework components" << std::endl;

  // Framework components
  imu_sensor_sptr_ = std::make_shared<mars::ImuSensorClass>("IMU");
  core_states_sptr_ = std::make_shared<mars::CoreState>();
  core_states_sptr_.get()->set_propagation_sensor(imu_sensor_sptr_);
  core_logic_ = mars::CoreLogic(core_states_sptr_);
  core_logic_.buffer_.set_max_buffer_size(800);

  core_logic_.verbose_ = verbose_output_;
  core_logic_.verbose_out_of_order_ = verbose_ooo_;
  core_logic_.discard_ooo_prop_meas_ = discard_ooo_prop_meas_;

  core_states_sptr_->set_noise_std(Eigen::Vector3d(0.013, 0.013, 0.013), Eigen::Vector3d(0.0013, 0.0013, 0.0013),
                                   Eigen::Vector3d(0.083, 0.083, 0.083), Eigen::Vector3d(0.0083, 0.0083, 0.0083));

  // Sensors
  position1_sensor_sptr_ = std::make_shared<mars::PositionSensorClass>("Position1", core_states_sptr_);
  Eigen::Matrix<double, 6, 1> position_meas_std;
  position_meas_std << 0.05, 0.05, 0.05;
  position1_sensor_sptr_->R_ = position_meas_std.cwiseProduct(position_meas_std);

  PositionSensorData position_calibration;
  position_calibration.state_.p_ip_ = Eigen::Vector3d(0.0, 0.0, 0.0);
  Eigen::Matrix<double, 3, 3> position_cov;
  position_cov.setZero();
  position_cov.diagonal() << 0.0025, 0.0025, 0.0025;  // 5cm
  position_calibration.sensor_cov_ = position_cov;
  position1_sensor_sptr_->set_initial_calib(std::make_shared<PositionSensorData>(position_calibration));

  // TODO is set here for now, but will be managed by core logic in later versions
  position1_sensor_sptr_->const_ref_to_nav_ = true;

  // Subscriber
  sub_imu_measurement_ =
      nh.subscribe("imu_in", sub_imu_cb_buffer_size_, &MarsWrapperPosition::ImuMeasurementCallback, this);
  sub_point_measurement_ =
      nh.subscribe("point_in", sub_sensor_cb_buffer_size_, &MarsWrapperPosition::PointMeasurementCallback, this);
  sub_pose_measurement_ =
      nh.subscribe("pose_in", sub_sensor_cb_buffer_size_, &MarsWrapperPosition::PoseMeasurementCallback, this);
  sub_pose_with_cov_measurement_ = nh.subscribe("pose_with_cov_in", sub_sensor_cb_buffer_size_,
                                                &MarsWrapperPosition::PoseWithCovMeasurementCallback, this);
  sub_odom_measurement_ =
      nh.subscribe("odom_in", sub_sensor_cb_buffer_size_, &MarsWrapperPosition::OdomMeasurementCallback, this);
  sub_transform_measurement_ = nh.subscribe("transform_in", sub_sensor_cb_buffer_size_,
                                            &MarsWrapperPosition::TransformMeasurementCallback, this);

  // Publisher
  pub_ext_core_state_ = nh.advertise<mars_ros::ExtCoreState>("core_ext_state_out", pub_cb_buffer_size_);
  pub_ext_core_state_lite_ = nh.advertise<mars_ros::ExtCoreStateLite>("core_ext_state_lite_out", pub_cb_buffer_size_);
  pub_core_pose_state_ = nh.advertise<geometry_msgs::PoseStamped>("core_pose_state_out", pub_cb_buffer_size_);
  pub_position1_state_ = nh.advertise<geometry_msgs::PoseStamped>("position_cal_state_out", pub_cb_buffer_size_);
}

bool MarsWrapperPosition::init()
{
  core_logic_.core_is_initialized_ = false;
  core_logic_.buffer_.ResetBufferData();
  position1_sensor_sptr_->is_initialized_ = false;

  return true;
}

bool MarsWrapperPosition::initServiceCallback(std_srvs::SetBool::Request& /*request*/, std_srvs::SetBool::Response& res)
{
  init();
  res.success = true;
  ROS_INFO_STREAM("Initialized filter trough ROS Service");

  return true;
}

void MarsWrapperPosition::configCallback(mars_ros::marsConfig& config, uint32_t /*level*/)
{
  // Config parameter overwrite
  publish_on_propagation_ = config.pub_on_prop;
  core_logic_.verbose_ = config.verbose;
  verbose_output_ = config.verbose;
  use_ros_time_now_ = config.use_ros_time_now;

  if (config.initialize)
  {
    init();
    ROS_INFO_STREAM("Initialized filter trough Reconfigure GUI");
  }

  config.initialize = false;
}

void MarsWrapperPosition::ImuMeasurementCallback(const sensor_msgs::ImuConstPtr& meas)
{
  // Map the measutement to the mars type
  Time timestamp;

  if (use_ros_time_now_)
  {
    timestamp = Time(ros::Time::now().toSec());
  }
  else
  {
    timestamp = Time(meas->header.stamp.toSec());
  }

  // Generate a measurement data block
  BufferDataType data;
  data.set_sensor_data(std::make_shared<IMUMeasurementType>(MarsMsgConv::ImuMsgToImuMeas(*meas)));

  // Call process measurement
  core_logic_.ProcessMeasurement(imu_sensor_sptr_, timestamp, data);

  // Initialize the first time at which the propagation sensor occures
  if (!core_logic_.core_is_initialized_)
  {
    core_logic_.Initialize(p_wi_init_, q_wi_init_);
  }

  if (publish_on_propagation_)
  {
    this->RunCoreStatePublisher();
  }
}

void MarsWrapperPosition::PointMeasurementCallback(const geometry_msgs::PointStampedConstPtr& meas)
{
  // Map the measurement to the mars sensor type
  Time timestamp(meas->header.stamp.toSec());

  PositionMeasurementType position_meas = MarsMsgConv::PointMsgToPositionMeas(*meas);
  PositionMeasurementUpdate(position1_sensor_sptr_, position_meas, timestamp);
}

void MarsWrapperPosition::PoseMeasurementCallback(const geometry_msgs::PoseStampedConstPtr& meas)
{
  // Map the measurement to the mars sensor type
  Time timestamp(meas->header.stamp.toSec());

  PositionMeasurementType position_meas = MarsMsgConv::PoseMsgToPositionMeas(*meas);
  PositionMeasurementUpdate(position1_sensor_sptr_, position_meas, timestamp);
}

void MarsWrapperPosition::PoseWithCovMeasurementCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& meas)
{
  // Map the measurement to the mars sensor type
  Time timestamp(meas->header.stamp.toSec());

  PositionMeasurementType position_meas = MarsMsgConv::PoseWithCovMsgToPositionMeas(*meas);
  PositionMeasurementUpdate(position1_sensor_sptr_, position_meas, timestamp);
}

void MarsWrapperPosition::TransformMeasurementCallback(const geometry_msgs::TransformStampedConstPtr& meas)
{
  // Map the measurement to the mars sensor type
  Time timestamp(meas->header.stamp.toSec());

  PositionMeasurementType position_meas = MarsMsgConv::TransformMsgToPositionMeas(*meas);
  PositionMeasurementUpdate(position1_sensor_sptr_, position_meas, timestamp);
}

void MarsWrapperPosition::OdomMeasurementCallback(const nav_msgs::OdometryConstPtr& meas)
{
  // Map the measurement to the mars sensor type
  Time timestamp(meas->header.stamp.toSec());

  PositionMeasurementType position_meas = MarsMsgConv::OdomMsgToPositionMeas(*meas);
  PositionMeasurementUpdate(position1_sensor_sptr_, position_meas, timestamp);
}

void MarsWrapperPosition::RunCoreStatePublisher()
{
  mars::BufferEntryType latest_state;
  core_logic_.buffer_.get_latest_state(&latest_state);
  mars::CoreStateType latest_core_state = static_cast<mars::CoreType*>(latest_state.data_.core_.get())->state_;

  pub_ext_core_state_.publish(MarsMsgConv::ExtCoreStateToMsg(latest_state.timestamp_.get_seconds(), latest_core_state));
  pub_ext_core_state_lite_.publish(
      MarsMsgConv::ExtCoreStateLiteToMsg(latest_state.timestamp_.get_seconds(), latest_core_state));

  pub_core_pose_state_.publish(
      MarsMsgConv::ExtCoreStateToPoseMsg(latest_state.timestamp_.get_seconds(), latest_core_state));
}

void MarsWrapperPosition::PositionMeasurementUpdate(std::shared_ptr<mars::PositionSensorClass> sensor_sptr,
                                                    const mars::PositionMeasurementType& position_meas,
                                                    const mars::Time& timestamp)
{
  Time timestamp_corr;

  if (use_ros_time_now_)
  {
    timestamp_corr = Time(ros::Time::now().toSec());
  }
  else
  {
    timestamp_corr = timestamp;
  }

  // TMP feedback init position
  p_wi_init_ = position_meas.position_;

  // Generate a measurement data block
  BufferDataType data;
  data.set_sensor_data(std::make_shared<PositionMeasurementType>(position_meas));

  // Call process measurement
  if (!core_logic_.ProcessMeasurement(sensor_sptr, timestamp_corr, data))
  {
    return;
  }

  // Publish the latest core state
  this->RunCoreStatePublisher();

  // Publish the latest sensor state
  mars::BufferEntryType latest_result;
  core_logic_.buffer_.get_latest_sensor_handle_state(sensor_sptr, &latest_result);
  mars::PositionSensorStateType position_sensor_state = sensor_sptr.get()->get_state(latest_result.data_.sensor_);

  pub_position1_state_.publish(
      MarsMsgConv::PositionStateToPoseWithCovMsg(latest_result.timestamp_.get_seconds(), position_sensor_state));
}


//---------------------------------------------------------------------------//
//  MIT License
//
//  Copyright (c) 2020-2021 Mikhail Komarov <nemo@nil.foundation>
//  Copyright (c) 2020-2021 Nikita Kaskov <nemo@nil.foundation>

//
//  Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
//
//  The above copyright notice and this permission notice shall be included in all
//  copies or substantial portions of the Software.
//
//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
//  SOFTWARE.
//---------------------------------------------------------------------------//

#ifndef FILECOIN_STORAGE_PROOFS_CORE_MERKLE_PROOF_HPP
#define FILECOIN_STORAGE_PROOFS_CORE_MERKLE_PROOF_HPP

#include <algorithm>
#include <vector>

#include <boost/variant.hpp>

#include <nil/crypto3/hash/algorithm/hash.hpp>

#include <nil/filecoin/storage/proofs/core/merkle/merkle.hpp>
#include <nil/filecoin/storage/proofs/core/proof/proof.hpp>
#include <nil/filecoin/storage/proofs/core/crypto/feistel.hpp>
#include <nil/filecoin/storage/proofs/core/path_element.hpp>

namespace nil {
    namespace filecoin {
        namespace merkletree {
            template<std::size_t A, std::size_t B, std::size_t C>
            std::size_t base_path_length(std::size_t leaves) {
                std::size_t l;
                if (C > 0) {
                    l = leaves / C / B;
                } else if (B > 0) {
                    l = leaves / B;
                } else {
                    l = leaves;
                }

                return graph_height<A>(l) - 1;
            }

            template<std::size_t A, std::size_t B, std::size_t C>
            std::size_t compound_path_length(std::size_t leaves) {
                std::size_t len = base_path_length<A, B, C>(leaves);
                if (B > 0) {
                    len += 1;
                }

                if (C > 0) {
                    len += 1;
                }

                return len;
            }

            template<std::size_t A, std::size_t B, std::size_t C>
            std::size_t compound_tree_height(std::size_t leaves) {
                // base layer
                std::size_t a = graph_height<A>(leaves) - 1;

                // sub tree layer
                std::size_t b;
                if (B > 0) {
                    b = B - 1;
                } else {
                    b = 0;
                }

                // top tree layer
                std::size_t c;
                if (C > 0) {
                    c = C - 1;
                } else {
                    c = 0;
                }

                return a + b + c;
            }

            template<typename Hash>
            struct Proof_basic_policy {
                typedef std::array<uint8_t, Hash::digest_size> hash_result_type;
                constexpr static const std::size_t hash_digest_size = Hash::digest_size;
            };

            template<typename Hash, std::size_t BaseTreeArity = 2>
            struct Proof {
                typedef typename Proof_basic_policy<Hash>::hash_result_type element;
                constexpr static const std::size_t element_size = Proof_basic_policy<Hash>::hash_digest_size;

                std::vector<element> lemma; // layer
                std::vector<std::size_t> path; // branch index

                /// Creates new MT inclusion proof
                Proof(const std::vector<element> &lemma, const std::vector<std::size_t> &path) :
                    lemma(lemma), path(path) { }

                template<typename Store>
                Proof(MerkleTree<Hash, Store, BaseTreeArity> tree, size_t leaf) {
                    size_t base = 0;
                    size_t j = leaf;

                    // level 1 width
                    size_t width = tree.leafs;
                    size_t branches = BaseTreeArity;
                    BOOST_ASSERT_MSG(width == utilities::next_pow2(width), "Must be a power of 2 tree");
                    BOOST_ASSERT_MSG(branches == utilities::next_pow2(branches),"branches must be a power of 2");
                    size_t shift = utilities::log2_pow2(branches);
                    lemma.push_back(tree.read_at(j));
                    while (base + 1 < tree.len) {
                        size_t hash_index = (j / branches) * branches;
                        for (size_t k = hash_index; k < hash_index + branches; ++k) {
                            if (k != j) {
                                lemma.push_back(tree.read_at(base + k));
                            }
                        }
                        path.push_back(j % branches); // path_index
                        base += width;
                        width >>= shift; // width /= branches;
                        j >>= shift; // j /= branches;
                    }

                    // root is final
                    lemma.push_back(tree.root);

                    // Sanity check: if the `MerkleTree` lost its integrity and `data` doesn't match the
                    // expected values for `leafs` and `row_count` this can get ugly.
                    BOOST_ASSERT_MSG(
                        lemma.size() == utilities::get_merkle_proof_lemma_len(tree.row_count, branches),
                        "Invalid proof lemma length");
                    BOOST_ASSERT_MSG(
                        path.size() == tree.row_count - 1,
                        "Invalid proof path length");
                }

                /// Return proof target leaf
                element item() {
                    return *lemma.begin();
                }

                /// Return tree root
                element root() {
                    return *(lemma.end() - 1);
                }

                /// Verifies MT inclusion proof
                bool validate() {
                    std::size_t size = lemma.size();
                    if (size < 2) {
                        return false;
                    }

                    std::size_t branches = BaseTreeArity;
                    auto a = Algorithm<element>::default();
                    auto h = this->item();
                    auto path_index = 1;

                    for (size_t i = 1; i < size - 1; i += branches - 1) {
                        a.reset();
                        h = {
                            std::vector<element> nodes;
                            nodes.reserve(branches);
                            auto cur_index = 0;
                            for (j = 0; j < branches; ++j) {
                                if j == self.path[path_index - 1] {
                                    nodes.push(h.clone());
                                } else {
                                    nodes.push(self.lemma[i + cur_index].clone());
                                    cur_index += 1;
                                }
                            }

                            if cur_index != branches - 1 {
                                return false;
                            }

                            path_index += 1;
                            a.multi_node(&nodes, i - 1)
                        };
                    }

                    return h == root();
                }

                    /// Verifies MT inclusion proof and that leaf_data is the original leaf data for which proof was generated.
                template<template<typename> class Algorithm>
                bool validate_with_data(leaf_data: &dyn Hashable<A>) {
                    auto a = Algorithm<T>::default();
                    leaf_data.hash(&a);
                    const auto item = a.hash();
                    a.reset();
                    const auto leaf_hash = a.leaf(item);

                    if (leaf_hash == item()) {
                        return validate<Algorithm>();
                    } else {
                        return false;
                    }
                }
            };
        }    // namespace merkletree
    }    // namespace filecoin
}    // namespace nil

#endif


#include <mass.h>
#include <tiny.h>

#include <algorithm> // needed for std::min and std::max
#include <cmath>     // needed for std::fabs

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

typedef tiny::MathTypes<double> math_types;
typedef math_types::vector3_type    V;
typedef math_types::quaternion_type Q;

class CuboidMesh
: public mass::FaceCallbackInterface<double>
{
public:

  double m_x1[4];  double m_y1[4];  double m_z1[4];
  double m_x2[4];  double m_y2[4];  double m_z2[4];
  double m_x3[4];  double m_y3[4];  double m_z3[4];
  double m_x4[4];  double m_y4[4];  double m_z4[4];
  double m_x5[4];  double m_y5[4];  double m_z5[4];
  double m_x6[4];  double m_y6[4];  double m_z6[4];

  double * m_x[6];  double * m_y[6];  double * m_z[6];

public:

  size_t get_number_of_vertices(size_t const & ) const
  {
    return 4u;
  }

  void get_x_coords(size_t const & face_no, double * coords) const
  {
    coords[0] = m_x[face_no][0];
    coords[1] = m_x[face_no][1];
    coords[2] = m_x[face_no][2];
    coords[3] = m_x[face_no][3];
  }

  void get_y_coords(size_t const & face_no, double * coords) const
  {
    coords[0] = m_y[face_no][0];
    coords[1] = m_y[face_no][1];
    coords[2] = m_y[face_no][2];
    coords[3] = m_y[face_no][3];
  }

  void get_z_coords(size_t const & face_no, double * coords) const
  {
    coords[0] = m_z[face_no][0];
    coords[1] = m_z[face_no][1];
    coords[2] = m_z[face_no][2];
    coords[3] = m_z[face_no][3];
  }

public:

  CuboidMesh(
         V const & v1
          , V const & v2
          , V const & v3
          , V const & v4
          , V const & v5
          , V const & v6
          , V const & v7
          , V const & v8
          )
  {
    // front face
    m_x1[0] = v1(0); m_y1[0] = v1(1); m_z1[0] = v1(2);
    m_x1[1] = v2(0); m_y1[1] = v2(1); m_z1[1] = v2(2);
    m_x1[2] = v3(0); m_y1[2] = v3(1); m_z1[2] = v3(2);
    m_x1[3] = v4(0); m_y1[3] = v4(1); m_z1[3] = v4(2);

    // back face
    m_x2[0] = v8(0); m_y2[0] = v8(1); m_z2[0] = v8(2);
    m_x2[1] = v7(0); m_y2[1] = v7(1); m_z2[1] = v7(2);
    m_x2[2] = v6(0); m_y2[2] = v6(1); m_z2[2] = v6(2);
    m_x2[3] = v5(0); m_y2[3] = v5(1); m_z2[3] = v5(2);

    // top face
    m_x3[0] = v4(0); m_y3[0] = v4(1); m_z3[0] = v4(2);
    m_x3[1] = v3(0); m_y3[1] = v3(1); m_z3[1] = v3(2);
    m_x3[2] = v7(0); m_y3[2] = v7(1); m_z3[2] = v7(2);
    m_x3[3] = v8(0); m_y3[3] = v8(1); m_z3[3] = v8(2);

    // bottom face
    m_x4[0] = v2(0); m_y4[0] = v2(1); m_z4[0] = v2(2);
    m_x4[1] = v1(0); m_y4[1] = v1(1); m_z4[1] = v1(2);
    m_x4[2] = v5(0); m_y4[2] = v5(1); m_z4[2] = v5(2);
    m_x4[3] = v6(0); m_y4[3] = v6(1); m_z4[3] = v6(2);

    // left face
    m_x5[0] = v1(0); m_y5[0] = v1(1); m_z5[0] = v1(2);
    m_x5[1] = v4(0); m_y5[1] = v4(1); m_z5[1] = v4(2);
    m_x5[2] = v8(0); m_y5[2] = v8(1); m_z5[2] = v8(2);
    m_x5[3] = v5(0); m_y5[3] = v5(1); m_z5[3] = v5(2);

    // right face
    m_x6[0] = v2(0); m_y6[0] = v2(1); m_z6[0] = v2(2);
    m_x6[1] = v6(0); m_y6[1] = v6(1); m_z6[1] = v6(2);
    m_x6[2] = v7(0); m_y6[2] = v7(1); m_z6[2] = v7(2);
    m_x6[3] = v3(0); m_y6[3] = v3(1); m_z6[3] = v3(2);

    m_x[0] = &m_x1[0]; m_x[1] = &m_x2[0]; m_x[2] = &m_x3[0]; m_x[3] = &m_x4[0]; m_x[4] = &m_x5[0]; m_x[5] = &m_x6[0];
    m_y[0] = &m_y1[0]; m_y[1] = &m_y2[0]; m_y[2] = &m_y3[0]; m_y[3] = &m_y4[0]; m_y[4] = &m_y5[0]; m_y[5] = &m_y6[0];
    m_z[0] = &m_z1[0]; m_z[1] = &m_z2[0]; m_z[2] = &m_z3[0]; m_z[3] = &m_z4[0]; m_z[4] = &m_z5[0]; m_z[5] = &m_z6[0];
  }

};

BOOST_AUTO_TEST_SUITE(mass);

BOOST_AUTO_TEST_CASE(test_transform)
{
  double const rho = 1.0;

  double const W = 1.0;
  double const H = 2.0;
  double const D = 3.0;

  mass::Properties<double> Bbody = compute_box( rho, W, H, D );

  for(size_t i = 0u; i < 20u; ++i)
  {
    // Make random rotation and translation
    Q const q = tiny::unit( Q::random() );
    V const T = V::random(-10,10);

    // Apply transform to a 2x2x2 box
    V const v1 = tiny::rotate(q, V::make(-W, -H,  D) ) + T;
    V const v2 = tiny::rotate(q, V::make( W, -H,  D) ) + T;
    V const v3 = tiny::rotate(q, V::make( W,  H,  D) ) + T;
    V const v4 = tiny::rotate(q, V::make(-W,  H,  D) ) + T;

    V const v5 = tiny::rotate(q, V::make(-W, -H, -D) ) + T;
    V const v6 = tiny::rotate(q, V::make( W, -H, -D) ) + T;
    V const v7 = tiny::rotate(q, V::make( W,  H, -D) ) + T;
    V const v8 = tiny::rotate(q, V::make(-W,  H, -D) ) + T;

    // Compute mass
    CuboidMesh const callback = CuboidMesh(v1,v2,v3,v4,v5,v6,v7,v8);

    mass::Properties<double> Cmodel = compute_mesh( rho, 6, &callback);

    // Transform body frame ground truth box values into same
    // model frame as compute_mesh is using
    mass::Properties<double> Binter = mass::rotate( q.real(), q.imag()(0), q.imag()(1), q.imag()(2),  Bbody );
    mass::Properties<double> Bmodel = mass::translate_to_model_frame( T(0), T(1), T(2), Binter );

    // Make sure compute_mesh has computed the correct values!
    BOOST_CHECK_CLOSE( Bmodel.m_m,   Cmodel.m_m,   0.1 );
    BOOST_CHECK_CLOSE( Bmodel.m_Ixx, Cmodel.m_Ixx, 0.1 );
    BOOST_CHECK_CLOSE( Bmodel.m_Iyy, Cmodel.m_Iyy, 0.1 );
    BOOST_CHECK_CLOSE( Bmodel.m_Izz, Cmodel.m_Izz, 0.1 );
    BOOST_CHECK_CLOSE( Bmodel.m_Ixy, Cmodel.m_Ixy, 0.1 );
    BOOST_CHECK_CLOSE( Bmodel.m_Ixz, Cmodel.m_Ixz, 0.1 );
    BOOST_CHECK_CLOSE( Bmodel.m_Iyz, Cmodel.m_Iyz, 0.1 );
    BOOST_CHECK_CLOSE( Bmodel.m_x,   Cmodel.m_x,   0.1 );
    BOOST_CHECK_CLOSE( Bmodel.m_y,   Cmodel.m_y,   0.1 );
    BOOST_CHECK_CLOSE( Bmodel.m_z,   Cmodel.m_z,   0.1 );
    BOOST_CHECK_CLOSE( Bmodel.m_Qs,  Cmodel.m_Qs,  0.1 );
    BOOST_CHECK_CLOSE( Bmodel.m_Qx,  Cmodel.m_Qx,  0.1 );
    BOOST_CHECK_CLOSE( Bmodel.m_Qy,  Cmodel.m_Qy,  0.1 );
    BOOST_CHECK_CLOSE( Bmodel.m_Qz,  Cmodel.m_Qz,  0.1 );

    // Now we wish to test if we can transform the compute_mesh
    // values into their body frame
    mass::Properties<double> Cinter = mass::translate_to_body_frame( Cmodel );
    mass::Properties<double> Cbody  = mass::rotate_to_body_frame( Cinter );

    BOOST_CHECK_CLOSE( Bbody.m_m,   Cbody.m_m,   0.1 );
    BOOST_CHECK_CLOSE( Bbody.m_Ixx, Cbody.m_Ixx, 0.1 );
    BOOST_CHECK_CLOSE( Bbody.m_Iyy, Cbody.m_Iyy, 0.1 );
    BOOST_CHECK_CLOSE( Bbody.m_Izz, Cbody.m_Izz, 0.1 );
    BOOST_CHECK_CLOSE( Bbody.m_Ixy, Cbody.m_Ixy, 0.1 );
    BOOST_CHECK_CLOSE( Bbody.m_Ixz, Cbody.m_Ixz, 0.1 );
    BOOST_CHECK_CLOSE( Bbody.m_Iyz, Cbody.m_Iyz, 0.1 );
    BOOST_CHECK_CLOSE( Bbody.m_x,   Cbody.m_x,   0.1 );
    BOOST_CHECK_CLOSE( Bbody.m_y,   Cbody.m_y,   0.1 );
    BOOST_CHECK_CLOSE( Bbody.m_z,   Cbody.m_z,   0.1 );

    // Finally we wish to extract the body to model frame
    // transformation and test it against the known transformation.

    // Test if mesh has computed the correct center of mass value
    BOOST_CHECK_CLOSE( Cmodel.m_x, T(0), 0.1 );
    BOOST_CHECK_CLOSE( Cmodel.m_y, T(1), 0.1 );
    BOOST_CHECK_CLOSE( Cmodel.m_z, T(2), 0.1 );

    // 2011-01-30 Kenny:  We can not just compare orientations... they
    // are not really unique. There could be upto three pi rotations (one
    // pi around x, one around y and one around z) permuations.
//    BOOST_CHECK_CLOSE( Cbody.m_Qs,  q.real(),     0.1 );
//    BOOST_CHECK_CLOSE( Cbody.m_Qx,  q.imag()[0],  0.1 );
//    BOOST_CHECK_CLOSE( Cbody.m_Qy,  q.imag()[1],  0.1 );
//    BOOST_CHECK_CLOSE( Cbody.m_Qz,  q.imag()[2],  0.1 );
  }
}

BOOST_AUTO_TEST_SUITE_END();


#pragma once
//===----------------------------------------------------------------------===//
#include "part.hpp"

#include <dtl/bitmap/iterator.hpp>
#include <dtl/dtl.hpp>
#include <dtl/math.hpp>

#include <boost/dynamic_bitset.hpp>
#include <memory>
//===----------------------------------------------------------------------===//
namespace dtl {
//===----------------------------------------------------------------------===//
/// Applies a fixed size partitioning to the given bitmap.
/// Updates are handled by decompressing/re-compressing the target partition.
template<
    /// The (compressed) bitmap type.
    typename B,
    /// The partition size in bits.
    std::size_t P>
class part_updirect
    : public part<B,P> {
public:
  /// C'tor (similar to all other implementations)
  explicit part_updirect(const boost::dynamic_bitset<$u32>& bitmap)
      : part<B,P>(bitmap) {}

  part_updirect(const part_updirect& other) = delete;
  part_updirect(part_updirect&& other) noexcept = default;
  part_updirect& operator=(const part_updirect& other) = delete;
  part_updirect& operator=(part_updirect&& other) noexcept = default;

  /// Return the name of the implementation.
  static std::string
  name() noexcept {
    return std::string("part_updirect<")
        + B::name() + std::string(",")
        + std::to_string(dtl::log_2(P))
        + std::string(">");
  }

  /// Set the i-th bit to the given value.
  void __forceinline__
  set(std::size_t i, u1 val) noexcept {
    const auto part_idx = i / P;
    // Decompress the partition.
    auto dec = dtl::to_bitmap_using_iterator(*this->parts_[part_idx]);
    // Apply the update.
    dec[i % P] = val;
    // Re-compress and install the partition.
    auto compressed_part_ptr = std::make_unique<B>(dec);
    std::swap(compressed_part_ptr, this->parts_[part_idx]);
  }

  /// Does nothing. Just for compatibility reasons.
  template<typename M>
  void __forceinline__
  merge() {}
};
//===----------------------------------------------------------------------===//
} // namespace dtl