// This file is part of the dune-gdt project:
//   https://github.com/dune-community/dune-gdt
// Copyright 2010-2017 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:
//   Felix Schindler (2013 - 2017)
//   Rene Milk       (2016 - 2017)
//   Tobias Leibner  (2014)

#ifndef DUNE_GDT_OPERATORS_RECONSTRUCTIONS_HH
#define DUNE_GDT_OPERATORS_RECONSTRUCTIONS_HH

#include <type_traits>
#include <limits>

#include <boost/numeric/conversion/cast.hpp>

#include <dune/common/dynmatrix.hh>

#include <dune/geometry/quadraturerules.hh>

#include <dune/xt/common/math.hh>
#include <dune/xt/la/type_traits.hh>
#include <dune/xt/grid/type_traits.hh>
#include <dune/xt/functions/interfaces.hh>
#include <dune/xt/functions/constant.hh>

#include <dune/gdt/discretefunction/default.hh>
#include <dune/gdt/local/integrands/elliptic-ipdg.hh>
#include <dune/gdt/local/operators/lambda.hh>
#include <dune/gdt/operators/base.hh>
#include <dune/gdt/spaces/rt/dune-pdelab-wrapper.hh>

namespace Dune {
namespace GDT {

// ============================================== //
// LocalizableDiffusiveFluxReconstructionOperator //
// ============================================== //

template <class GridLayer,
          class Source,
          class Range,
          LocalEllipticIpdgIntegrands::Method dg_method = LocalEllipticIpdgIntegrands::default_method>
class LocalizableDiffusiveFluxReconstructionOperator : public LocalizableOperatorBase<GridLayer, Source, Range>
{
  static_assert(XT::Functions::is_localizable_function<Source>::value, "");
  static_assert(is_discrete_function<Range>::value, "");
  static_assert(is_rt_space<typename Range::SpaceType>::value, "");

  typedef XT::Grid::extract_entity_t<GridLayer> E;
  static const constexpr size_t d = GridLayer::dimension;
  typedef double D;
  typedef double R;
  typedef XT::Functions::LocalizableFunctionInterface<E, D, d, R, 1> ScalarFunctionType;
  typedef XT::Functions::LocalizableFunctionInterface<E, D, d, R, d, d> TensorFunctionType;
  typedef LocalizableOperatorBase<GridLayer, Source, Range> BaseType;

public:
  using typename BaseType::GridLayerType;
  using typename BaseType::SourceType;
  using typename BaseType::RangeType;

  LocalizableDiffusiveFluxReconstructionOperator(GridLayerType grd_layr,
                                                 const ScalarFunctionType& diffusion_factor,
                                                 const TensorFunctionType& diffusion_tensor,
                                                 const SourceType& src,
                                                 RangeType& rng,
                                                 const size_t over_integrate = 0)
    : BaseType(grd_layr, src, rng)
    , diffusion_factor_(diffusion_factor)
    , diffusion_tensor_(diffusion_tensor)
    , one_(1.)
    , inner_integrand_(diffusion_factor_, diffusion_tensor_)
    , boundary_integrand_(diffusion_factor_, diffusion_tensor_)
    , over_integrate_(over_integrate)
    , tmp_matrix_(1, 1, 0.)
    , tmp_matrix_en_en_(1, 1, 0.)
    , tmp_matrix_en_ne_(1, 1, 0.)
    , tmp_basis_values_(rng.space().mapper().maxNumDofs())
    , local_operator_(
          // The local lambda which does the actual work in the local operator:
          // lmbd_source is src
          // local_range is the local_discrete_function of rng on the current entity
          [&](const auto& lmbd_source, auto& local_range) {
            const auto& entity = local_range.entity();
            const auto local_DoF_indices = local_range.space().local_DoF_indices(entity);
            const auto local_diffusion_factor = diffusion_factor_.local_function(entity);
            const auto local_diffusion_tensor = diffusion_tensor_.local_function(entity);
            const auto local_source = lmbd_source.local_function(entity);
            const auto local_basis = local_range.space().base_function_set(entity);
            const auto local_constant_one = one_.local_function(entity);
            // walk the intersections
            const auto intersection_it_end = this->grid_layer().iend(entity);
            for (auto intersection_it = this->grid_layer().ibegin(entity); intersection_it != intersection_it_end;
                 ++intersection_it) {
              const auto& intersection = *intersection_it;
              if (intersection.neighbor() && !intersection.boundary()) {
                const auto neighbor = intersection.outside();
                if (this->grid_layer().indexSet().index(entity) < this->grid_layer().indexSet().index(neighbor)) {
                  const auto local_diffusion_factor_neighbor = diffusion_factor_.local_function(neighbor);
                  const auto local_diffusion_tensor_neighbor = diffusion_tensor_.local_function(neighbor);
                  const auto local_source_neighbor = lmbd_source.local_function(neighbor);
                  const auto local_constant_one_neighbor = one_.local_function(neighbor);
                  const size_t local_intersection_index = intersection.indexInInside();
                  const size_t local_DoF_index = local_DoF_indices[local_intersection_index];
                  // do a face quadrature
                  R lhs = 0;
                  R rhs = 0;
                  const size_t integrand_order = inner_integrand_.order(*local_diffusion_factor,
                                                                        *local_diffusion_tensor,
                                                                        *local_diffusion_factor_neighbor,
                                                                        *local_diffusion_tensor_neighbor,
                                                                        *local_constant_one,
                                                                        *local_source,
                                                                        *local_constant_one_neighbor,
                                                                        *local_source_neighbor);
                  for (const auto& quadrature_point : QuadratureRules<D, d - 1>::rule(
                           intersection.type(), boost::numeric_cast<int>(integrand_order + over_integrate_))) {
                    const auto& xx_intersection = quadrature_point.position();
                    const auto xx_entity = intersection.geometryInInside().global(xx_intersection);
                    const auto normal = intersection.unitOuterNormal(xx_intersection);
                    const R integration_factor = intersection.geometry().integrationElement(xx_intersection);
                    const R weigth = quadrature_point.weight();
                    // evalaute
                    local_basis.evaluate(xx_entity, tmp_basis_values_);
                    const auto& basis_value = tmp_basis_values_[local_DoF_index];
                    tmp_matrix_en_en_ *= 0.0;
                    tmp_matrix_en_ne_ *= 0.0;
                    inner_integrand_.evaluate(*local_diffusion_factor,
                                              *local_diffusion_tensor,
                                              *local_diffusion_factor_neighbor,
                                              *local_diffusion_tensor_neighbor,
                                              *local_constant_one,
                                              *local_source,
                                              *local_constant_one_neighbor,
                                              *local_source_neighbor,
                                              intersection,
                                              xx_intersection,
                                              tmp_matrix_en_en_, // <- we are interested in this one
                                              tmp_matrix_,
                                              tmp_matrix_en_ne_, // <- and this one
                                              tmp_matrix_);
                    // compute integrals
                    assert(tmp_matrix_en_en_.rows() >= 1);
                    assert(tmp_matrix_en_en_.cols() >= 1);
                    assert(tmp_matrix_en_ne_.rows() >= 1);
                    assert(tmp_matrix_en_ne_.cols() >= 1);
                    lhs += integration_factor * weigth * (basis_value * normal);
                    rhs += integration_factor * weigth * (tmp_matrix_en_en_[0][0] + tmp_matrix_en_ne_[0][0]);
                  } // do a face quadrature
                  // set DoF
                  if (XT::Common::isnan(rhs) || XT::Common::isinf(rhs))
                    local_range.vector().set(local_DoF_index, 0.);
                  else
                    local_range.vector().set(local_DoF_index, rhs / lhs);
                }
              } else if (intersection.boundary() && !intersection.neighbor()) {
                const size_t local_intersection_index = intersection.indexInInside();
                const size_t local_DoF_index = local_DoF_indices[local_intersection_index];
                // do a face quadrature
                R lhs = 0;
                R rhs = 0;
                const size_t integrand_order = boundary_integrand_.order(
                    *local_diffusion_factor, *local_diffusion_tensor, *local_source, *local_constant_one);
                for (const auto& quadrature_point : QuadratureRules<D, d - 1>::rule(
                         intersection.type(), boost::numeric_cast<int>(integrand_order + over_integrate_))) {
                  const auto xx_intersection = quadrature_point.position();
                  const auto normal = intersection.unitOuterNormal(xx_intersection);
                  const R integration_factor = intersection.geometry().integrationElement(xx_intersection);
                  const R weigth = quadrature_point.weight();
                  const auto xx_entity = intersection.geometryInInside().global(xx_intersection);
                  // evalaute
                  local_basis.evaluate(xx_entity, tmp_basis_values_);
                  const auto& basis_value = tmp_basis_values_[local_DoF_index];
                  tmp_matrix_ *= 0.0;
                  boundary_integrand_.evaluate(*local_diffusion_factor,
                                               *local_diffusion_tensor,
                                               *local_constant_one,
                                               *local_source,
                                               intersection,
                                               xx_intersection,
                                               tmp_matrix_);
                  // compute integrals
                  assert(tmp_matrix_.rows() >= 1);
                  assert(tmp_matrix_.cols() >= 1);
                  lhs += integration_factor * weigth * (basis_value * normal);
                  rhs += integration_factor * weigth * tmp_matrix_[0][0];
                } // do a face quadrature
                // set DoF
                if (XT::Common::isnan(rhs) || XT::Common::isinf(rhs))
                  local_range.vector().set(local_DoF_index, 0.);
                else
                  local_range.vector().set(local_DoF_index, rhs / lhs);
              } else
                DUNE_THROW(XT::Common::Exceptions::internal_error, "Unknown intersection type!");
            } // walk the intersections
          })
  {
    this->range_.vector() *= 0.;
    this->append(local_operator_);
  }

  void prepare() override final
  {
    this->range_.vector() *= 0.;
  }

private:
  const ScalarFunctionType& diffusion_factor_;
  const TensorFunctionType& diffusion_tensor_;
  const XT::Functions::ConstantFunction<E, D, d, R, 1> one_;
  const LocalEllipticIpdgIntegrands::Inner<ScalarFunctionType, TensorFunctionType, dg_method> inner_integrand_;
  const LocalEllipticIpdgIntegrands::BoundaryLHS<ScalarFunctionType, TensorFunctionType, dg_method> boundary_integrand_;
  const size_t over_integrate_;
  DynamicMatrix<R> tmp_matrix_;
  DynamicMatrix<R> tmp_matrix_en_en_;
  DynamicMatrix<R> tmp_matrix_en_ne_;
  std::vector<FieldVector<R, d>> tmp_basis_values_;
  const LocalLambdaOperator<ScalarFunctionType, typename Range::SpaceType, typename Range::VectorType> local_operator_;
}; // class LocalizableDiffusiveFluxReconstructionOperator

// =================================== //
// DiffusiveFluxReconstructionOperator //
// =================================== //

template <class GridLayerType,
          class DiffusionFactorType,
          class DiffusionTensorType = void,
          LocalEllipticIpdgIntegrands::Method method = LocalEllipticIpdgIntegrands::default_method>
class DiffusiveFluxReconstructionOperator;

template <class GridLayerType,
          class DiffusionFactorType,
          class DiffusionTensorType,
          LocalEllipticIpdgIntegrands::Method method>
class DiffusiveFluxReconstructionOperator
{
  static_assert(GridLayerType::dimension == 2, "Only implemented for dimDomain 2 at the moment!");
  static_assert(Dune::XT::Functions::is_localizable_function<DiffusionFactorType>::value,
                "DiffusionFactorType has to be tagged as XT::Functions::is_localizable_function!");
  static_assert(Dune::XT::Functions::is_localizable_function<DiffusionTensorType>::value,
                "DiffusionTensorType has to be tagged as XT::Functions::is_localizable_function!");

public:
  using EntityType = XT::Grid::extract_entity_t<GridLayerType>;
  typedef typename GridLayerType::ctype DomainFieldType;
  static const size_t dimDomain = GridLayerType::dimension;
  typedef typename DiffusionFactorType::RangeFieldType FieldType;
  typedef typename DiffusionFactorType::DomainType DomainType;

private:
  static_assert(dimDomain == 2, "Not implemented!");

public:
  DiffusiveFluxReconstructionOperator(const GridLayerType& grid_layer,
                                      const DiffusionFactorType& diffusion_factor,
                                      const DiffusionTensorType& diffusion_tensor,
                                      const size_t over_integrate = 0)
    : grid_layer_(grid_layer)
    , diffusion_factor_(diffusion_factor)
    , diffusion_tensor_(diffusion_tensor)
    , over_integrate_(over_integrate)
  {
  }

  template <class GL, class V>
  void
  apply(const XT::Functions::LocalizableFunctionInterface<EntityType, DomainFieldType, dimDomain, FieldType, 1>& source,
        DiscreteFunction<DunePdelabRtSpaceWrapper<GL, 0, FieldType, dimDomain>, V>& range) const
  {
    const auto& rtn0_space = range.space();
    auto& range_vector = range.vector();
    const FieldType infinity = std::numeric_limits<FieldType>::infinity();
    for (size_t ii = 0; ii < range_vector.size(); ++ii)
      range_vector[ii] = infinity;
    const LocalEllipticIpdgIntegrands::Inner<DiffusionFactorType, DiffusionTensorType, method> inner_evaluation(
        diffusion_factor_, diffusion_tensor_);
    const LocalEllipticIpdgIntegrands::BoundaryLHS<DiffusionFactorType, DiffusionTensorType, method>
        boundary_evaluation(diffusion_factor_, diffusion_tensor_);
    const XT::Functions::ConstantFunction<EntityType, DomainFieldType, dimDomain, FieldType, 1> constant_one(1);
    DomainType normal(0);
    DomainType xx_entity(0);
    DynamicMatrix<FieldType> tmp_matrix(1, 1, 0);
    DynamicMatrix<FieldType> tmp_matrix_en_en(1, 1, 0);
    DynamicMatrix<FieldType> tmp_matrix_en_ne(1, 1, 0);
    std::vector<typename DunePdelabRtSpaceWrapper<GL, 0, FieldType, dimDomain>::BaseFunctionSetType::RangeType>
        basis_values(rtn0_space.mapper().maxNumDofs(),
                     typename DunePdelabRtSpaceWrapper<GL, 0, FieldType, dimDomain>::BaseFunctionSetType::RangeType(0));
    // walk the grid
    const auto entity_it_end = grid_layer_.template end<0>();
    for (auto entity_it = grid_layer_.template begin<0>(); entity_it != entity_it_end; ++entity_it) {
      const auto& entity = *entity_it;
      const auto local_DoF_indices = rtn0_space.local_DoF_indices(entity);
      const auto global_DoF_indices = rtn0_space.mapper().globalIndices(entity);
      assert(global_DoF_indices.size() == local_DoF_indices.size());
      const auto local_diffusion_factor = diffusion_factor_.local_function(entity);
      const auto local_diffusion_tensor = diffusion_tensor_.local_function(entity);
      const auto local_source = source.local_function(entity);
      const auto local_basis = rtn0_space.base_function_set(entity);
      const auto local_constant_one = constant_one.local_function(entity);
      // walk the intersections
      const auto intersection_it_end = grid_layer_.iend(entity);
      for (auto intersection_it = grid_layer_.ibegin(entity); intersection_it != intersection_it_end;
           ++intersection_it) {
        const auto& intersection = *intersection_it;
        if (intersection.neighbor() && !intersection.boundary()) {
          const auto neighbor = intersection.outside();
          if (grid_layer_.indexSet().index(entity) < grid_layer_.indexSet().index(neighbor)) {
            const auto local_diffusion_factor_neighbor = diffusion_factor_.local_function(neighbor);
            const auto local_diffusion_tensor_neighbor = diffusion_tensor_.local_function(neighbor);
            const auto local_source_neighbor = source.local_function(neighbor);
            const auto local_constant_one_neighbor = constant_one.local_function(neighbor);
            const size_t local_intersection_index = intersection.indexInInside();
            const size_t local_DoF_index = local_DoF_indices[local_intersection_index];
            // do a face quadrature
            FieldType lhs = 0;
            FieldType rhs = 0;
            const size_t integrand_order = inner_evaluation.order(*local_diffusion_factor,
                                                                  *local_diffusion_tensor,
                                                                  *local_diffusion_factor_neighbor,
                                                                  *local_diffusion_tensor_neighbor,
                                                                  *local_constant_one,
                                                                  *local_source,
                                                                  *local_constant_one_neighbor,
                                                                  *local_source_neighbor);
            const auto& quadrature = QuadratureRules<DomainFieldType, dimDomain - 1>::rule(
                intersection.type(), boost::numeric_cast<int>(integrand_order + over_integrate_));
            const auto quadrature_it_end = quadrature.end();
            for (auto quadrature_it = quadrature.begin(); quadrature_it != quadrature_it_end; ++quadrature_it) {
              const auto& xx_intersection = quadrature_it->position();
              xx_entity = intersection.geometryInInside().global(xx_intersection);
              normal = intersection.unitOuterNormal(xx_intersection);
              const FieldType integration_factor = intersection.geometry().integrationElement(xx_intersection);
              const FieldType weigth = quadrature_it->weight();
              // evalaute
              local_basis.evaluate(xx_entity, basis_values);
              const auto& basis_value = basis_values[local_DoF_index];
              tmp_matrix_en_en *= 0.0;
              tmp_matrix_en_ne *= 0.0;
              inner_evaluation.evaluate(*local_diffusion_factor,
                                        *local_diffusion_tensor,
                                        *local_diffusion_factor_neighbor,
                                        *local_diffusion_tensor_neighbor,
                                        *local_constant_one,
                                        *local_source,
                                        *local_constant_one_neighbor,
                                        *local_source_neighbor,
                                        intersection,
                                        xx_intersection,
                                        tmp_matrix_en_en, // <- we are interested in this one
                                        tmp_matrix,
                                        tmp_matrix_en_ne, // <- and this one
                                        tmp_matrix);
              // compute integrals
              assert(tmp_matrix_en_en.rows() >= 1);
              assert(tmp_matrix_en_en.cols() >= 1);
              assert(tmp_matrix_en_ne.rows() >= 1);
              assert(tmp_matrix_en_ne.cols() >= 1);
              lhs += integration_factor * weigth * (basis_value * normal);
              rhs += integration_factor * weigth * (tmp_matrix_en_en[0][0] + tmp_matrix_en_ne[0][0]);
            } // do a face quadrature
            // set DoF
            const size_t global_DoF_index = global_DoF_indices[local_DoF_index];
            // and make sure we are the first to do so
            assert(!(range_vector[global_DoF_index] < infinity));
            if (XT::Common::isnan(rhs) || XT::Common::isinf(rhs))
              range_vector[global_DoF_index] = 0.;
            else
              range_vector[global_DoF_index] = rhs / lhs;
          }
        } else if (intersection.boundary() && !intersection.neighbor()) {
          const size_t local_intersection_index = intersection.indexInInside();
          const size_t local_DoF_index = local_DoF_indices[local_intersection_index];
          // do a face quadrature
          FieldType lhs = 0;
          FieldType rhs = 0;
          const size_t integrand_order = boundary_evaluation.order(
              *local_diffusion_factor, *local_diffusion_tensor, *local_source, *local_constant_one);
          const auto& quadrature = QuadratureRules<DomainFieldType, dimDomain - 1>::rule(
              intersection.type(), boost::numeric_cast<int>(integrand_order + over_integrate_));
          const auto quadrature_it_end = quadrature.end();
          for (auto quadrature_it = quadrature.begin(); quadrature_it != quadrature_it_end; ++quadrature_it) {
            const auto xx_intersection = quadrature_it->position();
            normal = intersection.unitOuterNormal(xx_intersection);
            const FieldType integration_factor = intersection.geometry().integrationElement(xx_intersection);
            const FieldType weigth = quadrature_it->weight();
            xx_entity = intersection.geometryInInside().global(xx_intersection);
            // evalaute
            local_basis.evaluate(xx_entity, basis_values);
            const auto& basis_value = basis_values[local_DoF_index];
            tmp_matrix *= 0.0;
            boundary_evaluation.evaluate(*local_diffusion_factor,
                                         *local_diffusion_tensor,
                                         *local_constant_one,
                                         *local_source,
                                         intersection,
                                         xx_intersection,
                                         tmp_matrix);
            // compute integrals
            assert(tmp_matrix.rows() >= 1);
            assert(tmp_matrix.cols() >= 1);
            lhs += integration_factor * weigth * (basis_value * normal);
            rhs += integration_factor * weigth * tmp_matrix[0][0];
          } // do a face quadrature
          // set DoF
          const size_t global_DoF_index = global_DoF_indices[local_DoF_index];
          // and make sure we are the first to do so
          assert(!(range_vector[global_DoF_index] < infinity));
          if (XT::Common::isnan(rhs) || XT::Common::isinf(rhs))
            range_vector[global_DoF_index] = 0.;
          else
            range_vector[global_DoF_index] = rhs / lhs;
        } else
          DUNE_THROW(XT::Common::Exceptions::internal_error, "Unknown intersection type!");
      } // walk the intersections
    } // walk the grid
  } // ... apply(...)

private:
  const GridLayerType& grid_layer_;
  const DiffusionFactorType& diffusion_factor_;
  const DiffusionTensorType& diffusion_tensor_;
  const size_t over_integrate_;
}; // class DiffusiveFluxReconstructionOperator

/**
 *  \todo Add more static checks that GridLayerType and LocalizableFunctionType match.
 *  \todo Derive from operator interfaces.
 */
template <class GridLayerType, class LocalizableFunctionType, LocalEllipticIpdgIntegrands::Method method>
class DiffusiveFluxReconstructionOperator<GridLayerType, LocalizableFunctionType, void, method>
{
  static_assert(GridLayerType::dimension == 2, "Only implemented for dimDomain 2 at the moment!");
  static_assert(XT::Functions::is_localizable_function<LocalizableFunctionType>::value,
                "LocalizableFunctionType has to be tagged as XT::Functions::is_localizable_function!");

public:
  using EntityType = XT::Grid::extract_entity_t<GridLayerType>;
  typedef typename GridLayerType::ctype DomainFieldType;
  static const size_t dimDomain = GridLayerType::dimension;
  typedef typename LocalizableFunctionType::RangeFieldType FieldType;
  typedef typename LocalizableFunctionType::DomainType DomainType;

private:
  static_assert(dimDomain == 2, "Not implemented!");

public:
  DiffusiveFluxReconstructionOperator(const GridLayerType& grid_layer,
                                      const LocalizableFunctionType& diffusion,
                                      const size_t over_integrate = 0)
    : grid_layer_(grid_layer)
    , diffusion_(diffusion)
    , over_integrate_(over_integrate)
  {
  }

  template <class GL, class V>
  void
  apply(const XT::Functions::LocalizableFunctionInterface<EntityType, DomainFieldType, dimDomain, FieldType, 1>& source,
        DiscreteFunction<DunePdelabRtSpaceWrapper<GL, 0, FieldType, dimDomain>, V>& range) const
  {
    const auto& rtn0_space = range.space();
    auto& range_vector = range.vector();
    const FieldType infinity = std::numeric_limits<FieldType>::infinity();
    for (size_t ii = 0; ii < range_vector.size(); ++ii)
      range_vector[ii] = infinity;
    const LocalEllipticIpdgIntegrands::Inner<LocalizableFunctionType, void, method> inner_evaluation(diffusion_);
    const LocalEllipticIpdgIntegrands::BoundaryLHS<LocalizableFunctionType, void, method> boundary_evaluation(
        diffusion_);
    const XT::Functions::ConstantFunction<EntityType, DomainFieldType, dimDomain, FieldType, 1> constant_one(1);
    DomainType normal(0);
    DomainType xx_entity(0);
    DynamicMatrix<FieldType> tmp_matrix(1, 1, 0);
    DynamicMatrix<FieldType> tmp_matrix_en_en(1, 1, 0);
    DynamicMatrix<FieldType> tmp_matrix_en_ne(1, 1, 0);
    std::vector<typename DunePdelabRtSpaceWrapper<GL, 0, FieldType, dimDomain>::BaseFunctionSetType::RangeType>
        basis_values(rtn0_space.mapper().maxNumDofs(),
                     typename DunePdelabRtSpaceWrapper<GL, 0, FieldType, dimDomain>::BaseFunctionSetType::RangeType(0));
    // walk the grid
    const auto entity_it_end = grid_layer_.template end<0>();
    for (auto entity_it = grid_layer_.template begin<0>(); entity_it != entity_it_end; ++entity_it) {
      const auto& entity = *entity_it;
      const auto local_DoF_indices = rtn0_space.local_DoF_indices(entity);
      const auto global_DoF_indices = rtn0_space.mapper().globalIndices(entity);
      assert(global_DoF_indices.size() == local_DoF_indices.size());
      const auto local_diffusion = diffusion_.local_function(entity);
      const auto local_source = source.local_function(entity);
      const auto local_basis = rtn0_space.base_function_set(entity);
      const auto local_constant_one = constant_one.local_function(entity);
      // walk the intersections
      const auto intersection_it_end = grid_layer_.iend(entity);
      for (auto intersection_it = grid_layer_.ibegin(entity); intersection_it != intersection_it_end;
           ++intersection_it) {
        const auto& intersection = *intersection_it;
        if (intersection.neighbor() && !intersection.boundary()) {
          const auto neighbor = intersection.outside();
          if (grid_layer_.indexSet().index(entity) < grid_layer_.indexSet().index(neighbor)) {
            const auto local_diffusion_neighbor = diffusion_.local_function(neighbor);
            const auto local_source_neighbor = source.local_function(neighbor);
            const auto local_constant_one_neighbor = constant_one.local_function(neighbor);
            const size_t local_intersection_index = intersection.indexInInside();
            const size_t local_DoF_index = local_DoF_indices[local_intersection_index];
            // do a face quadrature
            FieldType lhs = 0;
            FieldType rhs = 0;
            const size_t integrand_order = inner_evaluation.order(*local_diffusion,
                                                                  *local_diffusion_neighbor,
                                                                  *local_constant_one,
                                                                  *local_source,
                                                                  *local_constant_one_neighbor,
                                                                  *local_source_neighbor);
            const auto& quadrature = QuadratureRules<DomainFieldType, dimDomain - 1>::rule(
                intersection.type(), boost::numeric_cast<int>(integrand_order + over_integrate_));
            const auto quadrature_it_end = quadrature.end();
            for (auto quadrature_it = quadrature.begin(); quadrature_it != quadrature_it_end; ++quadrature_it) {
              const auto& xx_intersection = quadrature_it->position();
              xx_entity = intersection.geometryInInside().global(xx_intersection);
              normal = intersection.unitOuterNormal(xx_intersection);
              const auto integration_factor = intersection.geometry().integrationElement(xx_intersection);
              const auto weigth = quadrature_it->weight();
              // evalaute
              local_basis.evaluate(xx_entity, basis_values);
              const auto& basis_value = basis_values[local_DoF_index];
              tmp_matrix_en_en *= 0.0;
              tmp_matrix_en_ne *= 0.0;
              inner_evaluation.evaluate(*local_diffusion,
                                        *local_diffusion_neighbor,
                                        *local_constant_one,
                                        *local_source,
                                        *local_constant_one_neighbor,
                                        *local_source_neighbor,
                                        intersection,
                                        xx_intersection,
                                        tmp_matrix_en_en, // <- we are interested in this one
                                        tmp_matrix,
                                        tmp_matrix_en_ne, // <- and this one
                                        tmp_matrix);
              // compute integrals
              assert(tmp_matrix_en_en.rows() >= 1);
              assert(tmp_matrix_en_en.cols() >= 1);
              assert(tmp_matrix_en_ne.rows() >= 1);
              assert(tmp_matrix_en_ne.cols() >= 1);
              lhs += integration_factor * weigth * (basis_value * normal);
              rhs += integration_factor * weigth * (tmp_matrix_en_en[0][0] + tmp_matrix_en_ne[0][0]);
            } // do a face quadrature
            // set DoF
            const size_t global_DoF_index = global_DoF_indices[local_DoF_index];
            // and make sure we are the first to do so
            assert(!(range_vector[global_DoF_index] < infinity));
            range_vector[global_DoF_index] = rhs / lhs;
          }
        } else if (intersection.boundary() && !intersection.neighbor()) {
          const size_t local_intersection_index = intersection.indexInInside();
          const size_t local_DoF_index = local_DoF_indices[local_intersection_index];
          // do a face quadrature
          FieldType lhs = 0;
          FieldType rhs = 0;
          const size_t integrand_order =
              boundary_evaluation.order(*local_diffusion, *local_source, *local_constant_one);
          const auto& quadrature = QuadratureRules<DomainFieldType, dimDomain - 1>::rule(
              intersection.type(), boost::numeric_cast<int>(integrand_order + over_integrate_));
          const auto quadrature_it_end = quadrature.end();
          for (auto quadrature_it = quadrature.begin(); quadrature_it != quadrature_it_end; ++quadrature_it) {
            const auto xx_intersection = quadrature_it->position();
            normal = intersection.unitOuterNormal(xx_intersection);
            const auto integration_factor = intersection.geometry().integrationElement(xx_intersection);
            const auto weigth = quadrature_it->weight();
            xx_entity = intersection.geometryInInside().global(xx_intersection);
            // evalaute
            local_basis.evaluate(xx_entity, basis_values);
            const auto& basis_value = basis_values[local_DoF_index];
            tmp_matrix *= 0.0;
            boundary_evaluation.evaluate(
                *local_diffusion, *local_constant_one, *local_source, intersection, xx_intersection, tmp_matrix);
            // compute integrals
            assert(tmp_matrix.rows() >= 1);
            assert(tmp_matrix.cols() >= 1);
            lhs += integration_factor * weigth * (basis_value * normal);
            rhs += integration_factor * weigth * tmp_matrix[0][0];
          } // do a face quadrature
          // set DoF
          const size_t global_DoF_index = global_DoF_indices[local_DoF_index];
          // and make sure we are the first to do so
          assert(!(range_vector[global_DoF_index] < infinity));
          range_vector[global_DoF_index] = rhs / lhs;
        } else
          DUNE_THROW(XT::Common::Exceptions::internal_error, "Unknown intersection type!");
      } // walk the intersections
    } // walk the grid
  } // ... apply(...)

private:
  const GridLayerType& grid_layer_;
  const LocalizableFunctionType& diffusion_;
  const size_t over_integrate_;
}; // class DiffusiveFluxReconstructionOperator

template <LocalEllipticIpdgIntegrands::Method m, class GL, class DF, class DT>
DiffusiveFluxReconstructionOperator<GL, DF, DT, m> make_diffusive_flux_reconstruction_operator(
    const GL& grid_layer, const DF& diffusion_factor, const DT& diffusion_tensor, const size_t over_integrate = 0)
{
  return DiffusiveFluxReconstructionOperator<GL, DF, DT, m>(
      grid_layer, diffusion_factor, diffusion_tensor, over_integrate);
}

} // namespace GDT
} // namespace Dune

#endif // DUNE_GDT_OPERATORS_RECONSTRUCTIONS_HH


/*
 * Test vectors generated from the crc-catalogue:
 * http://reveng.sourceforge.net/crc-catalogue/
 *
 *                 Copyright MartynP 2016.
 * Distributed under the Boost Software License, Version 1.0.
 *    (See accompanying file LICENSE_1_0.txt or copy at
 *          http://www.boost.org/LICENSE_1_0.txt)
 */

#include <cstdio>
#include <iostream>
#include <list>
#include <boost/dynamic_bitset.hpp>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "Test the bruteforce functionality"
#include <boost/test/unit_test.hpp>

#include "../crc.hpp"
#include "../bf_crc.hpp"

#define CRCFOURTEEN
#define CRCSIZTEEN
#define CRCTHIRTYTWO

char getRandomChar(){
  static char c = 'A' + rand()%24;
  return c;
}

uint32_t calculate_crc(uint32_t crc_width, uint8_t data[], size_t length, bf_crc::crc_model_t model)
{
	bf_crc::crc_t crc(crc_width);
	boost::dynamic_bitset<> msg = bf_crc::convert_uint8_to_bitset(data, length);

	crc.set(model.polynomial, model.initial, model.final_xor, model.reflected_input, model.reflected_output);
	crc.calc_crc(model.initial, msg);

	return crc.checksum();
}

uint32_t calculate_crc(uint32_t crc_width, uint8_t data[], size_t length, bf_crc::crc_model_t model, boost::dynamic_bitset<> *msg)
{
	bf_crc::crc_t crc(crc_width);

	*msg = bf_crc::convert_uint8_to_bitset(data, length);

	crc.set(model.polynomial, model.initial, model.final_xor, model.reflected_input, model.reflected_output);
	crc.calc_crc(model.initial, *msg);

	return crc.checksum();
}

#ifdef CRCFOURTEEN
BOOST_AUTO_TEST_CASE(crcFourteen)
{
	// 14 bit CRC's
	bf_crc *crc_bruteforce;
	uint16_t crc_width = 14;
	bf_crc::crc_t crc(crc_width);
	boost::dynamic_bitset<> msg;

	// Lets make sure things are random
	srand((unsigned)time(0));

	std::vector<bf_crc::test_vector_t> test_vectors;
	bf_crc::test_vector_t test_vector;

	/*
	 * CRC-14/DARC
	 * width=14 poly=0x0805 init=0x0000 refin=true refout=true xorout=0x0000 check=0x082d name="CRC-14/DARC"
	 */

	bf_crc::crc_model_t model(0x0805, 0x0000, 0x0000, true, true, my_crc_basic::AUTO);

	// REVENG Check
	uint8_t data_0[] = {0x31, 0x32, 0x33, 0x34, 0x35, 0x36, 0x37, 0x38, 0x39};
	test_vector.crc = calculate_crc(crc_width, data_0, sizeof(data_0), model, (boost::dynamic_bitset<>*)&msg);
	test_vector.message = msg;
	test_vectors.push_back(test_vector);

	// Check the CRC engine is still working...
	BOOST_CHECK(test_vector.crc == 0x082D);

	for (int i = 0; i < 10; i++) {

		uint8_t data[9];
		for (int b = 0; b < 9; b++)
			data[b] = getRandomChar();

		test_vector.crc = calculate_crc(crc_width, data, sizeof(data_0), model, (boost::dynamic_bitset<>*)&msg);
		test_vector.message = msg;
		test_vectors.push_back(test_vector);

	}

	crc_bruteforce = new bf_crc(crc_width, 		// CRC Width
								0, 			// Polynomial
								false, 		// Probe Final XOR?
								0, 			// Final XOR
								true,   	// Probe Initial?
								0, 			// Initial
								true, 		// Probe Reflected Input?
				    true,		// Probe Reflected Output?
				    my_crc_basic::AUTO);

	crc_bruteforce->set_verbose(true);
	crc_bruteforce->do_brute_force(4, test_vectors);

	// Get results
	std::vector<bf_crc::crc_model_t> results = crc_bruteforce->crc_model_match();
	bool match = false;
	for (size_t i = 0; i < results.size(); i++) {
		if (results[i].polynomial == model.polynomial &&
			results[i].initial == model.initial &&
			results[i].final_xor == model.final_xor &&
			results[i].reflected_output == model.reflected_output &&
			results[i].reflected_input == model.reflected_input)
			match = true;
	}

	// Check the correct model was one of those identified
	BOOST_CHECK(match == true);

}
#endif

#ifdef CRCSIXTEEN
BOOST_AUTO_TEST_CASE(crcSixteen)
{
	// 16 bit CRC's
	bf_crc *crc_bruteforce;
	uint16_t crc_width = 16;
	bf_crc::crc_t crc(crc_width);
	boost::dynamic_bitset<> msg;

	// Lets make sure things are random
	srand((unsigned)time(0));

	std::vector<bf_crc::test_vector_t> test_vectors;
	bf_crc::test_vector_t test_vector;

	/*
	 * CRC-16/CCITT-FALSE
	 * width=16 poly=0x1021 init=0xffff refin=false refout=false xorout=0x0000 check=0x29b1 name="CRC-16/CCITT-FALSE"
	 */

	// Model of the CRC to brute force
	bf_crc::crc_model_t model(0x1021, 0xFFFF, 0x0000, false, false);

	// REVENG Check
	uint8_t data_0[] = {0x31, 0x32, 0x33, 0x34, 0x35, 0x36, 0x37, 0x38, 0x39};
	test_vector.crc = calculate_crc(crc_width, data_0, sizeof(data_0), model, (boost::dynamic_bitset<>*)&msg);
	test_vector.message = msg;
	test_vectors.push_back(test_vector);

	// Check the CRC engine is still working...
	BOOST_CHECK(test_vector.crc == 0x29B1);

	for (int i = 0; i < 10; i++) {

		uint8_t data[9];
		for (int b = 0; b < 9; b++)
			data[b] = getRandomChar();

		test_vector.crc = calculate_crc(crc_width, data, sizeof(data_0), model, (boost::dynamic_bitset<>*)&msg);
		test_vector.message = msg;
		test_vectors.push_back(test_vector);

	}

	crc_bruteforce = new bf_crc(crc_width, 	// CRC Width
								0, 			// Polynomial
								false, 		// Probe Final XOR?
								0, 			// Final XOR
								true,   	// Probe Initial?
								0, 			// Initial
								false, 		// Probe Reflected Input?
				    false,		// Probe Reflected Output?
				    my_crc_basic::AUTO);

	crc_bruteforce->set_verbose(true);
	crc_bruteforce->do_brute_force(4, test_vectors);

	// Get results
	std::vector<bf_crc::crc_model_t> results = crc_bruteforce->crc_model_match();
	bool match = false;
	for (size_t i = 0; i < results.size(); i++) {
		if (results[i].polynomial == model.polynomial &&
			results[i].initial == model.initial &&
			results[i].final_xor == model.final_xor &&
			results[i].reflected_output == model.reflected_output &&
			results[i].reflected_input == model.reflected_input)
			match = true;
	}

	// Check the correct model was one of those identified
	BOOST_CHECK(match == true);

}
#endif

#ifdef CRCTHIRTYTWO
BOOST_AUTO_TEST_CASE(crcThirtyTwo)
{
	// 32 bit CRC's
	bf_crc *crc_bruteforce;
	uint16_t crc_width = 32;
	bf_crc::crc_t crc(crc_width);
	boost::dynamic_bitset<> msg;

	// Lets make sure things are random
	srand((unsigned)time(0));

	std::vector<bf_crc::test_vector_t> test_vectors;
	bf_crc::test_vector_t test_vector;

	/*
	 * CRC-32/BZIP2
	 * width=32 poly=0x04c11db7 init=0xffffffff refin=false refout=false xorout=0xffffffff check=0xfc891918 name="CRC-32/BZIP2"
	 */

	// Model of the CRC to be brute forced
	bf_crc::crc_model_t model(0x04c11db7, 0xffffffff, 0xffffffff, false, false, my_crc_basic::AUTO);

	// REVENG Check
	uint8_t data_0[] = {0x31, 0x32, 0x33, 0x34, 0x35, 0x36, 0x37, 0x38, 0x39};
	test_vector.crc = calculate_crc(crc_width, data_0, sizeof(data_0), model, (boost::dynamic_bitset<>*)&msg);
	test_vector.message = msg;
	test_vectors.push_back(test_vector);

	// Check the CRC engine worked
	BOOST_CHECK(test_vector.crc == 0xFC891918);

	// Make another 10 test vectors
	for (int i = 0; i < 10; i++) {

		// Create some random data
		uint8_t data[9];
		for (int b = 0; b < 9; b++)
			data[b] = getRandomChar();

		test_vector.crc = calculate_crc(crc_width, data, sizeof(data), model, (boost::dynamic_bitset<>*)&msg);
		test_vector.message = msg;
		test_vectors.push_back(test_vector);

	}

	crc_bruteforce = new bf_crc(crc_width, 	// CRC Width
								0, 			// Polynomial
								false, 		// Probe Final XOR?
								0xFFFFFFFF, // Final XOR
								false,   	// Probe Initial?
								0xFFFFFFFF, // Initial
								false, 		// Probe Reflected Input?
				    false,		// Probe Reflected Output?
				    my_crc_basic::AUTO);

	crc_bruteforce->set_verbose(true);
	crc_bruteforce->set_polynomial_start(0x00000000);
	crc_bruteforce->set_polynomial_end(0x10000000);
	crc_bruteforce->do_brute_force(4, test_vectors);

	// Get results
	std::vector<bf_crc::crc_model_t> results = crc_bruteforce->crc_model_match();
	bool match = false;
	for (size_t i = 0; i < results.size(); i++) {
		if (results[i].polynomial == model.polynomial &&
			results[i].initial == model.initial &&
			results[i].final_xor == model.final_xor &&
			results[i].reflected_output == model.reflected_output &&
			results[i].reflected_input == model.reflected_input)
			match = true;
	}

	// Test to make sure the model was identified
	BOOST_CHECK(match == true);

}
#endif


#include <bdd_model.hpp>

#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <map>
#include <chrono>

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>

using namespace verilog::bdd;
using namespace std::chrono;

int main(int nargs, char** argv){

  BDD a("a");
  BDD b("b");

  BDD c = a & b;

  std::cout << a;
  std::cout << b;
  std::cout << c;

  return 0;
}


#include <boost/multiprecision/integer.hpp>


//==============================================================================
//         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_TOOLBOX_ARITHMETIC_FUNCTIONS_SCALAR_TWO_ADD_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SCALAR_TWO_ADD_HPP_INCLUDED
#include <boost/simd/toolbox/arithmetic/functions/two_add.hpp>
#include <boost/fusion/tuple.hpp>
#include <boost/simd/include/functions/scalar/is_invalid.hpp>
#include <boost/simd/include/constants/zero.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::two_add_, tag::cpu_,
                          (A0),
                          ((scalar_<floating_<A0> >))
                          ((scalar_<floating_<A0> >))
                          ((scalar_<floating_<A0> >))
                          ((scalar_<floating_<A0> >))
                         )
  {
    typedef int result_type;
    inline result_type operator()(A0 const& a,A0 const& b,
                                  A0 & r0,A0 & r1) const
    {
      r0 = a+b;
      if (is_invalid(r0))
      {
        r1 = Zero<A0>();
      }
      else
      {
        A0 z = (r0-a);
        r1 =  (a-(r0-z))+(b-z);
      }
      return 0;
    }
  };
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::two_add_, tag::cpu_,
                          (A0),
                          ((scalar_<floating_<A0> >))
                          ((scalar_<floating_<A0> >))
                          ((scalar_<floating_<A0> >))
                         )
  {
    typedef A0 result_type;
    inline result_type operator()(A0 const& a0,A0 const& a1,A0 & a3) const
    {
      A0 a2;
      two_add(a0, a1, a2, a3);
      return a2;
    }
  };
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::two_add_, tag::cpu_,
                          (A0),
                          ((scalar_<floating_<A0> >))
                          ((scalar_<floating_<A0> >))
                         )
  {
    typedef typename boost::fusion::tuple<A0,A0>        result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      result_type res;
      two_add(a0,a1, boost::fusion::at_c<0>(res),boost::fusion::at_c<1>(res));
      return res;
    }
  };
} } }

#endif


#include <lwr_controllers/dynamic_sliding_mode_controller_task_space.h>
#include <utils/pseudo_inversion.h>
#include <utils/skew_symmetric.h>
#include <pluginlib/class_list_macros.h>
#include <kdl_parser/kdl_parser.hpp>
#include <Eigen/LU>
#include <math.h>

namespace lwr_controllers
{
	DynamicSlidingModeControllerTaskSpace::DynamicSlidingModeControllerTaskSpace() {}
	DynamicSlidingModeControllerTaskSpace::~DynamicSlidingModeControllerTaskSpace() {}

	bool DynamicSlidingModeControllerTaskSpace::init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n)
	{
        KinematicChainControllerBase<hardware_interface::EffortJointInterface>::init(robot, n);

		jnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(kdl_chain_));
		id_solver_.reset(new KDL::ChainDynParam(kdl_chain_,gravity_));
		fk_pos_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_));

		joint_ref_.resize(kdl_chain_.getNrOfJoints());
		tau_.resize(kdl_chain_.getNrOfJoints());
		J_.resize(kdl_chain_.getNrOfJoints());
		M_.resize(kdl_chain_.getNrOfJoints());
		C_.resize(kdl_chain_.getNrOfJoints());
		G_.resize(kdl_chain_.getNrOfJoints());

		S_.resize(kdl_chain_.getNrOfJoints());
		S0_.resize(kdl_chain_.getNrOfJoints());
		Sd_.resize(kdl_chain_.getNrOfJoints());
		Sq_.resize(kdl_chain_.getNrOfJoints());
		Sr_.resize(kdl_chain_.getNrOfJoints());
		sigma_.resize(kdl_chain_.getNrOfJoints());
		sigma_dot_.resize(kdl_chain_.getNrOfJoints());

		Kd_.resize(kdl_chain_.getNrOfJoints());
		gamma_.resize(kdl_chain_.getNrOfJoints());
		alpha_.resize(kdl_chain_.getNrOfJoints());
		lambda_.resize(kdl_chain_.getNrOfJoints());
		k_.resize(kdl_chain_.getNrOfJoints());

		sub_command_ = nh_.subscribe("command", 1, &DynamicSlidingModeControllerTaskSpace::command, this);

		pub_error_ = nh_.advertise<std_msgs::Float64MultiArray>("error", 1000);
		pub_pose_ = nh_.advertise<std_msgs::Float64MultiArray>("pose", 1000);
		pub_traj_ = nh_.advertise<std_msgs::Float64MultiArray>("traj", 1000);
		pub_marker_ = nh_.advertise<visualization_msgs::Marker>("marker",1000);

		return true;
	}

	void DynamicSlidingModeControllerTaskSpace::starting(const ros::Time& time)
	{
		// get joint positions
  		for(int i=0; i < joint_handles_.size(); i++)
  		{
    		joint_msr_states_.q(i) = joint_handles_[i].getPosition();
    		joint_msr_states_.qdot(i) = joint_handles_[i].getVelocity();
    		joint_des_states_.q(i) = joint_msr_states_.q(i);
  			sigma_(i) = 0;

  			// coefficients > 0
  			Kd_(i) = 10;
  			gamma_(i) = 1;
  			alpha_(i) = 12;
  			lambda_(i) = 1;
  			k_(i) = 3;
    	}

    	//x_des_ = KDL::Frame(KDL::Rotation::RPY(0,0,0),KDL::Vector(-0.4,0.3,1.5));

    	cmd_flag_ = 0;
    	step_ = 0;
	}

	void DynamicSlidingModeControllerTaskSpace::update(const ros::Time& time, const ros::Duration& period)
	{
		// get joint positions
  		for(int i=0; i < joint_handles_.size(); i++)
  		{
    		joint_msr_states_.q(i) = joint_handles_[i].getPosition();
    		joint_msr_states_.qdot(i) = joint_handles_[i].getVelocity();
    	}

    	if (cmd_flag_)
    	{
	    	// computing forward kinematics
		    fk_pos_solver_->JntToCart(joint_msr_states_.q,x_);

		    // computing Jacobian J(q)
		    jnt_to_jac_solver_->JntToJac(joint_msr_states_.q,J_);

		    // computing Jacobian pseudo-inversion
		    pseudo_inverse(J_.data,J_pinv_);

		    // computing end-effector position/orientation error w.r.t. desired frame
		    x_err_ = diff(x_,x_des_);

		    /* Trying quaternions, it seems to work better

		    // end-effector position/orientation error
	    	x_err_.vel = (x_des_.p - x_.p);

	    	// getting quaternion from rotation matrix
	    	x_.M.GetQuaternion(quat_curr_.v(0),quat_curr_.v(1),quat_curr_.v(2),quat_curr_.a);
	    	x_des_.M.GetQuaternion(quat_des_.v(0),quat_des_.v(1),quat_des_.v(2),quat_des_.a);

	    	skew_symmetric(quat_des_.v,skew_);

	    	for (int i = 0; i < skew_.rows(); i++)
	    	{
	    		v_temp_(i) = 0.0;
	    		for (int k = 0; k < skew_.cols(); k++)
	    			v_temp_(i) += skew_(i,k)*(quat_curr_.v(k));
	    	}

	    	x_err_.rot = (quat_curr_.a*quat_des_.v - quat_des_.a*quat_curr_.v) - v_temp_;
			*/
	    	// clearing error msg before publishing
    		msg_err_.data.clear();

		    for(int i = 0; i < e_ref_.size(); i++)
			{
				e_ref_(i) = x_err_(i);
		    	msg_err_.data.push_back(e_ref_(i));
			}

			joint_des_states_.qdot.data = J_pinv_*e_ref_;

			joint_des_states_.q.data = joint_msr_states_.q.data + period.toSec()*joint_des_states_.qdot.data;

	    	// computing S
	    	S_.data = (joint_msr_states_.qdot.data - joint_des_states_.qdot.data) + alpha_.data.cwiseProduct(joint_msr_states_.q.data - joint_des_states_.q.data);

			//for (int i = 0; i < joint_handles_.size(); i++)
				//S_(i) = (joint_msr_states_.qdot(i) - joint_des_states_.qdot(i)) + alpha_(i)*tanh(lambda_(i)*(joint_msr_states_.q(i) - joint_des_states_.q(i)));

	    	// saving S0 on the first step
	    	if (step_ == 0)
	    		S0_ = S_;

	    	// computing Sd
	    	for (int i = 0; i < joint_handles_.size(); i++)
	    		Sd_(i) = S0_(i)*exp(-k_(i)*(step_*period.toSec()));

	    	Sq_.data = S_.data + Sd_.data;//- Sd_.data;

	    	// computing sigma_dot as sgn(Sq)
	    	for (int i = 0; i < joint_handles_.size(); i++)
	    		sigma_dot_(i) = -(Sq_(i) < 0) + (Sq_(i) > 0);

	    	// integrating sigma_dot
	    	sigma_.data += period.toSec()*sigma_dot_.data;

	    	//for (int i = 0; i < joint_handles_.size(); i++)
	    		//sigma_(i) += period.toSec()*pow(Sq_(i),0.5);

	    	// computing Sr
	    	Sr_.data = Sq_.data + gamma_.data.cwiseProduct(sigma_.data);

	    	// computing tau
	    	tau_.data = -Kd_.data.cwiseProduct(Sr_.data);

	    	step_++;

	    	if (Equal(x_,x_des_,0.05))
	    	{

	    		ROS_INFO("On target");
	    		cmd_flag_ = 0;
	    		return;
	    	}
	    }

    	// set controls for joints
    	for (int i = 0; i < joint_handles_.size(); i++)
    	{
    		if (!cmd_flag_)
    			tau_(i) = PIDs_[i].computeCommand(joint_des_states_.q(i) - joint_msr_states_.q(i),period);//Kd_(i)*(alpha_(i)*(joint_des_states_.q(i) - joint_msr_states_.q(i)) + (joint_des_states_.qdot(i) - joint_msr_states_.qdot(i))) ;

	    	joint_handles_[i].setCommand(tau_(i));
    	}

    	// publishing markers for visualization in rviz
    	pub_marker_.publish(msg_marker_);
    	msg_id_++;

	    // publishing error for all tasks as an array of ntasks*6
	    pub_error_.publish(msg_err_);
	    // publishing actual and desired trajectory for each task (links) as an array of ntasks*3
	    pub_pose_.publish(msg_pose_);
	    pub_traj_.publish(msg_traj_);
	    ros::spinOnce();

	}

	void DynamicSlidingModeControllerTaskSpace::command(const std_msgs::Float64MultiArray::ConstPtr &msg)
	{
		if (msg->data.size() == 6)
		{
			x_des_ = KDL::Frame(KDL::Rotation::RPY(msg->data[3],
												   msg->data[4],
												   msg->data[5]),
								KDL::Vector(msg->data[0],
											msg->data[1],
											msg->data[2]));

			step_ = 0;
			cmd_flag_ = 1;
			//SetToZero(joint_des_states_);
		}
		else
		{
			ROS_INFO("Tasks parameters are [x,y,z,roll,pitch,yaw]");
			return;
		}
	}

	void DynamicSlidingModeControllerTaskSpace::set_marker(KDL::Frame x, int id)
	{
				msg_marker_.header.frame_id = "world";
				msg_marker_.header.stamp = ros::Time();
				msg_marker_.ns = "end_effector";
				msg_marker_.id = id;
				msg_marker_.type = visualization_msgs::Marker::SPHERE;
				msg_marker_.action = visualization_msgs::Marker::ADD;
				msg_marker_.pose.position.x = x.p(0);
				msg_marker_.pose.position.y = x.p(1);
				msg_marker_.pose.position.z = x.p(2);
				msg_marker_.pose.orientation.x = 0.0;
				msg_marker_.pose.orientation.y = 0.0;
				msg_marker_.pose.orientation.z = 0.0;
				msg_marker_.pose.orientation.w = 1.0;
				msg_marker_.scale.x = 0.005;
				msg_marker_.scale.y = 0.005;
				msg_marker_.scale.z = 0.005;
				msg_marker_.color.a = 1.0;
				msg_marker_.color.r = 0.0;
				msg_marker_.color.g = 1.0;
				msg_marker_.color.b = 0.0;
	}
}

PLUGINLIB_EXPORT_CLASS(lwr_controllers::DynamicSlidingModeControllerTaskSpace, controller_interface::ControllerBase)


//#include <iomanip>
//#include <cmath>
//#include <ctime>
//#include <algorithm>
//#include <vector>
//#include <iterator>
//#include <functional>
//#include <numeric>
#include <boost/program_options.hpp>

#include <iostream>
#include <fstream>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#define SWAP(a,b) {temp = (a); (a) = (b); (b) = temp;}

// *******************************************************
extern "C"
{
	/* Function prototypes. */
	void start( double *data, int numberOfPoints, float lower, float higher );
	int input( void );
	int rscale( int minbox, int maxbox, double boxratio );
	void dfa( double *seq, int npts, int nfit, int *rs, int nr, int sw );
	void setup( void );
	void cleanup( void );
	void help( void );
	double polyfit( double **x, double *y, int ndat, int nfit );
	void error( char error_text[] );
	double *vector( int nl, int nh );
	int *ivector( int nl, int nh );
	int *lvector( int nl, int nh );
	double **matrix( int nrl, int nrh, int ncl, int nch );
	void free_vector( double *v, int nl, int nh );
	void free_ivector( int *v, int nl, int nh );
	void free_lvector( int *v, int nl, int nh );
	void free_matrix( double **m, int nrl, int nrh, int ncl, int nch );

	/* Global variables. */
	char *pname; /* this program's name (for use in error messages) */
	double *seq; /* input data buffer; allocated and filled by input() */
	int *rs; /* box size array; allocated and filled by rscale() */
	double *mse; /* fluctuation array; allocated by setup(), filled by dfa() */
	int iflag = 1; /* integrate the input data if non-zero */
	int nfit = 2; /* order of the regression fit, plus 1 */
	int nr; /* number of box sizes */

	/**
	 * START.
	 */
	void start( double *data, int numberOfPoints, float lower, float higher )
	{

		int i;
		int sw = 0;
		int minbox = 0;
		int maxbox = 0;
		int npts;
		int temp;

		/* Read and interpret the command line. */
		pname = const_cast< char* > ( "dfa" );

		nfit = 2; // d ...

		minbox = lower;
		maxbox = higher;
		sw = 0; // sliding-window

		/* Allocate and fill the input data array seq[]. */
		//npts = input();
		seq = data;
		npts = numberOfPoints;

		/* Set minimum and maximum box sizes. */
		if ( minbox < 2 * nfit )
		minbox = 2 * nfit;
		if ( maxbox == 0 || maxbox> npts / 4 )
		maxbox = npts / 4;
		if ( minbox> maxbox )
		{
			SWAP(minbox, maxbox);
			if ( minbox < 2 * nfit )
			minbox = 2 * nfit;
		}

		/* Allocate and fill the box size array rs[].  rscale's third argument
		 specifies that the ratio between successive box sizes is 2^(1/8). */
		nr = rscale( minbox, maxbox, pow( 2.0, 1.0 / 8.0 ) );

		/* Allocate memory for dfa() and the functions it calls. */
		setup();

		/* Measure the fluctuations of the detrended input data at each box size
		 using the DFA algorithm; fill mse[] with these results. */
		dfa( seq, npts, nfit, rs, nr, sw );

		/* Output the results. */
		for ( i = 1; i <= nr; i++ )
		printf( "%g %g\n", log10( (double) rs[i] ), log10( mse[i] ) / 2.0 );

		/* Release allocated memory. */
		cleanup();
		exit( 0 );
	}

	/* Read input data, allocating and filling seq[], integrating if iflag != 0.
	 Following the convention used for other arrays in this program, seq[0] is
	 unused, and the first point is stored in seq[1].  The return value is the
	 number of points read.

	 This function allows the input buffer to grow as large as necessary, up to
	 the available memory (assuming that a int int is large enough to address
	 any memory location).  Note that the integration is done using double
	 precision arithmetic to avoid complete loss of precision when the integrated
	 data reach large amplitudes.  */
	int input()
	{
		//int maxdat = 0, npts = 0;
		int maxdat = 0, npts = 0;
		double y, yp = 0.0;

		while ( scanf( "%lf", &y ) == 1 )
		{
			if ( ++npts >= maxdat )
			{
				double *s;

				maxdat += 50000; /* allow the input buffer to grow (the
				 increment is arbitrary) */
				if ( ( s = (double*) realloc( seq, maxdat * sizeof(double) ) ) == NULL )
				{
					fprintf( stderr, "%s: insufficient memory, truncating input at row %d\n", pname, npts );
					break;
				}
				seq = s;
			}
			seq[npts] = iflag ? ( yp += y ) : y;
		}

		if ( npts < 1 )
			error( "no data read" );
		return ( npts );
	}

	int rslen; /* length of rs[] */

	/* rscale() allocates and fills rs[], the array of box sizes used by dfa()
	 below.  The box sizes range from (exactly) minbox to (approximately) maxbox,
	 and are arranged in a geometric series such that the ratio between
	 consecutive box sizes is (approximately) boxratio.  The return value is
	 the number of box sizes in rs[].
	 */
	int rscale( int minbox, int maxbox, double boxratio )
	{
		int ir, n;
		int rw;

		/* Determine how many scales are needed. */
		rslen = log10( maxbox / (double) minbox ) / log10( boxratio ) + 1.5;

		/* Thanks to Peter Domitrovich for pointing out that a previous version
		 of the above calculation undercounted the number of scales in some
		 situations. */
		rs = lvector( 1, rslen );

		for ( ir = 1, n = 2, rs[1] = minbox; n <= rslen && rs[n - 1] < maxbox; ir++ )
		{
			if ( ( rw = minbox * pow( boxratio, ir ) + 0.5 ) > rs[n - 1] )
			{
				rs[n++] = rw;
			}
		}

		if ( rs[--n] > maxbox )
		{
			--n;
		}

		return ( n );
	}

	double **x; /* matrix of abscissas and their powers, for polyfit(). */

	/* Detrended fluctuation analysis
	 seq:	input data array
	 npts:	number of input points
	 nfit:	order of detrending (2: linear, 3: quadratic, etc.)
	 rs:		array of box sizes (uniformly distributed on log scale)
	 nr:		number of entries in rs[] and mse[]
	 sw:		mode (0: non-overlapping windows, 1: sliding window)
	 This function returns the mean squared fluctuations in mse[].
	 */
	void dfa( double *seq, int npts, int nfit, int *rs, int nr, int sw )
	{
		int i, boxsize, inc, j;
		double stat;

		for ( i = 1; i <= nr; i++ )
		{
			boxsize = rs[i];

			if ( sw )
			{
				inc = 1;
				stat = (int) ( npts - boxsize + 1 ) * boxsize;
			}
			else
			{
				inc = boxsize;
				stat = (int) ( npts / boxsize ) * boxsize;
			}
			for ( mse[i] = 0.0, j = 0; j <= npts - boxsize; j += inc )
			{

				double polyfitResult = polyfit( x, seq + j, boxsize, nfit );
				mse[i] += polyfitResult;
			}
			mse[i] /= stat;
		}
	}

	/* workspace for polyfit() */
	double *beta, **covar, **covar0;
	int *indxc, *indxr, *ipiv;

	/* This function allocates workspace for dfa() and polyfit(), and sets
	 x[i][j] = i**(j-1), in preparation for polyfit(). */
	void setup()
	{
		int i;
		int j, k;

		beta = vector( 1, nfit );
		covar = matrix( 1, nfit, 1, nfit );
		covar0 = matrix( 1, nfit, 1, nfit );
		indxc = ivector( 1, nfit );
		indxr = ivector( 1, nfit );
		ipiv = ivector( 1, nfit );
		mse = vector( 1, nr );
		x = matrix( 1, rs[nr], 1, nfit );
		for ( i = 1; i <= rs[nr]; i++ )
		{
			printf("i %d\n", i );
			x[i][1] = 1.0;
			x[i][2] = i;
			for ( j = 3; j <= nfit; j++ )
				x[i][j] = x[i][j - 1] * i;
		}
	}

	/* This function frees all memory previously allocated by this program. */
	void cleanup()
	{
		free_matrix( x, 1, rs[nr], 1, nfit );
		free_vector( mse, 1, nr );
		free_ivector( ipiv, 1, nfit );
		free_ivector( indxr, 1, nfit );
		free_ivector( indxc, 1, nfit );
		free_matrix( covar0, 1, nfit, 1, nfit );
		free_matrix( covar, 1, nfit, 1, nfit );
		free_vector( beta, 1, nfit );
		free_lvector( rs, 1, rslen ); /* allocated by rscale() */
		free( seq ); /* allocated by input() */
	}

	static char *help_strings[] =
	{ "usage: %s [OPTIONS ...]\n", "where OPTIONS may include:", " -d K             detrend using a polynomial of degree K",
			"                   (default: K=1 -- linear detrending)", " -h               print this usage summary",
			" -i               input series is already integrated", " -l MINBOX        smallest box width (default: 2K+2)",
			" -s               sliding window DFA", " -u MAXBOX        largest box width (default: NPTS/4)",
			"The standard input should contain one column of data in text format.",
			"The standard output is two columns: log(n) and log(F) [base 10 logarithms],",
			"where n is the box size and F is the root mean square fluctuation.", NULL};

	void help( void )
	{
		int i;

		(void) fprintf( stderr, help_strings[0], pname );
		for ( i = 1; help_strings[i] != NULL; i++ )
			(void) fprintf( stderr, "%s\n", help_strings[i] );
	}

	/* polyfit() is based on lfit() and gaussj() from Numerical Recipes in C
	 (Press, Teukolsky, Vetterling, and Flannery; Cambridge U. Press, 1992).  It
	 fits a polynomial of degree (nfit-1) to a set of boxsize points given by
	 x[1...boxsize][2] and y[1...boxsize].  The return value is the sum of the
	 squared errors (chisq) between the (x,y) pairs and the fitted polynomial.
	 */
	double polyfit( double **x, double *y, int boxsize, int nfit )
	{
		int icol, irow, j, k;
		double big, chisq, pivinv, temp;
		int i;
		static int pboxsize = 0;

		/* This block sets up the covariance matrix.  Provided that boxsize
		 never decreases (which is true in this case), covar0 can be calculated
		 incrementally from the previous value. */
		if ( pboxsize != boxsize )
		{ /* this will be false most of the time */
			if ( pboxsize > boxsize ) /* this should never happen */
				pboxsize = 0;
			if ( pboxsize == 0 ) /* this should be true the first time only */
				for ( j = 1; j <= nfit; j++ )
					for ( k = 1; k <= nfit; k++ )
						covar0[j][k] = 0.0;
			for ( i = pboxsize + 1; i <= boxsize; i++ )
				for ( j = 1; j <= nfit; j++ )
					for ( k = 1, temp = x[i][j]; k <= j; k++ )
						covar0[j][k] += temp * x[i][k];
			for ( j = 2; j <= nfit; j++ )
				for ( k = 1; k < j; k++ )
					covar0[k][j] = covar0[j][k];
			pboxsize = boxsize;
		}
		for ( j = 1; j <= nfit; j++ )
		{
			beta[j] = ipiv[j] = 0;
			for ( k = 1; k <= nfit; k++ )
				covar[j][k] = covar0[j][k];
		}
		for ( i = 1; i <= boxsize; i++ )
		{
			beta[1] += ( temp = y[i] );
			beta[2] += temp * i;
		}
		if ( nfit > 2 )
			for ( i = 1; i <= boxsize; i++ )
				for ( j = 3, temp = y[i]; j <= nfit; j++ )
					beta[j] += temp * x[i][j];
		for ( i = 1; i <= nfit; i++ )
		{
			big = 0.0;
			for ( j = 1; j <= nfit; j++ )
				if ( ipiv[j] != 1 )
					for ( k = 1; k <= nfit; k++ )
					{
						if ( ipiv[k] == 0 )
						{
							if ( ( temp = covar[j][k] ) >= big || ( temp = -temp ) >= big )
							{
								big = temp;
								irow = j;
								icol = k;
							}
						} else if ( ipiv[k] > 1 )
							error( "singular matrix" );
					}
			++( ipiv[icol] );
			if ( irow != icol )
			{
				for ( j = 1; j <= nfit; j++ )
					SWAP(covar[irow][j], covar[icol][j]);
				SWAP(beta[irow], beta[icol]);
			}
			indxr[i] = irow;
			indxc[i] = icol;
			if ( covar[icol][icol] == 0.0 )
				error( "singular matrix" );
			pivinv = 1.0 / covar[icol][icol];
			covar[icol][icol] = 1.0;
			for ( j = 1; j <= nfit; j++ )
				covar[icol][j] *= pivinv;
			beta[icol] *= pivinv;
			for ( j = 1; j <= nfit; j++ )
				if ( j != icol )
				{
					temp = covar[j][icol];
					covar[j][icol] = 0.0;
					for ( k = 1; k <= nfit; k++ )
						covar[j][k] -= covar[icol][k] * temp;
					beta[j] -= beta[icol] * temp;
				}
		}
		chisq = 0.0;
		if ( nfit <= 2 )
			for ( i = 1; i <= boxsize; i++ )
			{
				temp = beta[1] + beta[2] * i - y[i];
				chisq += temp * temp;
			}
		else
			for ( i = 1; i <= boxsize; i++ )
			{
				temp = beta[1] + beta[2] * i - y[i];
				for ( j = 3; j <= nfit; j++ )
					temp += beta[j] * x[i][j];
				chisq += temp * temp;
			}
		return ( chisq );
	}

	/* The functions below are based on those of the same names in Numerical
	 Recipes (see above). */
	void error( char error_text[] )
	{
		fprintf( stderr, "%s: %s\n", pname, error_text );
		exit( 1 );
	}

	double *vector( int nl, int nh )
	/* allocate a double vector with subscript range v[nl..nh] */
	{
		double *v = (double *) malloc( ( size_t )( ( nh - nl + 2 ) * sizeof(double) ) );
		if ( v == NULL )
			error( "allocation failure in vector()" );
		return ( v - nl + 1 );
	}

	int *ivector( int nl, int nh )
	/* allocate an int vector with subscript range v[nl..nh] */
	{
		int *v = (int *) malloc( ( size_t )( ( nh - nl + 2 ) * sizeof(int) ) );
		if ( v == NULL )
			error( "allocation failure in ivector()" );
		return ( v - nl + 1 );
	}

	int *lvector( int nl, int nh )
	/* allocate a int int vector with subscript range v[nl..nh] */
	{
		int *v = (int *) malloc( ( size_t )( ( nh - nl + 2 ) * sizeof(int) ) );
		if ( v == NULL )
			error( "allocation failure in lvector()" );
		return ( v - nl + 1 );
	}

	double **matrix( int nrl, int nrh, int ncl, int nch )
	/* allocate a double matrix with subscript range m[nrl..nrh][ncl..nch] */
	{
		int i, nrow = nrh - nrl + 1, ncol = nch - ncl + 1;
		double **m;

		/* allocate pointers to rows */
		m = (double **) malloc( ( size_t )( ( nrow + 1 ) * sizeof(double*) ) );
		if ( !m )
			error( "allocation failure 1 in matrix()" );
		m += 1;
		m -= nrl;

		/* allocate rows and set pointers to them */
		m[nrl] = (double *) malloc( ( size_t )( ( nrow * ncol + 1 ) * sizeof(double) ) );
		if ( !m[nrl] )
			error( "allocation failure 2 in matrix()" );
		m[nrl] += 1;
		m[nrl] -= ncl;

		for ( i = nrl + 1; i <= nrh; i++ )
			m[i] = m[i - 1] + ncol;

		/* return pointer to array of pointers to rows */
		return ( m );
	}

	void free_vector( double *v, int nl, int nh )
	/* free a double vector allocated with vector() */
	{
		free( v + nl - 1 );
	}

	void free_ivector( int *v, int nl, int nh )
	/* free an int vector allocated with ivector() */
	{
		free( v + nl - 1 );
	}

	void free_lvector( int *v, int nl, int nh )
	/* free a int int vector allocated with lvector() */
	{
		free( v + nl - 1 );
	}

	void free_matrix( double **m, int nrl, int nrh, int ncl, int nch )
	/* free a double matrix allocated by matrix() */
	{
		free( m[nrl] + ncl - 1 );
		free( m + nrl - 1 );
	}

}

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

/**
 * @author: W.M. Otte (wim@invivonmr.uu.nl); Image Sciences Institute, UMC Utrecht, NL.
 * @date: 19-11-2009
 * This method was first proposed in:
 * Peng C-K, Buldyrev SV, Havlin S, Simons M, Stanley HE, Goldberger AL. Mosaic
 * organization of DNA nucleotides. Phys Rev E 1994;49:1685-1689.  [Available
 * on-line at http://prola.aps.org/abstract/PRE/v49/i2/p1685_1]
 *
 * A detailed description of the algorithm and its application to physiologic
 * signals can be found in:
 * Peng C-K, Havlin S, Stanley HE, Goldberger AL. Quantification of scaling
 * exponents and crossover phenomena in nonstationary heartbeat time series.
 * Chaos 1995;5:82-87. [Abstract online at http://www.ncbi.nlm.nih.gov/entrez/-
 * query.fcgi?cmd=Retrieve&db=PubMed&list_uids=11538314&dopt=Abstract]
 *
 * If you use this program in support of published research, please include a
 * citation of at least one of the two references above, as well as the standard
 * citation for PhysioNet:
 * Goldberger AL, Amaral LAN, Glass L, Hausdorff JM, Ivanov PCh, Mark RG,
 * Mietus JE, Moody GB, Peng CK, Stanley HE.  PhysioBank, PhysioToolkit, and
 * Physionet: Components of a New Research Resource for Complex Physiologic
 * Signals. Circulation 101(23):e215-e220 [Circulation Electronic Pages;
 * http://circ.ahajournals.org/cgi/content/full/101/23/e215]; 2000 (June 13).
 */
class DetrendedFluctuationAnalysis
{

public:

	typedef double ValueType;
	typedef std::vector< ValueType > VectorType;

	/**
	 * Detrended fluctuation analysis.
	 */
	void run( const std::string& inputFileName, float lower, float upper )
	{
		// [ 1 ] read input from text file ...
		double* data;
		int inputSize = getInput( inputFileName, data );

		// [ 2 ] insert in C-start function ...

		// TODO set proper default values ...
		int d = 1;

		// call C-old main function ...
		start( data, inputSize, lower, upper );
	}

protected:

	/**
	 * Return input from given text file as vector.
	 */
	int getInput( const std::string& input, double* inputDataPointer )
	{
		std::ifstream inFile;

		inFile.open( input.c_str() );
		if ( !inFile )
		{
			std::cout << "*** ERROR ***: Unable to open: " << input << "." << std::endl;
			exit( EXIT_FAILURE );
		}

		double x;
		VectorType output;

		while ( inFile >> x )
		{
			output.push_back( x );
		}
		inFile.close();

		/**
		 * Negative values will be converted to complex numbers in matlab,
		 * but not with the stl ...
		 *
		 * No support is given (yet) for complex number mle.
		 */
		if ( *( std::min_element( output.begin(), output.end() ) ) < 0 )
		{
			std::cerr << "*** ERROR ***: Negative input not supported!" << std::endl;
			exit( EXIT_FAILURE );
		}

		inputDataPointer = new double[output.size()];
		std::copy( output.begin(), output.end(), inputDataPointer );

		// dereference iterator and get address....
		//VectorType::iterator it = output.begin();
		// set pointer ...
		//inputDataPointer = &*it;

		return output.size();
	}
};

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

/**
 * Throw error if required option is not specified.
 */
void required_option( const boost::program_options::variables_map& vm, const std::string& required_option )
{
	if ( vm.count( required_option ) == 0 )
		throw std::logic_error( "Option: '" + required_option + "' is required!" );
}

/**
 * Fit powerlaw to list of numbers.
 */
int main( int argc, char* argv[] )
{
	namespace po = boost::program_options;

	// application description ...
	std::string description = "Detrended fluctuation analysis.\n";

	// options ...
	std::string input;

	double lower;
	double upper;

	try
	{

		po::options_description desc( "Available options" );

		desc.add_options()

		( "input,i", po::value< std::string >( &input ), "string: input file with distribution values in column format." )

		( "lower,l", po::value< double >( &lower )->default_value( 0.0 ), "float: lower." )

		( "upper,u", po::value< double >( &upper )->default_value( 0.0 ), "float: upper." )

		( "help,h", "bool: produce help message." );

		po::variables_map vm;
		po::store( po::parse_command_line( argc, argv, desc ), vm );
		po::notify( vm );

		// help message ...
		if ( vm.count( "help" ) )
		{
			std::cout << argv[0] << ": " << description << std::endl;
			std::cout << desc << "\n";

			return EXIT_SUCCESS;
		}

		// required options ...
		required_option( vm, "input" );

		// run application ...
		DetrendedFluctuationAnalysis dfa;

		dfa.run( input, lower, upper );

	} catch ( std::exception& e )
	{
		std::cerr << "*** ERROR ***: " << e.what() << "\n";
		std::cerr << "Use \"" << argv[0] << " --help\" for information about application usage." << std::endl;

		return EXIT_FAILURE;
	}

	return EXIT_SUCCESS;
}


/*
 * gpuBufferSensePrepGadget.cpp
 *
 *  Created on: Dec 10, 2014
 *      Author: dch
 */

#include "gpuBufferSensePrepGadget.h"
#include <ismrmrd/xml.h>
#include "GenericReconJob.h"
#include "cuNFFT.h"
#include "NFFTOperator.h"
#include "cuNDArray_math.h"
#include "vector_td_utilities.h"
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include "b1_map.h"
#include "cuCgSolver.h"
#include "hoNDArray_math.h"
#include "hoNDArray_utils.h"
#include "cuNDArray_fileio.h"
#include "cudaDeviceManager.h"
#include <numeric>

namespace Gadgetron {

gpuBufferSensePrepGadget::gpuBufferSensePrepGadget() {

}

gpuBufferSensePrepGadget::~gpuBufferSensePrepGadget() {

}

int gpuBufferSensePrepGadget::process_config(ACE_Message_Block* mb) {
	ISMRMRD::IsmrmrdHeader h;
	ISMRMRD::deserialize(mb->rd_ptr(),h);

	auto matrixsize = h.encoding.front().encodedSpace.matrixSize;

	profiles_per_frame_ = profiles_per_frame.value();
	kernel_width_ = kernel_width.value();
	oversampling_factor_ = buffer_convolution_oversampling_factor.value();

	unsigned int warp_size = cudaDeviceManager::Instance()->warp_size();
	image_dims_.push_back(((matrixsize.x+warp_size-1)/warp_size)*warp_size);
	image_dims_.push_back(((matrixsize.y+warp_size-1)/warp_size)*warp_size);

	image_dims_recon_.push_back(((static_cast<size_t>(std::ceil(matrixsize.x*reconstruction_os_factor.value()))+warp_size-1)/warp_size)*warp_size);
	image_dims_recon_.push_back(((static_cast<size_t>(std::ceil(matrixsize.y*reconstruction_os_factor.value()))+warp_size-1)/warp_size)*warp_size);

	image_dims_recon_os_ = uint64d2
			(((static_cast<size_t>(std::ceil(image_dims_recon_[0]*oversampling_factor_))+warp_size-1)/warp_size)*warp_size,
					((static_cast<size_t>(std::ceil(image_dims_recon_[1]*oversampling_factor_))+warp_size-1)/warp_size)*warp_size);

	// In case the warp_size constraint kicked in
	oversampling_factor_ = float(image_dims_recon_os_[0])/float(image_dims_recon_[0]);

	return GADGET_OK;

}

int gpuBufferSensePrepGadget::process(
		GadgetContainerMessage<IsmrmrdReconData>* m1) {

	IsmrmrdReconData* recondata= m1->getObjectPtr();

	if (recondata->rbit_.size() != 1){
		throw std::runtime_error("gpuBufferSensePrepGadget only support a single encoding space");
	}

	IsmrmrdReconBit& reconbit = recondata->rbit_[0];

	GenericReconJob job;

	IsmrmrdDataBuffered* buffer = &reconbit.data_;

	//Use reference data if available.
	if (reconbit.ref_){
		GDEBUG("Using Reference data for CSM estimation\n");
		buffer = &reconbit.ref_.value();
	}

	size_t ncoils = buffer->headers_[0].active_channels;

	std::vector<size_t> new_order = {0,1,2,4,5,6,3};

	boost::shared_ptr<cuNDArray<float>> dcw;
	boost::shared_ptr<cuNDArray<floatd2>> traj;
	if (buffer->trajectory_){
		auto & trajectory = *buffer->trajectory_;

		if (buffer->headers_[0].trajectory_dimensions == 3){
			auto traj_dcw = separate_traj_and_dcw(&trajectory);
			dcw = boost::make_shared<cuNDArray<float>>(std::get<1>(traj_dcw).get());
			traj = boost::make_shared<cuNDArray<floatd2>>(std::get<0>(traj_dcw).get());
		} else if (buffer->headers_[0].trajectory_dimensions == 2){
			auto old_traj_dims = *trajectory.get_dimensions();
			std::vector<size_t> traj_dims (old_traj_dims.begin()+1,old_traj_dims.end()); //Remove first element
			hoNDArray<floatd2> tmp_traj(traj_dims,(floatd2*)trajectory.get_data_ptr());
			traj = boost::make_shared<cuNDArray<floatd2>>(tmp_traj);
		} else {
			throw std::runtime_error("Unsupported number of trajectory dimensions");
		}
	}
	{
		auto tmpdim = *buffer->data_.get_dimensions();
		for (auto dim : tmpdim)
			std::cout << dim << " ";
		std::cout << std::endl;
		auto permuted = permute(*(hoNDArray<float_complext>*)&buffer->data_,new_order);
		cuNDArray<float_complext> data(permuted);
		if (dcw){
			float scale_factor = float(prod(image_dims_recon_os_))/asum(dcw.get());
			*dcw *= scale_factor;
		}

		auto reg_images = reconstruct_regularization(&data,traj.get(),dcw.get(),ncoils);
		//reg_images->squeeze();

		auto csm = estimate_b1_map<float,2>(reg_images.get());
		*reg_images *= csm;
		auto combined = sum(reg_images.get(),reg_images->get_number_of_dimensions()-1);

		auto tmp_combined = abs(reg_images.get());
		auto tmpcsm = abs(&csm);
		job.csm_host_ = csm.to_host();
		job.reg_host_ = combined->to_host();
	}

	IsmrmrdDataBuffered* mainbuffer = &reconbit.data_;

	//Permute as Sensegadgets expect last dimension to be coils. *Sigh*
	job.dat_host_ = boost::make_shared<hoNDArray<complext<float>>>(permute(*(hoNDArray<float_complext>*)&mainbuffer->data_,new_order));

	if (mainbuffer->trajectory_){
		auto & trajectory = *mainbuffer->trajectory_;
		if (mainbuffer->headers_[0].trajectory_dimensions >2 ){
			auto traj_dcw = separate_traj_and_dcw(&trajectory);
			job.tra_host_ = std::get<0>(traj_dcw);
			job.dcw_host_ = std::get<1>(traj_dcw);
		} else if (mainbuffer->headers_[0].trajectory_dimensions == 2){
			auto old_traj_dims = *trajectory.get_dimensions();
			std::vector<size_t> traj_dims (old_traj_dims.begin()+1,old_traj_dims.end()); //Remove first element
			hoNDArray<floatd2> tmp_traj(traj_dims,(floatd2*)trajectory.get_data_ptr());
			job.tra_host_ = boost::make_shared<hoNDArray<floatd2>>(tmp_traj);
			auto host_dcw = boost::make_shared<hoNDArray<float>>(traj_dims);
			fill(host_dcw.get(),1.0f);
			job.dcw_host_ = host_dcw;

		} else {
			throw std::runtime_error("Unsupported number of trajectory dimensions");
		}
	}
	{
		float scale_factor = float(prod(image_dims_recon_os_))/asum(job.dcw_host_.get());
		*job.dcw_host_  *= scale_factor;
	}

	auto data_dims = *job.dat_host_->get_dimensions();
	//Sense gadgets expect only 1 dimension for encoding, so collapse the first
	size_t elements = std::accumulate(data_dims.begin(),data_dims.end()-1,1,std::multiplies<size_t>());
	std::vector<size_t> new_data_dims = {elements,data_dims.back()};
	job.dat_host_->reshape(&new_data_dims);

	size_t traj_elements = job.tra_host_->get_number_of_elements();
	auto traj_dims = *job.tra_host_->get_dimensions();

	size_t kpoints_per_frame = traj_dims[0]*profiles_per_frame_;
	if (traj_elements%kpoints_per_frame){
		std::stringstream ss;
		ss << "Profiles per frame (" << profiles_per_frame_ << ") must be a divisor of total number of profiles (" << traj_elements/traj_dims[0] << ")";
		throw std::runtime_error(ss.str());
	}
	std::vector<size_t> new_traj_dims ={kpoints_per_frame,traj_elements/kpoints_per_frame};

	job.tra_host_->reshape(&new_traj_dims);
	job.dcw_host_->reshape(&new_traj_dims);

	//Let's invent some image headers!
	size_t total_frames = profiles_per_frame_ > 0 ? mainbuffer->headers_.get_number_of_elements()/profiles_per_frame_ : 1 ;
	job.image_headers_ = boost::shared_array<ISMRMRD::ImageHeader>(new ISMRMRD::ImageHeader[total_frames]);
	for (size_t i = 0; i < total_frames; i++){
		job.image_headers_[i] = create_image_header(mainbuffer->headers_[i*profiles_per_frame_],mainbuffer->sampling_,i,total_frames);
	}

	m1->release(); //We be done with everything now.

	auto header_message = new GadgetContainerMessage<ISMRMRD::ImageHeader>(job.image_headers_[0]);

	auto job_message = new GadgetContainerMessage<GenericReconJob>(job);

	header_message->cont(job_message);

	if (!this->next()->putq(header_message)){
		GDEBUG("Failed to put message on que");
		return GADGET_FAIL;
	} else
		return GADGET_OK;

	//cuNDArray<float_complext> reg_images = reconstruct_regularization(reconbit.data_);
}

boost::shared_ptr<cuNDArray<float_complext> > gpuBufferSensePrepGadget::reconstruct_regularization(
		cuNDArray<float_complext>* data, cuNDArray<floatd2>* traj, cuNDArray<float>* dcw, size_t ncoils ) {

	if (dcw) { //We have density compensation, so we can get away with gridding

		cuNFFT_impl<float,2> plan(from_std_vector<size_t,2>(image_dims_recon_),image_dims_recon_os_,kernel_width_);
		std::vector<size_t> csm_dims = image_dims_recon_;
		csm_dims.push_back(ncoils);
		auto result = new cuNDArray<float_complext>(csm_dims);
		GDEBUG("Coils %i \n\n",ncoils);

		std::vector<size_t> flat_dims = {traj->get_number_of_elements()};
		cuNDArray<floatd2> flat_traj(flat_dims,traj->get_data_ptr());
		GDEBUG("traj: %i data %i\n",traj->get_number_of_elements(),data->get_number_of_elements());
		GDEBUG("Preprocessing\n\n");
		plan.preprocess(&flat_traj,NFFT_prep_mode::NC2C);
		GDEBUG("Computing\n\n");
		plan.compute(data,*result,dcw,NFFT_comp_mode::BACKWARDS_NC2C);

		return boost::shared_ptr<cuNDArray<float_complext>>(result);

	} else { //No density compensation, we have to do iterative reconstruction.
		std::vector<size_t> csm_dims = image_dims_recon_;
		csm_dims.push_back(ncoils);

		auto E = boost::make_shared<NFFTOperator<cuNDArray,float,2>>();

		E->setup(from_std_vector<size_t,2>(image_dims_recon_),image_dims_recon_os_,kernel_width_);
		std::vector<size_t> flat_dims = {traj->get_number_of_elements()};
		cuNDArray<floatd2> flat_traj(flat_dims,traj->get_data_ptr());

		E->set_domain_dimensions(&csm_dims);
		cuCgSolver<float_complext> solver;
		solver.set_max_iterations(0);
		solver.set_encoding_operator(E);
		solver.set_output_mode(cuCgSolver<float_complext>::OUTPUT_VERBOSE);
		E->set_codomain_dimensions(data->get_dimensions().get());
		E->preprocess(&flat_traj);
		auto res = solver.solve(data);
		return res;
	}
}

std::tuple<boost::shared_ptr<hoNDArray<floatd2 > >, boost::shared_ptr<hoNDArray<float >>> gpuBufferSensePrepGadget::separate_traj_and_dcw(
		hoNDArray<float >* traj_dcw) {
	std::vector<size_t> dims = *traj_dcw->get_dimensions();
	std::vector<size_t> reduced_dims(dims.begin()+1,dims.end()); //Copy vector, but leave out first dim
	auto  dcw = boost::make_shared<hoNDArray<float>>(reduced_dims);

	auto traj = boost::make_shared<hoNDArray<floatd2>>(reduced_dims);

	auto dcw_ptr = dcw->get_data_ptr();
	auto traj_ptr = traj->get_data_ptr();
	auto ptr = traj_dcw->get_data_ptr();
	for (size_t i = 0; i < traj_dcw->get_number_of_elements()/3; i++){
		traj_ptr[i][0] = ptr[i*3];
		traj_ptr[i][1] = ptr[i*3+1];
		dcw_ptr[i] = ptr[i*3+2];
	}

	return std::make_tuple(traj,dcw);

}

ISMRMRD::ImageHeader gpuBufferSensePrepGadget::create_image_header(
		ISMRMRD::AcquisitionHeader& base_head, const SamplingDescription& samp, size_t idx, size_t num_frames) {

	ISMRMRD::ImageHeader header;
	header.version = base_head.version;

	header.matrix_size[0] = image_dims_recon_[0];
	header.matrix_size[1] = image_dims_recon_[1];
	header.matrix_size[2] = num_frames;

	header.field_of_view[0] = samp.recon_FOV_[0];
	header.field_of_view[1] = samp.recon_FOV_[1];
	header.field_of_view[2] = samp.recon_FOV_[2];

	header.channels = 1;
	header.slice = base_head.idx.slice;
	header.set = base_head.idx.set;

	header.acquisition_time_stamp = base_head.acquisition_time_stamp;
	memcpy(header.physiology_time_stamp, base_head.physiology_time_stamp, sizeof(uint32_t)*ISMRMRD::ISMRMRD_PHYS_STAMPS);

	memcpy(header.position, base_head.position, sizeof(float)*3);
	memcpy(header.read_dir, base_head.read_dir, sizeof(float)*3);
	memcpy(header.phase_dir, base_head.phase_dir, sizeof(float)*3);
	memcpy(header.slice_dir, base_head.slice_dir, sizeof(float)*3);
	memcpy(header.patient_table_position, base_head.patient_table_position, sizeof(float)*3);

	header.data_type = ISMRMRD::ISMRMRD_CXFLOAT;
	header.image_index = idx;
	header.image_series_index = 0;

	return header;

}

GADGET_FACTORY_DECLARE(gpuBufferSensePrepGadget)

} /* namespace Gadgetron */


/* rosrun kamu_robotu_map_analyzer map_analyzer2 /
home/alperen/MY_WORKSPACES/oko_slam/ros_ws/saved_maps/real_results/cartographer_maps/4Objects128pt3 */

// ROS
#include <ros/ros.h>
#include <std_msgs/String.h>
// OpenCV
#include <opencv2/opencv.hpp>
//#include <opencv2/highgui/highgui.hpp>

// File system and regular C++ headers
#include <string>
#include <iomanip>
#include <sstream>
#include <vector>

#include <boost/filesystem.hpp>
#include <dirent.h>
#include <sys/stat.h>
// GetCurrentDir
#include <unistd.h>

#include <yaml-cpp/yaml.h>

#define GetCurrentDir getcwd
// Function Prototypes
std::string GetCurrentWorkingDir(void);

// Helper function to find a cosine of angle between vectors from pt0->pt1 and pt0->pt2
static double angle(cv::Point pt1, cv::Point pt2, cv::Point pt0);
// Helper function to display text in the center of a contour
void setLabel(cv::Mat& im, const std::string label, std::vector<cv::Point>& contour);

int main(int argc, char** argv)
{
    ros::init(argc, argv, "map_analyzer2");
    ros::NodeHandle nh("~");

    // Get parameter from user
    std::string file_dir = argv[1];
    std::string yaml_file = file_dir + ".yaml";
    std::string map_file = file_dir + ".pgm";

    // Read Original Map Image
    cv::Mat input_map_image, input_map_image_gs;
    input_map_image = cv::imread(map_file);

    // Read yaml files
    YAML::Node config = YAML::LoadFile(yaml_file);
    float resolution = config["resolution"].as<float>();
    std::vector<float> origin_yaml = config["origin"].as<std::vector<float>>();
    int offset_coordinate_axis = input_map_image.size().height;
    int robot_initial_point_x = abs( origin_yaml[0]/resolution );
    int robot_initial_point_y = offset_coordinate_axis - abs( origin_yaml[1]/resolution );

    std::cout << "robot initial pose x: " << robot_initial_point_x << std::endl;
    std::cout << "robot initial pose y: " << robot_initial_point_y << std::endl;

    // Draw a Small Circle to Specify Initial Starting Point of Robot
    cv::circle( input_map_image, cv::Point(robot_initial_point_x,robot_initial_point_y), 1.0, cv::Scalar(0,0,255), -1, 8, 0 );
    // Draw Coordinate Axis whose origin is robot initial position
    cv::line( input_map_image, cv::Point(robot_initial_point_x,robot_initial_point_y) , cv::Point(robot_initial_point_x+25,robot_initial_point_y), cv::Scalar(0,0,255), 1,8); //X axis
    cv::line( input_map_image, cv::Point(robot_initial_point_x,robot_initial_point_y) , cv::Point(robot_initial_point_x,robot_initial_point_y-25), cv::Scalar(0,255,0), 1,8); //Y axis

//    cv::namedWindow( "Original Map", CV_WINDOW_NORMAL);
//    cv::resizeWindow("Original Map", 600, 800);
//    cv::imshow("Original Map",input_map_image);
//    cv::waitKey(0);

    // Convert original map image to grayscale
    cv::cvtColor( input_map_image, input_map_image_gs, CV_BGR2GRAY );
    ////// Smooth the image to filter noise and find edges using Canny
    cv::Mat map_im_blurred, map_im_edges;
    cv::GaussianBlur(input_map_image_gs, map_im_blurred, cv::Size(3,3),0,0);
    cv::Canny(map_im_blurred, map_im_edges, 10,30);
//    cv::namedWindow( "Map Edges", CV_WINDOW_NORMAL);
//    cv::resizeWindow("Map Edges", 600, 800);
//    cv::imshow("Map Edges",map_im_edges);
//    cv::waitKey(0);

    ////// Find Contours of the map with largest area and get a rectangle around
    // findContours uses edges to detect contours which possible belong to
    // closed structures like objects
    std::vector<std::vector<cv::Point> > map_im_contours;
    std::vector<cv::Vec4i> hierarchy;
    cv::findContours(map_im_edges, map_im_contours, hierarchy, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE);
    double largest_area = 0.0;
    int largest_contour_index = 0;
    double second_largest_area = 0.0;
    int second_largest_contour_index = 0;

    for( int i = 0; i< map_im_contours.size(); i++ )
    {
        double a = cv::contourArea( map_im_contours[i],false);
        //std::cout << "Area: " << a << std::endl;
        if(a>=largest_area && (a-largest_area) >= 500)
        {
            second_largest_area = largest_area;
            second_largest_contour_index = largest_contour_index;
            largest_area = a;
            largest_contour_index=i;
        }
        else if (a>second_largest_area && a<largest_area)
        {
            second_largest_area = a;
            second_largest_contour_index = i;
        }
//    // Debug
//    cv::drawContours(input_map_image,map_im_contours,i
//    ,cv::Scalar(255,0,0),0,8,hierarchy);
//    cv::namedWindow( "Map Edges2", CV_WINDOW_NORMAL);
//    cv::resizeWindow("Map Edges2", 600, 800);
//    cv::imshow("Map Edges2",input_map_image);
//    cv::waitKey(0);
//    cv::drawContours(input_map_image,map_im_contours,i
//    ,cv::Scalar(0,0,0),0,8,hierarchy);
    }

    //convex hulls
    std::vector<std::vector<cv::Point> >hull(map_im_contours.size());
    std::vector<std::vector<int> > hullsI(map_im_contours.size());
    std::vector<std::vector<cv::Vec4i>> defects(map_im_contours.size());
    for (int i = 0; i < map_im_contours.size(); i++)
    {
        cv::convexHull(map_im_contours[i], hull[i], false);
        cv::convexHull(map_im_contours[i], hullsI[i], false);
        if(hullsI[i].size() > 3 )
        {
            cv::convexityDefects(map_im_contours[i], hullsI[i], defects[i]);
        }
    }
    //REQUIRED contour is detected,then convex hell is found and also convexity defects are found and stored in defects

    if (largest_area>100){
        std::cout << "Area is found" << std::endl;
        cv::drawContours(input_map_image, hull, second_largest_contour_index, cv::Scalar(0, 255, 255), 2, 8, std::vector<cv::Vec4i>(), 0, cv::Point());

        /// Draw convexityDefects
        for(int j=0; j<defects[second_largest_contour_index].size(); ++j)
        {
            const cv::Vec4i& v = defects[second_largest_contour_index][j];
            float depth = v[3] / 256;
            if (depth > 10) //  filter defects by depth
            {
                int startidx = v[0];
                cv::Point ptStart(map_im_contours[second_largest_contour_index][startidx]);
                int endidx = v[1];
                cv::Point ptEnd(map_im_contours[second_largest_contour_index][endidx]);
                int faridx = v[2];
                cv::Point ptFar(map_im_contours[second_largest_contour_index][faridx]);

                cv::line(input_map_image, ptStart, ptEnd, cv::Scalar(0, 255, 0), 0.2);
                cv::line(input_map_image, ptStart, ptFar, cv::Scalar(0, 255, 0), 0.2);
                cv::line(input_map_image, ptEnd, ptFar, cv::Scalar(0, 255, 0), 0.2);
                cv::circle(input_map_image, ptFar, 1, cv::Scalar(0, 255, 0), 1);
            }
        }
    }

//    cv::namedWindow( "Convex Defects", CV_WINDOW_NORMAL);
//    cv::resizeWindow("Convex Defects", 600, 800);
//    cv::imshow( "Convex Defects", input_map_image );
//    cv::waitKey(0);

    cv::Rect bounding_rect=cv::boundingRect(map_im_contours[second_largest_contour_index]);
    cv::rectangle(input_map_image, bounding_rect,  cv::Scalar(255,0,0),1, 8,0);

    // Crop the original map image to eliminate redundant parts
    cv::Mat map_im_cropped;
    cv::Mat map_im_cropped_gs;
    map_im_cropped = input_map_image(bounding_rect);

//    cv::namedWindow( "Cropped Map Im", CV_WINDOW_NORMAL);
//    cv::resizeWindow("Cropped Map Im", 600, 800);
//    cv::imshow( "Cropped Map Im", map_im_cropped);
//    cv::waitKey(0);

    // Count edge numbers of contours to detect objects
    cv::cvtColor( map_im_cropped, map_im_cropped_gs, CV_BGR2GRAY );
    std::vector<std::vector<cv::Point> > cropped_contours;
    std::vector<cv::Vec4i> cropped_hierarchy;
    cv::findContours(map_im_cropped_gs, cropped_contours, cropped_hierarchy, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE);

    for( int i = 0; i< cropped_contours.size(); i++ )
    {
        std::vector<cv::Point> approx_cont;
        cv::approxPolyDP(cropped_contours[i], approx_cont, 0.1*cv::arcLength(map_im_contours[i],true),true);
        std::cout << "Edge Number:" << approx_cont.size() << std::endl;

        if ( !cv::isContourConvex(approx_cont) )
        {
            continue;
        }
        else if (approx_cont.size() == 3 && cv::contourArea( cropped_contours[i],false) < largest_area/10)
        {
            setLabel(map_im_cropped, "TRI", cropped_contours[i]);
        }
        if ( (approx_cont.size() >= 4 && approx_cont.size() <= 6) && cv::contourArea( cropped_contours[i],false) < (largest_area/100) )
        {
            int vtc = approx_cont.size();

            std::vector<double> cos;
            for (int j = 2; j < vtc+1; j++)
				cos.push_back(angle(approx_cont[j%vtc], approx_cont[j-2], approx_cont[j-1]));

			// Sort ascending the cosine values
			std::sort(cos.begin(), cos.end());

			// Get the lowest and the highest cosine
			double mincos = cos.front();
			double maxcos = cos.back();

			// Use the degrees obtained above and the number of vertices
			// to determine the shape of the contour
			if (vtc == 4 && mincos >= -0.1 && maxcos <= 0.3)
				setLabel(map_im_cropped, "RECT", cropped_contours[i]);
			else if (vtc == 5 && mincos >= -0.34 && maxcos <= -0.27)
				setLabel(map_im_cropped, "PENTA", cropped_contours[i]);
			else if (vtc == 6 && mincos >= -0.55 && maxcos <= -0.45)
				setLabel(map_im_cropped, "HEXA", cropped_contours[i]);
        }
        else if (approx_cont.size() >= 7 && cv::contourArea( cropped_contours[i],false) < (largest_area/50) )
        {
            // Detect and label circles
			double area = cv::contourArea(cropped_contours[i]);
			cv::Rect r = cv::boundingRect(cropped_contours[i]);
			int radius = r.width / 2;

			if (std::abs(1 - ((double)r.width / r.height)) <= 0.2 &&
			    std::abs(1 - (area / (CV_PI * std::pow(radius, 2)))) <= 0.2)
				setLabel(map_im_cropped, "CIR", cropped_contours[i]);
        }
    }

    cv::namedWindow( "Detected Objects", CV_WINDOW_NORMAL);
    cv::resizeWindow("Detected Objects", 600, 800);
    cv::imshow("Detected Objects",map_im_cropped);
    cv::waitKey(0);

    //std::string output_image = file_dir + "_processed.png";
    //cv::imwrite(output_image, map_im_cropped);

    return 0;
}

/**
 * Helper function to find a cosine of angle between vectors
 * from pt0->pt1 and pt0->pt2
 */
static double angle(cv::Point pt1, cv::Point pt2, cv::Point pt0)
{
	double dx1 = pt1.x - pt0.x;
	double dy1 = pt1.y - pt0.y;
	double dx2 = pt2.x - pt0.x;
	double dy2 = pt2.y - pt0.y;
	return (dx1*dx2 + dy1*dy2)/sqrt((dx1*dx1 + dy1*dy1)*(dx2*dx2 + dy2*dy2) + 1e-10);
}

/**
 * Helper function to display text in the center of a contour
 */
void setLabel(cv::Mat& im, const std::string label, std::vector<cv::Point>& contour)
{
	int fontface = cv::FONT_HERSHEY_SIMPLEX;
	double scale = 0.4;
	int thickness = 1;
	int baseline = 0;

	cv::Size text = cv::getTextSize(label, fontface, scale, thickness, &baseline);
	cv::Rect r = cv::boundingRect(contour);

	cv::Point pt(r.x + ((r.width - text.width) / 2), r.y + ((r.height + text.height) / 2));
	cv::rectangle(im, pt + cv::Point(0, baseline), pt + cv::Point(text.width, -text.height), CV_RGB(255,255,255), CV_FILLED);
	cv::putText(im, label, pt, fontface, scale, CV_RGB(0,0,0), thickness, 8);
}


#include "stdafx.h"

#include "problem.hpp"
#include "utility.hpp"

#include <fstream>
#include <iomanip>
#include <regex>
#include <utility>
#include <map>
#include <numeric>
#include <chrono>
#include <boost/algorithm/string.hpp>

struct advent_2017_16 : problem
{
	advent_2017_16() noexcept : problem(2017, 16) {
	}

protected:
	std::string dancers;

	void do_spin(std::ptrdiff_t amount) {
		std::rotate(std::begin(dancers), std::end(dancers) - amount, std::end(dancers));
	}

	void do_exchange(std::size_t a, std::size_t b) {
		std::swap(dancers[a], dancers[b]);
	}

	void do_partner(char a, char b) {
		std::iter_swap(std::find(std::begin(dancers), std::end(dancers), a),
		               std::find(std::begin(dancers), std::end(dancers), b));
	}

	std::string raw_commands;

	void prepare_input(std::ifstream& fin) override {
		std::getline(fin, raw_commands);
	}

	using dance_move = std::function<void(void)>;

	std::vector<dance_move> moves;

	void precompute() override {
		dancers.resize(16);
		std::iota(std::begin(dancers), std::end(dancers), 'a');

		std::regex pattern(R"((s([[:digit:]]+))|(x([[:digit:]]+)/([[:digit:]]+))|(p([[:lower:]])/([[:lower:]])),?)");
		std::sregex_iterator it(std::begin(raw_commands), std::end(raw_commands), pattern);
		std::sregex_iterator end;
		for(; it != end; ++it) {
			const std::smatch m = *it;
			if(m[1].matched) {
				std::ptrdiff_t amount = std::stoll(m[2].str());
				moves.push_back([=]() {
					do_spin(amount);
				});
			} else if(m[3].matched) {
				std::size_t a = std::stoull(m[4].str());
				std::size_t b = std::stoull(m[5].str());
				moves.push_back([=]() {
					do_exchange(a, b);
				});
			} else if(m[6].matched) {
				char a = m[7].str()[0];
				char b = m[8].str()[0];
				moves.push_back([=]() {
					do_partner(a, b);
				});
			}
		}
	}

	std::string part_1() override {
		for(const auto& m : moves) {
			m();
		}
		return dancers;
	}

	std::map<std::size_t, std::string> generate_all_permutations() {
		std::map<std::size_t, std::string> permutations;
		std::iota(std::begin(dancers), std::end(dancers), 'a');
		const std::string original = dancers;
		do {
			permutations[permutations.size()] = dancers;
			for(const auto& m : moves) {
				m();
			}
		} while(original != dancers);
		return permutations;
	}

	std::string part_2() override {
		std::map<std::size_t, std::string> permutations = generate_all_permutations();
		const std::size_t remainder = 1'000'000'000 % permutations.size();
		return permutations[remainder];
	}
};

REGISTER_SOLVER(2017, 16);