//   https://github.com/dune-community/dune-gdt
// Copyright 2010-2018 dune-gdt developers and contributors. All rights reserved.
// License: Dual licensed as BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)
//      or  GPL-2.0+ (http://opensource.org/licenses/gpl-license)
//          with "runtime exception" (http://www.dune-project.org/license.html)
// Authors:
//   Rene Milk      (2018)
//   Tobias Leibner (2017)

#ifndef DUNE_GDT_OPERATORS_FV_REALIZABILITY_HH
#define DUNE_GDT_OPERATORS_FV_REALIZABILITY_HH

#include "config.h"

#include <boost/iostreams/stream.hpp>
#include <boost/iostreams/device/null.hpp>

#include <dune/geometry/quadraturerules.hh>

#include <dune/xt/common/fvector.hh>

#include <dune/xt/grid/walker.hh>

#if HAVE_QHULL
#include <dune/xt/common/disable_warnings.hh>
#include <libqhullcpp/Qhull.h>
#include <libqhullcpp/QhullFacetList.h>
#include <dune/xt/common/reenable_warnings.hh>
#endif // HAVE_QHULL

#if HAVE_CLP
#include <coin/ClpSimplex.hpp>
#endif // HAVE_CLP

#include <dune/gdt/operators/fv/reconstruction/reconstructed_function.hh>
#include <dune/gdt/operators/interfaces.hh>
#include <dune/gdt/local/fluxes/entropybased.hh>

namespace Dune {
namespace GDT {

template <class AnalyticalFluxImp, class DiscreteFunctionImp, class BasisfunctionImp>
class LocalRealizabilityLimiterBase
    : public XT::Grid::Functor::Codim0<typename DiscreteFunctionImp::SpaceType::GridLayerType>
{
public:
  using AnalyticalFluxType = AnalyticalFluxImp;
  using DiscreteFunctionType = DiscreteFunctionImp;
  using BasisfunctionType = BasisfunctionImp;
  typedef typename DiscreteFunctionType::SpaceType::GridLayerType GridLayerType;
  typedef typename DiscreteFunctionType::EntityType EntityType;
  typedef typename GridLayerType::template Codim<0>::Geometry::LocalCoordinate DomainType;
  typedef typename DiscreteFunctionType::RangeType RangeType;
  typedef typename GridLayerType::IndexSet IndexSetType;
  typedef typename DiscreteFunctionType::RangeFieldType RangeFieldType;
  typedef typename DiscreteFunctionType::DomainFieldType DomainFieldType;
  static const size_t dimDomain = DiscreteFunctionType::dimDomain;
  static const size_t dimRange = DiscreteFunctionType::dimRange;
  typedef typename Dune::QuadratureRule<RangeFieldType, dimDomain> QuadratureType;
  using ReconstructedFunctionType =
      ReconstructedLocalizableFunction<GridLayerType, DomainFieldType, dimDomain, RangeFieldType, dimRange>;
  using EntropyFluxType = EntropyBasedLocalFlux<BasisfunctionType, GridLayerType, DiscreteFunctionType>;

  LocalRealizabilityLimiterBase(const AnalyticalFluxType& analytical_flux,
                                const DiscreteFunctionType& source,
                                ReconstructedFunctionType& reconstructed_function,
                                const BasisfunctionType& basis_functions,
                                const QuadratureType& quadrature,
                                const RangeFieldType epsilon,
                                const std::vector<RangeType>& basis_values,
                                const XT::Common::Parameter& param,
                                const std::string filename,
                                const RangeFieldType psi_vac)
    : analytical_flux_(analytical_flux)
    , source_(source)
    , reconstructed_function_(reconstructed_function)
    , basis_functions_(basis_functions)
    , quadrature_(quadrature)
    , epsilon_(epsilon)
    , basis_values_(basis_values)
    , param_(param)
    , filename_(filename)
    , psi_vac_(psi_vac)
    , u_vac_(basis_functions_.integrated() * psi_vac_ / 10.)
  {
    param_.set("boundary", {0.});
  }

  virtual ~LocalRealizabilityLimiterBase()
  {
  }

  void apply_limiter(const typename AnalyticalFluxType::EntityType& entity,
                     const RangeType theta_entity,
                     std::map<DomainType, RangeType, XT::Common::FieldVectorLess>& local_reconstructed_values,
                     const RangeType& u_bar,
                     bool add_epsilon = true)
  {
    assert(dynamic_cast<const EntropyFluxType*>(&analytical_flux_) != nullptr
           && "analytical_flux_ has to be derived from EntropyBasedLocalFlux");
    for (size_t ii = 0; ii < dimRange; ++ii) {
      auto theta_ii = add_epsilon ? theta_entity[ii] + epsilon_ : theta_entity[ii];
      if (theta_ii > 0.) {
        //        std::cout << "limited with theta: " << theta_ii << " and epsilon " << epsilon_ << std::endl;
        if (theta_ii > 1.)
          theta_ii = 1.;
        for (auto& pair : local_reconstructed_values) {
          auto& u_ii = pair.second[ii];
          u_ii = convex_combination(u_ii, u_bar[ii], theta_ii);
        }
      } // if (theta_ii > 0)
    } // ii

    for (auto& pair : local_reconstructed_values) {
      const auto x_in_inside_coords = entity.geometry().local(pair.first);
      auto& u = pair.second;
      const auto s = dynamic_cast<const EntropyFluxType*>(&analytical_flux_)
                         ->derived_local_function(entity)
                         ->get_alpha(x_in_inside_coords, u, param_, true, false)
                         .second;

      // if regularization was needed, we also need to replace u_n in that cell by its regularized version
      if (s > 0.) {
        if (!filename_.empty()) {
          static std::mutex outfile_lock;
          outfile_lock.lock();
          std::ofstream outfile(filename_, std::ios_base::app);
          outfile << param_.get("t")[0];
          for (size_t ii = 0; ii < dimDomain; ++ii)
            outfile << " " << entity.geometry().center()[ii];
          outfile << " " << s << " 1" << std::endl;
          outfile_lock.unlock();
        }
        const auto u_iso = dynamic_cast<const EntropyFluxType*>(&analytical_flux_)
                               ->basis_functions()
                               .calculate_isotropic_distribution(u)
                               .first;
        u = convex_combination(u, u_iso, s);
      } // if (s > 0)
    } // local_reconstructed_values
  }

  void apply_limiter(const typename AnalyticalFluxType::EntityType& entity,
                     const RangeFieldType theta_entity,
                     std::map<DomainType, RangeType, XT::Common::FieldVectorLess>& local_reconstructed_values,
                     const RangeType& u_bar,
                     bool add_epsilon = true)
  {
    assert(dynamic_cast<const EntropyFluxType*>(&analytical_flux_) != nullptr
           && "analytical_flux_ has to be derived from EntropyBasedLocalFlux");
    auto theta = add_epsilon ? theta_entity + epsilon_ : theta_entity;
    if (theta > 0.) {
      //      std::cout << "limited with theta: " << theta << " and epsilon " << epsilon_ << std::endl;
      if (theta > 1.)
        theta = 1.;
      for (auto& pair : local_reconstructed_values) {
        auto& u = pair.second;
        u = convex_combination(u, u_bar, theta);
      }
    }

    for (auto& pair : local_reconstructed_values) {
      const auto x_in_inside_coords = entity.geometry().local(pair.first);
      auto& u = pair.second;
      const auto s = dynamic_cast<const EntropyFluxType*>(&analytical_flux_)
                         ->derived_local_function(entity)
                         ->get_alpha(x_in_inside_coords, u, param_, true, false)
                         .second;

      // if regularization was needed, we also need to replace u_n in that cell by its regularized version
      if (s > 0.) {
        if (!filename_.empty()) {
          static std::mutex outfile_lock;
          outfile_lock.lock();
          std::ofstream outfile(filename_, std::ios_base::app);
          outfile << param_.get("t")[0];
          for (size_t ii = 0; ii < dimDomain; ++ii)
            outfile << " " << entity.geometry().center()[ii];
          outfile << " " << s << " 1" << std::endl;
          outfile_lock.unlock();
        }
        const auto u_iso = dynamic_cast<const EntropyFluxType*>(&analytical_flux_)
                               ->basis_functions()
                               .calculate_isotropic_distribution(u)
                               .first;
        u = convex_combination(u, u_iso, s);
      } // if (s > 0)
    } // local_reconstructed_values
  } // void apply_limiter(...)

protected:
  RangeType convex_combination(const RangeType& u, const RangeType& u_bar, const RangeFieldType& theta)
  {
    RangeType u_scaled = u;
    u_scaled *= 1 - theta;
    RangeType u_bar_scaled = u_bar;
    u_bar_scaled *= theta;
    return u_scaled + u_bar_scaled;
  }

  RangeFieldType convex_combination(const RangeFieldType& u, const RangeFieldType& u_bar, const RangeFieldType& theta)
  {
    return u_bar * theta + u * (1. - theta);
  }

  const AnalyticalFluxType& analytical_flux_;
  const DiscreteFunctionType& source_;
  ReconstructedFunctionType& reconstructed_function_;
  const BasisfunctionType& basis_functions_;
  const QuadratureType& quadrature_;
  const RangeFieldType epsilon_;
  const std::vector<RangeType>& basis_values_;
  XT::Common::Parameter param_;
  const std::string filename_;
  const RangeFieldType psi_vac_;
  const RangeType u_vac_;
};

template <class AnalyticalFluxImp, class DiscreteFunctionImp, class BasisfunctionImp = int>
class NonLimitingLocalRealizabilityLimiter
    : public LocalRealizabilityLimiterBase<AnalyticalFluxImp, DiscreteFunctionImp, BasisfunctionImp>
{
  using BaseType = LocalRealizabilityLimiterBase<AnalyticalFluxImp, DiscreteFunctionImp, BasisfunctionImp>;

public:
  using typename BaseType::EntityType;

  template <class... Args>
  NonLimitingLocalRealizabilityLimiter(Args&&... args)
    : BaseType(std::forward<Args>(args)...)
  {
  }

  void apply_local(const EntityType& entity)
  {
    auto& local_reconstructed_values = reconstructed_function_.local_values(entity);
    const auto u_bar = source_.local_function(entity)->evaluate(entity.geometry().local(entity.geometry().center()));
    BaseType::apply_limiter(entity, 0., local_reconstructed_values, u_bar, false);
  }

private:
  using BaseType::reconstructed_function_;
  using BaseType::source_;
}; // class NonLimitingLocalRealizabilityLimiter

template <class AnalyticalFluxImp, class DiscreteFunctionImp, class BasisfunctionImp>
class PositivityLocalRealizabilityLimiter
    : public LocalRealizabilityLimiterBase<AnalyticalFluxImp, DiscreteFunctionImp, BasisfunctionImp>
{
  using BaseType = LocalRealizabilityLimiterBase<AnalyticalFluxImp, DiscreteFunctionImp, BasisfunctionImp>;

public:
  using typename BaseType::GridLayerType;
  using typename BaseType::EntityType;
  using typename BaseType::DomainType;
  using typename BaseType::RangeType;
  using typename BaseType::RangeFieldType;
  using typename BaseType::QuadratureType;
  static const size_t dimDomain = BaseType::dimDomain;
  static const size_t dimRange = BaseType::dimRange;

  template <class... Args>
  PositivityLocalRealizabilityLimiter(Args&&... args)
    : BaseType(std::forward<Args>(args)...)
  {
  }

  void apply_local(const EntityType& entity)
  {
    auto& local_reconstructed_values = reconstructed_function_.local_values(entity);

    // get cell average
    const RangeType u_bar =
        source_.local_function(entity)->evaluate(entity.geometry().local(entity.geometry().center()));

    // vector to store thetas for each local reconstructed value
    RangeType thetas(0.);

    for (const auto& pair : local_reconstructed_values) {
      const auto& u = pair.second;
      for (size_t ii = 0; ii < u.size(); ++ii) {
        if (u[ii] >= u_bar[ii])
          continue;
        if (u_bar[ii] < u_vac_[ii])
          thetas[ii] = 1.;
        else if (u[ii] < u_vac_[ii])
          thetas[ii] = std::max(thetas[ii], (u_vac_[ii] - u[ii]) / (u_bar[ii] - u[ii]));
      } // ii
    } // ll

    BaseType::apply_limiter(entity, thetas, local_reconstructed_values, u_bar, false);
  } // void apply_local(...)

private:
  using BaseType::source_;
  using BaseType::reconstructed_function_;
  using BaseType::u_vac_;
}; // class PositivityLocalRealizabilityLimiter

template <class AnalyticalFluxImp, class DiscreteFunctionImp, class BasisfunctionImp>
class DgLocalRealizabilityLimiter
    : public LocalRealizabilityLimiterBase<AnalyticalFluxImp, DiscreteFunctionImp, BasisfunctionImp>
{
  using BaseType = LocalRealizabilityLimiterBase<AnalyticalFluxImp, DiscreteFunctionImp, BasisfunctionImp>;

public:
  using typename BaseType::AnalyticalFluxType;
  using typename BaseType::DiscreteFunctionType;
  using typename BaseType::EntityType;
  using typename BaseType::GridLayerType;
  using typename BaseType::QuadratureType;
  using typename BaseType::RangeType;
  using typename BaseType::RangeFieldType;
  using typename BaseType::ReconstructedFunctionType;
  using typename BaseType::BasisfunctionType;
  static const size_t dimDomain = BaseType::dimDomain;
  static const size_t dimRange = BaseType::dimRange;

  template <class... Args>
  DgLocalRealizabilityLimiter(Args&&... args)
    : BaseType(std::forward<Args>(args)...)
    , triangulation_(basis_functions_.triangulation())
  {
  }

  void apply_local(const EntityType& entity)
  {
    auto& local_reconstructed_values = reconstructed_function_.local_values(entity);

    // get cell average
    const RangeType u_bar =
        source_.local_function(entity)->evaluate(entity.geometry().local(entity.geometry().center()));

    // vector to store thetas for each local reconstructed value
    RangeType thetas(0.);

    for (const auto& pair : local_reconstructed_values) {
      const auto& u = pair.second;
      for (size_t ii = 0; ii < dimRange / 2; ++ii) {
        const auto& u0 = u[2 * ii];
        const auto& u1 = u[2 * ii + 1];
        const auto& ubar0 = u_bar[2 * ii];
        const auto& ubar1 = u_bar[2 * ii + 1];
        const auto& epsilon = u_vac_[2 * ii];
        const auto& vj = triangulation_[ii];
        const auto& vjplus1 = triangulation_[ii + 1];
        FieldVector<RangeFieldType, 3> thetas_ii;
        if (!is_epsilon_realizable(ubar0, ubar1, vj, vjplus1, epsilon)) {
          thetas[2 * ii] = 1.;
        } else {
          thetas_ii[0] = (epsilon - u0) / (ubar0 - u0);
          thetas_ii[1] = (u0 * vj - u1 + epsilon * std::sqrt(std::pow(vj, 2) + 1)) / ((ubar1 - u1) - (ubar0 - u0) * vj);
          thetas_ii[2] = (u0 * vjplus1 - u1 - epsilon * std::sqrt(std::pow(vjplus1, 2) + 1))
                         / ((ubar1 - u1) - (ubar0 - u0) * vjplus1);
          for (size_t kk = 0; kk < 3; ++kk)
            if (thetas_ii[kk] >= 0. && thetas_ii[kk] <= 1.)
              thetas[2 * ii] = std::max(thetas[2 * ii], thetas_ii[kk]);
        } // else (!realizable)
        thetas[2 * ii + 1] = thetas[2 * ii];
      } // ii
    } // local_reconstructed_values
    BaseType::apply_limiter(entity, thetas, local_reconstructed_values, u_bar, false);
  } // void apply_local(...)

private:
  bool is_epsilon_realizable(const RangeFieldType ubar0,
                             const RangeFieldType ubar1,
                             const RangeFieldType v0,
                             const RangeFieldType v1,
                             const RangeFieldType eps) const
  {
    bool ret = (ubar0 >= eps) && (ubar1 <= v1 * ubar0 - eps * std::sqrt(std::pow(v1, 2) + 1))
               && (v0 * ubar0 + eps * std::sqrt(std::pow(v0, 2) + 1) <= ubar1);
    return ret;
  }

  using BaseType::source_;
  using BaseType::reconstructed_function_;
  using BaseType::basis_functions_;
  using BaseType::u_vac_;
  typename BasisfunctionImp::TriangulationType triangulation_;
}; // class DgLocalRealizabilityLimiter

#if HAVE_QHULL

template <class AnalyticalFluxImp, class DiscreteFunctionImp, class BasisfunctionImp>
class ConvexHullLocalRealizabilityLimiter
    : public LocalRealizabilityLimiterBase<AnalyticalFluxImp, DiscreteFunctionImp, BasisfunctionImp>
{
  using BaseType = LocalRealizabilityLimiterBase<AnalyticalFluxImp, DiscreteFunctionImp, BasisfunctionImp>;

public:
  using typename BaseType::GridLayerType;
  using typename BaseType::EntityType;
  using typename BaseType::DomainType;
  using typename BaseType::RangeType;
  using typename BaseType::RangeFieldType;
  using typename BaseType::QuadratureType;
  using typename BaseType::ReconstructedFunctionType;
  using typename BaseType::BasisfunctionType;
  static const size_t dimDomain = BaseType::dimDomain;
  static const size_t dimRange = BaseType::dimRange;
  typedef typename std::vector<std::pair<RangeType, RangeFieldType>> PlaneCoefficientsType;

  template <class... Args>
  ConvexHullLocalRealizabilityLimiter(Args&&... args)
    : BaseType(std::forward<Args>(args)...)
  {
    if (is_instantiated_)
      DUNE_THROW(InvalidStateException,
                 "This class uses several static variables to save its state between time "
                 "steps, so using several instances at the same time may result in undefined "
                 "behavior!");
    is_instantiated_ = true;
    if (!plane_coefficients_)
      calculate_plane_coefficients();
  }

  ~ConvexHullLocalRealizabilityLimiter()
  {
    is_instantiated_ = false;
  }

  void apply_local(const EntityType& entity)
  {
    auto& local_reconstructed_values = reconstructed_function_.local_values(entity);

    // get cell average
    const RangeType u_bar =
        source_.local_function(entity)->evaluate(entity.geometry().local(entity.geometry().center()));

    // vector to store thetas for each local reconstructed value
    std::vector<RangeFieldType> thetas(local_reconstructed_values.size(), -epsilon_);

    size_t ll = -1;
    for (const auto& pair : local_reconstructed_values) {
      ++ll;
      // rescale u_l, u_bar
      auto u_l = pair.second;
      auto u_bar_minus_u_l = u_bar - u_l;
      const auto factor = basis_functions_.realizability_limiter_max(u_l, u_bar);
      u_l /= factor;
      u_bar_minus_u_l /= factor;

      for (const auto& coeffs : *plane_coefficients_) {
        const RangeType& a = coeffs.first;
        const RangeFieldType& b = coeffs.second;
        RangeFieldType theta_li = (b - a * u_l) / (a * u_bar_minus_u_l);
        if (XT::Common::FloatCmp::le(theta_li, 1.))
          thetas[ll] = std::max(thetas[ll], theta_li);
      } // coeffs
    } // ll
    for (auto& theta : thetas)
      theta = std::min(epsilon_ + theta, 1.);

    auto theta_entity = *std::max_element(thetas.begin(), thetas.end());
    if (theta_entity > 0.) {
      for (auto& pair : local_reconstructed_values) {
        auto& u = pair.second;
        auto u_scaled = u;
        u_scaled *= (1 - theta_entity);
        auto u_bar_scaled = u_bar;
        u_bar_scaled *= theta_entity;
        u = u_scaled + u_bar_scaled;
      }
    }
  } // void apply_local(...)

private:
  // calculate half space representation of realizable set
  void calculate_plane_coefficients()
  {
    using orgQhull::Qhull;
    Qhull qhull;
    std::vector<FieldVector<RangeFieldType, dimRange>> points(quadrature_.size() + 1);
    points[0] = FieldVector<RangeFieldType, dimRange>(0);
    size_t ii = 1;
    for (const auto& quad_point : quadrature_)
      points[ii++] = basis_functions_.evaluate(quad_point.position());

    std::cout << "Starting qhull..." << std::endl;
    qhull.runQhull("Realizable set", int(dimRange), int(points.size()), &(points[0][0]), "Qt T1");
    std::cout << "qhull done" << std::endl;
    //    qhull.outputQhull("n");
    const auto facet_end = qhull.endFacet();
    plane_coefficients_ = std::make_shared<PlaneCoefficientsType>(qhull.facetList().count());
    ii = 0;
    for (auto facet = qhull.beginFacet(); facet != facet_end; facet = facet.next(), ++ii) {
      for (size_t jj = 0; jj < dimRange; ++jj)
        (*plane_coefficients_)[ii].first[jj] = *(facet.hyperplane().coordinates() + jj);
      (*plane_coefficients_)[ii].second = -facet.hyperplane().offset();
    }
  }

  using BaseType::basis_functions_;
  using BaseType::source_;
  using BaseType::reconstructed_function_;
  using BaseType::quadrature_;
  using BaseType::epsilon_;
  static bool is_instantiated_;
  static std::shared_ptr<PlaneCoefficientsType> plane_coefficients_;
}; // class ConvexHullLocalRealizabilityLimiter

template <class AnalyticalFluxImp, class DiscreteFunctionImp, class BasisfunctionImp>
bool ConvexHullLocalRealizabilityLimiter<AnalyticalFluxImp, DiscreteFunctionImp, BasisfunctionImp>::is_instantiated_ =
    false;

template <class AnalyticalFluxImp, class DiscreteFunctionImp, class BasisfunctionImp>
std::shared_ptr<typename ConvexHullLocalRealizabilityLimiter<AnalyticalFluxImp, DiscreteFunctionImp, BasisfunctionImp>::
                    PlaneCoefficientsType>
    ConvexHullLocalRealizabilityLimiter<AnalyticalFluxImp, DiscreteFunctionImp, BasisfunctionImp>::plane_coefficients_;

template <class AnalyticalFluxImp, class DiscreteFunctionImp, class BasisfunctionImp>
class DgConvexHullLocalRealizabilityLimiter
    : public LocalRealizabilityLimiterBase<AnalyticalFluxImp, DiscreteFunctionImp, BasisfunctionImp>
{
  using ThisType = DgConvexHullLocalRealizabilityLimiter;
  using BaseType = LocalRealizabilityLimiterBase<AnalyticalFluxImp, DiscreteFunctionImp, BasisfunctionImp>;

public:
  using typename BaseType::GridLayerType;
  using typename BaseType::EntityType;
  using typename BaseType::DomainType;
  using typename BaseType::RangeType;
  using typename BaseType::RangeFieldType;
  using typename BaseType::QuadratureType;
  using typename BaseType::ReconstructedFunctionType;
  using typename BaseType::BasisfunctionType;
  static const size_t dimDomain = BaseType::dimDomain;
  static const size_t dimRange = BaseType::dimRange;
  static const size_t block_size = (dimDomain == 1) ? 2 : 4;
  static const size_t num_blocks = dimRange / block_size;
  typedef FieldVector<RangeFieldType, block_size - 1> BlockRangeType;
  typedef typename std::vector<std::pair<BlockRangeType, RangeFieldType>> BlockPlaneCoefficientsType;
  typedef FieldVector<BlockPlaneCoefficientsType, num_blocks> PlaneCoefficientsType;

public:
  template <class... Args>
  DgConvexHullLocalRealizabilityLimiter(Args&&... args)
    : BaseType(std::forward<Args>(args)...)
  {
    if (is_instantiated_)
      DUNE_THROW(InvalidStateException,
                 "This class uses several static variables to save its state between time "
                 "steps, so using several instances at the same time may result in undefined "
                 "behavior!");
    is_instantiated_ = true;
    if (!plane_coefficients_)
      calculate_plane_coefficients();
  }

  ~DgConvexHullLocalRealizabilityLimiter()
  {
    is_instantiated_ = false;
  }

  void apply_local(const EntityType& entity)
  {
    auto& local_reconstructed_values = reconstructed_function_.local_values(entity);

    // get cell average
    const RangeType u_bar =
        source_.local_function(entity)->evaluate(entity.geometry().local(entity.geometry().center()));

    // vector to store thetas for each local reconstructed value
    std::vector<RangeFieldType> thetas(local_reconstructed_values.size(), -epsilon_);

    size_t ll = -1;
    for (const auto& pair : local_reconstructed_values) {
      ++ll;
      // rescale u_l, u_bar
      auto u_l = pair.second;
      if (XT::Common::FloatCmp::eq(u_l, u_bar))
        continue;
      const auto factor = basis_functions_.realizability_limiter_max(u_l, u_bar);
      u_l /= factor;
      auto u_bar_scaled = u_bar;
      u_bar_scaled /= factor;
      // check positivity of first moment on each spherical triangle
      for (size_t jj = 0; jj < num_blocks; ++jj) {
        const size_t offset = jj * block_size;
        if (u_l[offset] >= u_bar_scaled[offset])
          continue;
        thetas[ll] = std::max(thetas[ll], u_l[offset] / (u_l[offset] - u_bar_scaled[offset]));
      }
      // check convex hull property of other moments
      for (size_t jj = 0; jj < num_blocks; ++jj) {
        const size_t offset = jj * block_size;
        BlockRangeType q_k, q_bar_k;
        for (size_t mm = 0; mm < block_size - 1; ++mm) {
          q_k[mm] = u_l[offset + mm + 1];
          q_bar_k[mm] = u_bar_scaled[offset + mm + 1];
        }
        for (const auto& coeffs : (*plane_coefficients_)[jj]) {
          const BlockRangeType& a = coeffs.first;
          const RangeFieldType& b = coeffs.second;
          const auto u0 = u_l[offset];
          const auto ubar0 = u_bar_scaled[offset];
          RangeFieldType theta_li = (b * u0 - a * q_k) / (a * (q_bar_k - q_k) - b * (ubar0 - u0));
          if (!(theta_li > 1.))
            thetas[ll] = std::max(thetas[ll], theta_li);
        } // coeffs
      } // jj
    } // ll

    auto theta_entity = *std::max_element(thetas.begin(), thetas.end());
    BaseType::apply_limiter(entity, theta_entity, local_reconstructed_values, u_bar);
  } // void apply_local(...)

private:
  // calculate half space representation of realizable set
  void calculate_plane_coefficients()
  {
    plane_coefficients_ = std::make_shared<PlaneCoefficientsType>();
    FieldVector<std::vector<FieldVector<RangeFieldType, block_size - 1>>, num_blocks> points;
    FieldVector<QuadratureType, num_blocks> blocked_quadrature;
    for (const auto& quad_point : quadrature_) {
      const auto face_indices = basis_functions_.get_face_indices(quad_point.position());
      const size_t num_adjacent_faces = face_indices.size();
      for (const auto& kk : face_indices)
        blocked_quadrature[kk].emplace_back(quad_point.position(), quad_point.weight() / num_adjacent_faces);
    } // ii
    size_t num_faces;
    for (size_t jj = 0; jj < num_blocks; ++jj) {
      points[jj].resize(blocked_quadrature[jj].size() + 1);
      for (size_t ii = 0; ii < blocked_quadrature[jj].size(); ++ii) {
        const auto val = basis_functions_.evaluate(blocked_quadrature[jj][ii].position(), false, num_faces);
        for (size_t ll = 0; ll < block_size - 1; ++ll)
          points[jj][ii][ll] = val[block_size * jj + 1 + ll];
      } // ii
      points[jj][blocked_quadrature[jj].size()] = FieldVector<RangeFieldType, block_size - 1>(0.);
    }
    std::vector<std::thread> threads(num_blocks);
    // Launch a group of threads
    for (size_t jj = 0; jj < num_blocks; ++jj)
      threads[jj] = std::thread(&ThisType::calculate_plane_coefficient_block, this, std::ref(points[jj]), jj);
    // Join the threads with the main thread
    for (size_t jj = 0; jj < num_blocks; ++jj)
      threads[jj].join();
  }

  void calculate_plane_coefficient_block(std::vector<FieldVector<RangeFieldType, block_size - 1>>& points, size_t jj)
  {
    orgQhull::Qhull qhull;
    boost::iostreams::stream<boost::iostreams::null_sink> null_ostream((boost::iostreams::null_sink()));
    qhull.setOutputStream(&null_ostream);
    qhull.setErrorStream(&null_ostream);
    qhull.runQhull("Realizable set", int(block_size) - 1, int(points.size()), &(points[0][0]), "Qt T1");
    const auto facet_end = qhull.endFacet();
    BlockPlaneCoefficientsType block_plane_coefficients(qhull.facetList().count());
    //    std::cout << "num_vertices: " << qhull.vertexList().count() << std::endl;
    size_t ii = 0;
    for (auto facet = qhull.beginFacet(); facet != facet_end; facet = facet.next(), ++ii) {
      for (size_t ll = 0; ll < block_size - 1; ++ll)
        block_plane_coefficients[ii].first[ll] = *(facet.hyperplane().coordinates() + ll);
      block_plane_coefficients[ii].second = -facet.hyperplane().offset();
    } // ii
    (*plane_coefficients_)[jj] = block_plane_coefficients;
  }

  using BaseType::basis_functions_;
  using BaseType::source_;
  using BaseType::reconstructed_function_;
  using BaseType::quadrature_;
  using BaseType::epsilon_;
  static bool is_instantiated_;
  static std::shared_ptr<PlaneCoefficientsType> plane_coefficients_;
}; // class ConvexHullLocalRealizabilityLimiter

template <class AnalyticalFluxImp, class DiscreteFunctionImp, class BasisfunctionImp>
bool DgConvexHullLocalRealizabilityLimiter<AnalyticalFluxImp, DiscreteFunctionImp, BasisfunctionImp>::is_instantiated_ =
    false;

template <class AnalyticalFluxImp, class DiscreteFunctionImp, class BasisfunctionImp>
std::shared_ptr<typename DgConvexHullLocalRealizabilityLimiter<AnalyticalFluxImp,
                                                               DiscreteFunctionImp,
                                                               BasisfunctionImp>::PlaneCoefficientsType>
    DgConvexHullLocalRealizabilityLimiter<AnalyticalFluxImp, DiscreteFunctionImp, BasisfunctionImp>::
        plane_coefficients_;

#else // HAVE_QHULL

template <class AnalyticalFluxImp, class DiscreteFunctionImp, class BasisfunctionImp>
class ConvexHullLocalRealizabilityLimiter
{
  static_assert(Dune::AlwaysFalse<DiscreteFunctionImp>::value, "You are missing Qhull!");
};

template <class AnalyticalFluxImp, class DiscreteFunctionImp, class BasisfunctionImp>
class DgConvexHullLocalRealizabilityLimiter
{
  static_assert(Dune::AlwaysFalse<DiscreteFunctionImp>::value, "You are missing Qhull!");
};

#endif // HAVE_QHULL

#if HAVE_CLP
template <class AnalyticalFluxImp, class DiscreteFunctionImp, class BasisfunctionImp>
class ClpLocalRealizabilityLimiter
    : public LocalRealizabilityLimiterBase<AnalyticalFluxImp, DiscreteFunctionImp, BasisfunctionImp>
{
  using BaseType = LocalRealizabilityLimiterBase<AnalyticalFluxImp, DiscreteFunctionImp, BasisfunctionImp>;

public:
  using typename BaseType::GridLayerType;
  using typename BaseType::EntityType;
  using typename BaseType::DomainType;
  using typename BaseType::RangeType;
  using typename BaseType::RangeFieldType;
  using typename BaseType::QuadratureType;
  static const size_t dimDomain = BaseType::dimDomain;
  static const size_t dimRange = BaseType::dimRange;

  template <class... Args>
  ClpLocalRealizabilityLimiter(Args&&... args)
    : BaseType(std::forward<Args>(args)...)
  {
  }

  void apply_local(const EntityType& entity)
  {
    auto& local_reconstructed_values = reconstructed_function_.local_values(entity);
    assert(local_reconstructed_values.size() == 2 * dimDomain);

    // get cell average
    const RangeType u_bar =
        source_.local_function(entity)->evaluate(entity.geometry().local(entity.geometry().center()));

    RangeFieldType theta_entity(0.);

    // if a component is already really small, we do not want to reconstruct in that direction
    for (size_t ii = 0; ii < dimRange; ++ii)
      if (u_bar[ii] < u_vac_[ii])
        for (auto& pair : local_reconstructed_values)
          pair.second[ii] = u_bar[ii];

    for (const auto& pair : local_reconstructed_values) {
      const auto& u = pair.second;
      if (XT::Common::FloatCmp::eq(u_bar, u))
        continue;

      // solve LP:
      // min \theta s.t.
      // (\sum x_i v_i) + \theta (u - \bar{u}) = u
      // x_i, \theta >= 0
      theta_entity = std::max(theta_entity, solve_linear_program(u_bar, u));
    } // ll
    BaseType::apply_limiter(entity, theta_entity, local_reconstructed_values, u_bar);
  } // void apply_local(...)

private:
  void setup_linear_program()
  {
    if (!*lp_) {
      // We start with creating a model with dimRange rows and num_quad_points+1 columns */
      constexpr int num_rows = static_cast<int>(dimRange);
      assert(quadrature_.size() < std::numeric_limits<int>::max());
      int num_cols = static_cast<int>(quadrature_.size() + 1); /* variables are x_1, ..., x_{num_quad_points}, theta */
      *lp_ = std::make_unique<ClpSimplex>(false);
      auto& lp = **lp_;
      // set number of rows
      lp.resize(num_rows, 0);

      // Clp wants the row indices that are non-zero in each column. We have a dense matrix, so provide all indices
      // 0..num_rows
      std::array<int, num_rows> row_indices;
      for (size_t ii = 0; ii < num_rows; ++ii)
        row_indices[ii] = ii;

      // set columns for quadrature points
      assert(int(basis_values_.size()) == num_cols - 1);
      for (int ii = 0; ii < num_cols - 1; ++ii) {
        const auto& v_i = basis_values_[ii];
        // First argument: number of elements in column
        // Second/Third argument: indices/values of column entries
        // Fourth/Fifth argument: lower/upper column bound, i.e. lower/upper bound for x_i. As all x_i should be
        // positive, set to 0/inf, which is the default.
        // Sixth argument: Prefactor in objective for x_i, this is 0 for all x_i, which is also the default;
        lp.addColumn(num_rows, row_indices.data(), &(v_i[0]));
      }

      // add theta column (set to random values, will be set correctly in solve_linear_program)
      // The bounds for theta should be [0,1], but we allow allow theta to be slightly
      // negative so we can check if u_l is on the boundary and if so, move it a little
      // away from the boundary
      // Also sets the prefactor in the objective to 1 for theta.
      lp.addColumn(num_rows, row_indices.data(), &(basis_values_[0][0]), -0.1, 1., 1.);
      lp.setLogLevel(0);
    } // if (!lp_)
  }

  //  RangeFieldType solve_linear_program(const RangeType& u_bar, const RangeType& u_l, const size_t index)
  RangeFieldType solve_linear_program(const RangeType& u_bar, const RangeType& u_l)
  {
    setup_linear_program();
    auto& lp = **lp_;
    constexpr int num_rows = static_cast<int>(dimRange);
    int num_cols = static_cast<int>(quadrature_.size() + 1); /* variables are x_1, ..., x_{num_quad_points}, theta */
    RangeFieldType theta;
    const auto u_l_minus_u_bar = u_l - u_bar;

    // set rhs (equality constraints, so set both bounds equal
    for (size_t ii = 0; ii < num_rows; ++ii) {
      lp.setRowLower(ii, u_l[ii]);
      lp.setRowUpper(ii, u_l[ii]);
    }

    // Clp wants the row indices that are non-zero in each column. We have a dense matrix, so provide all indices
    // 0..num_rows
    std::array<int, num_rows> row_indices;
    for (size_t ii = 0; ii < num_rows; ++ii)
      row_indices[ii] = ii;

    // delete and reset theta column
    const int last_col = num_cols - 1;
    lp.deleteColumns(1, &last_col);
    lp.addColumn(num_rows, row_indices.data(), &(u_l_minus_u_bar[0]), -0.1, 1., 1.);

    // Now solve
    lp.primal();
    theta = lp.objectiveValue();
    if (!lp.isProvenOptimal())
      theta = 1.;

    return theta;
  }

  using BaseType::source_;
  using BaseType::reconstructed_function_;
  using BaseType::quadrature_;
  using BaseType::epsilon_;
  using BaseType::basis_values_;
  using BaseType::u_vac_;
  XT::Common::PerThreadValue<std::unique_ptr<ClpSimplex>> lp_;
}; // class ClpLocalRealizabilityLimiter

#else // HAVE_CLP

template <class AnalyticalFluxImp, class DiscreteFunctionImp, class BasisfunctionImp>
class ClpLocalRealizabilityLimiter
{
  static_assert(Dune::AlwaysFalse<DiscreteFunctionImp>::value, "You are missing Clp!");
};

#endif // HAVE_CLP

template <class LocalRealizabilityLimiterImp, class Traits>
class RealizabilityLimiter;

namespace internal {

template <class LocalRealizabilityLimiterImp>
struct RealizabilityLimiterTraits
{
  using LocalRealizabilityLimiterType = LocalRealizabilityLimiterImp;
  using AnalyticalFluxType = typename LocalRealizabilityLimiterImp::AnalyticalFluxType;
  using BasisfunctionType = typename LocalRealizabilityLimiterImp::BasisfunctionType;
  using QuadratureType = typename LocalRealizabilityLimiterImp::QuadratureType;
  using RangeFieldType = typename LocalRealizabilityLimiterImp::RangeFieldType;
  using FieldType = RangeFieldType;
  using JacobianType = NoJacobian;
  using RangeType = typename LocalRealizabilityLimiterImp::RangeType;
  using ReconstructedFunctionType = typename LocalRealizabilityLimiterImp::ReconstructedFunctionType;
  using derived_type = RealizabilityLimiter<LocalRealizabilityLimiterType, RealizabilityLimiterTraits>;
};

} // namespace internal

template <class LocalRealizabilityLimiterImp,
          class Traits = internal::RealizabilityLimiterTraits<LocalRealizabilityLimiterImp>>
class RealizabilityLimiter : public OperatorInterface<Traits>
{
public:
  using LocalRealizabilityLimiterType = typename Traits::LocalRealizabilityLimiterType;
  using AnalyticalFluxType = typename Traits::AnalyticalFluxType;
  using BasisfunctionType = typename Traits::BasisfunctionType;
  using QuadratureType = typename Traits::QuadratureType;
  using RangeFieldType = typename Traits::RangeFieldType;
  using RangeType = typename Traits::RangeType;
  using ReconstructedFunctionType = typename Traits::ReconstructedFunctionType;

  RealizabilityLimiter(const AnalyticalFluxType& analytical_flux,
                       const BasisfunctionType& basis_functions,
                       const QuadratureType& quadrature,
                       const RangeFieldType epsilon = 1e-8,
                       const std::string filename = "",
                       const RangeFieldType psi_vac = 5e-9)
    : analytical_flux_(analytical_flux)
    , basis_functions_(basis_functions)
    , quadrature_(quadrature)
    , epsilon_(epsilon)
    , basis_values_(quadrature_.size())
    , filename_(filename)
    , psi_vac_(psi_vac)
  {
    for (size_t ii = 0; ii < quadrature_.size(); ++ii)
      basis_values_[ii] = basis_functions_.evaluate(quadrature_[ii].position());
  }

  template <class SourceType>
  void apply(const SourceType& source, ReconstructedFunctionType& range, const XT::Common::Parameter& param) const
  {
    static_assert(is_discrete_function<SourceType>::value,
                  "SourceType has to be derived from DiscreteFunction (use the non-reconstructed values!)");
    LocalRealizabilityLimiterType local_realizability_limiter(analytical_flux_,
                                                              source,
                                                              range,
                                                              basis_functions_,
                                                              quadrature_,
                                                              epsilon_,
                                                              basis_values_,
                                                              param,
                                                              filename_,
                                                              psi_vac_);
    auto walker = XT::Grid::Walker<typename SourceType::SpaceType::GridLayerType>(source.space().grid_layer());
    walker.append(local_realizability_limiter);
    walker.walk(true);
  } // void apply(...)

private:
  const AnalyticalFluxType& analytical_flux_;
  const BasisfunctionType& basis_functions_;
  const QuadratureType& quadrature_;
  const RangeFieldType epsilon_;
  std::vector<RangeType> basis_values_;
  const std::string filename_;
  const RangeFieldType psi_vac_;
}; // class RealizabilityLimiter<...>

} // namespace GDT
} // namespace Dune

#endif // DUNE_GDT_OPERATORS_FV_REALIZABILITY_HH


#include <boost/python/class.hpp>
#include <boost/python/args.hpp>
#include <boost/python/return_value_policy.hpp>
#include <boost/python/copy_const_reference.hpp>
#include <boost/python/return_internal_reference.hpp>
#include <boost/python/with_custodian_and_ward.hpp>
#include <cctbx/sgtbx/wyckoff.h>
#include <scitbx/boost_python/is_polymorphic_workaround.h>

SCITBX_BOOST_IS_POLYMORPHIC_WORKAROUND(cctbx::sgtbx::wyckoff::mapping)

namespace cctbx { namespace sgtbx { namespace boost_python {

namespace {

  struct wyckoff_position_wrappers
  {
    typedef wyckoff::position w_t;

    static void
    wrap()
    {
      using namespace boost::python;
      typedef return_value_policy<copy_const_reference> ccr;
      class_<w_t>("wyckoff_position", no_init)
        .def("multiplicity", &w_t::multiplicity)
        .def("letter", &w_t::letter)
        .def("special_op", &w_t::special_op, ccr())
        .def("point_group_type", &w_t::point_group_type)
        .def("unique_ops", &w_t::unique_ops, (arg("space_group")))
      ;
    }
  };

  struct wyckoff_mapping_wrappers
  {
    typedef wyckoff::mapping w_t;

    static void
    wrap()
    {
      using namespace boost::python;
      typedef return_value_policy<copy_const_reference> ccr;
      typedef return_internal_reference<> rir;
      class_<w_t>("wyckoff_mapping", no_init)
        .def("unit_cell", &w_t::unit_cell, rir())
        .def("original_site", &w_t::original_site)
        .def("position", &w_t::position, rir())
        .def("sym_op", &w_t::sym_op, ccr())
        .def("representative_site", &w_t::representative_site)
        .def("exact_site", &w_t::exact_site)
        .def("distance_moved", &w_t::distance_moved)
        .def("special_op", &w_t::special_op)
      ;
    }
  };

  struct wyckoff_table_wrappers
  {
    typedef wyckoff::table w_t;

    static void
    wrap()
    {
      using namespace boost::python;
      typedef return_value_policy<copy_const_reference> ccr;
      typedef return_internal_reference<> rir;
      class_<w_t>("wyckoff_table", no_init)
        .def(init<space_group_type const&>((arg("space_group_type"))))
        .def("space_group_type", &w_t::space_group_type, rir())
        .def("size", &w_t::size)
        .def("position",
           (wyckoff::position const&(w_t::*)(std::size_t) const)
           &w_t::position, rir())
        .def("position",
           (wyckoff::position const&(w_t::*)(char) const)
           &w_t::position, (arg("letter")), rir())
        .def("lookup_index", &w_t::lookup_index, (arg("letter")))
        .def("mapping",
           (wyckoff::mapping(w_t::*)(site_symmetry const&) const)
           &w_t::mapping, (arg("site_symmetry")),
           with_custodian_and_ward_postcall<0,1>())
        .def("mapping",
          (wyckoff::mapping(w_t::*)(
            uctbx::unit_cell const&,
            fractional<> const&,
            double) const) &w_t::mapping, (
              arg("unit_cell"),
              arg("original_site"),
              arg("special_position_radius")=0.5),
                with_custodian_and_ward_postcall<0,1>())
      ;
    }
  };

} // namespace <anoymous>

  void wrap_wyckoff()
  {
    wyckoff_position_wrappers::wrap();
    wyckoff_mapping_wrappers::wrap();
    wyckoff_table_wrappers::wrap();
  }

}}} // namespace cctbx::sgtbx::boost_python


/*
 * Copyright (C) 2011-2014, Computing Systems Laboratory (CSLab), NTUA.
 * Copyright (C) 2011-2012, Vasileios Karakasis
 * Copyright (C) 2011-2012, Theodoros Gkountouvas
 * Copyright (C) 2013-2014, Athena Elafrou
 * All rights reserved.
 *
 * This file is distributed under the BSD License. See LICENSE.txt for details.
 */

/*
 * \file CsxBench.hpp
 * \brief Benchmark utilities
 *
 * \author Vasileios Karakasis
 * \author Theodoros Gkountouvas
 * \author Athena Elafrou
 * \date 2011–2014
 * \copyright This file is distributed under the BSD License. See LICENSE.txt
 * for details.
 */

#ifndef SPARSEX_INTERNALS_CSX_BENCH_HPP
#define SPARSEX_INTERNALS_CSX_BENCH_HPP

#include <sparsex/internals/SpmMt.hpp>

#ifdef __cplusplus
// C++ only

#include <sparsex/internals/Affinity.hpp>
#include <sparsex/internals/Csr.hpp>
#include <sparsex/internals/CsxKernels.hpp>
#include <sparsex/internals/CsxUtil.hpp>
#include <sparsex/internals/Mmf.hpp>
#include <sparsex/internals/SpmvMethod.hpp>
#include <sparsex/internals/ThreadPool.hpp>
#include <sparsex/internals/Types.hpp>
#include <sparsex/internals/Timer.hpp>
#include <sparsex/internals/Vector.hpp>
#include <thread>
#include <boost/thread/barrier.hpp>
#include <boost/bind.hpp>

using namespace std;
using namespace sparsex::io;
using namespace sparsex::runtime;
using namespace sparsex::timing;

extern double csx_time;

int GetOptionOuterLoops();
float spmv_bench_mt(spm_mt_t *spm_mt, size_t loops, size_t rows_nr,
                    size_t cols_nr);
void spmv_check_mt(CSR<spx_uindex_t, spx_value_t> *spm, spm_mt_t *spm_mt,
                   size_t loops, size_t rows_nr, size_t cols_nr);
float spmv_bench_sym_mt(spm_mt_t *spm_mt, size_t loops, size_t rows_nr,
                        size_t cols_nr);
void spmv_check_sym_mt(CSR<spx_uindex_t, spx_value_t> *spm, spm_mt_t *spm_mt,
                       size_t loops, size_t rows_nr, size_t cols_nr);

/**
 *  Check the CSX SpMV result against the baseline single-thread CSR
 *  implementation.
 *
 *  @param the (mulithreaded) CSX matrix.
 *  @param the MMF input file.
 */
template<typename IndexType, typename ValueType>
void CheckLoop(spm_mt_t *spm_mt, char *mmf_name);

/**
 *  Run CSX SpMV and record the performance information.
 */
template<typename ValueType>
void BenchLoop(spm_mt_t *spm_mt, char *mmf_name);

/**
 *  Read an MMF file and create the corresponding CSR format.
 */
template<typename IndexType, typename ValueType>
void MMFtoCSR(const char *filename, IndexType **rowptr, IndexType **colind,
              ValueType **values, size_t *nrows, size_t *ncols, size_t *nnz);

/* Implementations */
template<typename IndexType, typename ValueType>
void CheckLoop(spm_mt_t *spm_mt, char *mmf_name)
{
  CSR<IndexType, ValueType> *csr = new CSR<IndexType, ValueType>;
  MMFtoCSR<IndexType, ValueType>(mmf_name, &csr->rowptr_, &csr->colind_,
				 &csr->values_, &csr->nr_rows_,
				 &csr->nr_cols_, &csr->nr_nzeros_);

  cout << "Checking... " << flush;
  if (!spm_mt->symmetric) {
    spmv_check_mt(csr, spm_mt, 1, csr->GetNrRows(), csr->GetNrCols());
  } else{
    spmv_check_sym_mt(csr, spm_mt, 1, csr->GetNrRows(), csr->GetNrCols());
  }
  cout << "Check Passed" << endl;

  // Cleanup
  delete[] csr->rowptr_;
  delete[] csr->colind_;
  delete[] csr->values_;
  delete csr;
}

template<typename IndexType, typename ValueType>
void CheckResult(vector_t *result, vector_t *x, char *mmf_name)
{
  CSR<IndexType, ValueType> *csr = new CSR<IndexType, ValueType>;
  MMFtoCSR<IndexType, ValueType>(mmf_name, &csr->rowptr_, &csr->colind_,
				 &csr->values_, &csr->nr_rows_,
				 &csr->nr_cols_, &csr->nr_nzeros_);

  cout << "Checking... " << flush;
  vector_t *y_csr = VecCreate(csr->nr_rows_);
  csr_spmv(csr, x, y_csr);
  if (VecCompare(y_csr, result) < 0)
    exit(1);
  cout << "Check Passed" << endl;

  // Cleanup
  delete[] csr->rowptr_;
  delete[] csr->colind_;
  delete[] csr->values_;
  delete csr;
}

template<typename IndexType, typename ValueType>
void BenchLoop(spm_mt_t *spm_mt, char *mmf_name)
{
  uint64_t nrows, ncols, nnz;
  double secs, flops;
  long loops_nr = 128;

  ReadMmfSizeLine(mmf_name, nrows, ncols, nnz);
  int nr_outer_loops = GetOptionOuterLoops();

  double csx_time = 0;    // FIXME: preprocessing time no more available here
  for (int i = 0; i < nr_outer_loops; ++i) {
    if (!spm_mt->symmetric) {
      secs = spmv_bench_mt(spm_mt, loops_nr, nrows, ncols);
      flops = (double)(loops_nr*nnz*2)/((double)1000*1000*secs);
      printf("m:%s f:%s s:%lu pt:%f t:%f r:%f\n", "csx",
	     basename(mmf_name),
	     CsxSize<IndexType, ValueType>(spm_mt), csx_time,
	     secs, flops);
    } else {
      secs = spmv_bench_sym_mt(spm_mt, loops_nr, nrows, ncols);
      flops = (double)(loops_nr*nnz*2)/((double)1000*1000*secs);
      printf("m:%s f:%s ms:%lu s:%lu pt:%f t:%f r:%f\n", "csx-sym",
	     basename(mmf_name),
	     CsxSymMapSize<IndexType, ValueType>(spm_mt),
	     CsxSymSize<IndexType, ValueType>(spm_mt),
	     csx_time, secs, flops);
    }
  }
}

template<typename IndexType, typename ValueType>
void MMFtoCSR(const char *filename, IndexType **rowptr, IndexType **colind,
              ValueType **values, size_t *nrows, size_t *ncols, size_t *nnz)
{
  MMF<IndexType, ValueType> mmf(filename);
  *nrows = mmf.GetNrRows();
  *ncols = mmf.GetNrCols();
  *nnz = mmf.GetNrNonzeros();
  *values = new ValueType[mmf.GetNrNonzeros()];
  *colind = new IndexType[mmf.GetNrNonzeros()];
  *rowptr = new IndexType[mmf.GetNrRows() + 1];

  typename MMF<IndexType, ValueType>::iterator iter = mmf.begin();
  typename MMF<IndexType, ValueType>::iterator iter_end = mmf.end();
  IndexType row_i = 0, val_i = 0, row_prev = 0;
  IndexType row, col;
  ValueType val;

  (*rowptr)[row_i++] = val_i;
  for (;iter != iter_end; ++iter) {
    row = (*iter).GetRow() - 1;
    col = (*iter).GetCol() - 1;
    val = (*iter).GetValue();
    assert(row >= row_prev);
    if (row != row_prev) {
      for (IndexType i = 0; i < row - row_prev; i++) {
	(*rowptr)[row_i++] = val_i;
      }
      row_prev = row;
    }
    (*values)[val_i] = val;
    (*colind)[val_i] = col;
    val_i++;
  }

  (*rowptr)[row_i++] = val_i;
}

#endif

SPX_BEGIN_C_DECLS__
void check_result(vector_t *result, vector_t *x, char *filename);
SPX_END_C_DECLS__

#endif // SPARSEX_INTERNALS_CSX_BENCH_HPP


#include <iostream>
#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>
#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>

#include "envitools/Box.hpp"
#include "envitools/CSVIO.hpp"
#include "envitools/ContrastMetrics.hpp"
#include "envitools/EnviUtils.hpp"

namespace fs = boost::filesystem;
namespace po = boost::program_options;
namespace et = envitools;
namespace cm = et::ContrastMetrics;

int main(int argc, char* argv[])
{
    ///// Parse the cmd line /////
    fs::path imgDir, csvPath, forePtsPath, backPtsPath, roiPath;

    // clang-format off
    po::options_description options("Options");
    options.add_options()
        ("help,h","Show this message")
        ("input-dir,i",po::value<std::string>()->required(),
            "Input directory of wavelengths")
        ("foreground-pts,f",po::value<std::string>(),
            "Ink points for Michelson")
        ("background-pts,b",po::value<std::string>(),
            "Papyrus points for Michelson contrast")
        ("roi,r",po::value<std::string>(),
            "Region of interest for RMS contrast")
        ("output-file,o", po::value<std::string>()->required(),
            "Output file path");
    // clang-format on

    // parsedOptions will hold the values of all parsed options as a Map
    po::variables_map parsedOptions;
    po::store(
        po::command_line_parser(argc, argv).options(options).run(),
        parsedOptions);

    // show the help message
    if (parsedOptions.count("help") || argc < 4) {
        std::cout << options << std::endl;
        return EXIT_SUCCESS;
    }

    // warn of missing options
    try {
        po::notify(parsedOptions);
    } catch (po::error& e) {
        std::cerr << "ERROR: " << e.what() << std::endl;
        return EXIT_FAILURE;
    }

    // Do we have the options to calculate contrast?
    auto doMichelson = parsedOptions.count("foreground-pts") > 0 &&
                       parsedOptions.count("background-pts") > 0;
    auto doRMS = parsedOptions.count("roi") > 0;

    // Stop if there's nothing to do
    if (!doMichelson && !doRMS) {
        std::cerr << "No arguments given for computing contrast" << std::endl;
        return EXIT_FAILURE;
    }

    // Get the output path
    csvPath = parsedOptions["output-file"].as<std::string>();

    ///// Collect the tif files /////
    imgDir = parsedOptions["input-dir"].as<std::string>();
    std::vector<fs::path> imgPaths = et::FindByExtension(imgDir, ".tif");
    if (imgPaths.empty()) {
        std::cerr << "Error opening/parsing image directory. No files found."
                  << std::endl;
        return EXIT_FAILURE;
    }

    ///// Setup Michelson Points and Regions ////
    std::vector<cv::Vec2i> forePts, backPts;
    if (doMichelson) {
        // Get the points file paths
        forePtsPath = parsedOptions["foreground-pts"].as<std::string>();
        backPtsPath = parsedOptions["background-pts"].as<std::string>();
        if (!boost::filesystem::exists(forePtsPath) ||
            !boost::filesystem::exists(backPtsPath)) {
            std::cerr << "Michelson points file does not exist" << std::endl;
            return EXIT_FAILURE;
        }
        // Load the pts
        forePts = et::CSVIO::ReadPointCSV(forePtsPath);
        backPts = et::CSVIO::ReadPointCSV(backPtsPath);
    }

    ///// Setup RMS Regions ////
    std::vector<et::Box> rmsBoxes;
    if (doRMS) {
        roiPath = parsedOptions["roi"].as<std::string>();
        if (!boost::filesystem::exists(roiPath)) {
            std::cerr << "Provided ROI file does not exist" << std::endl;
            return EXIT_FAILURE;
        }
        rmsBoxes = et::CSVIO::ReadROICSV(roiPath);
    }

    ///// Calculate contrast /////
    std::map<std::string, std::vector<double>> perWavelengthResults;
    std::vector<double> contrasts;
    cv::Mat image;
    std::cout << "Calculating contrast metrics..." << std::endl;
    for (auto path : imgPaths) {
        // Reset temp storage
        contrasts.clear();

        // Get wavelength for key
        auto id = et::ParseWavelength(path);
        std::cerr << "Wavelength: " << id << "\r";

        // Load the image
        image = cv::imread(path.string(), -1);
        if (image.empty()) {
            std::cout << "Could not open or find the image: ";
            std::cout << path.string() << std::endl;
            continue;
        }

        // Images are floating point. Tone map them to a reasonable scale
        image = et::ToneMap(image, float(2.2));

        // Michelson Contrast
        if (doMichelson) {
            contrasts.push_back(cm::MichelsonContrast(image, forePts, backPts));
        }

        // RMS Contrast
        if (doRMS) {
            for (auto box : rmsBoxes) {
                contrasts.push_back(cm::RMSContrast(image, box));
            }
        }

        // Add to the map
        perWavelengthResults[id] = contrasts;
    }
    std::cout << std::endl;

    ///// Make the CSV header /////
    std::vector<std::string> header;
    header.emplace_back("wavelength");
    if (doMichelson) {
        header.emplace_back("michel");
    }
    // Make one column for each RMS roi we loaded
    if (doRMS) {
        for (size_t i = 0; i < rmsBoxes.size(); i++) {
            header.emplace_back("rms" + std::to_string(i));
        }
    }

    ///// Write to a CSV /////
    std::cout << "Writing CSV..." << std::endl;
    et::CSVIO::WriteCSV(csvPath, perWavelengthResults, header);

    std::cout << "Done." << std::endl;
    return 0;
}


//==================================================================================================
/**

  Copyright 2016 NumScale SAS

  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_SIMD_FUNCTION_UNARY_MINUS_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_UNARY_MINUS_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/detail/traits.hpp>
#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/constant/valmin.hpp>
#include <boost/simd/function/bitwise_xor.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/is_equal.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/function/saturated.hpp>
#include <boost/simd/constant/mzero.hpp>
#include <boost/simd/constant/zero.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;

  BOOST_DISPATCH_OVERLOAD_IF( unary_minus_
                            , (typename A0,typename X)
                            , (detail::is_native<X>)
                            , bd::cpu_
                            , bs::pack_<bd::arithmetic_<A0>,X>
                            )
  {
    BOOST_FORCEINLINE A0 operator()(const A0& a0) const BOOST_NOEXCEPT
    {
      return bs::Zero<A0>() - a0;
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF( unary_minus_
                            , (typename A0,typename X)
                            , (detail::is_native<X>)
                            , bd::cpu_
                            , bs::pack_<bd::floating_<A0>,X>
                            )
  {
    BOOST_FORCEINLINE A0 operator()(const A0& a0) const BOOST_NOEXCEPT
    {
      return bitwise_xor(bs::Mzero<A0>(),a0);
    }
  };
  BOOST_DISPATCH_OVERLOAD_IF ( unary_minus_
                          , (typename T, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::saturated_tag
                          , bs::pack_<bd::signed_<T>, X>
                           )
  {
    BOOST_FORCEINLINE T operator()(const saturated_tag &, T const& a0) const BOOST_NOEXCEPT
    {
      return if_else(a0 == Valmin<T>(),Valmax<T>(),bs::unary_minus(a0));
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF ( unary_minus_
                          , (typename T, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::saturated_tag
                          , bs::pack_<bd::unspecified_<T>, X>
                          )
  {
    BOOST_FORCEINLINE T operator()(const saturated_tag &, T const& a) const BOOST_NOEXCEPT
    {
      return unary_minus(a);
    }
  };

} } }

#endif


//==================================================================================================
/**
  Copyright 2016 Numscale SAS

  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_X86_SSSE3_SIMD_FUNCTION_SLIDE_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_SSSE3_SIMD_FUNCTION_SLIDE_HPP_INCLUDED

#include <boost/simd/meta/as_arithmetic.hpp>
#include <boost/simd/detail/overload.hpp>
#include <boost/simd/detail/brigand.hpp>
#include <boost/simd/function/bitwise_cast.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD ( slide_
                          , (typename T, typename Offset)
                          , bs::ssse3_
                          , bs::pack_< bd::fundamental_<T>, bs::sse_ >
                          , bs::pack_< bd::fundamental_<T>, bs::sse_ >
                          , bd::constant_<bd::integer_<Offset>>
                          )
  {
    BOOST_FORCEINLINE T operator()(T const& a0, T const& a1, Offset const&) const BOOST_NOEXCEPT
    {
      // Work on byte level
      using byte_t = typename T::template retype<std::uint8_t,16>;

      // Compute relative offsets
      using bitcount = std::integral_constant < std::size_t
                                              , sizeof(as_arithmetic_t<typename T::value_type>)
                                              * Offset::value
                                              >;

      // Shift everything in place
      return bitwise_cast<T>( byte_t(_mm_alignr_epi8 ( bitwise_cast<byte_t>(a1), bitwise_cast<byte_t>(a0)
                                              , bitcount::value
                                              )
                                    )
                            );
    }
  };
} } }

#endif


#include "cageclient.hh"
#include <boost/function.hpp>
#include <boost/program_options.hpp>
#include <boost/program_options/variables_map.hpp>
#include <chrono>
#include <cmath>
#include <geometry_msgs/TransformStamped.h>
#include <nav_msgs/Odometry.h>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_broadcaster.h>

namespace bo = boost::program_options;
template <typename Type>
bool parseOption(bo::variables_map &vm, std::string option, Type& store){
  if(vm.count(option)){
    store=vm[option].as<Type>();
    return true;
  }
  return false;
}

int main(int argc, char **argv) try
{
    bo::options_description opts("Options");
    opts.add_options()("help,h", "Show usage")("version,v", "Show program version");
    opts.add_options()("device,d", bo::value<std::string>());

    bo::variables_map vm;
    std::string device, port;
    std::vector<std::string> command;
    bool scan = false;
    int canspeed = 0;
    std::cout << "CageRos" << std::endl;
    std::cout << "   2019/02/20  yoshida@furo.org" << std::endl;

    try
    {
        bo::store(bo::parse_command_line(argc, argv, opts), vm);
        if (vm.count("help"))
        {
            std::cout << opts << std::endl;
            exit(0);
        }
        if (vm.count("version"))
        {
            exit(0);
        }
        if (!parseOption(vm, "device", device))
        {
            std::cerr << "No device specified." << std::endl;
            exit(0);
        }
    }
    catch (std::exception &e)
    {
        std::cerr << e.what() << std::endl;
        std::cout << opts << std::endl;
        exit(0);
    }

    ros::init(argc, argv, "cage_ros");

    CageAPI Cage(device);
    if(!Cage.connect()){
        std::cerr<<"Failed to connect:"<<Cage.getError()<<std::endl;
        return -1;
    }

    ros::NodeHandle nh;
    ros::Publisher pub_odom = nh.advertise<nav_msgs::Odometry>("odom", 100);
    ros::Publisher pub_odomgt = nh.advertise<nav_msgs::Odometry>("odom_gt", 100);
    ros::Publisher pub_imu = nh.advertise<sensor_msgs::Imu>("imu", 100);
    tf2_ros::TransformBroadcaster tfb;

    tf2::Quaternion iq;
    iq.setRPY(0., 0., 0.);
    tf2::Transform imuTrans(iq, tf2::Vector3{0, 0, 0});

    boost::function<void(const geometry_msgs::Twist &r_msg)>
        fromRos =
            [&Cage](const auto &msg) {
                std::cout << "setVW:"<< msg.linear.x <<"," << msg.angular.z << std::endl;
                Cage.setVW(msg.linear.x, msg.angular.z);
            };
    auto sub = nh.subscribe<geometry_msgs::Twist>("cmd_vel", 10, fromRos);

    ros::AsyncSpinner spinner(1);
    spinner.start();

    double odox=0, odoy=0, odoth=0, vx, av, dt;
    double simClock=0;
    {
      CageAPI::vehicleStatus st;
      if (!Cage.getStatusOne(st, 100)) {
        std::cerr << "getStatus failed:" << Cage.getError() << std::endl;
        exit(0);
      }
      simClock=st.simClock;
      odox=st.wx;
      odoy=st.wy;
      auto q = tf2::Quaternion(st.ox, st.oy, st.oz, st.ow);
      double ir,ip,iy;
      tf2::Matrix3x3(q).getRPY(ir,ip,iy);
      odoth=iy;
    }

    const double treadWidth=Cage.VehicleInfo.TreadWidth;
    const double perimeterR = Cage.VehicleInfo.WheelPerimeterR;
    const double perimeterL = Cage.VehicleInfo.WheelPerimeterL;
    const double reductionRatio=Cage.VehicleInfo.ReductionRatio;

    std::cout<<"Vehicle Parameters\n tread, wheelPerimeterR,L reductionRatio"
    <<treadWidth<<" "<<perimeterR<<" "<<perimeterL<<" "<<reductionRatio<<std::endl;

    while (ros::ok()) {
      CageAPI::vehicleStatus st;
      if (!Cage.getStatusOne(st, 100)) {
        std::cerr << "getStatus failed:" << Cage.getError() << std::endl;
        continue;
      }
      // to ROS
      // simple odometry
      dt=st.simClock-simClock;
      double ts;
      int32_t tsnsec= static_cast <int32_t>(std::modf(st.simClock,&ts)*1e9);
      int32_t tssec=static_cast<int32_t>(ts);

      if(dt<0.001) continue; //1ms
      simClock=st.simClock;

      double vr = -st.rrpm * perimeterR/reductionRatio / 60.;
      double vl = st.lrpm * perimeterL/reductionRatio / 60.;
      vx = (vr+vl)/2.;
      //av=(vr-vl)/treadWidth; // use wheel rotation and body parameters
      av=st.rz;  // use angular velocity sensor
      double dx = vx * cos(odoth)*dt;
      double dy=vx*sin(odoth)*dt;
      odox+=dx;
      odoy+=dy;
      odoth+=av*dt;
      //std::cout<<"rrpm, lrpm, vr, vl, dt"<<st.rrpm<<" "<<st.lrpm<<" "<<vr<<"  "<<vl<<"  "<<dt<<std::endl;
      std::cout << "rx, ry, rz " << std::setw(12) << std::fixed << st.rz << " "
                << std::setw(12) << std::fixed << st.ry << " " << std::setw(12)
                << std::fixed << st.rz << std::endl;
      // publish
      nav_msgs::Odometry odo;
      odo.header.frame_id="odom";
      odo.header.stamp.sec = tssec;
      odo.header.stamp.nsec = tsnsec;
      odo.child_frame_id = "base_link";
      odo.pose.pose.position.x = odox;
      odo.pose.pose.position.y = odoy;
      odo.pose.pose.position.z = 0.;
      tf2::Quaternion q;
      q.setRPY(0., 0., odoth);
      odo.pose.pose.orientation = tf2::toMsg(q);
      odo.pose.covariance = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                             0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                             0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
      odo.twist.twist.linear.x = vx;
      odo.twist.twist.linear.y = 0.;
      odo.twist.twist.linear.z = 0.;
      odo.twist.twist.angular.x = 0.;
      odo.twist.twist.angular.y = 0.;
      odo.twist.twist.angular.z = av;
      odo.twist.covariance = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                              0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                              0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
      pub_odom.publish(odo);
      geometry_msgs::TransformStamped odotf;
      odotf.header.frame_id="odom";
      odotf.header.stamp.sec=tssec;
      odotf.header.stamp.nsec=tsnsec;
      odotf.child_frame_id="base_link";
      odotf.transform.translation.x = odo.pose.pose.position.x;
      odotf.transform.translation.y = odo.pose.pose.position.y;
      odotf.transform.translation.z = odo.pose.pose.position.z;
      odotf.transform.rotation=tf2::toMsg(q);
      tfb.sendTransform(odotf);

      nav_msgs::Odometry odogt;
      odogt.header.frame_id = "odom";
      odogt.header.stamp.sec = tssec;
      odogt.header.stamp.nsec = tsnsec;
      odogt.child_frame_id = "base_link_gt";
      odogt.pose.pose.position.x = st.wx;
      odogt.pose.pose.position.y = st.wy;
      odogt.pose.pose.position.z = st.wz;
      q=tf2::Quaternion(st.ox,st.oy,st.oz,st.ow);
      odogt.pose.pose.orientation = tf2::toMsg(q);
      odogt.pose.covariance = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                             0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                             0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
      odogt.twist.twist.linear.x = vx;
      odogt.twist.twist.linear.y = 0.;
      odogt.twist.twist.linear.z = 0.;
      odogt.twist.twist.angular.x = 0.;
      odogt.twist.twist.angular.y = 0.;
      odogt.twist.twist.angular.z = st.rz;
      odogt.twist.covariance = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                              0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                              0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
      pub_odomgt.publish(odogt);
      geometry_msgs::TransformStamped odogttf;
      odogttf.header.frame_id = "odom";
      odogttf.header.stamp.sec = tssec;
      odogttf.header.stamp.nsec = tsnsec;
      odogttf.child_frame_id = "base_link_gt";
      odogttf.transform.translation.x = odogt.pose.pose.position.x;
      odogttf.transform.translation.y = odogt.pose.pose.position.y;
      odogttf.transform.translation.z = odogt.pose.pose.position.z;
      odogttf.transform.rotation = tf2::toMsg(q);
      tfb.sendTransform(odogttf);

      sensor_msgs::Imu imu;
      imu.header.frame_id = "imu";
      imu.header.stamp.sec = tssec;
      imu.header.stamp.nsec = tsnsec;
      imu.orientation.w = st.ow;
      imu.orientation.x=st.ox;
      imu.orientation.y=st.oy;
      imu.orientation.z=st.oz;
      imu.orientation_covariance = {0, 0, 0, 0, 0, 0, 0, 0, 0};
      imu.angular_velocity.x = st.rx;
      imu.angular_velocity.y = st.ry;
      imu.angular_velocity.z = st.rz;
      imu.angular_velocity_covariance = {0, 0, 0, 0, 0, 0, 0, 0, 0};
      imu.linear_acceleration.x = st.ax;
      imu.linear_acceleration.y = st.ay;
      imu.linear_acceleration.z = st.az;
      imu.linear_acceleration_covariance = {0, 0, 0, 0, 0, 0, 0, 0, 0};
      pub_imu.publish(imu);

      geometry_msgs::TransformStamped tf_imu;
      tf_imu.header.stamp.sec=tssec;
      tf_imu.header.stamp.nsec=tsnsec;
      tf_imu.header.frame_id="base_link";
      tf_imu.child_frame_id="imu";
      tf_imu.transform=tf2::toMsg(imuTrans);
      tfb.sendTransform(tf_imu);
    }
    spinner.stop();
    return 0;
}
catch (const std::exception &ex)
{
    std::cerr << "EXCEPTION: " << ex.what() << std::endl;
    return 1;
}


/*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2014-2015, Timm Linder, Social Robotics Lab, University of Freiburg
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions are met:
*
*  * Redistributions of source code must retain the above copyright notice, this
*    list of conditions and the following disclaimer.
*  * Redistributions in binary form must reproduce the above copyright notice,
*    this list of conditions and the following disclaimer in the documentation
*    and/or other materials provided with the distribution.
*  * Neither the name of the copyright holder nor the names of its contributors
*    may be used to endorse or promote products derived from this software
*    without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
*  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
*  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
*  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
*  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
*  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
*  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
*  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
*  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include <pluginlib/class_list_macros.h>
#include "euclidean_nn_fuser.h"

#include <Eigen/Core>
#include <limits>
#include <eigen_conversions/eigen_msg.h>

namespace spencer_detected_person_association
{
    float EuclideanNNFuserNodelet::computeDistance(const spencer_tracking_msgs::CompositeDetectedPerson& d1, const spencer_tracking_msgs::CompositeDetectedPerson& d2)
    {
        // Compute the Euclidean distance in X and Y
        float distance = hypot(d1.pose.pose.position.x - d2.pose.pose.position.x, d1.pose.pose.position.y - d2.pose.pose.position.y);

        // Gating: If it fails, set distance to infinity
        double gatingDistance = 0.5; getPrivateNodeHandle().getParamCached("gating_distance", gatingDistance);
        if(distance > gatingDistance) distance = std::numeric_limits<float>::infinity();

        return distance;
    }

    void EuclideanNNFuserNodelet::fusePoses(const spencer_tracking_msgs::CompositeDetectedPerson& d1, const spencer_tracking_msgs::CompositeDetectedPerson& d2, geometry_msgs::PoseWithCovariance& fusedPose)
    {
        // Obtain and normalize weights of arithmetic mean
        double weight1 = 0.5; getPrivateNodeHandle().getParamCached("pose_weight_for_topic1", weight1);
        double weight2 = 0.5; getPrivateNodeHandle().getParamCached("pose_weight_for_topic2", weight2);

        const float weightSum = weight1 + weight2;
        weight1 /= weightSum; weight2 /= weightSum;

        // Compute the arithmetic mean of the X,Y,Z positions as well as the corresponding covariances.
        fusedPose.pose.position.x = weight1 * d1.pose.pose.position.x + weight2 * d2.pose.pose.position.x;
        fusedPose.pose.position.y = weight1 * d1.pose.pose.position.y + weight2 * d2.pose.pose.position.y;
        fusedPose.pose.position.z = weight1 * d1.pose.pose.position.z + weight2 * d2.pose.pose.position.z;

        // Interpolate the orientation using SLERP
        Eigen::Quaterniond q1, q2, qInterpolated;
        tf::quaternionMsgToEigen(d1.pose.pose.orientation, q1);
        tf::quaternionMsgToEigen(d2.pose.pose.orientation, q2);
        qInterpolated = q1.slerp(weight2, q2);
        tf::quaternionEigenToMsg(qInterpolated, fusedPose.pose.orientation);

        // Take component-wise minimum of the covariance matrices
        for(size_t i = 0; i < d1.pose.covariance.size(); i++) {
            // FIXME: Is this a good idea for tracking? This is similar to using the 'stronger peak' of the two distributions,
            // as we hopefully don't get less-informed by fusing the two detections (if our association makes sense)
            // There is probably no 'correct way' as the mixture of two Gaussians is not a Gaussian
            fusedPose.covariance[i] = std::min(d1.pose.covariance[i], d2.pose.covariance[i]);
        }
    }
}

PLUGINLIB_DECLARE_CLASS(spencer_detected_person_association, EuclideanNNFuserNodelet, spencer_detected_person_association::EuclideanNNFuserNodelet, nodelet::Nodelet)


#define BOOST_TEST_MODULE ea_solver_unit_tests

#include <boost/test/included/unit_test.hpp>
#include <armadillo>
#include "../src/numerical/evolutionary_algorithm/ea_solver.h"
#include "test_utils.cpp"
#include "../src/logging/easylogging++.h"

INITIALIZE_EASYLOGGINGPP

using namespace arma;

auto solver = EASolver(SIGNALS, 500, 1000, 0.0001, 100, 50);

BOOST_AUTO_TEST_CASE(test_common_cases)
{
    auto tester = SolverTester<EASolver>(solver);
    tester.test_common(0.1);
}


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

    @copyright 2016 NumScale SAS

  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_FUNCTION_ROL_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ROL_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-bitwise
    This function object returns the first entry rotated left
    by the value of the second entry.

    @par Header <boost/simd/function/rol.hpp>

    @par Note
    This function asserts if the integral parameter is out of range

    @see  ror, rrol, rror

    @par Example:

      @snippet rol.cpp rol

    @par Possible output:

      @snippet rol.txt rol

  **/
  Value rol(Value const& x, IntegerValue const& n);
} }
#endif

#include <boost/simd/function/scalar/rol.hpp>
#include <boost/simd/function/simd/rol.hpp>

#endif


#include <waypoint_following/waypoint_follower.hpp>
#include <boost/geometry/geometry.hpp>
#include <string>
#include <vector>
#include <fstream>
#include <sstream>
#include <algorithm>
using namespace boost::geometry;
namespace waypoint_following
{
  using std::placeholders::_1;
  using std::placeholders::_2;
  class WaypointFollower::WaypointFileReader
  {
  public:
    std::shared_ptr<Waypoints> waypoints;

    WaypointFileReader(const std::string & waypoint_file_path)
    {
      waypoints = readWaypointsFromFile(waypoint_file_path);
    }

    std::shared_ptr<Waypoints> readWaypointsFromFile(const std::string & waypoint_file_path)
    {
      std::shared_ptr<Waypoints> pts(new Waypoints());
      std::ifstream f(waypoint_file_path.c_str());
      std::string line;
      std::getline(f, line); // Ignore first line
      while (std::getline(f, line))
      {
        auto tokens = tokenize(line, ",");
        if (tokens->size() == 5)
        {
          double lat = std::stod((*tokens)[0]);
          double lon = std::stod((*tokens)[1]);
          double alt = std::stod((*tokens)[2]);
          double rpm = std::stod((*tokens)[3]);
          double heading = std::stod((*tokens)[4]);
          pts->push_back(Waypoint{lat, lon, alt, rpm, heading});
        }
      }
      f.close();
      return pts;
    }

    std::shared_ptr<std::vector<std::string>> tokenize(const std::string & s, const std::string & del = " ")
    {
      std::shared_ptr<std::vector<std::string>> tokens(new std::vector<std::string>());
      int start = 0;
      int end = s.find(del);
      while (end != -1) {
          start = end + del.size();
          end = s.find(del, start);
          tokens->push_back(s.substr(start, end - start));
      }
      return tokens;
    }
  };

  class WaypointFollower::HeadingCalculator
  {
    Waypoint calc(const NavSatFix::SharedPtr nav)
    {
      return Waypoint{0.0, 0.0, 0.0, 0.0, 0.0};
    }
  };

  class WaypointFollower::Localizer
  {
  public:
    Localizer(std::shared_ptr<Waypoints> waypoints)
    {
      this->waypoints = waypoints;
      std::vector<model::d2::point_xy<double>> points = {};
      for (const auto & wp : *waypoints)
      {
        points.push_back(model::d2::point_xy<double>(wp.latitude, wp.longitude));
      }
      append(path_poly, points);
    }
    model::polygon<model::d2::point_xy<double>> path_poly;
    std::shared_ptr<Waypoints> waypoints;
    int cloest_idx = 0;
    double getCrossTrackError(const Waypoint & wp)
    {
      auto pt = model::d2::point_xy<double>(wp.latitude, wp.longitude);
      double cte = distance(pt, path_poly) * (within(pt, path_poly) ? 1.0 : -1.0);
      return cte;
    }

    double getHeadingError(const Waypoint & location)
    {
      // find the points
      std::vector<double> distances;
      distances.reserve(waypoints->size());
      for (const auto & wp: *waypoints)
      {
        distances.push_back(sqrt(pow(wp.latitude - location.latitude, 2) + pow(wp.longitude - location.longitude, 2)));
      }
      cloest_idx = std::min_element(distances.begin(), distances.end()) - distances.begin();

      // Calculate heading differences. left is negative and right is positive
      double target_heading = (*waypoints)[cloest_idx].heading;
      double real_heading = location.heading;
      double tl = 0.0;
      double tr = 0.0;
      if (target_heading > real_heading){
        tl = target_heading - real_heading;
        tr = real_heading + 2 * M_PI - target_heading;
      }
      else
      {
        tl = 2 * M_PI - real_heading + target_heading;
        tr = real_heading - target_heading;
      }
      return tl < tr ? -tl : tr;
    }

    double getTargetSpeed()
    {
      return (*waypoints)[cloest_idx].rpm;
    }
  };

  WaypointFollower::WaypointFollower(const rclcpp::NodeOptions & options)
  : rclcpp::Node("waypoint_follower_node", options)
  {
    speed_pub_ = create_publisher<Float64>("commands/motor/speed", rclcpp::QoS{5});
    steering_pub_ = create_publisher<Float64>("commands/servo/position", rclcpp::QoS{5});

    speed_sub_ = create_subscription<Float64>("commands/pilot/speed", rclcpp::QoS{5},
                      std::bind(&WaypointFollower::SpeedReceiveCallback, this, _1));
    gnss_sub_ = create_subscription<NavSatFix>("", rclcpp::QoS{5},
                      std::bind(&WaypointFollower::GNSSReceiveCallback, this, _1));
    //speed_k = declare_parameter("speed_k").get<double>();
    kp = declare_parameter("KP").get<double>();
    ki = declare_parameter("KI").get<double>();
    kd = declare_parameter("KD").get<double>();
    std::string waypoint_file = declare_parameter("waypoint_file").get<std::string>();
    steering_controller_ = std::unique_ptr<MiniPID>(new MiniPID(kp, ki, kd));
    file_reader_= std::unique_ptr<WaypointFileReader>(new WaypointFileReader(waypoint_file));
    heading_calc_ =  std::unique_ptr<HeadingCalculator>(new HeadingCalculator());
    loc_ = std::unique_ptr<Localizer>(new Localizer(file_reader_->waypoints));
  }

  void WaypointFollower::SpeedReceiveCallback(const Float64::SharedPtr speed)
  {
    real_speed = speed->data;
  }
  void WaypointFollower::GNSSReceiveCallback(const NavSatFix::SharedPtr nav)
  {
    double heading = 0.0;
    current_location = Waypoint{nav->latitude, nav->longitude, nav->altitude, 0.0, heading};
    calcAndSendCommands();
  }

  void WaypointFollower::calcAndSendCommands()
  {
    double cte = loc_->getCrossTrackError(current_location);
    double heading_error = loc_->getHeadingError(current_location);
    double steering = steering_controller_->getOutput(0.5 * cte + 0.5 * heading_error / (2 * M_PI));
    double target_speed = loc_->getTargetSpeed();
    auto steering_msg = Float64();
    steering_msg.data = steering;
    steering_pub_->publish(steering_msg);
    auto speed_msg = Float64();
    speed_msg.data = target_speed;
    speed_pub_->publish(speed_msg);

  }
}

#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(waypoint_following::WaypointFollower)


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_SWAR_FUNCTIONS_SIMD_SSE_SSE4_1_GROUPS_HPP_INCLUDED
#define BOOST_SIMD_SWAR_FUNCTIONS_SIMD_SSE_SSE4_1_GROUPS_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_SSE4_1_SUPPORT

#include <boost/simd/swar/functions/groups.hpp>
#include <boost/simd/include/functions/simd/shr.hpp>
#include <boost/simd/include/functions/simd/bitwise_and.hpp>
#include <boost/simd/include/functions/simd/bitwise_or.hpp>
#include <boost/dispatch/meta/downgrade.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT         ( groups_, boost::simd::tag::sse4_1_, (A0)
                            , ((simd_<uint32_<A0>,boost::simd::tag::sse_>))
                              ((simd_<uint32_<A0>,boost::simd::tag::sse_>))
                            )
  {
    typedef typename dispatch::meta::downgrade<A0>::type result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return _mm_packus_epi32( (a0 & 0x7FFFFFFF) | shri(a0 & 0xF0000000, 1)
                             , (a1 & 0x7FFFFFFF) | shri(a1 & 0xF0000000, 1)
                             );
    }
  };
} } }

#endif
#endif


#include <boost/python/class.hpp>
#include <boost/python/args.hpp>
#include <cctbx/sgtbx/space_group.h>

namespace cctbx { namespace sgtbx { namespace boost_python {

namespace {

  struct phase_info_wrappers
  {
    typedef phase_info w_t;

    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("phase_info", no_init)
        .def(init<
          space_group const&,
          miller::index<> const&,
          optional<bool> >((
            arg("space_group"),
            arg("miller_index"),
            arg("no_test_sys_absent")=false)))
        .def("sys_abs_was_tested", &w_t::sys_abs_was_tested)
        .def("is_sys_absent", &w_t::is_sys_absent)
        .def("is_centric", &w_t::is_centric)
        .def("ht", &w_t::ht)
        .def("t_den", &w_t::t_den)
        .def("ht_angle", &w_t::ht_angle, (arg("deg")=false))
        .def("is_valid_phase", &w_t::is_valid_phase, (
          arg("phi"),
          arg("deg")=false,
          arg("tolerance")=1e-5))
        .def("nearest_valid_phase",
          (double(w_t::*)(double, bool) const) &w_t::nearest_valid_phase, (
            arg("phi"),
            arg("deg")=false))
        .def("valid_structure_factor",
          (std::complex<double>(w_t::*)(std::complex<double> const&) const)
            &w_t::valid_structure_factor, (arg("f")))
      ;
    }
  };

} // namespace <anoymous>

  void wrap_phase_info()
  {
    phase_info_wrappers::wrap();
  }

}}} // namespace cctbx::sgtbx::boost_python


/*
 * SequenceCounter.cpp
 *
 *  Created on: 2014/09/13
 *      Author: miwa
 */
#include "Utils.h"
#include "SequenceCounter.h"
#include <boost/algorithm/string.hpp>
#include <boost/filesystem.hpp>
#include <gzstream.h>
#include <string>
#include <sstream>
#include <fstream>
#include <cmath>
#include <deque>
using namespace std;
using namespace boost::filesystem;

int coin::CooccurrenceCounter::calc_max_product(double memory_size) const{
	double rlimit = 0.85 * memory_size * 1073741824/(sizeof(double));
	double n = nwords_;
	while(abs(rlimit - n * (log(n) + 0.1544313298)) > 1e-3) n = rlimit / (log(n) + 0.1544313298);
	return (int)n;
}

long long coin::CooccurrenceCounter::calc_excess_limit(double memory_size) const{
	return (long long)(0.15 * memory_size * 1073741824 / (sizeof(std::_Rb_tree_node<std::pair<double, int64_t> >)));
}

std::string coin::CooccurrenceCounter::filename(const std::string &file, int id) const{
	ostringstream os;
	os << file << ".tmp." << id;
	return os.str();
}

void coin::CooccurrenceCounter::count(const string& gzfile, const std::string &outfile){
	igzstream in(gzfile.c_str());
	if(!in){
		cerr << gzfile << " not found!!" << endl;
		return;
	}
	string line;
	string word;
	while(in && getline(in, line)){
		preprocess(line);
		istringstream iss(line);
		vector<int> local_words;
		while(iss >> word){
			if(word_indices_.find(word) != word_indices_.end()){
				local_words.push_back(word_indices_[word]);
			}else{
				local_words.push_back(-1);
			}
		}
		int size = local_words.size();
		// for all pairs
		for(int i = 0;i < size;++i){
			int32_t w1 = local_words[i];
			if(w1 == -1)continue;
			for(int j = i+1;j < size;++j){
				int32_t w2 = local_words[j];
				if(w2 == -1)continue;
				update_count(w1, w2, 1., outfile);
				update_count(w2, w1, 1., outfile);
			}
		}
	}
	in.close();
}

void coin::CooccurrenceCounter::write_excesses(const string& file) const{
	ofstream out(file, ios::out | ios::binary | ios::trunc);
	if(!out){
		cerr << "can not write to " << file << "!!" << endl;
		return;
	}
	out.write((char *)&excesses_, sizeof(int));
	for(pair<int64_t, double> excess_count:excess_counts_){
		out.write((char *)&excess_count.first, sizeof(int64_t));
		out.write((char *)&excess_count.second, sizeof(double));
	}
}

void coin::CooccurrenceCounter::update_count(int32_t w1, int32_t w2, double count, const std::string &outfile){
	total_ += count;
	if(w2 < count_sizes_[w1]){
		counts_[w1][w2] += count;
	}else{
		int64_t l12 = to_scalar(w1, w2);
		if(excess_counts_.find(l12) == excess_counts_.end()){
			excess_counts_[l12] = count;
			excesses_++;
			if(excesses_ > excess_limit_){
				write_excesses(filename(outfile,nexcess_files_));
				nexcess_files_++;
				excesses_ = 0;
				excess_counts_.clear();
			}
		}else{
			excess_counts_[l12] += count;
		}
	}
}

void coin::CooccurrenceCounter::write_cooccurrences(const std::string &file){
	long long cooccurrences = 0;
	ofstream out(file, ios::out | ios::binary | ios::trunc);
	for(int w1 = 0;w1 < nwords_;++w1){
		for(int w2 = 0; w2 < count_sizes_[w1];++w2){
			if(counts_[w1][w2] == 0.)continue;
			out.write((char *)&w1, sizeof(int32_t));
			out.write((char *)&w2, sizeof(int32_t));
			out.write((char *)&counts_[w1][w2], sizeof(double));
			if(cooccurrences % 1000000 == 0){
				cerr << cooccurrences << " pair: "<< word_strings_[w1] << "<=>" << word_strings_[w2] << ":" << counts_[w1][w2] << endl;
			}
			cooccurrences++;
		}
	}
	cerr << cooccurrences << " co-occurrence pairs stored in memory" << endl;
	if(excesses_ > 0){
		write_excesses(filename(file,nexcess_files_));
		nexcess_files_++;
		excesses_ = 0;
		excess_counts_.clear();
	}
	map<int64_t, double> counts;
	ifstream *excesses[nexcess_files_];
	string files[nexcess_files_];
	int sizes[nexcess_files_];
	int64_t current[nexcess_files_];
	for(int i = 0;i < nexcess_files_;++i){
		files[i] = filename(file, i);
		excesses[i] = new ifstream(files[i], ios::in | ios::binary);
		excesses[i]->read((char *)&sizes[i], sizeof(int));
		double count;
		excesses[i]->read((char *)&current[i], sizeof(int64_t));
		excesses[i]->read((char *)&count, sizeof(double));
		if(counts.find(current[i]) != counts.end()){
			counts[current[i]] += count;
		}else{
			counts[current[i]] = count;
		}
		sizes[i]--;
	}
	while(nexcess_files_ > 0){
		int64_t index = counts.begin()->first;
		double value = counts.begin()->second;
		counts.erase(index);
		pair<int32_t, int32_t> w = to_pair(index);
		out.write((char *)&w.first, sizeof(int32_t));
		out.write((char *)&w.second, sizeof(int32_t));
		out.write((char *)&value, sizeof(double));
		if(cooccurrences % 1000000 == 0){
			cerr << cooccurrences << " pair: "<< word_strings_[w.first] << "<=>" << word_strings_[w.second] << ":" << value << endl;
		}
		cooccurrences++;
		for(int i = 0;i < nexcess_files_;++i){
			if(index == current[i] && sizes[i] != 0){
				double count;
				excesses[i]->read((char *)&current[i], sizeof(int64_t));
				excesses[i]->read((char *)&count, sizeof(double));
				if(counts.find(current[i]) != counts.end()){
					counts[current[i]] += count;
				}else{
					counts[current[i]] = count;
				}
				sizes[i]--;
			}
		}
		bool need_check = false;
		do{
			need_check = false;
			for(int i = 0;i < nexcess_files_;++i){
				if(sizes[i] == 0){
					excesses[i]->close();
					remove(files[i].c_str());
					delete excesses[i];
					if(nexcess_files_ - 1 != i){
						excesses[i] = excesses[nexcess_files_ - 1];
						files[i] = files[nexcess_files_ - 1];
						sizes[i] = sizes[nexcess_files_-1];
						current[i] = current[nexcess_files_ - 1];
						need_check = true;
					}
					nexcess_files_--;
					break;
				}
			}
		}while(need_check);
	}
	for(pair<int64_t, double> count:counts){
		int64_t index = count.first;
		double value = count.second;
		pair<int32_t, int32_t> w = to_pair(index);
		out.write((char *)&w.first, sizeof(int32_t));
		out.write((char *)&w.second, sizeof(int32_t));
		out.write((char *)&value, sizeof(double));
		if(cooccurrences % 1000000 == 0){
			cerr << cooccurrences << " pair: "<< word_strings_[w.first] << "<=>" << word_strings_[w.second] << ":" << value << endl;
		}
		cooccurrences++;
	}
	cerr << cooccurrences << " pairs in total ";
	out.close();
}

void coin::CooccurrenceCounter::shuffle(const std::string &file) const{
	char *buf_in = new char[BUFSIZE];
	char *buf_out = new char[BUFSIZE];
	string in_filename = file;
	string tmp_filename = in_filename+".tmp";
	ifstream in_file(in_filename, ios::in | ios::binary);
	ofstream out_file(tmp_filename, ios::out | ios::binary);
	in_file.rdbuf()->pubsetbuf(buf_in, BUFSIZE);
	out_file.rdbuf()->pubsetbuf(buf_out, BUFSIZE);
	vector<CooccEntry> entries;
	entries.reserve(SHUFFLE_SIZE+1);
	int nentries = 0;
	while(true){
		int32_t w1, w2;
		double count;
		in_file.read((char *)&w1, sizeof(int32_t));
		if(in_file.eof())break;
		in_file.read((char *)&w2, sizeof(int32_t));
		in_file.read((char *)&count, sizeof(double));
		entries.push_back(CooccEntry(w1, w2, count));
		nentries++;
		if(nentries > SHUFFLE_SIZE){
			std::shuffle(entries.begin(), entries.end(), default_random_engine(0));
			for(CooccEntry entry:entries){
				out_file.write((char*)&entry.w1, sizeof(int32_t));
				out_file.write((char*)&entry.w2, sizeof(int32_t));
				out_file.write((char*)&entry.count, sizeof(double));
			}
			entries.clear();
			nentries = 0;
		}
	}
	in_file.close();
	if(nentries > 0){
		std::shuffle(entries.begin(), entries.end(), default_random_engine(0));
		for(CooccEntry entry:entries){
			out_file.write((char*)&entry.w1, sizeof(int32_t));
			out_file.write((char*)&entry.w2, sizeof(int32_t));
			out_file.write((char*)&entry.count, sizeof(double));
		}
	}
	out_file.close();
	delete[] buf_in;
	delete[] buf_out;
	try{
		remove(path(in_filename));
		rename(path(tmp_filename), path(in_filename));
	}catch(filesystem_error& ex) {
		cerr << ex.what() << endl;
		exit(-1);
	}
}

void coin::SequenceCounter::count(const string& gzfile, const std::string &outfile){
	igzstream in(gzfile.c_str());
	if(!in){
		cerr << gzfile << " not found!!" << endl;
		return;
	}
	string line;
	string word;
	while(in && getline(in, line)){
		preprocess(line);
		istringstream iss(line);
		vector<int> local_words;
		while(iss >> word){
			if(word_indices_.find(word) != word_indices_.end()){
				local_words.push_back(word_indices_[word]);
			}else{
				local_words.push_back(-1);
			}
		}
		int size = local_words.size();
		for(int i = 0;i < size;i++){
			int32_t w1 = local_words[i];
			if(w1 == -1)continue;
			for(int j = i+1;j < size && j <= i+nwindow_;j++){
				int32_t w2 = local_words[j];
				if(w2 == -1)continue;
				double c = 1.;
				if(weight() == 1){
					c = 1. / (j - i);
				}else if(weight() == 2){
					c = 1. + (1. - (j - i)) / nwindow_;
				}
				update_count(w1, w2, c, outfile);
				if(symmetric()){
					update_count(w2, w1, c, outfile);
				}
			}
		}
	}
	in.close();
}