#pragma once

#include <Eigen/Core>

#include "metric.hpp"
#include "std_traits.hpp"

//! \file eigen.hpp
//! \brief Contains traits and classes that provide Eigen support for PicoTree.

namespace pico_tree {

namespace internal {

//! \brief A trait that determines if Derived inherits from Eigen::MatrixBase<>.
template <typename Derived>
struct is_matrix_base
    : public std::is_base_of<
          Eigen::MatrixBase<typename std::decay<Derived>::type>,
          typename std::decay<Derived>::type> {};

//! \brief Dimension trait for Eigen vectors.
template <typename Derived, bool IsRowMajor>
struct EigenVectorDim;

//! \brief Provides compile time dimension for ColMajor Eigen types.
template <typename Derived>
struct EigenVectorDim<Derived, false> {
  static_assert(
      is_matrix_base<Derived>::value, "DERIVED_TYPE_IS_NOT_AN_EIGEN_MATRIX");
  static_assert(
      Derived::ColsAtCompileTime == 1, "DERIVED_TYPE_IS_NOT_A_VECTOR");
  //! \brief Compile time dimension of Derived.
  static int constexpr Dim = Derived::RowsAtCompileTime;
};

//! \brief Provides compile time dimension for RowMajor Eigen types.
template <typename Derived>
struct EigenVectorDim<Derived, true> {
  static_assert(
      is_matrix_base<Derived>::value, "DERIVED_TYPE_IS_NOT_AN_EIGEN_MATRIX");
  static_assert(
      Derived::RowsAtCompileTime == 1, "DERIVED_TYPE_IS_NOT_A_VECTOR");
  //! \brief Compile time dimension of Derived.
  static int constexpr Dim = Derived::ColsAtCompileTime;
};

//! \brief EigenPointTraits provides an interface for the different point types
//! that can be used with EigenTraits.
//! \details Unlike the specialization of StdPointTraits for Eigen types, the
//! internal implementation supports matrix expressions.
template <typename Derived>
struct EigenPointTraits : public EigenVectorDim<Derived, Derived::IsRowMajor> {
  //! \brief The scalar type of point coordinates.
  using ScalarType = typename std::decay<typename Derived::Scalar>::type;

  //! \brief Returns a pointer to the coordinates of \p point.
  inline static ScalarType const* Coords(
      Eigen::MatrixBase<Derived> const& point) {
    return point.derived().data();
  }

  //! \brief Returns the spatial dimension of \p point.
  inline static int Sdim(Eigen::MatrixBase<Derived> const& point) {
    return static_cast<int>(point.size());
  }
};

//! \brief Provides the Point interface for the different implementations of
//! EigenTraitsImpl.
template <typename Derived>
struct EigenPointBase {
  //! \brief The scalar type of point coordinates.
  using ScalarType = typename std::decay<typename Derived::Scalar>::type;

  //! \brief Returns the spatial dimension of \p point.
  template <typename OtherDerived>
  inline static int PointSdim(Eigen::MatrixBase<OtherDerived> const& point) {
    static_assert(
        std::is_same<
            ScalarType,
            typename std::decay<typename OtherDerived::Scalar>::type>::value,
        "INCOMPATIBLE_SCALAR_TYPES");
    return EigenPointTraits<OtherDerived>::Sdim(point);
  }

  //! \brief Returns a pointer to the coordinates of \p point.
  template <typename OtherDerived>
  inline static ScalarType const* PointCoords(
      Eigen::MatrixBase<OtherDerived> const& point) {
    static_assert(
        std::is_same<
            ScalarType,
            typename std::decay<typename OtherDerived::Scalar>::type>::value,
        "INCOMPATIBLE_SCALAR_TYPES");
    return EigenPointTraits<OtherDerived>::Coords(point);
  }
};

//! \brief Space and Point traits for Eigen types.
template <typename Derived, typename Index_, bool RowMajor>
struct EigenTraitsImpl;

//! \brief Space and Point traits for ColMajor Eigen types.
template <typename Derived, typename Index_>
struct EigenTraitsImpl<Derived, Index_, false>
    : public EigenPointBase<Derived> {
  //! \brief Spatial dimension. Eigen::Dynamic equals pico_tree::kDynamicDim.
  static constexpr int Dim = Derived::RowsAtCompileTime;
  //! \brief The point type used by Derived.
  using PointType = Eigen::Block<Derived const, Dim, 1, true>;
  //! \brief The index type of point coordinates.
  using IndexType = Index_;

  //! \brief Returns the dimension of the space in which the points reside.
  //! I.e., the amount of coordinates each point has.
  inline static int SpaceSdim(Eigen::MatrixBase<Derived> const& matrix) {
    return static_cast<int>(matrix.rows());
  }

  //! \brief Returns the number of points.
  inline static IndexType SpaceNpts(Eigen::MatrixBase<Derived> const& matrix) {
    return static_cast<IndexType>(matrix.cols());
  }

  //! \brief Returns the point at index \p idx.
  inline static PointType PointAt(
      Eigen::MatrixBase<Derived> const& matrix, IndexType const idx) {
    return matrix.col(idx);
  }
};

//! \brief Space and Point traits for RowMajor Eigen types.
template <typename Derived, typename Index_>
struct EigenTraitsImpl<Derived, Index_, true> : public EigenPointBase<Derived> {
  //! \brief Spatial dimension. Eigen::Dynamic equals pico_tree::kDynamicDim.
  static constexpr int Dim = Derived::ColsAtCompileTime;
  //! \brief The point type used by Derived.
  using PointType = Eigen::Block<Derived const, 1, Dim, true>;
  //! \brief The index type of point coordinates.
  using IndexType = Index_;

  //! \brief Returns the dimension of the space in which the points reside.
  //! I.e., the amount of coordinates each point has.
  inline static int SpaceSdim(Eigen::MatrixBase<Derived> const& matrix) {
    return static_cast<int>(matrix.cols());
  }

  //! \brief Returns the number of points.
  inline static IndexType SpaceNpts(Eigen::MatrixBase<Derived> const& matrix) {
    return static_cast<IndexType>(matrix.rows());
  }

  //! \brief Returns the point at index \p idx.
  inline static PointType PointAt(
      Eigen::MatrixBase<Derived> const& matrix, IndexType const idx) {
    return matrix.row(idx);
  }
};

//! \brief This struct simply reduces some of the template argument overhead.
template <typename Derived, typename Index_>
struct EigenTraitsBase
    : public EigenTraitsImpl<Derived, Index_, Derived::IsRowMajor> {
  static_assert(
      Derived::RowsAtCompileTime == Eigen::Dynamic ||
          Derived::ColsAtCompileTime == Eigen::Dynamic,
      "FIXED_SIZE_MATRICES_ARE_NOT_SUPPORTED");

  //! \brief The SpaceType of these traits.
  using SpaceType = Derived;
};

}  // namespace internal

//! \brief EigenTraits provides an interface for spaces and points when working
//! with Eigen types.
//! \details It supports dynamic matrices and maps of dynamic matrices. Support
//! for fixed size matrices or maps of those is disabled. Fixed size matrices
//! are mostly useful when they are small. See section "Fixed vs. Dynamic size"
//! of the following link:
//! * https://eigen.tuxfamily.org/dox/group__TutorialMatrixClass.html
//!

//! Special care needs to be taken to work with fixed size matrices as well,
//! adding to the complexity of this library with very little in return. This
//! results in the choice to not support them through EigenTraits.
//!

//! Special care:
//! * Aligned members of fixed size cannot be copied or moved (a move is a
//! copy). https://eigen.tuxfamily.org/dox/group__TopicPassingByValue.html
//! * They may need to be aligned in memory. As members are aligned with
//! respect to the containing class the EigenAdaptor would need to be aligned
//! as well.
//! https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
//! \tparam Derived An Eigen::Matrix<> or Eigen::Map<Eigen::Matrix<>>.
//! \tparam Index_ Type used for indexing. Defaults to int.
template <typename Derived, typename Index_ = int>
struct EigenTraits;

//! \brief EigenTraits provides an interface for Eigen::Matrix<>.
//! \tparam Index_ Type used for indexing. Defaults to int.
template <
    typename Scalar_,
    int Rows_,
    int Cols_,
    int Options_,
    int MaxRows_,
    int MaxCols_,
    typename Index_>
struct EigenTraits<
    Eigen::Matrix<Scalar_, Rows_, Cols_, Options_, MaxRows_, MaxCols_>,
    Index_>
    : internal::EigenTraitsBase<
          Eigen::Matrix<Scalar_, Rows_, Cols_, Options_, MaxRows_, MaxCols_>,
          Index_> {};

//! \brief EigenTraits provides an interface for Eigen::Map<Eigen::Matrix<>>.
//! \tparam Index_ Type used for indexing. Defaults to int.
template <
    typename Scalar_,
    int Rows_,
    int Cols_,
    int Options_,
    int MaxRows_,
    int MaxCols_,
    int MapOptions_,
    typename StrideType_,
    typename Index_>
struct EigenTraits<
    Eigen::Map<
        Eigen::Matrix<Scalar_, Rows_, Cols_, Options_, MaxRows_, MaxCols_>,
        MapOptions_,
        StrideType_>,
    Index_>
    : internal::EigenTraitsBase<
          Eigen::Map<
              Eigen::
                  Matrix<Scalar_, Rows_, Cols_, Options_, MaxRows_, MaxCols_>,
              MapOptions_,
              StrideType_>,
          Index_> {};

//! \brief EigenTraits provides an interface for
//! std::reference_wrapper<Eigen::Matrix<>>.
//! \tparam Index_ Type used for indexing. Defaults to int.
template <
    typename Scalar_,
    int Rows_,
    int Cols_,
    int Options_,
    int MaxRows_,
    int MaxCols_,
    typename Index_>
struct EigenTraits<
    std::reference_wrapper<
        Eigen::Matrix<Scalar_, Rows_, Cols_, Options_, MaxRows_, MaxCols_>>,
    Index_>
    : public EigenTraits<
          Eigen::Matrix<Scalar_, Rows_, Cols_, Options_, MaxRows_, MaxCols_>,
          Index_> {
  //! \brief The SpaceType of these traits.
  using SpaceType = std::reference_wrapper<
      Eigen::Matrix<Scalar_, Rows_, Cols_, Options_, MaxRows_, MaxCols_>>;
};

//! \brief EigenTraits provides an interface for
//! std::reference_wrapper<Eigen::Matrix<> const>.
//! \tparam Index_ Type used for indexing. Defaults to int.
template <
    typename Scalar_,
    int Rows_,
    int Cols_,
    int Options_,
    int MaxRows_,
    int MaxCols_,
    typename Index_>
struct EigenTraits<
    std::reference_wrapper<
        Eigen::
            Matrix<Scalar_, Rows_, Cols_, Options_, MaxRows_, MaxCols_> const>,
    Index_>
    : public EigenTraits<
          Eigen::Matrix<Scalar_, Rows_, Cols_, Options_, MaxRows_, MaxCols_>,
          Index_> {
  //! \brief The SpaceType of these traits.
  using SpaceType = std::reference_wrapper<
      Eigen::Matrix<Scalar_, Rows_, Cols_, Options_, MaxRows_, MaxCols_> const>;
};

//! \brief StdPointTraits provides an interface for Eigen::Matrix<>.
template <
    typename Scalar_,
    int Rows_,
    int Cols_,
    int Options_,
    int MaxRows_,
    int MaxCols_>
struct StdPointTraits<
    Eigen::Matrix<Scalar_, Rows_, Cols_, Options_, MaxRows_, MaxCols_>>
    : internal::EigenPointTraits<
          Eigen::Matrix<Scalar_, Rows_, Cols_, Options_, MaxRows_, MaxCols_>> {
};

//! \brief StdPointTraits provides an interface for Eigen::Map<Eigen::Matrix<>>.
template <
    typename Scalar_,
    int Rows_,
    int Cols_,
    int Options_,
    int MaxRows_,
    int MaxCols_,
    int MapOptions_,
    typename StrideType_>
struct StdPointTraits<Eigen::Map<
    Eigen::Matrix<Scalar_, Rows_, Cols_, Options_, MaxRows_, MaxCols_>,
    MapOptions_,
    StrideType_>>
    : internal::EigenPointTraits<Eigen::Map<
          Eigen::Matrix<Scalar_, Rows_, Cols_, Options_, MaxRows_, MaxCols_>,
          MapOptions_,
          StrideType_>> {};

//! \brief EigenL1 metric for measuring the Taxicab or Manhattan distance
//! between points.
template <typename Scalar_>
class EigenL1 {
 public:
  //! \brief This tag specifies the supported space by this metric.
  using SpaceTag = EuclideanSpaceTag;
  using ScalarType = Scalar_;

  //! \brief Calculates the distance between points \p p0 and \p p1.
  template <typename Derived0, typename Derived1>
  inline ScalarType operator()(
      Eigen::MatrixBase<Derived0> const& p0,
      Eigen::MatrixBase<Derived1> const& p1) const {
    return (p0 - p1).cwiseAbs().sum();
  }

  //! \brief Calculates the difference between two coordinates.
  inline ScalarType operator()(ScalarType const x, ScalarType const y) const {
    return std::abs(x - y);
  }

  //! \brief Returns the absolute value of \p x.
  inline ScalarType operator()(ScalarType const x) const { return std::abs(x); }
};

//! \brief The EigenL2Squared semimetric measures squared Euclidean distances
//! between points.
template <typename Scalar_>
class EigenL2Squared {
 public:
  //! \brief This tag specifies the supported space by this metric.
  using SpaceTag = EuclideanSpaceTag;
  using ScalarType = Scalar_;

  //! \brief Calculates the distance between points \p p0 and \p p1.
  template <typename Derived0, typename Derived1>
  inline ScalarType operator()(
      Eigen::MatrixBase<Derived0> const& p0,
      Eigen::MatrixBase<Derived1> const& p1) const {
    return (p0 - p1).squaredNorm();
  }

  //! \brief Calculates the difference between two coordinates.
  inline ScalarType operator()(ScalarType const x, ScalarType const y) const {
    ScalarType const d = x - y;
    return d * d;
  }

  //! \brief Returns the squared value of \p x.
  inline ScalarType operator()(ScalarType const x) const { return x * x; }
};

}  // namespace pico_tree


// Copyright 2010-2013 The Trustees of Indiana University.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:

// 1. Redistributions of source code must retain the above copyright notice, this
//    list of conditions and the following disclaimer.
// 2. 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 OWNER 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.
//
//  Authors: Jeremiah Willcock
//           Andrew Lumsdaine

#ifndef BOOST_DISTRIBUTED_ERDOS_RENYI_GENERATOR_HPP
#define BOOST_DISTRIBUTED_ERDOS_RENYI_GENERATOR_HPP

#include "splittable_ecuyer1988.hpp"
#include <boost/iterator.hpp>
#include <boost/random/geometric_distribution.hpp>
#include <boost/random/uniform_01.hpp>

namespace boost {
  template <typename VertexDistribution, typename Generator = boost::random::splittable_ecuyer1988, typename VertexType = size_t>
  class distributed_erdos_renyi_iterator:
          public boost::iterator_facade<distributed_erdos_renyi_iterator<VertexDistribution, Generator, VertexType>,
                                        std::pair<VertexType, VertexType>,
                                        std::input_iterator_tag,
                                        std::pair<VertexType, VertexType> >
  {
    VertexDistribution owner;
    typedef size_t rank_type;
    rank_type my_rank;
    typename Generator::split_iterator_pair generators; // One for each source vertex
    Generator current_generator;
    VertexType current_source, current_target, nverts;
    bool is_undirected, allow_self_loops;
    boost::geometric_distribution<VertexType> distrib;

    void next_source_vertex() {
      // fprintf(stderr, "next_source_vertex()\n");
      ++current_source;
      current_target = (VertexType)(-1);
      ++generators.first;
      if (generators.first != generators.second) {
        current_generator = *generators.first;
      }
    }

    void step_to_owned_vertex() {
      // Find a source that I own
      while (current_source != nverts &&
             rank_type(owner(current_source)) != my_rank) {
        next_source_vertex();
      }
    }

    void raw_increment() { // Does not always produce a valid vertex
      if (current_source == nverts) return;
      boost::random::uniform_01_wrapper<Generator> wr(current_generator);
      VertexType delta = distrib(wr);
      // fprintf(stderr, "delta = %zu\n", delta);
      // First test is to deal with overflows, plus handle that the current
      // target might be -1 and so delta == nverts would be valid
      if (delta > nverts || current_target + delta >= nverts) {
        next_source_vertex();
        step_to_owned_vertex();
        // Return target as -1, so raw_increment() will be repeated
      } else {
        current_target += delta;
      }
      // fprintf(stderr, "New loc is (%zu, %zu)\n", current_source, current_target);
    }

    public:
    distributed_erdos_renyi_iterator(): owner(), my_rank(0), current_source(0), current_target((VertexType)(-1)), nverts(0) {}

    distributed_erdos_renyi_iterator(Generator& gen, VertexType nverts, double prob, bool is_undirected, bool allow_self_loops, VertexDistribution owner, rank_type my_rank): owner(owner), my_rank(my_rank), current_source(0), current_target((VertexType)(-1)), nverts(nverts), is_undirected(is_undirected), allow_self_loops(allow_self_loops), distrib(1. - prob) {
      generators = gen.split_off_n(nverts);
      raw_increment();
    }

    std::pair<VertexType, VertexType> dereference() const {
      return std::make_pair(current_source, current_target);
    }

    bool equal(const distributed_erdos_renyi_iterator& o) const {
      // Only really works on end iterators or those generated with the same parameters
      return (nverts - current_source) == (o.nverts - o.current_source) && current_target == o.current_target;
    }

    void increment() {
      do {
        raw_increment();
      } while (current_source != nverts &&
               (current_target == (VertexType)(-1) ||
                (!allow_self_loops && current_source == current_target) ||
                (is_undirected && current_target > current_source)));
    }
  };

}

#endif // BOOST_DISTRIBUTED_ERDOS_RENYI_GENERATOR_HPP


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014   MetaScale SAS
//
//          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 <nt2/table.hpp>
#include <nt2/include/functions/plus.hpp>
#include <nt2/include/functions/multiplies.hpp>
#include <boost/fusion/include/at.hpp>
#include <vector>

#include <nt2/sdk/bench/benchmark.hpp>
#include <nt2/sdk/bench/metric/gflops.hpp>
#include <nt2/sdk/bench/protocol/max_duration.hpp>
#include <nt2/sdk/bench/setup/geometric.hpp>
#include <nt2/sdk/bench/setup/constant.hpp>
#include <nt2/sdk/bench/setup/combination.hpp>
#include <nt2/sdk/bench/stats/median.hpp>

using namespace nt2::bench;
using namespace nt2;

template<typename T> struct axpy_nt2
{
  template<typename Setup>
  axpy_nt2(Setup const& s)
              :  alpha(boost::fusion::at_c<1>(s))
              ,  size_(boost::fusion::at_c<0>(s))
  {
    X.resize(nt2::of_size(size_)); Y.resize(nt2::of_size(size_));
    for(std::size_t i = 1; i<=size_; ++i) X(i) = Y(i) = T(i-1);
  }

  void operator()()
  {
    Y = alpha * X + Y;
  }

  friend std::ostream& operator<<(std::ostream& os, axpy_nt2<T> const& p)
  {
    return os << "(" << p.size() << ")";
  }

  std::size_t size() const { return size_; }
  double flops() const { return 2.*size_; }

private:
  T alpha;
  std::size_t size_;
  nt2::table<T> X, Y;
};

NT2_REGISTER_BENCHMARK_TPL( axpy_nt2, NT2_SIMD_REAL_TYPES )
{
  std::size_t size_min = args("size_min", 16);
  std::size_t size_max = args("size_max", 4096*8);
  std::size_t size_step = args("size_step", 2);
  T alpha = args("alpha", 1.);

  run_during_with< axpy_nt2<T> > ( 1.
                                , and_( geometric(size_min,size_max,size_step)
                                      , constant(alpha)
                                      )
                                , gflops<stats::median_>()
                                );
}


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

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

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

#include <boost/simd/function/scalar/bitwise_cast.hpp>
#include <boost/simd/function/scalar/shift_left.hpp>
#include <boost/simd/function/scalar/shr.hpp>
#include <boost/simd/function/scalar/unary_minus.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( rshr_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::scalar_< bd::arithmetic_<A0> >
                          , bd::scalar_< bd::integer_<A1> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0, A1 a1) const BOOST_NOEXCEPT
    {
      return (a1>0) ? shr(a0, a1): shift_left(a0, -a1);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( rshr_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::scalar_< bd::arithmetic_<A0> >
                          , bd::scalar_< bd::unsigned_<A1> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0, A1 a1) const BOOST_NOEXCEPT { return  shr(a0, a1); }
  };

  BOOST_DISPATCH_OVERLOAD ( rshr_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >, bd::scalar_< bd::integer_<A1> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0, A1 a1) const BOOST_NOEXCEPT
    {
      using i_t = bd::as_integer_t<A0, signed>;
      return bitwise_cast<A0>(rshr(bitwise_cast<i_t>(a0),a1));
    }
  };
} } }

#endif


#include <boost/numeric/interval/arith.hpp>


//
// This file is subject to the terms and conditions defined in
// file 'LICENSE.txt', which is part of this source code package.
//
// Principal developer : Felipe Bordeu (Felipe.Bordeu@ec-nantes.fr)
//

#include "iostream"
#include <cstring>      // Needed for memset

#include <cmath>

#include <algorithm>

//#include <Eigen/Dense>
//#typedef Matrix<double, Dynamic, Dynamic> MatrixXd;

#include <signal.h>

#include "MatLab.h"
#include "NetWork.h"
#include "cRecompactCore.h"

int main( int argc, char**argv ){
    signal(SIGCHLD, SIG_IGN);
    std::cout << "*****************************" << std::endl;
    std::cout << "* cRecompact routine        *" << std::endl;
    std::cout << "* By Felipe Bordeu          *" << std::endl;
    std::cout << "* GeM Ecole Centrale Nantes *" << std::endl;
    std::cout << "* 2013                      *" << std::endl;
    std::cout << "*****************************" << std::endl;

    std::cout << "Connecting to port 5556" << std::endl;
    NetWork mynet("5556");

    pid_t ID;
    while(true) {

        struct sockaddr_storage their_addr;
        std::cout << "Waiting for a client..." << std::endl;
        int new_sd = mynet.Accept(their_addr);
        ID = fork();

        if(ID == 0) {

            std::cout << "Waiting to recieve data..."  << std::endl;
            MatLabDataMatrix<double> flag;

            flag.recv(new_sd);
            while(flag(0,0) >0 ) {

                // we recive a option structure

                MatLabDataMatrix<double> ops;
                bool opt = false;
                if(flag(0,0) > 1) {
                  std::cout << "reciving options" << std::endl;
                  opt = true;
                  ops.recv(new_sd);
                  for(int ii=0; ii< 9 ; ++ii){
                    std::cout << "op " << ii << " " << ops(0+ii,0)<< std::endl;
                  }
                }

                std::cout << "waintig for a job " << std::endl;
                MatLabDataMatrix<double> NumberOfDims;
                NumberOfDims.recv(new_sd);
                std::cout << "Number of Dimensions : " <<   NumberOfDims(0,0) << std::endl;

                std::vector<MatLabDataMatrix<double> >FF;
                FF.resize(NumberOfDims(0,0));
                for (unsigned i =0; i < NumberOfDims(0,0); ++i) {
                    std::cout << "reciving data " << i << "..";
                    FF[i].recv(new_sd);
                    std::cout << ".. Done" << std::endl;
                }
                std::cout << "Data in C " << std::endl;
                std::vector<MatLabDataMatrix<double> > sol;
                if(NumberOfDims(0,0)!=0){
                    PGD_Options options;

                    if(opt){
                      options.reweight_modes       = ops(0+0,0);
                      options.fp_tol               = ops(0+1,0);
                      options.res_reduc            = ops(0+2,0);
                      options.max_added_modes      = ops(0+3,0);
                      options.fp_max_iter          = ops(0+4,0);
                      options.improve_modes        = ops(0+5,0);
                      options.improve_modes_max    = ops(0+6,0);
                      options.verbose              = ops(0+7,0);
                      options.lastImproveModesLoop = ops(0+8,0);
                    }else {
                      options.fp_tol = 1e-8;
                      options.res_reduc = 1e-8;
                      options.improve_modes_max = std::max(10, (int) round(FF[0].dsizes[1]/10) );
                      options.max_added_modes = FF[0].dsizes[1];
                    }
                    //sleep(10);
                    Recompact(FF, sol,options);
                    }

                for (unsigned i =0; i < NumberOfDims(0,0); ++i) {
                    sol[i].send(new_sd);
                }

                // zero to quit or 1 to compress a new
                flag.recv(new_sd);
            }
            std::cout << "Clossing connection" << std::endl;
            close(new_sd);
            break;
        } else {
          std::cout << " Runing new process for client id " << ID << std::endl;
          close(new_sd);
        }
    }
};


/* Copyright (C) 2010-2019, The Regents of The University of Michigan.
 All rights reserved.

 This software was developed as part of the The Vulcan project in the Intelligent Robotics Lab
 under the direction of Benjamin Kuipers, kuipers@umich.edu. Use of this code is governed by an
 MIT-style License that can be found at "https://github.com/h2ssh/Vulcan".
*/

/**
 * \file     particle_filter.cpp
 * \author   Collin Johnson
 *
 * Definition of ParticleFilter and create_particle_filter() factory.
 */

#include "hssh/metrical/localization/particle_filter.h"
#include "core/angle_functions.h"
#include "hssh/metrical/data.h"
#include "hssh/metrical/localization/debug_info.h"
#include "hssh/metrical/localization/motion_model.h"
#include "hssh/metrical/localization/observation_model.h"
#include "hssh/metrical/localization/particle_filter_utils.h"
#include "hssh/metrical/localization/particle_sampler.h"
#include "hssh/metrical/localization/sample_set_distribution_calculator.h"
#include "hssh/metrical/occupancy_grid.h"
#include "laser/laser_scan_lines.h"
#include "utils/timestamp.h"
#include <cassert>
#include <gnuplot-iostream.h>
#include <iostream>

#include <future>
#include <thread>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/median.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/variance.hpp>

// #define DEBUG_POSTERIOR_CALCULATION
// #define DEBUG_MEAN
// #define DEBUG_COVARIANCE
// #define DEBUG_UNCERTAINTY
// #define DEBUG_SAMPLING

namespace vulcan
{
namespace hssh
{

void display_posterior_weight_statistics(const std::vector<particle_t>& posterior);

ParticleFilter::ParticleFilter(const particle_filter_params_t& params,
                               std::unique_ptr<MotionModel> motion,
                               std::unique_ptr<ObservationModel> observation,
                               std::unique_ptr<ParticleSampler> sampler,
                               std::unique_ptr<SampleSetDistributionCalculator> calcuator)
: isInitialUpdate(true)
, kldSampling(params.kldParams)
, motionModel(std::move(motion))
, observationModel(std::move(observation))
, sampler(std::move(sampler))
, calculator(std::move(calcuator))
, previousTime(0)
, currentTime(0)
, params(params)
{
    deltaOut.open("estimate_diff.log");
}

ParticleFilter::~ParticleFilter(void)
{
}

pose_distribution_t ParticleFilter::initializeFilterAtPose(const pose_t& pose)
{
    // Initialize filter with uniformly weighted particles drawn from pose distribution
    double sampleWeight = 1.0 / params.initialNumSamples;

    currentPose = createDistributionForResetPose(pose);
    currentPose.uncertainty.prepareForSampling();

    prior.samples.clear();

    particle_t newParticle;

    for (std::size_t n = 0; n < params.initialNumSamples; ++n) {
        auto sampledPose = currentPose.uncertainty.sample();
        newParticle.pose = pose_t(sampledPose(0), sampledPose(1), sampledPose(2));
        newParticle.parent = newParticle.pose;
        newParticle.weight = sampleWeight;

        prior.samples.push_back(newParticle);
    }

    prior.gaussianApproximation = currentPose.uncertainty;
    isInitialUpdate = true;

    return currentPose;
}

pose_distribution_t ParticleFilter::initializeWithSampleSet(const std::vector<particle_t>& samples)
{
    prior.samples = samples;
    for (auto& s : prior.samples) {
        s.weight = 1.0 / prior.samples.size();
    }

    currentPose = calculate_sample_set_distribution(samples);
    prior.gaussianApproximation = currentPose.uncertainty;

    // Ensure the parents are set for all samples
    for (auto& sample : prior.samples) {
        sample.parent = sample.pose;
    }

    isInitialUpdate = true;

    return currentPose;
}

pose_distribution_t ParticleFilter::updateFilter(const metric_slam_data_t& data,
                                                 const OccupancyGrid& map,
                                                 particle_filter_debug_info_t* debug)
{
    if (debug) {
        debug->particles.clear();
        debug->particleScores.clear();
        debug->scan = data.laser;
    }

    if (isInitialUpdate) {
        calculateInitialParticleWeights(data.scanLines, map, debug);
        isInitialUpdate = false;
        previousTime = data.endTime;
    }

    currentTime = data.endTime;

    // Only update the particles if motion was detected. If the robot didn't move, then
    // obviously don't do anything.
    bool hasRobotMoved =
      motionModel->calculateUpdateMotion(data, prior.gaussianApproximation, data.endTime - data.startTime);

    if (hasRobotMoved)   // || params.shouldLocalizeWhenNoMotionDetected)
    {
        calculateProposalDistribution(motionModel->getMotionDistribution());
        observationModel->initializeModel(data.scanLines, proposalDistribution);
        calculatePosteriorDistribution(data.scanLines, map, debug);

        assert(posterior.samples.size() > 1);

        posterior.gaussianApproximation = calculator->calculateGaussianForSamples(posterior.samples);
        prior = posterior;
    }

    previousTime = currentTime;

    if (debug) {
        debug->particles = posterior.samples;
        debug->proposalDistribution = proposalDistribution;
    }

#ifdef DEBUG_SAMPLING
    display_posterior_weight_statistics(posterior.samples);
#endif

    //     double xDiff = posterior.gaussianApproximation[0] - currentPose.x;
    //     double yDiff = posterior.gaussianApproximation[1] - currentPose.y;
    //     double thetaDiff = angle_diff(posterior.gaussianApproximation[2], currentPose.theta);
    //     double trans = std::sqrt(xDiff*xDiff + yDiff*yDiff);
    //     double rot = thetaDiff;
    //
    //     double odomTrans = std::sqrt(std::pow(motionModel->getMotionDistribution()[0], 2.0)
    //         + std::pow(motionModel->getMotionDistribution()[1], 2.0));
    //     double odomRot = motionModel->getMotionDistribution()[2];
    //
    //     sumXDiff += xDiff - motionModel->getMotionDistribution()[0];
    //     sumYDiff += yDiff - motionModel->getMotionDistribution()[1];
    //     sumThetaDiff += angle_diff(thetaDiff, motionModel->getMotionDistribution()[2]);
    //     sumTransDiff += trans - odomTrans;
    //     sumRotDiff += rot - odomRot;
    //     ++numUpdates;
    //
    //     if(std::abs(rot - odomRot) > M_PI / 18)
    //     {
    //         assert(!"Orientation jumped!");
    //     }
    //
    //     deltaOut << numUpdates << ' '
    //         << (xDiff - motionModel->getMotionDistribution()[0]) << ' '
    //         << (yDiff - motionModel->getMotionDistribution()[1]) << ' '
    //         << (thetaDiff - motionModel->getMotionDistribution()[2]) << ' '
    //         << (trans - odomTrans) << ' '
    //         << (rot - odomRot) << ' '
    //         << (sumXDiff / numUpdates) << ' '
    //         << (sumYDiff / numUpdates) << ' '
    //         << (sumThetaDiff / numUpdates) << ' '
    //         << (sumTransDiff / numUpdates) << ' '
    //         << (sumRotDiff / numUpdates) << '\n';
    //
    //     std::cout << "Avg diff: (" << (sumXDiff / numUpdates) << ", " << (sumYDiff / numUpdates) << ", "
    //         << (sumThetaDiff / numUpdates) << ") Trans:" << (sumTransDiff / numUpdates) << " Rot:"
    //         << (sumRotDiff / numUpdates) << " Num updates:" << numUpdates << '\n';

    // If the pose jumped, fall back on odometry, but keep the same uncertainty
    //     if(
    //         (std::abs(currentPose.x - posterior.gaussianApproximation[0]) > 0.25) ||
    //         (std::abs(currentPose.y - posterior.gaussianApproximation[1]) > 0.25) ||
    //         (posterior.gaussianApproximation(0, 0) > 0.1) ||
    //         (posterior.gaussianApproximation(1, 1) > 0.1))
    //     {
    //         std::cout << "\n\nERROR: Localization jumped: Cov:" << posterior.gaussianApproximation.getCovariance()
    //             << "\n\n\nFalling back on odometry";
    //
    //         pose_t odomPose = motionModel->movePoseByMean(currentPose.toPose());
    //         currentPose.x = odomPose.x;
    //         currentPose.y = odomPose.y;
    //         currentPose.theta = odomPose.theta;
    //         posterior.gaussianApproximation[0] = odomPose.x;
    //         posterior.gaussianApproximation[1] = odomPose.y;
    //         posterior.gaussianApproximation[2] = odomPose.theta;
    //     }
    //     else
    //     {
    assert(posterior.gaussianApproximation.dimensions() == 3);

    currentPose.x = posterior.gaussianApproximation[0];
    currentPose.y = posterior.gaussianApproximation[1];
    currentPose.theta = posterior.gaussianApproximation[2];
    currentPose.uncertainty = posterior.gaussianApproximation;
    //     }

    currentPose.uncertainty = posterior.gaussianApproximation;
    currentPose.timestamp = data.endTime;

    assert(currentPose.timestamp);

#ifdef DEBUG_UNCERTAINTY
    Matrix covariance = currentPose.uncertainty.getCovariance();

    float stdX = std::sqrt(covariance(0, 0));
    float stdY = std::sqrt(covariance(1, 1));
    float stdTheta = std::sqrt(covariance(2, 2));

    std::cout << "INFO: ParticleFilter: Mean:" << currentPose.toPose() << " Std:(" << stdX << ',' << stdY << ','
              << stdTheta << ")\n";
#endif

    return currentPose;
}

void ParticleFilter::changeReferenceFrame(const pose_t& referenceFrame)
{
    for (auto& sample : prior.samples) {
        sample.pose = sample.pose.transformToNewFrame(referenceFrame);
    }

    pose_t transformedMean = pose_t(currentPose).transformToNewFrame(referenceFrame);

    currentPose.x = transformedMean.x;
    currentPose.y = transformedMean.y;
    currentPose.theta = transformedMean.theta;

    // HACK: Not quite correct because the covariance would need to be transformed by the Jacobian of the rotation
    // matrix but should be fine for just the single update the will happen before the new reference frame has kicked
    // fully into gear
    Vector meanVector = currentPose.uncertainty.getMean();
    meanVector(0) = transformedMean.x;
    meanVector(1) = transformedMean.y;
    meanVector(2) = transformedMean.theta;
    currentPose.uncertainty.setDistributionStatistics(meanVector, currentPose.uncertainty.getCovariance());
}

void ParticleFilter::calculatePosteriorDistribution(const laser::laser_scan_lines_t& scan,
                                                    const OccupancyGrid& map,
                                                    particle_filter_debug_info_t* debug)
{
    assert(prior.gaussianApproximation.dimensions() == 3);

    particle_t meanSample(prior.gaussianApproximation[0],
                          prior.gaussianApproximation[1],
                          prior.gaussianApproximation[2]);

    kldSampling.startNewCalculation(meanSample);
    prior.gaussianApproximation.prepareForSampling();

    proposal.samples.clear();
    posterior.samples.clear();

    int samplesDrawn = 0;
    int samplesToDraw = 0;

    do {
        samplesToDraw = std::min(kldSampling.currentNumberOfSamplesNeeded(), params.maxSamplesDrawn) - samplesDrawn;

        sampler->drawSamplesFromPrior(samplesToDraw, prior, proposal.samples);

        for (int n = samplesDrawn; n < samplesDrawn + samplesToDraw; ++n) {
            posterior.samples.push_back(processNewSample(proposal.samples[n], scan, map, debug));
            posterior.samples.back().id = n;
        }

        samplesDrawn += samplesToDraw;
    } while (!kldSampling.haveDrawnEnoughSamples(samplesDrawn) && (samplesToDraw > 0));

    //     std::vector<particle_t> samples;
    //     prior.gaussianApproximation.prepareForSampling();
    //
    //     int numSamples = params.kldParams.minSamples;
    //     const int kMaxThreads = 6;
    //     const int kNumThreads = kMaxThreads; //std::min(kMaxThreads, (numSamples / kMaxThreads) + 1);
    //
    //     int samplesPerThread = numSamples;// / kNumThreads; // Add one to the positions/thread to account for
    //     truncating due to integer division
    //                                                     // The last thread will take up to 3 fewer isovist
    //                                                     calculations
    //     numSamples = samplesPerThread * kNumThreads;
    //     samples.resize(numSamples);
    //     using SampleIter = std::vector<particle_t>::iterator;
    //     auto sampleFunc = [&](SampleIter begin, SampleIter end) {
    //         std::vector<particle_t> sampled;
    //         sampled.reserve(samplesPerThread);
    //         sampler->drawSamplesFromPrior(samplesPerThread, prior, sampled);
    //         for(int n = 0; n < samplesPerThread; ++n)
    //         {
    //             *begin = processNewSample(sampled[n], scan, map, nullptr);
    //             begin->id = n;
    //             ++begin;
    //         }
    //
    //         assert(begin == end);
    //     };
    //
    //     // Launch the threads
    //     std::vector<std::future<void>> asyncSamples;
    //     for(int n = 0; n < kNumThreads; ++n)
    //     {
    //         int start = samplesPerThread * n;
    //         int end = samplesPerThread * (n+1);
    //         assert(end <= numSamples);
    //
    //         asyncSamples.push_back(std::async(std::launch::async,
    //                                         sampleFunc,
    //                                         samples.begin()+start,
    //                                         samples.begin()+end));
    //     }
    //
    //     // Wait for all the samples to finish
    //     for(auto& f : asyncSamples)
    //     {
    //         f.get();
    //     }
    //
    //     for(auto& sample : samples)
    //     {
    //         posterior.samples.push_back(sample);
    //     }
    //
    //     std::cout << "Drew " << posterior.samples.size() << " samples.\n";

    //     std::vector<double> weights(posterior.samples.size());
    //     std::transform(posterior.samples.begin(), posterior.samples.end(), weights.begin(), [](const particle_t& p) {
    //         return p.weight;
    //     });
    //     std::sort(weights.begin(), weights.end(), std::greater<double>());
    //
    //     static Gnuplot plot;
    //     double cutoff = weights.front() * 0.9;
    //     auto cutoffIt = std::find_if(weights.begin(), weights.end(), [cutoff](double w) { return w < cutoff; });
    //     int numGood = std::distance(weights.begin(), cutoffIt);
    //     plot << "unset arrow\n";
    //     plot << "set xrange [0:500]\n";
    //     plot << "set arrow from " << numGood << ",0 to " << numGood << ',' << cutoff <<" nohead\n";
    //     plot << "set arrow from 0," << cutoff << " to " << numGood << ',' << cutoff << " nohead\n";
    //     plot << "set xtics (" << numGood << ' ' << numGood << ")\n";
    //     plot << "set ytics (" << cutoff << ' ' << cutoff << ")\n";
    //     plot << "plot '-' with lines title 'Particle Weights', " << cutoff << " title '" << numGood << "'\n";
    // //     plot[scan.scan.laserId] << "plot '-' with lines title 'Dist Weights', '-' with lines title 'Line
    // Weights'\n";
    //     plot.send1d(weights);
    //     plot[scan.scan.laserId].send1d(lineWeights);
    //     plot[scan.scan.laserId].send1d(weights);

    normalize_sample_weights(posterior.samples);

#ifdef DEBUG_SAMPLING
    std::cout << "INFO: ParticleFilter: Drew " << samplesDrawn << " samples\n";
#endif
}

void ParticleFilter::calculateInitialParticleWeights(const laser::laser_scan_lines_t& scan,
                                                     const OccupancyGrid& map,
                                                     particle_filter_debug_info_t* debug)
{
    observationModel->initializeModel(scan, currentPose.uncertainty);

    for (auto& sample : prior.samples) {
        sample.weight = observationModel->sampleLikelihood(sample, scan, map, debug);
    }

    normalize_sample_weights(prior.samples);
    currentPose = calculate_sample_set_distribution(prior.samples);

    posterior = prior;
}

void ParticleFilter::calculateProposalDistribution(const MultivariateGaussian& motionTransform)
{
    Matrix rotation(3, 3);

    rotation.zeros();
    rotation(0, 0) = std::cos(currentPose.theta);
    rotation(0, 1) = -std::sin(currentPose.theta);
    rotation(1, 0) = std::sin(currentPose.theta);
    rotation(1, 1) = std::cos(currentPose.theta);
    rotation(2, 2) = 1.0;

    Vector proposalMean = currentPose.uncertainty.getMean() + rotation * motionTransform.getMean();
    Matrix proposalCovariance =
      currentPose.uncertainty.getCovariance() + arma::trans(rotation) * motionTransform.getCovariance() * rotation;

    proposalMean(2) = wrap_to_pi(proposalMean(2));
    proposalDistribution.setDistributionStatistics(proposalMean, proposalCovariance);
}

particle_t ParticleFilter::processNewSample(const particle_t& parent,
                                            const laser::laser_scan_lines_t& scan,
                                            const OccupancyGrid& map,
                                            particle_filter_debug_info_t* debug)
{
    auto newSample = motionModel->moveSample(parent);

    if (newSample.pose.timestamp == parent.parent.timestamp) {
        std::cout << "Same time for pose and parent!\n";
    }

    newSample.parent.timestamp = previousTime;
    newSample.pose.timestamp = currentTime;

    // Very unlikely the robot is in an unobserved cell
    newSample.weight = observationModel->sampleLikelihood(newSample, scan, map, debug);
    //     kldSampling.addSample(newSample);

#ifdef DEBUG_POSTERIOR_CALCULATION
    std::cout << "INFO: ParticleFilter: sample:" << newSample.pose << " weight:" << newSample.weight << '\n';
#endif

    return newSample;
}

pose_distribution_t ParticleFilter::createDistributionForResetPose(const pose_t& pose)
{
    Vector mean(3);
    Matrix cov(3, 3);

    cov.zeros(3, 3);

    mean(0) = pose.x;
    mean(1) = pose.y;
    mean(2) = pose.theta;

    cov(0, 0) = params.positionInitialVariance;
    cov(1, 1) = params.positionInitialVariance;
    cov(2, 2) = params.orientationInitialVariance;

    return MultivariateGaussian(mean, cov);
}

void display_posterior_weight_statistics(const std::vector<particle_t>& posterior)
{
    using namespace boost::accumulators;
    accumulator_set<double, stats<tag::mean, tag::variance, tag::max, tag::min, tag::median>> statsAcc;

    for (auto& particle : posterior) {
        statsAcc(particle.weight);
    }

    std::cout << "Particle weights stats: Mean:" << mean(statsAcc) << " Var:" << variance(statsAcc)
              << " Min:" << min(statsAcc) << " Max:" << max(statsAcc) << " Med:" << median(statsAcc) << '\n';
}

}   // namespace hssh
}   // namespace vulcan


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG, www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also tools/license/license.mtl.txt in the distribution.

#ifndef MTL_VEC_EXP_INCLUDE
#define MTL_VEC_EXP_INCLUDE

#include <boost/numeric/mtl/vector/map_view.hpp>
#include <boost/numeric/mtl/matrix/map_view.hpp>

namespace mtl {

    namespace vec {

	/// Exponential of \a v element-wise
	template <typename Vector>
	exp_view<Vector> exp(const Vector& v)
	{
	    return exp_view<Vector>(v);
	}

#      ifdef MTL_WITH_MATH_ELEVEN
	/// Binary exponential of \a v element-wise
	template <typename Vector>
	exp2_view<Vector> exp2(const Vector& v)
	{
	    return exp2_view<Vector>(v);
	}
#      endif

	/// Decimal exponential of \a v element-wise
	template <typename Vector>
	exp10_view<Vector> exp10(const Vector& v)
	{
	    return exp10_view<Vector>(v);
	}

    } // namespace vec

    namespace mat {

	/// Exponential of \a A element-wise
	template <typename Matrix>
	exp_view<Matrix> exp(const Matrix& A)
	{
	    return exp_view<Matrix>(A);
	}

    } // namespace mat

} // namespace mtl

#endif // MTL_VEC_EXP_INCLUDE


#include <sm/eigen/gtest.hpp>
#include <aslam/backend/Optimizer.hpp>
#include <aslam/backend/OptimizationProblem.hpp>
#include <aslam/backend/ErrorTerm.hpp>
#include <boost/ptr_container/ptr_vector.hpp>
#include <aslam/backend/test/ErrorTermTestHarness.hpp>
#include <aslam/backend/test/SampleDvAndError.hpp>

TEST(CallbackTestSuite, testCallback)
{
  try {
    using namespace aslam::backend;
    boost::shared_ptr<OptimizationProblem> problem_ptr(new OptimizationProblem);
    OptimizationProblem& problem = *problem_ptr;
    const int P = 3;
    const int E = 2;
    // Add some design variables.
    std::vector< boost::shared_ptr<Point2d> > p2d;
    for (int p = 0; p < P; ++p) {
      boost::shared_ptr<Point2d> point(new Point2d(Eigen::Vector2d::Random()));
      p2d.push_back(point);
      problem.addDesignVariable(point);
      point->setBlockIndex(p);
      point->setActive(true);
    }
    // Add some error terms.
    std::vector< boost::shared_ptr<LinearErr> > errorTerms;
    for (int p = 0; p < P; ++p) {
      for (int e = 0; e < E; ++e) {
        boost::shared_ptr<LinearErr> err(new LinearErr(p2d[p].get()));
        errorTerms.push_back(err);
        problem.addErrorTerm(err);
      }
    }

    // Now let's optimize.
    OptimizerOptions options;
    options.verbose = false;
    options.linearSolver = "dense";
    options.levenbergMarquardtLambdaInit = 10;
    options.doSchurComplement = false;
    options.doLevenbergMarquardt = false;
    options.maxIterations = 3;
    Optimizer optimizer(options);
    optimizer.setProblem(problem_ptr);

    using namespace callback;
    int countInit = 0, countResUpdate = 0, countCostUpdate = 0, countDesignUpdate = 0;
    double startJ, lastUpdatedJ, expectedCost;
    optimizer.callback().add<event::OPTIMIZATION_INITIALIZED>(
        [&](const Event & arg) {
          countInit ++;
          lastUpdatedJ = startJ = arg.currentCost;
          ASSERT_EQ(typeid(event::OPTIMIZATION_INITIALIZED), typeid(arg));
        }
      );
    optimizer.callback().add<event::DESIGN_VARIABLES_UPDATED>(
        [&](const Event & arg) {
          countDesignUpdate ++;
          ASSERT_EQ(typeid(event::DESIGN_VARIABLES_UPDATED), typeid(arg));
        }
      );
    optimizer.callback().add<event::RESIDUALS_UPDATED>(
        [&](const event::RESIDUALS_UPDATED & arg) {
          countResUpdate ++;
          expectedCost = 0;
          for(auto && e : errorTerms){
            expectedCost += e->getSquaredError();
          }
          ASSERT_EQ(typeid(event::RESIDUALS_UPDATED), typeid(arg));
        }
      );
    optimizer.callback().add<event::COST_UPDATED>(
        [&](const event::COST_UPDATED & arg) { // void with argument
          countCostUpdate ++;
          ASSERT_DOUBLE_EQ(expectedCost, arg.currentCost);
          lastUpdatedJ = arg.currentCost;
          ASSERT_EQ(typeid(event::COST_UPDATED), typeid(arg));
        }
      );
    optimizer.callback().add({typeid(event::COST_UPDATED), typeid(event::COST_UPDATED)}, // register twice
        [&](const Event &) { // void with argument
          countCostUpdate ++;
        }
      );
    optimizer.callback().add<event::COST_UPDATED>(
        [&](const Event &) { // ProceedInstruction with argument
          countCostUpdate ++;
          return ProceedInstruction::CONTINUE;
        }
      );
    optimizer.callback().add<event::COST_UPDATED>(
        [&]() { // ProceedInstruction without argument
          countCostUpdate ++;
          return ProceedInstruction::CONTINUE;
        }
      );

    auto ret = optimizer.optimize();

    ASSERT_EQ(1, countInit);
    EXPECT_EQ(ret.iterations + 1, countResUpdate);
    EXPECT_EQ((ret.iterations + 1) * 5, countCostUpdate);
    EXPECT_EQ(ret.iterations, countDesignUpdate);
    EXPECT_EQ(ret.JStart, startJ);
    EXPECT_EQ(ret.JFinal, lastUpdatedJ);
  } catch (const std::exception& e) {
    FAIL() << e.what();
  }
}


/*
// Copyright (c) 2000-2009, Texas Engineering Experiment Station (TEES), a
// component of the Texas A&M University System.

// All rights reserved.

// The information and source code contained herein is the exclusive
// property of TEES and may not be disclosed, examined or reproduced
// in whole or in part without explicit written authorization from TEES.
*/

#ifndef STAPL_CONTAINERS_GRAPH_MESH_IMPLICIT_REGULAR_MESH_HPP
#define STAPL_CONTAINERS_GRAPH_MESH_IMPLICIT_REGULAR_MESH_HPP

#include <stapl/runtime.hpp>
#include <stapl/containers/graph/partitioners/regular_spatial_decomposition.hpp>
#include <stapl/containers/sequential/graph/graph_util.h>
#include <stapl/containers/sequential/graph/adj_list_vertex_edge.h>
#include <stapl/utility/tuple.hpp>
#include <utility>
#include <stapl/containers/graph/mesh/implicit_regular_mesh_domain.hpp>
#include <stapl/containers/graph/mesh/geom_vector.hpp>
#include <boost/iterator/counting_iterator.hpp>
#include <boost/iterator/transform_iterator.hpp>
#include <boost/bind.hpp>
#include <boost/function.hpp>
#include <stapl/containers/graph/views/graph_view.hpp>
#include <stapl/views/metadata/extraction/generic.hpp>
#include <stapl/containers/iterators/container_iterator.hpp>
#include <stapl/containers/iterators/container_accessor.hpp>

namespace stapl {

//////////////////////////////////////////////////////////////////////
/// @brief Cell face of a regular mesh.
/// @tparam Dim mesh dimension.
/// @todo put in detail namespace.
//////////////////////////////////////////////////////////////////////
template<int Dim>
class regular_face_property
{
public:
  typedef geom_vector<Dim, double>   gvector_t;
  typedef gvector_t                  normal_type;

private:
  normal_type m_normal;

public:
  regular_face_property(void) = default;

  //////////////////////////////////////////////////////////////////////
  /// @param norm cell face normal.
  //////////////////////////////////////////////////////////////////////
  regular_face_property(normal_type const& norm)
    : m_normal(norm)
  { }

  normal_type get_normal(void)
  { return m_normal; }

  void define_type(typer& t)
  { t.member(m_normal); }

}; // class regular_face_property

//////////////////////////////////////////////////////////////////////
/// @brief Functor that computes the index of a neighboring cell and
///        the direction in which it lies from the cell provided.
/// @tparam DOM mesh domain type.
/// @tparam N mesh dimension.
/// @todo put in detail namespace.
//////////////////////////////////////////////////////////////////////
template <typename DOM,int N>
struct regular_mesh_cell_neighbor
{
  typedef typename DOM::tuple_type tuple_type;
  typedef typename regular_face_property
      <tuple_size<tuple_type>::value>::normal_type   normal_type;
  typedef std::pair<tuple_type, normal_type> result_type;

  //////////////////////////////////////////////////////////////////////
  /// @param dom mesh domain.
  /// @param t1 cell index.
  /// @param idx neighbor index
  /// @return pair with neighboring cell position and normal.
  //////////////////////////////////////////////////////////////////////
  result_type operator()(DOM const& dom,
                         tuple_type const& t1, size_t idx) const
  {
    normal_type normal;
    tuple_type tuple_tmp(t1);
    /*Try positive direction of axis N*/
    get<N>(tuple_tmp) = get<N>(tuple_tmp)+1;
    if (idx==0)
    { //if it is the good neighbor to return
      normal.template set<N>(1);
      if (dom.contains(tuple_tmp))
      //if neighbor exists, return it
        return result_type(tuple_tmp, normal);
      else
      //if neighbor is boundary, return myself
        return result_type(t1, normal);
    }
    else
      idx--;

    /*Try negative direction of axis N*/
    if (idx==0)
    { //if it is the good neighbor to return
      normal.template set<N>(-1);
      if (static_cast<int>(get<N>(tuple_tmp))-2<0)
      //if neighbor is boundary (negative position), return myself
        return result_type(t1, normal);
      get<N>(tuple_tmp) = get<N>(tuple_tmp)-2;
      if (dom.contains(tuple_tmp))
      //if neighbor exists, return it
        return result_type(tuple_tmp, normal);
      else
      //if neighbor is boundary, return myself
        return result_type(t1, normal);
    }
    else
      idx--;
    return regular_mesh_cell_neighbor<DOM,N-1>()(dom,t1,idx);
  }
};

//////////////////////////////////////////////////////////////////////
/// @brief Functor getting a cell neighbor.
/// @tparam DOM mesh domain type.
/// @todo put in detail namespace.
///
/// Template specialization to stop the recursion in finding the cell
/// neighbor.
//////////////////////////////////////////////////////////////////////
template <typename DOM>
struct regular_mesh_cell_neighbor<DOM,-1>
{
  typedef typename DOM::tuple_type tuple_type;
  typedef typename regular_face_property
      <tuple_size<tuple_type>::value>::normal_type   normal_type;
  typedef std::pair<tuple_type, normal_type> result_type;
  result_type operator()(const DOM &dom,
                         tuple_type const& t1, size_t idx) const
  {
    return result_type();
  }
};

//////////////////////////////////////////////////////////////////////
/// @brief Regular mesh edge functor creating the edges of a cell on-the-fly.
/// @tparam Dom mesh domain type.
/// @todo put in detail namespace.
//////////////////////////////////////////////////////////////////////
template<typename DOM>
struct regular_mesh_edge_func
{
public:
  typedef typename DOM::tuple_type tuple_type;
  typedef size_t                                             vertex_descriptor;
  typedef edge_descriptor_impl<vertex_descriptor>              edge_descriptor;
  typedef regular_face_property<tuple_size<tuple_type>::value> edge_property;
  typedef sequential::graph_property_edge<vertex_descriptor,
                                          edge_property>       edge_type;
  typedef edge_type result_type;

private:
  DOM m_dom;

public:
  //////////////////////////////////////////////////////////////////////
  /// @param domain mesh domain.
  //////////////////////////////////////////////////////////////////////
  regular_mesh_edge_func(DOM const& domain)
    : m_dom(domain)
  { }

  //////////////////////////////////////////////////////////////////////
  /// @brief Returns number of edges of a cell.
  /// @return number of edges.
  //////////////////////////////////////////////////////////////////////
  size_t size(void) const
  {
    return 2*tuple_size<tuple_type>::value;
  }

  //////////////////////////////////////////////////////////////////////
  /// @brief Build a cell edge.
  /// @param parent index of source cell of the edge.
  /// @param idx neighbor index.
  /// @return cell edge.
  //////////////////////////////////////////////////////////////////////
  edge_type operator() (size_t const& parent, size_t const& idx) const
  {
    typedef regular_mesh_cell_neighbor<DOM,tuple_size<tuple_type>::value-1>
                                                  regular_mesh_cell_neighbor_t;
    typedef typename regular_mesh_cell_neighbor_t::result_type result_t;
    tuple_type parent_pos = m_dom.reverse_linearize(parent);
    result_t result = regular_mesh_cell_neighbor<
                                        DOM, tuple_size<tuple_type>::value-1>()
                                                      (m_dom, parent_pos, idx);

    return edge_type(edge_descriptor(parent, m_dom.linearize(result.first)),
                                                 edge_property(result.second));
  }

  void define_type(typer& t)
  { t.member(m_dom); }
};

//////////////////////////////////////////////////////////////////////
/// @brief Implicit mesh cell built on-the-fly.
/// @tparam EdgeGenFunc edge generator functor type.
/// @todo put in detail namespace.
//////////////////////////////////////////////////////////////////////
template<typename EdgeGenFunc>
class implicit_vertex
{
public:
  typedef EdgeGenFunc                             edge_functor;
  typedef size_t                                  vertex_descriptor;
  typedef properties::no_property                 property_type;
  typedef property_type                           property_reference;
  typedef typename edge_functor::edge_property    edge_property;
  typedef typename edge_functor::edge_descriptor  edge_descriptor;
  typedef typename edge_functor::edge_type        edge_type;
  typedef boost::transform_iterator<
            boost::function<
               edge_type (size_t const&)>,
            boost::counting_iterator<
               vertex_descriptor> >               adj_edge_iterator;
  typedef const adj_edge_iterator                 const_adj_edge_iterator;

private:
  vertex_descriptor m_descriptor;
  EdgeGenFunc       m_f;

public:
  //////////////////////////////////////////////////////////////////////
  /// @param d vertex descriptor.
  /// @param f edge generator functor.
  //////////////////////////////////////////////////////////////////////
  implicit_vertex(size_t const& d, EdgeGenFunc f)
    : m_descriptor(d), m_f(f)
  { }

  vertex_descriptor descriptor(void) const
  {
    return m_descriptor;
  }

  //////////////////////////////////////////////////////////////////////
  /// @brief Returns an edge iterator on the first edge of the vertex.
  /// @return edge iterator on the first edge of the vertex.
  //////////////////////////////////////////////////////////////////////
  adj_edge_iterator begin(void)
  {
    return boost::make_transform_iterator(boost::counting_iterator<
                                            vertex_descriptor>(0),
                                          boost::bind(m_f, m_descriptor, _1));
  }
  //////////////////////////////////////////////////////////////////////
  /// @brief Returns an edge iterator after the last edge of the vertex.
  /// @return edge iterator after the last edge of the vertex.
  //////////////////////////////////////////////////////////////////////
  adj_edge_iterator end(void)
  {
    return boost::make_transform_iterator(boost::counting_iterator<
                                            vertex_descriptor>(m_f.size()),
                                          boost::bind(m_f, m_descriptor, _1));
  }

  const_adj_edge_iterator begin(void) const
  {
    return boost::make_transform_iterator(boost::counting_iterator<
                                            vertex_descriptor>(0),
                                          boost::bind(m_f, m_descriptor, _1));
  }

  const_adj_edge_iterator end(void) const
  {
    return boost::make_transform_iterator(boost::counting_iterator<
                                            vertex_descriptor>(m_f.size()),
                                          boost::bind(m_f, m_descriptor, _1));
  }

  //////////////////////////////////////////////////////////////////////
  /// @brief Returns the number of edges of the vertex.
  /// @return number of edges of the vertex.
  //////////////////////////////////////////////////////////////////////
  size_t size(void) const
  {
    return m_f.size();
  }

  void define_type(typer& t)
  {
    t.member(m_descriptor);
    t.member(m_f);
  }
};

template<typename EdgeGenFunc, typename Accessor>
class proxy<implicit_vertex<EdgeGenFunc>, Accessor>
  : public Accessor
{
private:
  friend class proxy_core_access;
  typedef implicit_vertex<EdgeGenFunc> target_t;

public:
  typedef typename target_t::edge_functor        edge_functor;
  typedef typename target_t::vertex_descriptor   vertex_descriptor;
  typedef typename target_t::property_type       property_type;
  typedef typename target_t::property_reference  property_reference;
  typedef typename target_t::edge_property       edge_property;
  typedef typename target_t::edge_descriptor     edge_descriptor;
  typedef typename target_t::edge_type           edge_type;
  typedef typename target_t::adj_edge_iterator   adj_edge_iterator;
  typedef typename target_t::const_adj_edge_iterator const_adj_edge_iterator;

  explicit proxy(Accessor const& acc)
    : Accessor(acc)
  { }

  operator target_t(void) const
  { return Accessor::read(); }

  vertex_descriptor descriptor(void) const
  { return Accessor::read().descriptor(); }

  adj_edge_iterator begin(void)
  { return Accessor::read().begin(); }

  adj_edge_iterator end(void)
  { return Accessor::read().end(); }

  const_adj_edge_iterator begin(void) const
  { return Accessor::read().begin(); }

  const_adj_edge_iterator end(void) const
  { return Accessor::read().end(); }

  size_t size(void) const
  { return Accessor::read().size();}
};

//////////////////////////////////////////////////////////////////////
/// @brief Functor to set up the mesh decomposition, partitioning the
///        mesh along the dimension with the highest number of cells.
/// @tparam N mesh dimension-1.
/// @todo put in detail namespace.
//////////////////////////////////////////////////////////////////////
template<int N>
struct decompose_largest_dim
{
  //////////////////////////////////////////////////////////////////////
  /// @param sizes size of the mesh in each dimension.
  /// @param max_value maximum number of cells among the dimensions.
  /// @param num_parts number of partitions for decomposition.
  /// @param decomp number of partitions in each dimension.
  //////////////////////////////////////////////////////////////////////
  template<typename Tuple1, typename Tuple2>
  void operator()(Tuple1 const& sizes,
                  size_t const& max_value,
                  size_t const& num_parts,
                  Tuple2& decomp)
  {
    if (get<N>(sizes) == max_value) {
      get<N>(decomp) = num_parts;
      return;
    }
    decompose_largest_dim<N-1>()(sizes, max_value, num_parts, decomp);
  }
};

//////////////////////////////////////////////////////////////////////
/// @brief Functor to set up the mesh decomposition, partitioning the
///        mesh along the dimension with the highest number of cells.
/// @todo put in detail namespace.
///
/// Template specialization to stop the recursion
//////////////////////////////////////////////////////////////////////
template<>
struct decompose_largest_dim<-1>
{
  template<typename Tuple1, typename Tuple2>
  void operator()(Tuple1 const& sizes,
                  size_t const& max_value,
                  size_t const& num_parts,
                  Tuple2& decomp)
  { }
};

template<typename DI>
class flat_container;

namespace detail {

//////////////////////////////////////////////////////////////////////
/// @brief Functor returning the element passed in constructor.
/// @todo Use general identity
//////////////////////////////////////////////////////////////////////
template<typename T>
class mesh_identity
{
private:
  T const& m_t;

public:
  typedef T result_type;

  mesh_identity(T const& t)
   : m_t(t)
  { }

  template<typename Q>
  T const& operator()(Q const&) const
  { return m_t; }
};

} // namespace detail

//////////////////////////////////////////////////////////////////////
/// @brief Functor returning the locality metadata information of the
///        mesh container.
/// @tparam C container type.
/// @todo There should probably be a custom domain extractor
///       for regular meshes
/// @todo put in detail namespace.
//////////////////////////////////////////////////////////////////////
template<typename C>
struct implicit_regular_mesh_metadata
{
  typedef C                                                    container_type;
  typedef detail::extract_domain<C>                            get_dom_t;
  typedef typename get_dom_t::result_type                      domain_type;
  typedef typename domain_type::index_type                     index_type;
  typedef typename container_type::tuple_type                  tuple_type;
  typedef C                                                    component_type;

  typedef metadata_entry<domain_type, component_type*, size_t> dom_info_type;

  typedef metadata::flat_container<dom_info_type>              md_cont_type;
  typedef std::pair<bool, md_cont_type*>                       return_type;

  //////////////////////////////////////////////////////////////////////
  /// @param c pointer to the container.
  ///
  /// @return a pair indicating if the metadata container is static and
  ///   balance distributed and a pointer to the metadata container.
  //////////////////////////////////////////////////////////////////////
  return_type operator()(C* c) const
  {
    const size_t n = c->get_num_locations();

    tuple_type decomp = tuple_ops::transform(
      tuple_type(), detail::mesh_identity<size_t>(1)
    );

    tuple_type local_sizes = get_dom_t()(c).get_local_sizes();

    size_t max_value = tuple_ops::fold(local_sizes,size_t(0),max<size_t>());

    decompose_largest_dim<tuple_size<tuple_type>::value-1>()
      (get_dom_t()(c).get_local_sizes(), max_value, n, decomp);

    spatial_regular_domain_decompose<domain_type>
      npart(get_dom_t()(c), decomp);

    size_t num_part = npart.size();

    md_cont_type* out_part = new md_cont_type(num_part);

    if (num_part>0)
    {
      if (out_part->get_location_id() < num_part)
      {
        size_t id = out_part->get_location_id();

        (*out_part)[id] =
          dom_info_type(
            id, npart[id], NULL,
            LQ_CERTAIN, get_affinity(),
            c->get_rmi_handle(), c->get_location_id()
          );
      }
    }

    return std::make_pair(true, out_part);
  }
}; // struct implicit_regular_mesh_metadata

template <typename Container>
struct implicit_regular_mesh_distribution
  : public p_object
{
private:
  typedef detail::extract_domain<Container>                   get_dom_t;

  typename get_dom_t::result_type&                            m_domain;

public:
  typedef typename get_dom_t::result_type::tuple_type         tuple_type;
  typedef typename get_dom_t::result_type                     domain_type;
  typedef Container                                           component_type;
  typedef metadata_entry<domain_type, component_type*, size_t>   dom_info_type;

  // partition_type defined only to satisfy is_fixed_size_md
  typedef balanced_partition<domain_type>                     partition_type;

  implicit_regular_mesh_distribution(domain_type& domain)
    : m_domain(domain)
  { }

  domain_type const& domain(void) const
  {
    return m_domain;
  }

  future<dom_info_type> metadata_at(size_t gid)
  {
    const size_t n  = this->get_num_locations();
    const size_t id = this->get_location_id();

    tuple_type decomp = tuple_ops::transform(
      tuple_type(), detail::mesh_identity<size_t>(1)
    );

    tuple_type local_sizes = m_domain.get_local_sizes();

    size_t max_value = tuple_ops::fold(local_sizes, size_t(0), max<size_t>());

    decompose_largest_dim<tuple_size<tuple_type>::value-1>()
                             (m_domain.get_local_sizes(),max_value, n, decomp);

    spatial_regular_domain_decompose<domain_type> npart(m_domain, decomp);

    size_t num_part = npart.size();

    for (size_t i = 0; i != num_part; ++i)
    {
      domain_type ndom(npart[i]);

      if (ndom.contains(gid))
      {
        return make_ready_future(dom_info_type(
                 id, ndom, nullptr,
                 LQ_CERTAIN, get_affinity(),
                 this->get_rmi_handle(), this->get_location_id()));
      }
    }

    abort("GID not found in distributed implicit regular mesh domain");

    return make_ready_future(dom_info_type(
             id, npart[0], nullptr,
             LQ_CERTAIN, get_affinity(),
             this->get_rmi_handle(), this->get_location_id()));
  }
}; // struct implicit_regular_mesh_distribution

//////////////////////////////////////////////////////////////////////
/// @brief Implicit container representing a regular mesh.
/// @tparam N mesh dimension.
//////////////////////////////////////////////////////////////////////
template<int N>
struct implicit_regular_mesh_container
  : public p_object
{
  typedef implicit_regular_mesh_container<N>            this_type;
  typedef implicit_regular_mesh_distribution<this_type> distribution_type;
  typedef implicit_regular_mesh_metadata<this_type>     loc_dist_metadata;
  typedef typename size_tuple<N>::type                  tuple_type;
  typedef implicit_regular_mesh_domain<tuple_type>      domain_type;
  typedef domain_type                                   closed_domain_type;
  typedef regular_mesh_edge_func<domain_type>           edge_gen_func_type;
  typedef implicit_vertex< edge_gen_func_type >         value_type;
  typedef value_type                                    reference;
  typedef const value_type                              const_reference;
  typedef reference                                     vertex_reference;
  typedef typename vertex_reference::adj_edge_iterator  adj_edge_iterator;
  typedef typename vertex_reference::const_adj_edge_iterator
                                                        const_adj_edge_iterator;
  typedef typename vertex_reference::property_type      vertex_property;
  typedef typename vertex_reference::edge_property      edge_property;
  typedef typename vertex_reference::edge_descriptor    edge_descriptor;
  typedef typename vertex_reference::vertex_descriptor  vertex_descriptor;
  typedef size_t                                        gid_type;
  typedef container_accessor<this_type>                 accessor_type;
  typedef container_iterator<this_type, accessor_type,
                      std::random_access_iterator_tag>  iterator;

private:
  domain_type       m_dom;
  distribution_type m_dist;

public:
  //////////////////////////////////////////////////////////////////////
  /// @param size size of the mesh in each dimension.
  //////////////////////////////////////////////////////////////////////
  implicit_regular_mesh_container(tuple_type const& size)
    : m_dom(size), m_dist(m_dom)
  { }

  distribution_type& distribution(void)
  {
    return m_dist;
  }

  //////////////////////////////////////////////////////////////////////
  /// @brief Returns cell whose id is @ref idx.
  /// @param idx cell id.
  /// @return cell.
  //////////////////////////////////////////////////////////////////////
  value_type operator[](gid_type const& idx) const
  {
    return value_type(idx, edge_gen_func_type(m_dom));
  }

  //////////////////////////////////////////////////////////////////////
  /// @brief Returns cell located @ref n positions after cell id
  ///        @ref idx in the domain.
  /// @param idx cell id.
  /// @param n number of positions.
  /// @return cell.
  //////////////////////////////////////////////////////////////////////
  value_type advance(gid_type const& idx, long const& n)
  {
    return value_type(m_dom.advance(idx, n), edge_gen_func_type(m_dom));
  }

  //////////////////////////////////////////////////////////////////////
  /// @brief Returns a cell iterator on the first cell of the mesh.
  /// @return cell iterator on the first cell of the mesh.
  //////////////////////////////////////////////////////////////////////
  iterator begin(void)
  {
    return iterator(this, m_dom.first());
  }

  //////////////////////////////////////////////////////////////////////
  /// @brief Returns a cell iterator after the last cell of the mesh.
  /// @return cell iterator after the last cell of the mesh.
  //////////////////////////////////////////////////////////////////////
  iterator end(void)
  {
    return iterator(this, m_dom.open_last());
  }

  //////////////////////////////////////////////////////////////////////
  /// @brief Returns a cell iterator on cell whose id is @ref gid.
  /// @param gid mesh cell id.
  /// @return cell iterator on cell whose id is @ref gid.
  //////////////////////////////////////////////////////////////////////
  iterator make_iterator(gid_type const& gid)
  {
    return iterator(this, gid);
  }

  //////////////////////////////////////////////////////////////////////
  /// @brief Creates an iterator to the specified gid, which is restricted
  /// to the provided domain.
  /// @param domain The domain to which the iterator will be restricted.
  /// @param gid The gid of the vertex to which the iterator will point.
  /// @return The iterator pointing to the specified gid, restricted to the
  /// given domain.
  //////////////////////////////////////////////////////////////////////
  iterator make_iterator(domain_type const& domain, gid_type const& gid)
  {
    return iterator(this,domain,gid);
  }

  //////////////////////////////////////////////////////////////////////
  /// @brief Returns cell whose id is @ref idx.
  /// @param idx cell id.
  /// @return cell.
  //////////////////////////////////////////////////////////////////////
  value_type get_element(gid_type const& idx) const
  {
    return this->operator[](idx);
  }

  //////////////////////////////////////////////////////////////////////
  /// @brief Returns the version of the container.
  /// @return Version number of 0
  //////////////////////////////////////////////////////////////////////
  size_t version(void) const
  {
    return 0;
  }

  //////////////////////////////////////////////////////////////////////
  /// @brief Returns the number of the vertices of the mesh.
  /// @return number of the vertices of the mesh.
  //////////////////////////////////////////////////////////////////////
  size_t size(void) const
  {
    return m_dom.size();
  }

  //////////////////////////////////////////////////////////////////////
  /// @brief Returns the cell domain.
  //////////////////////////////////////////////////////////////////////
  domain_type domain(void) const
  {
    return m_dom;
  }
}; // struct implicit_regular_mesh_container

//////////////////////////////////////////////////////////////////////
/// @brief Helper struct to get the type of a graph view over a mesh.
//////////////////////////////////////////////////////////////////////
template<int N>
struct implicit_regular_mesh_view_type
{
  typedef graph_view<implicit_regular_mesh_container<N> > type;
};

//////////////////////////////////////////////////////////////////////
/// @brief Function creating an implicit regular mesh.
/// @tparam N mesh dimension.
/// @param size size of the mesh in each dimension.
/// @return graph view over the created mesh.
//////////////////////////////////////////////////////////////////////
template<int N>
typename implicit_regular_mesh_view_type<N>::type
make_implicit_regular_mesh_view
  (typename implicit_regular_mesh_container<N>::tuple_type const& size)
{
  return typename implicit_regular_mesh_view_type<N>::type
                                (new implicit_regular_mesh_container<N>(size));
}

} // namespace stapl

#endif // STAPL_CONTAINERS_GRAPH_MESH_IMPLICIT_REGULAR_MESH_HPP


#include <boost/graph/topology.hpp>


// Copyright (c) 2019, Torsten Sattler
// 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 <COPYRIGHT HOLDER> 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.
//
// author: Torsten Sattler, torsten.sattler.de@googlemail.com

#include <algorithm>
#include <cmath>
#include <cstddef>
#include <cstdint>
#include <limits>
#include <random>
#include <vector>

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <Eigen/Geometry>

#include "hybrid_line_estimator.h"

namespace ransac_lib {

HybridLineEstimator::HybridLineEstimator(
    const Eigen::Matrix2Xd& points, const Eigen::Matrix4Xd& points_with_normals,
    const std::vector<double>& prior_probabilities) {
  points_ = points;
  num_points_ = points_.cols();
  points_with_normals_ = points_with_normals;
  num_points_with_normals_ = points_with_normals_.cols();
  prior_probabilities_ = prior_probabilities;
}

void HybridLineEstimator::LeastSquares(
    const std::vector<std::vector<int>>& sample, Eigen::Vector3d* line) const {
  const int kNumSamplesPoints = static_cast<int>(sample[0].size());
  const int kNumSamplesPointsWithNormals = static_cast<int>(sample[1].size());
  const int kNumSamples = kNumSamplesPoints + kNumSamplesPointsWithNormals;

  if (kNumSamples < 6) return;
  // We fit the line by estimating the eigenvectors of the covariance matrix
  // of the data.
  Eigen::Vector2d mean(0.0, 0.0);
  for (int i = 0; i < kNumSamplesPoints; ++i) {
    mean += points_.col(sample[0][i]);
  }
  for (int i = 0; i < kNumSamplesPointsWithNormals; ++i) {
    mean += points_with_normals_.col(sample[1][i]).head<2>();
  }
  mean /= static_cast<double>(kNumSamples);

  // Builds the covariance matrix C.
  Eigen::Matrix2d C = Eigen::Matrix2d::Zero();

  for (int i = 0; i < kNumSamplesPoints; ++i) {
    Eigen::Vector2d d = points_.col(sample[0][i]) - mean;
    C += d * d.transpose();
  }
  for (int i = 0; i < kNumSamplesPointsWithNormals; ++i) {
    Eigen::Vector2d d = points_with_normals_.col(sample[1][i]).head<2>() - mean;
    C += d * d.transpose();
  }
  C /= static_cast<double>(kNumSamples - 1);

  Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eig_solver(C);
  if (eig_solver.info() != Eigen::Success) return;

  line->head<2>() = eig_solver.eigenvectors().col(1);

  // Re-estimates the translation along the line to account for subtraction
  // of mean.
  (*line)[2] = -line->head<2>().dot(mean);
}

// Evaluates the line on the i-th data point of the t-th data type.
double HybridLineEstimator::EvaluateModelOnPoint(const Eigen::Vector3d& line,
                                                 int t, int i) const {
  double residual = 0.0;
  if (t == 0) {
    residual = line.dot(points_.col(i).homogeneous());
  } else {
    residual = line.dot(points_with_normals_.col(i).head<2>().homogeneous());
  }
  return residual * residual;
}

int HybridLineEstimator::TwoPointSolver(
    const std::vector<int>& sample, std::vector<Eigen::Vector3d>* lines) const {
  lines->clear();
  if (sample.size() < 2u) return 0;

  lines->resize(1);
  Eigen::Vector3d p1(points_(0, sample[0]), points_(1, sample[0]), 1.0);
  Eigen::Vector3d p2(points_(0, sample[1]), points_(1, sample[1]), 1.0);
  (*lines)[0] = p1.cross(p2);
  // Normalizes the line such that the normal of the line has unit length.
  double normal_norm = (*lines)[0].head<2>().norm();
  if (normal_norm == 0.0) {
    lines->clear();
    return 0;
  }

  (*lines)[0] /= normal_norm;

  return 1;
}

int HybridLineEstimator::PointNormalSolver(
    const std::vector<int>& sample, std::vector<Eigen::Vector3d>* lines) const {
  lines->clear();
  if (sample.size() < 1u) return 0;

  lines->resize(1);
  Eigen::Vector2d normal = points_with_normals_.col(sample[0]).tail<2>();
  normal.normalize();
  double c = -normal.dot(points_with_normals_.col(sample[0]).head<2>());
  (*lines)[0] = Eigen::Vector3d(normal[0], normal[1], c);
  // Normalizes the line such that the normal of the line has unit length.
  double normal_norm = (*lines)[0].head<2>().norm();
  if (normal_norm == 0.0) {
    lines->clear();
    return 0;
  }

  (*lines)[0] /= normal_norm;

  return 1;
}

}  // namespace ransac_lib


/* Copyright (C) 2018-2019 Thomas Jespersen, TKJ Electronics. All rights reserved.
 *
 * This program is free software: you can redistribute it and/or modify it
 * under the terms of the MIT License
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 * See the MIT License for further details.
 *
 * Contact information
 * ------------------------------------------
 * Thomas Jespersen, TKJ Electronics
 * Web      :  http://www.tkjelectronics.dk
 * e-mail   :  thomasj@tkjelectronics.dk
 * ------------------------------------------
 */

#include <kugle_misc/QuaternionVelocityControl.h>

#include <math.h>
#include <stdlib.h>
#include <cmath>

#include <ros/ros.h>
#include <boost/thread/thread.hpp>
#include <kugle_misc/Quaternion.h>
#include <kugle_misc/FirstOrderLPF.h>

namespace kugle_misc {

QuaternionVelocityControl::QuaternionVelocityControl(double Rate, double ReferenceSmoothingTau) : rate_(Rate), dx_ref_filt_(1.0/Rate, ReferenceSmoothingTau), dy_ref_filt_(1.0/Rate, ReferenceSmoothingTau)
{
    ros::NodeHandle nParam("~");

	// Reset integral quaternion to the unit quaternion (no integral)
	q_tilt_integral_[0] = 1;
	q_tilt_integral_[1] = 0;
	q_tilt_integral_[2] = 0;
	q_tilt_integral_[3] = 0;

	// Publish to quaternion reference topic
    quaternionPub_ = nh_.advertise<geometry_msgs::Quaternion>("cmd_quaternion", 1000);

	// Subscribe to odometry topic
    odometrySub_ = nh_.subscribe("odom", 1000, &QuaternionVelocityControl::OdometryCallback, this);

    // Subscribe to velocity reference
    referenceSub_ = nh_.subscribe("cmd_vel", 1000, &QuaternionVelocityControl::VelocityReferenceCallback, this);

    // Subscribe to velocity reference
    referenceInertialSub_ = nh_.subscribe("cmd_vel_inertial", 1000, &QuaternionVelocityControl::VelocityReferenceInertialCallback, this);

    // Load parameters
    nParam.param("velocity_error_clamp", params_.VelocityErrorClamp, params_.VelocityErrorClamp);
    nParam.param("max_tilt", params_.MaxTilt, params_.MaxTilt);
    nParam.param("integral_gain", params_.IntegralGain, params_.IntegralGain);
    nParam.param("max_integral_correction", params_.MaxIntegralCorrection, params_.MaxIntegralCorrection);

    // Reset internal module states
    currentAttitude_.w = 1;
    currentAttitude_.x = 0;
    currentAttitude_.y = 0;
    currentAttitude_.z = 0;
    currentVelocity_.x = 0;
    currentVelocity_.y = 0;
    velocityRefGivenInHeadingFrame_ = true;
    desiredVelocity_.x = 0;
    desiredVelocity_.y = 0;
    desiredAngularYawVelocity_ = 0;
    lastReferenceUpdateTime_ = ros::Time::now();

    shouldExit_ = false;
    thread_ = boost::thread(boost::bind(&QuaternionVelocityControl::Thread, this));
}

QuaternionVelocityControl::~QuaternionVelocityControl()
{
    shouldExit_ = true;
    thread_.join();
}

void QuaternionVelocityControl::Thread()
{
    ros::Rate loop_rate(rate_); // 10 hz loop rate
    double dt = 1.0 / rate_;

    geometry_msgs::Quaternion quaternionMsg;
    double desiredYaw = 0;
    double q[4];
    double dxy[2];
    double velocityRef[2];
    double q_ref[4];

    while (ros::ok() && !shouldExit_)
    {
        q[0] = currentAttitude_.w;
        q[1] = currentAttitude_.x;
        q[2] = currentAttitude_.y;
        q[3] = currentAttitude_.z;

        dxy[0] = currentVelocity_.x; // notice that this velocity is in heading frame
        dxy[1] = currentVelocity_.y;

        velocityRef[0] = desiredVelocity_.x;
        velocityRef[1] = desiredVelocity_.y;

        desiredYaw += dt * desiredAngularYawVelocity_;

        Step(q, dxy, velocityRef, velocityRefGivenInHeadingFrame_, desiredYaw, dt, q_ref);

        quaternionMsg.w = q_ref[0];
        quaternionMsg.x = q_ref[1];
        quaternionMsg.y = q_ref[2];
        quaternionMsg.z = q_ref[3];

        quaternionPub_.publish(quaternionMsg);
        ros::spinOnce(); // walks the callback queue and calls registered callbacks for any outstanding events (incoming msgs, svc reqs, timers)
        loop_rate.sleep();
    }
}

void QuaternionVelocityControl::OdometryCallback(const nav_msgs::Odometry::ConstPtr& msg)
{
    //ROS_INFO_STREAM("Received odometry, translational velocity: (" << msg->twist.twist.linear.x << ", " << msg->twist.twist.linear.y << ")");
    currentAttitude_ = msg->pose.pose.orientation;
    currentVelocity_ = msg->twist.twist.linear; // notice that this velocity is in heading frame
}

void QuaternionVelocityControl::VelocityReferenceCallback(const geometry_msgs::Twist::ConstPtr& msg)
{
    desiredVelocity_ = msg->linear;
    desiredAngularYawVelocity_ = msg->angular.z;
    velocityRefGivenInHeadingFrame_ = true;
    lastReferenceUpdateTime_ = ros::Time::now();
}

void QuaternionVelocityControl::VelocityReferenceInertialCallback(const geometry_msgs::Twist::ConstPtr& msg)
{
    desiredVelocity_ = msg->linear;
    desiredAngularYawVelocity_ = msg->angular.z;
    velocityRefGivenInHeadingFrame_ = false;
    lastReferenceUpdateTime_ = ros::Time::now();
}

void QuaternionVelocityControl::Step(const double q[4], const double dxy[2], const double velocityRef[2], const bool velocityRefGivenInHeadingFrame, const double headingRef, const double dt, double q_ref_out[4])
{
	//const double Velocity_Inertial_q[4] = {0, dx_filt.sample(fmin(fmax(*dx, -CLAMP_VELOCITY), CLAMP_VELOCITY)), dy_filt.sample(fmin(fmax(*dy, -CLAMP_VELOCITY), CLAMP_VELOCITY)), 0};
	double Velocity_Heading_q[4] = {0, dxy[0], dxy[1], 0};
	double Velocity_Heading[2];

	double Velocity_Reference_Filtered[2];
	Velocity_Reference_Filtered[0] = dx_ref_filt_.Filter(velocityRef[0]);
	Velocity_Reference_Filtered[1] = dy_ref_filt_.Filter(velocityRef[1]);

	if (velocityRefGivenInHeadingFrame) { // reference given in heading frame
		// Calculate velocity error
        Velocity_Heading_q[1] -= Velocity_Reference_Filtered[0];
        Velocity_Heading_q[2] -= Velocity_Reference_Filtered[1];
	}
	else  // reference given in inertial frame
    {
        double Velocity_Ref_Inertial_q[4] = {0, Velocity_Reference_Filtered[0], Velocity_Reference_Filtered[1], 0};
        double Velocity_Ref_Heading_q[4];
        // Rotate reference from inertial frame to heading frame
        //Velocity_Ref_Heading = [0,1,0,0;0,0,1,0] * Phi(q)' * Gamma(q) * [0;Velocity_Ref;0];
        double tmp_q[4];
        Quaternion_Gamma(q, Velocity_Ref_Inertial_q, tmp_q); // Gamma(q) * [0;Velocity_Ref;0];
        Quaternion_PhiT(q, tmp_q, Velocity_Ref_Heading_q); // Phi(q)' * Gamma(q) * [0;velocityRef;0];

        Velocity_Heading_q[1] -= Velocity_Ref_Heading_q[1];
        Velocity_Heading_q[2] -= Velocity_Ref_Heading_q[2];
    }

    // OBS. The above could also be replaced with a Quaternion transformation function that takes in a 3-dimensional vector

	Velocity_Heading[0] = std::min(fmax(Velocity_Heading_q[1], -params_.VelocityErrorClamp), params_.VelocityErrorClamp);
	Velocity_Heading[1] = std::min(fmax(Velocity_Heading_q[2], -params_.VelocityErrorClamp), params_.VelocityErrorClamp);

	/*Velocity_Heading_Integral[0] += INTEGRAL_GAIN/_SampleRate * fmin(fmax(Velocity_Heading[0], -CLAMP_VELOCITY), CLAMP_VELOCITY); // INTEGRAL_GAIN * dt * velocity
	Velocity_Heading_Integral[1] += INTEGRAL_GAIN/_SampleRate * fmin(fmax(Velocity_Heading[1], -CLAMP_VELOCITY), CLAMP_VELOCITY);

	Velocity_Heading[0] += Velocity_Heading_Integral[0]; // correct with integral error
	Velocity_Heading[1] += Velocity_Heading_Integral[1];*/

	double q_heading[4]; // heading reference quaternion based on heading reference
	q_heading[0] = cos(headingRef/2);
	q_heading[1] = 0;
	q_heading[2] = 0;
	q_heading[3] = sin(headingRef/2);

	double normVelocity_Heading = sqrt(Velocity_Heading[0]*Velocity_Heading[0] + Velocity_Heading[1]*Velocity_Heading[1]);
	if (normVelocity_Heading < zero_velocity) {
		// Return upright quaternion (with heading reference) if we have no velocity, since we are then where we are supposed to be
		q_ref_out[0] = q_heading[0];
		q_ref_out[1] = q_heading[1];
		q_ref_out[2] = q_heading[2];
		q_ref_out[3] = q_heading[3];
		return;
	}

	//CorrectionDirection = [0, 1; -1, 0] * Velocity_Heading / normVelocity_Heading;
	double CorrectionDirection[2] = {Velocity_Heading[1] / normVelocity_Heading,
																 -Velocity_Heading[0] / normVelocity_Heading};

	// CorrectionAmountRadian = min(max((normVelocity_Heading / 5), -1), 1) * deg2rad(30); % max 5 m/s resulting in 30 degree tilt <= this is the proportional gain
	double CorrectionAmountRadian = std::min(std::max(normVelocity_Heading / params_.VelocityErrorClamp, -1.0), 1.0) * deg2rad(params_.MaxTilt);

	double q_tilt[4]; // tilt reference quaternion defined in heading frame
	q_tilt[0] = cos(CorrectionAmountRadian/2);
	q_tilt[1] = sin(CorrectionAmountRadian/2)*CorrectionDirection[0];
	q_tilt[2] = sin(CorrectionAmountRadian/2)*CorrectionDirection[1];
	q_tilt[3] = 0;

	if (velocityRef[0] > zero_velocity || velocityRef[1] > zero_velocity) {
		CorrectionAmountRadian = 0; // only do integral action when velocity reference is zero
	}

	double q_tilt_integral__incremental[4];
	q_tilt_integral__incremental[0] = cos(CorrectionAmountRadian/2 * params_.IntegralGain * dt);
	q_tilt_integral__incremental[1] = sin(CorrectionAmountRadian/2 * params_.IntegralGain * dt)*CorrectionDirection[0];
	q_tilt_integral__incremental[2] = sin(CorrectionAmountRadian/2 * params_.IntegralGain * dt)*CorrectionDirection[1];
	q_tilt_integral__incremental[3] = 0;

	double q_tilt_integral__old[4];
	for (int i = 0; i < 4; i++) q_tilt_integral__old[i] = q_tilt_integral_[i];

	Quaternion_Phi(q_tilt_integral__old, q_tilt_integral__incremental, q_tilt_integral_);
	Quaternion_AngleClamp(q_tilt_integral_, deg2rad(params_.MaxIntegralCorrection), q_tilt_integral_);

	double q_tilt_withintegral[4];
	Quaternion_Phi(q_tilt, q_tilt_integral_, q_tilt_withintegral);

	// Add heading to quaternion reference = q_heading o q_tilt
	Quaternion_Phi(q_heading, q_tilt_withintegral, q_ref_out);
}

} // end namespace kugle_misc