#include "eigen_numpy.h"

#include <Eigen/Eigen>
#if EIGEN_VERSION_AT_LEAST(3, 3, 0)
#include <unsupported/Eigen/CXX11/Tensor>
#endif // EIGEN_VERSION_AT_LEAST(3, 3, 0)
#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION
#include <numpy/arrayobject.h>

// These macros were renamed in NumPy 1.7.1.
#if !defined(NPY_ARRAY_C_CONTIGUOUS) && defined(NPY_C_CONTIGUOUS)
#define NPY_ARRAY_C_CONTIGUOUS NPY_C_CONTIGUOUS
#endif

#if !defined(NPY_ARRAY_ALIGNED) && defined(NPY_ALIGNED)
#define NPY_ARRAY_ALIGNED NPY_ALIGNED
#endif

namespace bp = boost::python;

using namespace Eigen;

template <typename SCALAR>
struct NumpyEquivalentType {};

template <> struct NumpyEquivalentType<double> {enum {type_code = NPY_DOUBLE};};
template <> struct NumpyEquivalentType<float> {enum {type_code = NPY_FLOAT};};
template <> struct NumpyEquivalentType<int> {enum {type_code = NPY_INT};};
template <> struct NumpyEquivalentType<unsigned short>{enum {type_code = NPY_USHORT};};
template <> struct NumpyEquivalentType<unsigned char>{enum {type_code = NPY_UBYTE};};
template <> struct NumpyEquivalentType<std::complex<double> > {enum {type_code = NPY_CDOUBLE};};

template <typename SourceType, typename DestType >
static void copy_array(const SourceType* source, DestType* dest,
                       const npy_int &nb_rows, const npy_int &nb_cols,
    const bool &isSourceTypeNumpy = false, const bool &isDestRowMajor = true,
    const bool& isSourceRowMajor = true,
    const npy_int &numpy_row_stride = 1, const npy_int &numpy_col_stride = 1)
{
  // determine source strides
  int row_stride = 1, col_stride = 1;
  if (isSourceTypeNumpy) {
    row_stride = numpy_row_stride;
    col_stride = numpy_col_stride;
  } else {
    if (isSourceRowMajor) {
      row_stride = nb_cols;
    } else {
      col_stride = nb_rows;
    }
  }

  if (isDestRowMajor) {
    for (int r=0; r<nb_rows; r++) {
      for (int c=0; c<nb_cols; c++) {
        *dest = source[r*row_stride + c*col_stride];
        dest++;
      }
    }
  } else {
    for (int c=0; c<nb_cols; c++) {
      for (int r=0; r<nb_rows; r++) {
        *dest = source[r*row_stride + c*col_stride];
        dest++;
      }
    }
  }
}

template<class MatType> // MatrixXf or MatrixXd
struct EigenMatrixToPython {
  static PyObject* convert(const MatType& mat) {
    npy_intp shape[2] = { mat.rows(), mat.cols() };
    PyArrayObject* python_array = (PyArrayObject*)PyArray_SimpleNew(
        2, shape, NumpyEquivalentType<typename MatType::Scalar>::type_code);

    copy_array(mat.data(),
               (typename MatType::Scalar*)PyArray_DATA(python_array),
               mat.rows(),
               mat.cols(),
               false,
               true,
               MatType::Flags & Eigen::RowMajorBit);
    return (PyObject*)python_array;
  }
};

template<typename MatType>
struct EigenMatrixFromPython {
  typedef typename MatType::Scalar T;

  EigenMatrixFromPython() {
    bp::converter::registry::push_back(&convertible,
                                       &construct,
                                       bp::type_id<MatType>());
  }

  static void* convertible(PyObject* obj_ptr) {
    PyArrayObject *array = reinterpret_cast<PyArrayObject*>(obj_ptr);
    if (!PyArray_Check(array)) {
      //LOG(ERROR) << "PyArray_Check failed";
      return 0;
    }
    if (PyArray_NDIM(array) > 2) {
      //LOG(ERROR) << "dim > 2";
      return 0;
    }
    if (PyArray_ObjectType(obj_ptr, 0) != NumpyEquivalentType<typename MatType::Scalar>::type_code) {
      //LOG(ERROR) << "types not compatible";
      return 0;
    }
    int flags = PyArray_FLAGS(array);
    if (!(flags & NPY_ARRAY_C_CONTIGUOUS)) {
      //LOG(ERROR) << "Contiguous C array required";
      return 0;
    }
    if (!(flags & NPY_ARRAY_ALIGNED)) {
      //LOG(ERROR) << "Aligned array required";
      return 0;
    }
    return obj_ptr;
  }

  static void construct(PyObject* obj_ptr,
                        bp::converter::rvalue_from_python_stage1_data* data) {
    const int R = MatType::RowsAtCompileTime;
    const int C = MatType::ColsAtCompileTime;

    using bp::extract;

    PyArrayObject *array = reinterpret_cast<PyArrayObject*>(obj_ptr);
    int ndims = PyArray_NDIM(array);
    npy_intp* dimensions = PyArray_DIMS(array);

    int dtype_size = (PyArray_DESCR(array))->elsize;
    int s1 = PyArray_STRIDE(array, 0);
    //CHECK_EQ(0, s1 % dtype_size);
    int s2 = 0;
    if (ndims > 1) {
      s2 = PyArray_STRIDE(array, 1);
      //CHECK_EQ(0, s2 % dtype_size);
    }

    int nrows = R;
    int ncols = C;
    if (ndims == 2) {
      if (R != Eigen::Dynamic) {
        //CHECK_EQ(R, array->dimensions[0]);
      } else {
        nrows = dimensions[0];
      }

      if (C != Eigen::Dynamic) {
        //CHECK_EQ(C, array->dimensions[1]);
      } else {
        ncols = dimensions[1];
      }
    } else {
      //CHECK_EQ(1, ndims);
      // Vector are a somehow special case because for Eigen, everything is
      // a 2D array with a dimension set to 1, but to numpy, vectors are 1D
      // arrays
      // So we could get a 1x4 array for a Vector4

      // For a vector, at least one of R, C must be 1
      //CHECK(R == 1 || C == 1);

      if (R == 1) {
        if (C != Eigen::Dynamic) {
          //CHECK_EQ(C, array->dimensions[0]);
        } else {
          ncols = dimensions[0];
        }
        // We have received a 1xC array and want to transform to VectorCd,
        // so we need to transpose
        // TODO: An alternative is to add wrappers for RowVector, but maybe
        // implicit transposition is more natural
        std::swap(s1, s2);
      } else {
        if (R != Eigen::Dynamic) {
          //CHECK_EQ(R, array->dimensions[0]);
        } else {
          nrows = dimensions[0];
        }
      }
    }

    T* raw_data = reinterpret_cast<T*>(PyArray_DATA(array));

    typedef Map<Matrix<T, Dynamic, Dynamic, RowMajor>, Aligned, Stride<Dynamic, Dynamic> > MapType;

    void* storage=((bp::converter::rvalue_from_python_storage<MatType>*)
                   (data))->storage.bytes;

    new (storage) MatType;
    MatType* emat = (MatType*)storage;
    // TODO: This is a (potentially) expensive copy operation. There should
    // be a better way
    *emat = MapType(raw_data, nrows, ncols,
                Stride<Dynamic, Dynamic>(s1/dtype_size, s2/dtype_size));
    data->convertible = storage;
  }
};

template<class TransformType> // MatrixXf or MatrixXd
struct EigenTransformToPython {
  static PyObject* convert(const TransformType& transform) {
      return EigenMatrixToPython<typename TransformType::MatrixType>::convert(transform.matrix());
  }
};

template<typename TransformType>
struct EigenTransformFromPython {
  EigenTransformFromPython() {
    bp::converter::registry::push_back(&convertible,
                                       &construct,
                                       bp::type_id<TransformType>());
  }

  static void* convertible(PyObject* obj_ptr) {
    return EigenMatrixFromPython<typename TransformType::MatrixType>::convertible(obj_ptr);
  }

  static void construct(PyObject* obj_ptr,
                        bp::converter::rvalue_from_python_stage1_data* data) {
    return EigenMatrixFromPython<typename TransformType::MatrixType>::construct(obj_ptr, data);
  }
};

#if EIGEN_VERSION_AT_LEAST(3, 3, 0)

//Assumes row-major destination
template<typename SourceType, typename DestType>
static void copy_tensor(
    const SourceType* source, DestType* dest,
    const int& num_dimensions,
    const npy_intp* shape,
    const int& size,
    const bool& is_source_row_major = false) {
  if (is_source_row_major) {
    for(int i_element = 0; i_element < size; i_element++,dest++){
      *dest = source[i_element];
    }
    return;
  }

  int col_stride = shape[0];
  std::vector<int> strides;
  std::vector<int> remaining_dims;
  int cumulative_stride = shape[0] * shape[1];
  int chunk_size = 1;

  for (int ix_dimension = 2; ix_dimension < num_dimensions; ix_dimension++) {
    int dim = shape[ix_dimension];
    strides.push_back(cumulative_stride);
    remaining_dims.push_back(dim);
    cumulative_stride *= dim;
    chunk_size *= dim;
  }

  std::reverse(remaining_dims.begin(), remaining_dims.end());
  std::reverse(strides.begin(), strides.end());

  for (int r = 0; r < shape[0]; r++) {
    for (int c = 0; c < shape[1]; c++) {
      int row_and_col_offset = r + c * col_stride;
      for (int ix_element = 0; ix_element < chunk_size; ix_element++) {
        int ix_subelement = ix_element;
        int remaining_offset = 0;
        for (size_t ix_dim = 0; ix_dim < remaining_dims.size(); ix_dim++) {
          div_t division_result = div(ix_subelement, remaining_dims[ix_dim]);
          ix_subelement = division_result.quot;
          int coord = division_result.rem;
          remaining_offset += coord * strides[ix_dim];
        }
        *dest = source[row_and_col_offset + remaining_offset];
        dest++;
      }
    }
  }
}

template<class TensorType>
struct EigenTensorToPython {
  static PyObject* convert(const TensorType& tensor) {

    const int num_dimensions = TensorType::NumDimensions;
    npy_intp* shape = static_cast<npy_intp*>(malloc(sizeof(npy_intp) * num_dimensions));
    //npy_int shape2[num_dimensions]; //will error, check
    for (int i_dimension = 0; i_dimension < num_dimensions; i_dimension++) {
      shape[i_dimension] = static_cast<npy_intp>(tensor.dimension(i_dimension));
    }

    PyArrayObject* python_array = (PyArrayObject*) PyArray_SimpleNew(
        num_dimensions, shape, NumpyEquivalentType<typename TensorType::Scalar>::type_code);

    copy_tensor(tensor.data(),
        (typename TensorType::Scalar*) PyArray_DATA(python_array),
        num_dimensions,
        shape,
        tensor.size(),
        static_cast<Eigen::StorageOptions>(TensorType::Layout) == Eigen::RowMajor);
    free(shape);
    return (PyObject*) python_array;

  }
};

template<typename TensorType>
struct EigenTensorFromPython {
  typedef typename TensorType::Scalar T;
  EigenTensorFromPython() {
    bp::converter::registry::push_back(&convertible,
        &construct,
        bp::type_id<TensorType>());
  }

  static void* convertible(PyObject* obj_ptr) {
    PyArrayObject* array = reinterpret_cast<PyArrayObject*>(obj_ptr);
    if (!PyArray_Check(array)) {
      //LOG(ERROR) << "PyArray_Check failed";
      return 0;
    }

    int dimension_count = PyArray_NDIM(array);
    if (dimension_count <= 2) {
      //This should be a Matrix or Vector, not Eigen::Tensor (default behavior)
      //LOG(ERROR) << "PyArray_Check failed";
      return 0;
    } else if (dimension_count != TensorType::NumDimensions) {
      //LOG(ERROR) << "PyArray_Check failed";
      return 0;
    }
    if (PyArray_ObjectType(obj_ptr, 0) != NumpyEquivalentType<T>::type_code) {
      //LOG(ERROR) << "types not compatible";
      return 0;
    }
    int flags = PyArray_FLAGS(array);
    if (!(flags & NPY_ARRAY_C_CONTIGUOUS)) {
      //LOG(ERROR) << "Contiguous C array required";
      return 0;
    }
    if (!(flags & NPY_ARRAY_ALIGNED)) {
      //LOG(ERROR) << "Aligned array required";
      return 0;
    }
    return obj_ptr;
  }

  static void construct(PyObject* obj_ptr,
      bp::converter::rvalue_from_python_stage1_data* data) {

    using bp::extract;

    PyArrayObject* array = reinterpret_cast<PyArrayObject*>(obj_ptr);
    npy_intp* numpy_array_dimensions = PyArray_DIMS(array);

    T* raw_data = reinterpret_cast<T*>(PyArray_DATA(array));

    typedef TensorMap<Tensor<T, TensorType::NumDimensions, RowMajor>, Aligned> TensorMapType;
    typedef TensorLayoutSwapOp<Tensor<T, TensorType::NumDimensions, RowMajor>> TensorSwapLayoutType;

    void* storage = ((bp::converter::rvalue_from_python_storage<TensorType>*)
        (data))->storage.bytes;

    std::array<Index, TensorType::NumDimensions> tensor_dimensions;
    std::array<int, TensorType::NumDimensions> inverse_dimensions;
    for (size_t i_dim = 0, inv_dim = TensorType::NumDimensions - 1; i_dim < tensor_dimensions.size();
        i_dim++, inv_dim--) {
      tensor_dimensions[i_dim] = static_cast<Index>(numpy_array_dimensions[i_dim]);
      inverse_dimensions[i_dim] = inv_dim;
    }

    new (storage) TensorType;
    TensorType* etensor = (TensorType*) storage;

    // TODO: This is a (potentially) expensive copy operation. There should be a better way
    auto mapped_t = TensorMapType(raw_data, tensor_dimensions);
    *etensor = TensorSwapLayoutType(mapped_t).shuffle(inverse_dimensions);
    data->convertible = storage;
  }
};

#endif // EIGEN_VERSION_AT_LEAST(3, 3, 0)

#define EIGEN_MATRIX_CONVERTER(Type) \
  EigenMatrixFromPython<Type>();  \
  bp::to_python_converter<Type, EigenMatrixToPython<Type> >();

#define EIGEN_TRANSFORM_CONVERTER(Type) \
  EigenTransformFromPython<Type>();  \
  bp::to_python_converter<Type, EigenTransformToPython<Type> >();

#define MAT_CONV(R, C, T) \
  typedef Matrix<T, R, C> Matrix ## R ## C ## T; \
  EIGEN_MATRIX_CONVERTER(Matrix ## R ## C ## T);

// This require a MAT_CONV for that Matrix type to be registered first
#define MAP_CONV(R, C, T) \
  typedef Map<Matrix ## R ## C ## T> Map ## R ## C ## T; \
  EIGEN_MATRIX_CONVERTER(Map ## R ## C ## T);

#define T_CONV(R, C, T) \
  typedef Transpose<Matrix ## R ## C ## T> Transpose ## R ## C ## T; \
  EIGEN_MATRIX_CONVERTER(Transpose ## R ## C ## T);

#define BLOCK_CONV(R, C, BR, BC, T) \
  typedef Block<Matrix ## R ## C ## T, BR, BC> Block ## R ## C ## BR ## BC ## T; \
  EIGEN_MATRIX_CONVERTER(Block ## R ## C ## BR ## BC ## T);

#if EIGEN_VERSION_AT_LEAST(3, 3, 0)

//For two-way Eigen <--> numpy Tensor converters
#define EIGEN_TENSOR_CONVERTER(Type) \
  EigenTensorFromPython<Type>(); \
  bp::to_python_converter<Type, EigenTensorToPython<Type> >();

//For one-way Eigen --> numpy converters (for row-major Eigen stuff)
#define TENSOR_ROW_MAJOR_CONV(T, D)\
  typedef Tensor<T, D, RowMajor> TensorRm ## T ## D; \
  bp::to_python_converter<TensorRm ## T ## D, EigenTensorToPython<TensorRm ## T ## D> >();

#define TENSOR_CONV(T, D) \
  typedef Tensor<T, D> Tensor ## T ## D; \
  EIGEN_TENSOR_CONVERTER(Tensor ## T ## D);

#endif // EIGEN_VERSION_AT_LEAST(3, 3, 0)

static const int X = Eigen::Dynamic;

#if PY_VERSION_HEX >= 0x03000000
void*
#else
void
#endif
SetupEigenConverters() {
  static bool is_setup = false;
  if (is_setup) return NUMPY_IMPORT_ARRAY_RETVAL;
  is_setup = true;

  import_array();

  EIGEN_MATRIX_CONVERTER(Matrix2f);
  EIGEN_MATRIX_CONVERTER(Matrix2d);
  EIGEN_MATRIX_CONVERTER(Matrix3f);
  EIGEN_MATRIX_CONVERTER(Matrix3d);
  EIGEN_MATRIX_CONVERTER(Matrix4f);
  EIGEN_MATRIX_CONVERTER(Matrix4d);

  EIGEN_MATRIX_CONVERTER(Vector2f);
  EIGEN_MATRIX_CONVERTER(Vector3f);
  EIGEN_MATRIX_CONVERTER(Vector4f);
  EIGEN_MATRIX_CONVERTER(Vector2d);
  EIGEN_MATRIX_CONVERTER(Vector3d);
  EIGEN_MATRIX_CONVERTER(Vector4d);

  EIGEN_TRANSFORM_CONVERTER(Affine2f);
  EIGEN_TRANSFORM_CONVERTER(Affine3f);
  EIGEN_TRANSFORM_CONVERTER(Affine2d);
  EIGEN_TRANSFORM_CONVERTER(Affine3d);

  EIGEN_TRANSFORM_CONVERTER(Isometry2f);
  EIGEN_TRANSFORM_CONVERTER(Isometry3f);
  EIGEN_TRANSFORM_CONVERTER(Isometry2d);
  EIGEN_TRANSFORM_CONVERTER(Isometry3d);

  EIGEN_TRANSFORM_CONVERTER(Projective2f);
  EIGEN_TRANSFORM_CONVERTER(Projective3f);
  EIGEN_TRANSFORM_CONVERTER(Projective2d);
  EIGEN_TRANSFORM_CONVERTER(Projective3d);

  MAT_CONV(2, 3, double);
  MAT_CONV(X, 3, double);
  MAT_CONV(X, X, double);
  MAT_CONV(X, 1, double);
  MAT_CONV(1, 4, double);
  MAT_CONV(1, X, double);
  MAT_CONV(3, 4, double);
  MAT_CONV(2, X, double);

#if EIGEN_VERSION_AT_LEAST(3, 3, 0)

  TENSOR_ROW_MAJOR_CONV(float, 3);
  TENSOR_ROW_MAJOR_CONV(float, 4);
  TENSOR_ROW_MAJOR_CONV(double, 3);
  TENSOR_ROW_MAJOR_CONV(double, 4);

  TENSOR_CONV(int, 3);
  TENSOR_CONV(int, 4);
  TENSOR_CONV(float, 3);
  TENSOR_CONV(float, 4);
  TENSOR_CONV(double, 3);
  TENSOR_CONV(double, 4);

#endif // EIGEN_VERSION_AT_LEAST(3, 3, 0)

#if PY_VERSION_HEX >= 0x03000000
  return 0;
#endif
}


//==============================================================================
//         Copyright 2003 - 2013   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <boost/simd/ieee/include/functions/bitfloating.hpp>

#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>
#include <boost/dispatch/meta/as_floating.hpp>
#include <boost/simd/include/constants/mone.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/constants/bitincrement.hpp>

NT2_TEST_CASE_TPL ( bitfloating_int_convert,  BOOST_SIMD_INT_CONVERT_TYPES)
{
  using boost::simd::bitfloating;
  using boost::simd::tag::bitfloating_;
  typedef typename boost::dispatch::meta::call<bitfloating_(T)>::type r_t;
  typedef typename boost::dispatch::meta::as_floating<T>::type  wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
#if !defined(BOOST_SIMD_NO_DENORMALS)
  NT2_TEST_EQUAL(bitfloating(boost::simd::One<T>()), boost::simd::Bitincrement<r_t>());
  NT2_TEST_EQUAL(bitfloating(boost::simd::Mone<T>()), -boost::simd::Bitincrement<r_t>());
#endif
  NT2_TEST_EQUAL(bitfloating(boost::simd::Zero<T>()), boost::simd::Zero<r_t>());
}

NT2_TEST_CASE_TPL ( bitfloating_uint_convert,  BOOST_SIMD_UINT_CONVERT_TYPES)
{
  using boost::simd::bitfloating;
  using boost::simd::tag::bitfloating_;
  typedef typename boost::dispatch::meta::call<bitfloating_(T)>::type r_t;
  typedef typename boost::dispatch::meta::as_floating<T>::type  wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
  NT2_TEST_EQUAL(bitfloating(boost::simd::One<T>()), boost::simd::Bitincrement<r_t>());
  NT2_TEST_EQUAL(bitfloating(boost::simd::Zero<T>()), boost::simd::Zero<r_t>());
}


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

#include <Eigen/Core>
#include <cmath>
#include <fstream>
#include <iostream>
#include <vector>

#include "g2o/core/factory.h"
#include "g2o/stuff/command_args.h"
#include "g2o/stuff/sampler.h"
#include "g2o/types/slam3d/edge_se3.h"
#include "g2o/types/slam3d/vertex_se3.h"

using namespace std;
using namespace g2o;

int main(int argc, char** argv) {
  // command line parsing
  int nodesPerLevel;
  int numLaps;
  bool randomSeed;
  double radius;
  std::vector<double> noiseTranslation;
  std::vector<double> noiseRotation;
  string outFilename;
  CommandArgs arg;
  arg.param("o", outFilename, "-", "output filename");
  arg.param("nodesPerLevel", nodesPerLevel, 50,
            "how many nodes per lap on the sphere");
  arg.param("laps", numLaps, 50,
            "how many times the robot travels around the sphere");
  arg.param("radius", radius, 100., "radius of the sphere");
  arg.param("noiseTranslation", noiseTranslation, std::vector<double>(),
            "set the noise level for the translation, separated by semicolons "
            "without spaces e.g: \"0.1;0.1;0.1\"");
  arg.param("noiseRotation", noiseRotation, std::vector<double>(),
            "set the noise level for the rotation, separated by semicolons "
            "without spaces e.g: \"0.001;0.001;0.001\"");
  arg.param("randomSeed", randomSeed, false,
            "use a randomized seed for generating the sphere");
  arg.parseArgs(argc, argv);

  if (noiseTranslation.size() == 0) {
    cerr << "using default noise for the translation" << endl;
    noiseTranslation.push_back(0.01);
    noiseTranslation.push_back(0.01);
    noiseTranslation.push_back(0.01);
  }
  cerr << "Noise for the translation:";
  for (size_t i = 0; i < noiseTranslation.size(); ++i)
    cerr << " " << noiseTranslation[i];
  cerr << endl;
  if (noiseRotation.size() == 0) {
    cerr << "using default noise for the rotation" << endl;
    noiseRotation.push_back(0.005);
    noiseRotation.push_back(0.005);
    noiseRotation.push_back(0.005);
  }
  cerr << "Noise for the rotation:";
  for (size_t i = 0; i < noiseRotation.size(); ++i)
    cerr << " " << noiseRotation[i];
  cerr << endl;

  Eigen::Matrix3d transNoise = Eigen::Matrix3d::Zero();
  for (int i = 0; i < 3; ++i)
    transNoise(i, i) = std::pow(noiseTranslation[i], 2);

  Eigen::Matrix3d rotNoise = Eigen::Matrix3d::Zero();
  for (int i = 0; i < 3; ++i) rotNoise(i, i) = std::pow(noiseRotation[i], 2);

  Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Zero();
  information.block<3, 3>(0, 0) = transNoise.inverse();
  information.block<3, 3>(3, 3) = rotNoise.inverse();

  vector<VertexSE3*> vertices;
  vector<EdgeSE3*> odometryEdges;
  vector<EdgeSE3*> edges;
  int id = 0;
  for (int f = 0; f < numLaps; ++f) {
    for (int n = 0; n < nodesPerLevel; ++n) {
      VertexSE3* v = new VertexSE3;
      v->setId(id++);

      Eigen::AngleAxisd rotz(-M_PI + 2 * n * M_PI / nodesPerLevel,
                             Eigen::Vector3d::UnitZ());
      Eigen::AngleAxisd roty(
          -0.5 * M_PI + id * M_PI / (numLaps * nodesPerLevel),
          Eigen::Vector3d::UnitY());
      Eigen::Matrix3d rot = (rotz * roty).toRotationMatrix();

      Eigen::Isometry3d t;
      t = rot;
      t.translation() = t.linear() * Eigen::Vector3d(radius, 0, 0);
      v->setEstimate(t);
      vertices.push_back(v);
    }
  }

  // generate odometry edges
  for (size_t i = 1; i < vertices.size(); ++i) {
    VertexSE3* prev = vertices[i - 1];
    VertexSE3* cur = vertices[i];
    Eigen::Isometry3d t = prev->estimate().inverse() * cur->estimate();
    EdgeSE3* e = new EdgeSE3;
    e->setVertex(0, prev);
    e->setVertex(1, cur);
    e->setMeasurement(t);
    e->setInformation(information);
    odometryEdges.push_back(e);
    edges.push_back(e);
  }

  // generate loop closure edges
  for (int f = 1; f < numLaps; ++f) {
    for (int nn = 0; nn < nodesPerLevel; ++nn) {
      VertexSE3* from = vertices[(f - 1) * nodesPerLevel + nn];
      for (int n = -1; n <= 1; ++n) {
        if (f == numLaps - 1 && n == 1) continue;
        VertexSE3* to = vertices[f * nodesPerLevel + nn + n];
        Eigen::Isometry3d t = from->estimate().inverse() * to->estimate();
        EdgeSE3* e = new EdgeSE3;
        e->setVertex(0, from);
        e->setVertex(1, to);
        e->setMeasurement(t);
        e->setInformation(information);
        edges.push_back(e);
      }
    }
  }

  GaussianSampler<Eigen::Vector3d, Eigen::Matrix3d> transSampler;
  transSampler.setDistribution(transNoise);
  GaussianSampler<Eigen::Vector3d, Eigen::Matrix3d> rotSampler;
  rotSampler.setDistribution(rotNoise);

  if (randomSeed) {
    std::random_device r;
    std::seed_seq seedSeq{r(), r(), r(), r(), r()};
    vector<int> seeds(2);
    seedSeq.generate(seeds.begin(), seeds.end());
    cerr << "using seeds:";
    for (size_t i = 0; i < seeds.size(); ++i) cerr << " " << seeds[i];
    cerr << endl;
    transSampler.seed(seeds[0]);
    rotSampler.seed(seeds[1]);
  }

  // noise for all the edges
  for (size_t i = 0; i < edges.size(); ++i) {
    EdgeSE3* e = edges[i];
    Eigen::Quaterniond gtQuat = (Eigen::Quaterniond)e->measurement().linear();
    Eigen::Vector3d gtTrans = e->measurement().translation();

    Eigen::Vector3d quatXYZ = rotSampler.generateSample();
    double qw = 1.0 - quatXYZ.norm();
    if (qw < 0) {
      qw = 0.;
      cerr << "x";
    }
    Eigen::Quaterniond rot(qw, quatXYZ.x(), quatXYZ.y(), quatXYZ.z());
    rot.normalize();
    Eigen::Vector3d trans = transSampler.generateSample();
    rot = gtQuat * rot;
    trans = gtTrans + trans;

    Eigen::Isometry3d noisyMeasurement = (Eigen::Isometry3d)rot;
    noisyMeasurement.translation() = trans;
    e->setMeasurement(noisyMeasurement);
  }

  // concatenate all the odometry constraints to compute the initial state
  for (size_t i = 0; i < odometryEdges.size(); ++i) {
    EdgeSE3* e = edges[i];
    VertexSE3* from = static_cast<VertexSE3*>(e->vertex(0));
    VertexSE3* to = static_cast<VertexSE3*>(e->vertex(1));
    HyperGraph::VertexSet aux;
    aux.insert(from);
    e->initialEstimate(aux, to);
  }

  // write output
  ofstream fileOutputStream;
  if (outFilename != "-") {
    cerr << "Writing into " << outFilename << endl;
    fileOutputStream.open(outFilename.c_str());
  } else {
    cerr << "writing to stdout" << endl;
  }

  string vertexTag = Factory::instance()->tag(vertices[0]);
  string edgeTag = Factory::instance()->tag(edges[0]);

  ostream& fout = outFilename != "-" ? fileOutputStream : cout;
  for (size_t i = 0; i < vertices.size(); ++i) {
    VertexSE3* v = vertices[i];
    fout << vertexTag << " " << v->id() << " ";
    v->write(fout);
    fout << endl;
  }

  for (size_t i = 0; i < edges.size(); ++i) {
    EdgeSE3* e = edges[i];
    VertexSE3* from = static_cast<VertexSE3*>(e->vertex(0));
    VertexSE3* to = static_cast<VertexSE3*>(e->vertex(1));
    fout << edgeTag << " " << from->id() << " " << to->id() << " ";
    e->write(fout);
    fout << endl;
  }

  return 0;
}


#include <boost/graph/adjacency_list.hpp>

#include <boost/graph/depth_first_search.hpp>
#include <boost/graph/breadth_first_search.hpp>

#include <iostream>

template<class Vertex, class Edge>
struct graph : boost::adjacency_list<boost::vecS,
												boost::vecS,
												boost::directedS,
												Vertex,
												Edge> {

};

class dfs_visitor : public boost::default_dfs_visitor {

public:

  template < typename Vertex, typename Graph >
  void discover_vertex(Vertex u, const Graph & g) const {
	std::cout << "disc:\t" << u << std::endl;
  }

  template < typename Vertex, typename Graph >
  void initialize_vertex(Vertex u, const Graph & g) const {
	std::cout << "init:\t" << u << std::endl;
  }

  template < typename Vertex, typename Graph >
    void finish_vertex(Vertex u, const Graph & g) const {
	std::cout << "finish:\t" << u << std::endl;
  }

  template < typename Vertex, typename Graph >
  void start_vertex(Vertex u, const Graph & g) const {
	std::cout << "start:\t" << u << std::endl;
  }

  template < typename Edge, typename Graph >
  void tree_edge(Edge e, const Graph & g) const {
	std::cout << "tree:\t" << e << std::endl;
  }

  template < typename Edge, typename Graph >
  void back_edge(Edge e, const Graph & g) const {
	std::cout << "back:\t" << e << std::endl;
  }

};

class bfs_visitor : public boost::default_bfs_visitor {

public:

  template < typename Vertex, typename Graph >
  void discover_vertex(Vertex u, const Graph & g) const {
	std::cout << "disc:\t" << u << std::endl;
  }

  template < typename Vertex, typename Graph >
  void initialize_vertex(Vertex u, const Graph & g) const {
	std::cout << "init:\t" << u << std::endl;
  }

  template < typename Vertex, typename Graph >
    void finish_vertex(Vertex u, const Graph & g) const {
	std::cout << "finish:\t" << u << std::endl;
  }

  template < typename Vertex, typename Graph >
  void start_vertex(Vertex u, const Graph & g) const {
	std::cout << "start:\t" << u << std::endl;
  }

  template < typename Edge, typename Graph >
  void tree_edge(Edge e, const Graph & g) const {
	std::cout << "tree:\t" << e << std::endl;
  }

  template < typename Edge, typename Graph >
  void back_edge(Edge e, const Graph & g) const {
	std::cout << "back:\t" << e << std::endl;
  }

};

int main(int, char** ) {

  struct vertex {
	boost::default_color_type color;
  };

  struct edge {};
  graph<vertex, edge> g;

  unsigned u = add_vertex(g);
  unsigned v = add_vertex(g);
  unsigned w = add_vertex(g);
  unsigned x = add_vertex(g);

  add_edge(u, v, g);
  add_edge(v, w, g);
  add_edge(v, x, g);

  vertex vp = g[v];

  std::cout << "dfs"<< std::endl;
  dfs_visitor dfs;
  depth_first_search(g, visitor(dfs).color_map(get(&vertex::color, g)));

  std::cout << "bfs"<< std::endl;
  bfs_visitor bfs;
  unsigned start = u;
  breadth_first_search(g, start, visitor(bfs).color_map(get(&vertex::color, g)));

}


#include "nfasl/Dot.hpp"
#include "nfasl/Nfasl.hpp"

#include "Algo.hpp"

#include <boost/lexical_cast.hpp>
#include <boost/graph/graphviz.hpp>
#include <fstream>
#include <set>

namespace nfasl {

  //using namespace boost;

  /*
struct VertexP { std::string tag; };
struct EdgeP { std::string symbol; };
struct GraphP { std::string orientation; };

typedef adjacency_list<vecS, vecS, directedS, VertexP, EdgeP, GraphP> Graph;

int main() {
    Graph g(GraphP{"LR"});
    // Then fill the graph
    add_edge(
        add_vertex(VertexP{ "tag1" }, g),
        add_vertex(VertexP{ "tag2" }, g),
        EdgeP{ "symbol" }, g
    );

    {
        std::ofstream dot_file("automaton.dot");
        dynamic_properties dp;
        dp.property("node_id", get(&VertexP::tag, g));
        dp.property("label", get(&VertexP::tag, g));
        dp.property("label", get(&EdgeP::symbol, g));
        dp.property("rankdir", boost::make_constant_property<Graph*>(std::string("LR")));
        dp.property("dummy", boost::make_function_property_map<Graph*>([](Graph const* g) { return g->m_property->orientation; }));

        write_graphviz_dp(dot_file, g, dp);
    }
}
  */
using namespace boost;
  //typedef boost::GraphvizDigraph Graph;
typedef adjacency_list<vecS, vecS, bidirectionalS,
                       property<vertex_color_t, default_color_type>//,
                       //                       property<vertex_shape_t, default_shape_type>
                       > Graph;
typedef graph_traits<Graph>::vertex_descriptor VertexDescriptor;

typedef size_t Name;

enum NodeType
  {
   NodeType_Initial = 0x1,
   NodeType_Final = 0x2
  };

class DotBuilder {
  Graph& graph;
  std::map<Name, VertexDescriptor> vertices;
public:
  DotBuilder(Graph& graph_) : graph(graph_) {}

  void addVertex(Name name, size_t typ) {
    assert(vertices.find(name) == vertices.end());
    // Add the vertex
    VertexDescriptor vertex = add_vertex(graph);
    // Add its property
    #if 0
    const boost::property_map<Graph, boost::vertex_attribute_t>::
        type& vertAttrMap = boost::get(boost::vertex_attribute, graph);
    vertAttrMap[vertex]["label"] = boost::lexical_cast<std::string>(name);
    if (typ | NodeType_Initial) {
      vertAttrMap[vertex]["shape"] = "doublecircle";
    } else {
      vertAttrMap[vertex]["shape"] = "circle";
    }
    if (typ | NodeType_Final) {
      vertAttrMap[vertex]["color"] = "green";
    }
    #endif
    vertices[name] = vertex;
  }

  void addEdge(Name src, Name dst, Predicate phi) {
    add_edge(vertices.at(src), vertices.at(dst), graph);
  }
};

static void toDot(const Nfasl& a, Graph& graph) {
  DotBuilder builder{graph};
  for (size_t q = 0; q < a.stateCount; ++q) {
    size_t typ = 0;
    if (q == a.initial) {
      typ |= NodeType_Initial;
    }
    if (set_member(a.finals, q)) {
      typ |= NodeType_Final;
    }
    builder.addVertex(q, typ);
  }
  for (size_t q = 0; q < a.stateCount; ++q) {
    for (auto const& rule : a.transitions[q]) {
      builder.addEdge(q, rule.state, rule.phi);
    }
  }
}

void toDot(const Nfasl& a, const std::string& file) {
  Graph graph;
  toDot(a, graph);

  std::ofstream stream{file};
  boost::write_graphviz(stream, graph);
}

} // namespace nfasl


#ifndef YASMIC_COMPRESSED_ROW_MATRIX
#define YASMIC_COMPRESSED_ROW_MATRIX

#if _MSC_VER >= 1400
    // disable the warning for deprecated c++ commands
    #pragma warning( push )
	#pragma warning( disable : 4996 )
#endif // _MSC_VER >= 1400

#include <iterator>
#include <boost/iterator/iterator_facade.hpp>
#include <boost/iterator/counting_iterator.hpp>
#include <boost/iterator/transform_iterator.hpp>

#include <boost/tuple/tuple.hpp>
#include <boost/iterator/zip_iterator.hpp>

#include <vector>

#include <functional>
#include <yasmic/tuple_utility.hpp>
#include <limits>

#include <yasmic/generic_matrix_operations.hpp>

namespace yasmic
{
	namespace impl
	{
		/**
		 * The nonzero iterator for the crm matrix.
		 */

		template <class RowIter, class ColIter, class ValIter>
		class compressed_row_nonzero_const_iterator
		: public boost::iterator_facade<
            compressed_row_nonzero_const_iterator<RowIter, ColIter, ValIter>,
			yasmic::simple_nonzero<
				typename std::iterator_traits<RowIter>::value_type,
				typename std::iterator_traits<ValIter>::value_type,
				typename std::iterator_traits<RowIter>::value_type> const,
            boost::forward_traversal_tag,
            yasmic::simple_nonzero<
				typename std::iterator_traits<RowIter>::value_type,
				typename std::iterator_traits<ValIter>::value_type,
				typename std::iterator_traits<RowIter>::value_type> const >
        {
        public:
            compressed_row_nonzero_const_iterator() {}

            compressed_row_nonzero_const_iterator(
                RowIter ri, RowIter rend, ColIter ci, ValIter vi)
            : _ri(ri), _rend(rend), _ci(ci), _vi(vi), _id(0), _row(0)
            {
				_ri++;
            	// skip over any empty rows
            	while ( _ri != _rend && *(_ri) == _id )
                {
                   	// keep incrementing the row
                   	++_ri; ++_row;
                }
            }

			compressed_row_nonzero_const_iterator(
                RowIter ri, RowIter rend, ColIter ci, ValIter vi,
				typename std::iterator_traits<RowIter>::value_type id,
				typename std::iterator_traits<RowIter>::value_type row = 0)
			: _ri(ri), _rend(rend), _ci(ci), _vi(vi), _id(id), _row(row)
			{
			}

        private:
            friend class boost::iterator_core_access;

            inline void increment()
            {
            	// just increment everything!
            	++_ci; ++_vi; ++_id;

                if (_id == *_ri)
                {
                	++_ri; ++_row;

                	// while we aren't at the end and the row isn't empty
                	// (if *_ri == _id, then the row is empty because _id
                	// is the current index in the column/val array.)
                	while ( _ri != _rend && *(_ri) == _id )
                    {
                    	// keep incrementing the row
                    	++_ri; ++_row;
                    }
                }
            }

            bool equal(compressed_row_nonzero_const_iterator const& other) const
            {
				return (_ri == other._ri && _ci == other._ci);
            }

            yasmic::simple_nonzero<
				typename std::iterator_traits<RowIter>::value_type,
				typename std::iterator_traits<ValIter>::value_type,
				typename std::iterator_traits<RowIter>::value_type>
            dereference() const
            {
            	//return boost::make_tuple(_row, *_ci, *_vi);
				return make_simple_nonzero(_row, *_ci, *_vi, _id);
            }

            typename std::iterator_traits<RowIter>::value_type _row;
            typename std::iterator_traits<RowIter>::value_type _id;
            RowIter _ri, _rend;
            ColIter _ci;
            ValIter _vi;
        };

		template <class IndexType, class ColIter, class ValIter>
		class crm_row_nonzero_const_iterator
		: public boost::iterator_facade<
            crm_row_nonzero_const_iterator<IndexType, ColIter, ValIter>,
			yasmic::simple_nonzero<
				typename std::iterator_traits<ColIter>::value_type,
				typename std::iterator_traits<ValIter>::value_type,
				IndexType> const,
            boost::forward_traversal_tag,
            yasmic::simple_nonzero<
				typename std::iterator_traits<ColIter>::value_type,
				typename std::iterator_traits<ValIter>::value_type,
				IndexType> const >
		{
		public:
			crm_row_nonzero_const_iterator() {}

            crm_row_nonzero_const_iterator(
                IndexType r, IndexType nzi, ColIter ci, ValIter vi)
            :  _r(r), _nzi(nzi), _ci(ci), _vi(vi)
            {}

		private:
			IndexType _r;
			IndexType _nzi;
			ColIter _ci;
			ValIter _vi;

            friend class boost::iterator_core_access;

			void increment()
            {
				++_nzi;
				++_ci;
				++_vi;
			}

			bool equal(crm_row_nonzero_const_iterator const& other) const
			{
				return (_ci == other._ci);
			}

			yasmic::simple_nonzero<
				typename std::iterator_traits<ColIter>::value_type,
				typename std::iterator_traits<ValIter>::value_type,
				IndexType>
            dereference() const
			{
				return (make_simple_nonzero(_r, *_ci, *_vi, _nzi));
			}
		};

		template <class RowIter, class ColIter, class ValIter>
		struct crm_row_nonzero_iter_help
		{
			typedef crm_row_nonzero_const_iterator<
				typename std::iterator_traits<RowIter>::value_type,
				ColIter, ValIter> row_iter;
		};
	}

	template <class RowIter, class ColIter, class ValIter>
	class compressed_row_matrix
	{
    public:
		typedef typename std::iterator_traits<RowIter>::value_type size_type;

		typedef size_type index_type;

		typedef typename std::iterator_traits<ValIter>::value_type value_type;

		typedef size_type nz_index_type;

		typedef simple_nonzero<index_type, value_type, nz_index_type> nonzero_descriptor;

		typedef impl::compressed_row_nonzero_const_iterator<RowIter, ColIter, ValIter>
             nonzero_iterator;

		typedef boost::counting_iterator<size_type> row_iterator;

		typedef nonzero_descriptor row_nonzero_descriptor;
		/*typedef impl::compressed_row_nonzero_const_iterator<RowIter, ColIter, ValIter>
             row_nonzero_iterator;*/

		typedef typename impl::crm_row_nonzero_iter_help<RowIter, ColIter, ValIter>::row_iter
			row_nonzero_iterator;

		typedef ValIter value_iterator;

		typedef void column_iterator;
		typedef void column_nonzero_iterator;

		//typedef unsymmetric_tag symmetry_category;
        typedef void properties;

		/**
		 * Simple constructor for all the iterators.  This function
		 * requires one pass through the data in order to determine
		 * the number of rows, columns, and non-zeros.
		 */
		compressed_row_matrix(RowIter rstart, RowIter rend,
            ColIter cstart, ColIter cend, ValIter vstart, ValIter vend)
        : _rstart(rstart), _rend(rend), _cstart(cstart), _cend(cend),
          _vstart(vstart), _vend(vend), _nnz(0), _nrows(0), _ncols(0)
        {
			RowIter ri = _rstart;
			ColIter ci = _cstart;
			ValIter vi = _vstart;

			// elminate the first entry, so we get the correct # of rows
			++ri;
			while (ri != _rend)
			{
				++_nrows;  ++ri;
			}

			while (ci != _cend)
			{
				_ncols = std::max(*ci,_ncols);
				++ci; ++vi;
				++_nnz;
			}

			if (vi != _vend)
			{
				// warning, incorrect data!
			}
        }

		compressed_row_matrix(RowIter rstart, RowIter rend,
            ColIter cstart, ColIter cend, ValIter vstart, ValIter vend,
            size_type nrows, size_type ncols, size_type nnz)
        : _rstart(rstart), _rend(rend), _cstart(cstart), _cend(cend),
          _vstart(vstart), _vend(vend), _nrows(nrows), _ncols(ncols), _nnz(nnz)
        {}

        nonzero_iterator begin_nonzeros() const
        {
        	return (nonzero_iterator(_rstart, _rend, _cstart, _vstart));
        }

        nonzero_iterator end_nonzeros() const
        {
        	return (nonzero_iterator(_rend-1, _rend, _cend, _vstart));
        }

        inline std::pair<size_type, size_type> dimensions() const
        {
        	return (std::make_pair(_nrows, _ncols));
        }

        inline size_type nnz() const
        {
        	return (_nnz);
        }

        row_iterator begin_rows() const
        {
        	return (row_iterator(0));
        }

        row_iterator end_rows() const
        {
        	return (row_iterator(_nrows));
        }

		row_nonzero_iterator begin_row(index_type r) const
		{
			index_type nzi = *(_rstart + (r));
			return (row_nonzero_iterator(r, nzi, _cstart + nzi, _vstart + nzi));
		}

		row_nonzero_iterator end_row(index_type r) const
        {
			index_type nzi = *(_rstart + (r+1));
			return (row_nonzero_iterator(r, nzi, _cstart + nzi, _vstart + nzi));
		}

		value_iterator begin_values() const
		{
			return (_vstart);
		}

		value_iterator end_values() const
		{
			return (_vend);
		}

        RowIter _rstart,_rend;
        ColIter _cstart,_cend;
        ValIter _vstart,_vend;

        size_type _nrows, _ncols, _nnz;
	};

	template <class RowIter, class ColIter, class ValIter, class BFunc >
	void pack_storage(compressed_row_matrix<RowIter, ColIter, ValIter>& m, BFunc b = BFunc())
	{
		// remove any duplicate entries.  This doesn't actually free any
		// memory, but reduces the non-zero count to the appropriate level
		// for the matrix.

		typedef compressed_row_matrix<RowIter, ColIter, ValIter> Matrix;
		typedef smatrix_traits<Matrix> traits;

		std::vector<typename traits::index_type> wa(ncols(m));

		typename traits::nz_index_type cur_elem = 0;

		typename traits::index_type unused = std::numeric_limits<typename traits::index_type>::max();

		std::fill(wa.begin(), wa.end(), unused);

		RowIter ri, riend;
		ri = m._rstart;
		riend = m._rend-1;

		ColIter ci = m._cstart;
		ValIter vi = m._vstart;

		for(; ri != riend; ++ri)
		{
			typename traits::nz_index_type pos_start = *ri;
			typename traits::nz_index_type pos_end = *(ri+1);

			*ri = cur_elem;

			//
			// look at the elements in order
			//

			for (typename traits::nz_index_type k=pos_start; k<pos_end; k++)
			{
				// the column
				typename traits::index_type c = ci[k];

				if (wa[c] == unused)
				{
					// this is a new element
					ci[cur_elem] = c;
					vi[cur_elem] = vi[k];

					wa[c] = cur_elem;

					// now we've used this element...
					cur_elem++;
				}
				else
				{
					// this is a duplicate element
					//vi[wa[c]] += vi[k];
					vi[wa[c]] = b(vi[wa[c]], vi[k]);
				}
			}

			// reset just the entries we used (which are stored in the ci
			// array)
			for (typename traits::nz_index_type k=*ri; k < cur_elem; k++)
			{
				wa[ci[k]] = unused;
			}

		}

		*ri = cur_elem;
	}

    /* ========================================================
     *  Routines to pack storage
     * ===================================================== */

	template <class RowIter, class ColIter, class ValIter >
	void pack_storage(compressed_row_matrix<RowIter, ColIter, ValIter>& m)
	{
		pack_storage(m, std::plus<typename std::iterator_traits<ValIter>::value_type > ());
	}

    /* ========================================================
     *  Routines to sort storage
     * ===================================================== */

	/**
	 * The version with a custom sorting operation.
	 */
	template <class RowIter, class ColIter, class ValIter, class StrictWeakOrdering>
	void sort_storage(compressed_row_matrix<RowIter, ColIter, ValIter>& m,
		StrictWeakOrdering comp)
	{
        //BOOST_STATIC_ASSERT(0 == 1);
	}

	namespace impl
	{
		template <class ColIter, class ValIter>
		struct crm_col_val_iter_helper_type
		{
			typedef boost::tuple<
					typename std::iterator_traits<ColIter>::value_type,
					typename std::iterator_traits<ValIter>::value_type >
				value_type;

			typedef boost::tuple<
					typename std::iterator_traits<ColIter>::value_type&,
					typename std::iterator_traits<ValIter>::value_type& >
				ref_type;
		};

		template <class ColIter, class ValIter>
		class crm_col_val_iter
			: public boost::iterator_facade<
				crm_col_val_iter<ColIter, ValIter>,
				typename crm_col_val_iter_helper_type<ColIter, ValIter>::value_type,
				std::random_access_iterator_tag,
				typename crm_col_val_iter_helper_type<ColIter, ValIter>::ref_type,
				typename std::iterator_traits<ColIter>::difference_type>
		{
		public:
			crm_col_val_iter()
			{}

			crm_col_val_iter(ColIter ci, ValIter vi)
				: _ci(ci), _vi(vi)
			{
			}

			ColIter _ci;
			ValIter _vi;

		private:
			friend class boost::iterator_core_access;
            typedef typename std::iterator_traits<ColIter>::difference_type my_difference_type;

			void increment()
			{
				++_ci; ++_vi;
			}

			void decrement()
			{
				--_ci; --_vi;
			}

			bool equal(crm_col_val_iter const& other) const
			{
				return (_ci == other._ci);
			}

			typename crm_col_val_iter_helper_type<ColIter, ValIter>::ref_type dereference() const
			{
				//_tmp = boost::make_tuple(*_ci, *_vi);
				//return (_tmp);
				return (typename crm_col_val_iter_helper_type<ColIter, ValIter>::ref_type(*_ci, *_vi));
			}

			void advance(my_difference_type n)
			{
				_ci += n;
				_vi += n;
			}

			my_difference_type distance_to(crm_col_val_iter const& other) const
			{
				return ( other._ci - _ci);
			}

		};

		template <class ColIter, class ValIter>
		struct crm_col_val_iter_tuple_compare
			: public std::binary_function<
					typename crm_col_val_iter_helper_type<ColIter, ValIter>::value_type,
					typename crm_col_val_iter_helper_type<ColIter, ValIter>::value_type,
					bool>
		{
			typedef typename crm_col_val_iter_helper_type<ColIter, ValIter>::value_type T;
			bool operator()(const  T& t1, const T& t2)
			{
				return (boost::get<0>(t1) < boost::get<0>(t2));
			}
		};

		template <class ColIter, class ValIter>
		crm_col_val_iter<ColIter, ValIter>
		crm_make_col_val_iter(ColIter ci, ValIter vi)
		{
			return crm_col_val_iter<ColIter, ValIter>(ci, vi);
		};
	}

	/**
	 * Sort the storage of a matrix.  This function sorts the entries
	 * in each row by column.
	 */
	template <class RowIter, class ColIter, class ValIter>
	void sort_storage(compressed_row_matrix<RowIter, ColIter, ValIter>& m)
	{
		typedef compressed_row_matrix<RowIter, ColIter, ValIter> Matrix;
		typedef smatrix_traits<Matrix> traits;

		typedef typename traits::index_type itype;
		typedef typename traits::value_type vtype;

		RowIter ri, riend;
		ri = m._rstart;
		riend = m._rend-1;

		ColIter ci = m._cstart;
		ValIter vi = m._vstart;

		for(; ri != riend; ++ri)
		{
			typename traits::nz_index_type pos_start = *ri;
			typename traits::nz_index_type pos_end = *(ri+1);

			std::sort(
				impl::crm_make_col_val_iter(ci + pos_start, vi + pos_start),
				impl::crm_make_col_val_iter(ci + pos_end, vi + pos_end),
				impl::crm_col_val_iter_tuple_compare<ColIter,ValIter>());
		}
	}

    /* ========================================================
     *  Routines to multiply
     * ===================================================== */

	/**
	 * Specialize the multiply operator for this matrix type.
	 */
	template <class RowIter, class ColIter, class ValIter, class Iter1, class Iter2>
	void mult(const compressed_row_matrix<RowIter, ColIter, ValIter>& m, Iter1 x, Iter2 y)
	{
		RowIter ri = m._rstart;
		ColIter ci = m._cstart;
		ValIter vi = m._vstart;

		typedef compressed_row_matrix<RowIter, ColIter, ValIter> matrix;
		typedef typename smatrix_traits<matrix>::index_type itype;
		typedef typename smatrix_traits<matrix>::size_type stype;

		stype nr = nrows(m);

		for (itype r=0; r < nr; ++r)
		{
			typedef typename smatrix_traits<matrix>::nz_index_type nzitype;
			typedef typename smatrix_traits<matrix>::value_type vtype;

			vtype ip = 0.0;

			for (nzitype cp = ri[r]; cp < ri[r+1]; ++cp)
			{
				ip += vi[cp]*x[ci[cp]];
			}

			y[r] = ip;
		}
	}

	/**
	 * Specialize the transpose multiply operator for this matrix type.
	 */
	template <class RowIter, class ColIter, class ValIter, class Iter1, class Iter2>
	void trans_mult(const compressed_row_matrix<RowIter, ColIter, ValIter>& m, Iter1 x, Iter2 y)
	{
		RowIter ri = m._rstart;
		ColIter ci = m._cstart;
		ValIter vi = m._vstart;

		typedef compressed_row_matrix<RowIter, ColIter, ValIter> matrix;
		typedef typename smatrix_traits<matrix>::index_type itype;
		typedef typename smatrix_traits<matrix>::size_type stype;

		stype nc = ncols(m);

		// first zero the vector
		for (itype c=0; c < nc; ++c)
		{
			y[c] = 0.0;
		}

		for (itype c=0; c < nc; ++c)
		{
			typedef typename smatrix_traits<matrix>::nz_index_type nzitype;
			typedef typename smatrix_traits<matrix>::value_type vtype;

			vtype cv = x[c];

			for (nzitype cp = ri[c]; cp < ri[c+1]; ++cp)
			{
				y[ci[cp]] += vi[cp]*cv;
			}
		}
	}

    /* ========================================================
     *  Routines to find a value
     * ===================================================== */

    /**
	 * Specialize the value operator for this matrix type.
	 */
	template <class RowIter, class ColIter, class ValIter>
	typename smatrix_traits< compressed_row_matrix<RowIter, ColIter, ValIter> >::value_type value(
        typename smatrix_traits< compressed_row_matrix<RowIter, ColIter, ValIter> >::index_type row,
        typename smatrix_traits< compressed_row_matrix<RowIter, ColIter, ValIter> >::index_type col,
        const compressed_row_matrix<RowIter, ColIter, ValIter>& m)
	{
        typedef compressed_row_matrix<RowIter, ColIter, ValIter> matrix;

		RowIter ri = m._rstart;
		ColIter ci = m._cstart;
		ValIter vi = m._vstart;

        typedef typename smatrix_traits<matrix>::nz_index_type nzi_type;

        for (nzi_type cp = ri[row]; cp < ri[row+1]; ++cp)
        {
            if (col == ci[cp])
            {
                return (vi[cp]);
            }
        }

        return (0);
	}

    /**
     * csr_matrix is a more user manageable compressed sparse row matrix type.
     * It is based on the same ideas as the compressed_row_matrix, but designed
     * to be less general and more specific.
     *
     * csr_matrix uses compressed_row_matrix for some of its operations.
     *
     * The idea is that an application will use the csr_matrix structure to
     * manage a sparse matrix and design algorithms that are NOT more
     * generally applicable.
     */
    /*template <class IndexType, class ValueType, class SizeType=IndexType>
    struct csr_matrix
    {
        SizeType nrows;
        SizeType ncols;
        SizeType nnz;

        IndexType* ai;

        IndexType* aj;
        ValueType* a;
    };

    template <class IndexType, class ValueType, class SizeType>
    smatrix_traits<csr_matrix<IndexType, ValueType, SizeType> >
    {
        typedef compressed_row_matrix<IndexType*, IndexType*, ValueType*>
            compressed_row_matrix_type;

        typedef IndexType index_type;
        typedef ValueType value_type;
        typedef typename compressed_row_matrix_type::nonzero_iterator nonzero_iterator;
        typedef typename compressed_row_matrix_type::nonzero_descriptor nonzero_descriptor;

	    typedef typename compressed_row_matrix_type::row_iterator row_iterator;

	    typedef typename compressed_row_matrix_type::row_nonzero_iterator row_nonzero_iterator;
	    typedef typename compressed_row_matrix_type::row_nonzero_descriptor row_nonzero_descriptor;

	    typedef typename SizeType nz_index_type;

	    typedef void column_iterator;
	    //typedef typename Mat::column_nonzero_iterator column_nonzero_iterator;

	    typedef unsymmetric_tag symmetry_category;
	    };
    }

    template <class IndexType, class ValueType, class SizeType>
    SizeType nrows(const csr_matrix<IndexType, ValueType, SizeType>& m)
    { return m.nrows; }

    template <class IndexType, class ValueType, class SizeType>
    SizeType ncols(const csr_matrix<IndexType, ValueType, SizeType>& m)
    { return m.ncols; }

    template <class IndexType, class ValueType, class SizeType>
    SizeType nnz(const csr_matrix<IndexType, ValueType, SizeType>& m)
    { return m.nnz; }*/

}

#if _MSC_VER >= 1400
	// disable the warning for deprecated c++ commands
	#pragma warning( pop )
#endif // _MSC_VER >= 1400

#endif // YASMIC_COMPRESSED_ROW_MATRIX


/*
 * MIT License
 *
 * Copyright (c) 2015 Alexis LE GOADEC
 *
 * 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.
 *
 */

/*
 * Modélisation de la matrice d'adjacence associée à l'hypergraphe.
 *
 */

#ifndef MODEL_INCLUDE_ADJACENTMATRIX_HH_
#define MODEL_INCLUDE_ADJACENTMATRIX_HH_

#include "LibType.hh"
#include <boost/tuple/tuple.hpp>

class HyperEdge;

class HyperVertex;

/**
 * Déclaration de la matrice d'adjacence.
 */
class AdjacentMatrix {

public:

	/**
	 * Constructeur par défaut.
	 */
	AdjacentMatrix();

	/**
	 * Constructeur de taille particulière.
	 * @param x L'abscisse
	 * @param y L'ordonnée de la matrice.
	 */
	AdjacentMatrix(const unsigned int&, const unsigned int&);

	/**
	 * Modification de la taille de la matrice.
	 * @param x L'abscisse
	 * @param y L'ordonnée de la matrice.
	 */
	void resize(const unsigned int&, const unsigned int&);

	/**
	 * Ajouter un hyper-vertex à la matrice.
	 * @param HyperVertex L'hyper-vertex à ajouter.
	 */
	void addHyperVertex(const boost::shared_ptr<HyperVertex>&);

	/**
	 * Ajouter une hyper-arête à la matrice.
	 * @param HyperEdge L'hyper-arête à ajouter.
	 */
	void addHyperEdge(const boost::shared_ptr<HyperEdge>&);

	/**
	 * Vérifier si l'hyper-vertex est dans l'hyper-arête.
	 * @param L'hyper-vertex à vérifier.
	 * @param L'hyper-arête dont la précence de l'hyper-vertex est à vérifier.
	 * @return True si l'hyper-vertex est dans l'hyper-arête, False sinon.
	 */
	bool isVertexInEdge(const boost::shared_ptr<HyperVertex>&, const boost::shared_ptr<HyperEdge>&) const;

	/**
	 * Vérifier si une hyper-arête est dans la liste des hyper-arêtes d'un hyper-vertex.
	 * @param L'hyper-arête à vérifier.
	 * @param L'hyper-vertex.
	 * @return True si l'hyper-vertex contient l'hyper-arête dans sa liste, False sinon.
	 */
	bool isEdgeInVertex(const boost::shared_ptr<HyperEdge>&, const boost::shared_ptr<HyperVertex>&) const;

	/**
	 * Vérifier si l'hyper-vertex ayant l'identifiant i se trouve dans l'hyper-arête ayant l'identifiant j.
	 * @param L'identifiant de l'hyper-vertex.
	 * @param L'identifiant de l'hyper-arête.
	 * @return True si c'est le cas, False sinon.
	 */
	bool isVertexInEdge(const int&, const int&) const;

	/**
	 * Vérifier si l'hyper-arête ayant l'identifiant i se trouve dans la liste de l'hyper-vertex ayant l'identifiant j.
	 * @param L'identifiant de l'hyper-arête.
	 * @param L'identifiant de l'hyper-vertex.
	 * @return True si c'est le cas, False sinon.
	 */
	bool isEdgeInVertex(const int&, const int&) const;

	/**
	 * Obtenir la matrice d'adjacence booléenne.
	 * @return La matrice d'adjacence booléenne.
	 */
	LibType::AdjacentMatrixContainerBool& getBoolAdjacentMatrix();

	/**
	 * Obtenir la matrice d'adjacence entière.
	 * @return La matrice d'adjacence entière.
	 */
	LibType::AdjacentMatrixContainerInt& getUIntAdjacentMatrix();

	/**
	 * Obtenir le degré d'un hyper-vertex.
	 * @param L'hyper-vertex dont on souhaite obtenir le degré.
	 * @return Nn entier positif correspondant au degrès de l'hyper-vertex.
	 */
	unsigned int getVertexDegree(const boost::shared_ptr<HyperVertex>&) const;

	/**
	 * Obtenir l'effectif d'une hyper-arête.
	 * @param L'hyper-arête dont on souhaite obtenir l'effectif.
	 * @return Un entier positif correspondant au nombre d'hyper-vertex dans l'hyper-arête.
	 */
	unsigned int getEdgeSize(const boost::shared_ptr<HyperEdge>&) const;

	/**
	 * Obtenir le co-rang de l'hypergraphe.
	 * @return Un entie correspondant au co-rang.
	 */
	unsigned int getCoRank() const;

	/**
	 * Obtenir le rang de l'hypergraphe.
	 * @return Un entie correspondant au rang.
	 */
	unsigned int getRank() const;

	/**
	 * Obtenir les dimensions de la matrice d'adjacence.
	 * @return Un tuple dont le premier nombre est l'abscisse, le second est l'ordonnée.
	 */
	boost::tuple<unsigned int, unsigned int>&
	getMatrixDimension();

	/**
	 * Fonction-test affichant la matrice sur la sortie standard - NE PAS UTILISER.
	 */
	void display() const;

protected:

	/**
	 * L'abscisse.
	 */
	unsigned int _m;

	/**
	 * L'ordonnée.
	 */
	unsigned int _n;

	/**
	 * Les données de la matrice d'adjacence booléenne.
	 */
	LibType::AdjacentMatrixContainerBool _adjacentMatrixBool;

	/**
	 * Les données de la matrice d'adjacence entière.
	 */
	LibType::AdjacentMatrixContainerInt  _adjacentMatrixUInt;

};

#endif


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

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/integral.hpp>
#include <boost/hana/map.hpp>
#include <boost/hana/maybe.hpp>
#include <boost/hana/pair.hpp>
#include <boost/hana/type.hpp>
using namespace boost::hana;

int main() {
    //! [main]
    auto m = map(
        pair(type<int>, 'i'),
        pair(type<float>, 'f')
    );
    BOOST_HANA_CONSTEXPR_ASSERT(lookup(m, type<int>) == just('i'));
    BOOST_HANA_CONSTEXPR_ASSERT(lookup(m, type<float>) == just('f'));
    BOOST_HANA_CONSTANT_ASSERT(lookup(m, type<void>) == nothing);
    BOOST_HANA_CONSTANT_ASSERT(lookup(m, int_<3>) == nothing);
    //! [main]
}


/******************************************************************************
Copyright (c) 2020, Farbod Farshidian. All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

 * Redistributions of source code must retain the above copyright notice, this
  list of conditions and the following disclaimer.

 * Redistributions in binary form must reproduce the above copyright notice,
  this list of conditions and the following disclaimer in the documentation
  and/or other materials provided with the distribution.

 * Neither the name of the copyright holder nor the names of its
  contributors may be used to endorse or promote products derived from
  this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 ******************************************************************************/

#include <iostream>
#include <string>

#include "ocs2_quadrotor/QuadrotorInterface.h"
#include "ocs2_quadrotor/dynamics/QuadrotorSystemDynamics.h"

#include <ocs2_core/cost/QuadraticStateCost.h>
#include <ocs2_core/cost/QuadraticStateInputCost.h>
#include <ocs2_core/initialization/OperatingPoints.h>
#include <ocs2_core/misc/LoadData.h>

// Boost
#include <boost/filesystem/operations.hpp>
#include <boost/filesystem/path.hpp>

namespace ocs2 {
namespace quadrotor {

/******************************************************************************************************/
/******************************************************************************************************/
/******************************************************************************************************/
QuadrotorInterface::QuadrotorInterface(const std::string& taskFile, const std::string& libraryFolder) {
  // check that task file exists
  boost::filesystem::path taskFilePath(taskFile);
  if (boost::filesystem::exists(taskFilePath)) {
    std::cerr << "[QuadrotorInterface] Loading task file: " << taskFilePath << std::endl;
  } else {
    throw std::invalid_argument("[QuadrotorInterface] Task file not found: " + taskFilePath.string());
  }
  // create library folder if it does not exist
  boost::filesystem::path libraryFolderPath(libraryFolder);
  boost::filesystem::create_directories(libraryFolderPath);
  std::cerr << "[QuadrotorInterface] Generated library path: " << libraryFolderPath << std::endl;

  // Default initial condition
  loadData::loadEigenMatrix(taskFile, "initialState", initialState_);
  std::cerr << "x_init:   " << initialState_.transpose() << std::endl;

  // Solver settings
  ddpSettings_ = ddp::loadSettings(taskFile, "ddp");
  mpcSettings_ = mpc::loadSettings(taskFile, "mpc");

  /*
   * ReferenceManager & SolverSynchronizedModule
   */
  referenceManagerPtr_.reset(new ReferenceManager);

  /*
   * Optimal control problem
   */
  // Cost
  matrix_t Q(STATE_DIM, STATE_DIM);
  matrix_t R(INPUT_DIM, INPUT_DIM);
  matrix_t Qf(STATE_DIM, STATE_DIM);
  loadData::loadEigenMatrix(taskFile, "Q", Q);
  loadData::loadEigenMatrix(taskFile, "R", R);
  loadData::loadEigenMatrix(taskFile, "Q_final", Qf);
  std::cerr << "Q:  \n" << Q << "\n";
  std::cerr << "R:  \n" << R << "\n";
  std::cerr << "Q_final:\n" << Qf << "\n";

  problem_.costPtr->add("cost", std::unique_ptr<StateInputCost>(new QuadraticStateInputCost(Q, R)));
  problem_.finalCostPtr->add("finalCost", std::unique_ptr<StateCost>(new QuadraticStateCost(Qf)));

  // Dynamics
  auto quadrotorParameters = quadrotor::loadSettings(taskFile, "QuadrotorParameters", true);
  problem_.dynamicsPtr.reset(new QuadrotorSystemDynamics(quadrotorParameters));

  // Rollout
  auto rolloutSettings = rollout::loadSettings(taskFile, "rollout");
  rolloutPtr_.reset(new TimeTriggeredRollout(*problem_.dynamicsPtr, rolloutSettings));

  // Initialization
  vector_t initialInput = vector_t::Zero(INPUT_DIM);
  initialInput(0) = quadrotorParameters.quadrotorMass_ * quadrotorParameters.gravity_;
  operatingPointPtr_.reset(new OperatingPoints(initialState_, initialInput));
}

}  // namespace quadrotor
}  // namespace ocs2