// This file is part of the dune-gdt project:
//   http://users.dune-project.org/projects/dune-gdt
// Copyright holders: Felix Schindler
// License: BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)
//
// Contributors: Kirsten Weber

#ifndef DUNE_GDT_EVALUATION_ELLIPTIC_HH
#define DUNE_GDT_EVALUATION_ELLIPTIC_HH

#include <tuple>

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

#include <dune/common/dynmatrix.hh>
#include <dune/common/typetraits.hh>

#include <dune/stuff/common/fmatrix.hh>
#include <dune/stuff/common/memory.hh>
#include <dune/stuff/functions/constant.hh>
#include <dune/stuff/functions/interfaces.hh>

#include "interface.hh"

namespace Dune {
namespace GDT {
namespace LocalEvaluation {

// forward
template< class DiffusionFactorImp, class DiffusionTensorImp = void >
class Elliptic;

namespace internal {

/**
 * \brief Traits for the Elliptic evaluation (variant for given diffusion factor and tensor).
 * \sa    EllipticTraits (below) for a variant if only a diffusion is given.
 */
template< class DiffusionFactorImp, class DiffusionTensorImp >
class EllipticTraits
{
  static_assert(Stuff::is_localizable_function< DiffusionFactorImp >::value,
                "DiffusionFactorType has to be a localizable function!");
public:
  typedef typename DiffusionFactorImp::EntityType      EntityType;
  typedef typename DiffusionFactorImp::DomainFieldType DomainFieldType;
  static const size_t                                  dimDomain = DiffusionFactorImp::dimDomain;

  // we need to distinguish three cases here (since DiffusionTensorImp may be void):
  // given a two functions, a factor and a tensor
  // given a single factor (then set the tensor to default)
  // given a dingle tensor (then set the factor to default)
private:
  typedef EntityType      E;
  typedef DomainFieldType D;
  static const size_t     d = dimDomain;

  template< bool factor, bool tensor, bool anything = true >
  struct Helper { static_assert(AlwaysFalse< DiffusionTensorImp >::value, "Unsupported combination of functions given!"); };

  // given both
  template< bool anything >
  struct Helper< false, false, anything >
  {
    static_assert(Stuff::is_localizable_function< DiffusionTensorImp >::value,
                  "DiffusionTensorType has to be a localizable function!");
    static_assert(std::is_same< typename DiffusionFactorImp::EntityType,
                                typename DiffusionTensorImp::EntityType >::value,
                  "EntityTypes have to agree!");
    static_assert(std::is_same< typename DiffusionFactorImp::DomainFieldType,
                                typename DiffusionTensorImp::DomainFieldType >::value,
                  "DomainFieldTypes have to agree!");
    static_assert(DiffusionFactorImp::dimDomain == DiffusionTensorImp::dimDomain,
                  "Dimensions have to agree!");
    static_assert(DiffusionFactorImp::dimRange     == 1, "DiffusionFactorType has to be scalar!");
    static_assert(DiffusionFactorImp::dimRangeCols == 1, "DiffusionFactorType has to be scalar!");
    static_assert(DiffusionTensorImp::dimRange     == DiffusionTensorImp::dimDomain,
                  "DiffusionTensorType has to be matrix valued!");
    static_assert(DiffusionTensorImp::dimRangeCols == DiffusionTensorImp::dimDomain,
                  "DiffusionTensorType has to be matrix valued!");
    typedef DiffusionFactorImp FactorType;
    typedef DiffusionTensorImp TensorType;
  };

  // given only one, and this is scalar
  template< bool anything >
  struct Helper< true, false, anything >
  {
    typedef DiffusionFactorImp FactorType;
    typedef Stuff::Functions::Constant< E, D, d, D, d, d > TensorType;
  };

  // given only one, and this is a tensor
  template< bool anything >
  struct Helper< false, true, anything >
  {
    typedef Stuff::Functions::Constant< E, D, d, D, 1, 1 > FactorType;
    typedef DiffusionFactorImp                             TensorType;
  };

  static const bool single_factor_given =    DiffusionFactorImp::dimRange == 1
                                          && DiffusionFactorImp::dimRangeCols == DiffusionFactorImp::dimRange
                                          && std::is_same< DiffusionTensorImp, void >::value;
  static const bool single_tensor_given =    DiffusionFactorImp::dimRange != 1
                                          && DiffusionFactorImp::dimRange == DiffusionFactorImp::dimRangeCols
                                          && std::is_same< DiffusionTensorImp, void >::value;
public:
  typedef typename Helper< single_factor_given, single_tensor_given >::FactorType DiffusionFactorType;
  typedef typename Helper< single_factor_given, single_tensor_given >::TensorType DiffusionTensorType;
  typedef Elliptic< DiffusionFactorType, DiffusionTensorType > derived_type;
  typedef std::tuple< std::shared_ptr< typename DiffusionFactorType::LocalfunctionType >,
                      std::shared_ptr< typename DiffusionTensorType::LocalfunctionType > > LocalfunctionTupleType;
}; // class EllipticTraits

} // namespace internal

/**
 * \brief Computes an elliptic evaluation.
 */
template< class DiffusionFactorImp, class DiffusionTensorImp >
class Elliptic
  : public LocalEvaluation::Codim0Interface< internal::EllipticTraits< DiffusionFactorImp, DiffusionTensorImp >, 2 >
{
  typedef LocalEvaluation::Codim0Interface< internal::EllipticTraits< DiffusionFactorImp, DiffusionTensorImp >, 2 >
      BaseType;
public:
  typedef internal::EllipticTraits< DiffusionFactorImp, DiffusionTensorImp > Traits;
  typedef typename Traits::DiffusionFactorType    DiffusionFactorType;
  typedef typename Traits::DiffusionTensorType    DiffusionTensorType;
  typedef typename Traits::LocalfunctionTupleType LocalfunctionTupleType;
  typedef typename Traits::EntityType             EntityType;
  typedef typename Traits::DomainFieldType        DomainFieldType;
  static const size_t                             dimDomain = Traits::dimDomain;
private:
  typedef Stuff::Common::ConstStorageProvider< DiffusionFactorType > DiffusionFactorProvider;
  typedef Stuff::Common::ConstStorageProvider< DiffusionTensorType > DiffusionTensorProvider;
  using typename BaseType::E;
  using typename BaseType::D;
  using BaseType::d;

public:
  Elliptic(const DiffusionFactorType& diffusion_factor,
           const DiffusionTensorType& diffusion_tensor)
    : diffusion_factor_(diffusion_factor)
    , diffusion_tensor_(diffusion_tensor)
  {}

  Elliptic(const DiffusionFactorType& diffusion_factor)
    : diffusion_factor_(diffusion_factor)
    , diffusion_tensor_(new DiffusionTensorType(Stuff::Functions::internal::UnitMatrix< typename DiffusionTensorType::RangeFieldType, dimDomain >::value()))
  {}

  template< typename DiffusionType // This disables the ctor if dimDomain == 1, since factor and tensor are then identical
          , typename = typename std::enable_if< (std::is_same< DiffusionType, DiffusionTensorType >::value) // and the ctors
                                                && (dimDomain > 1) && sizeof(DiffusionType) >::type >       // ambiguous.
  Elliptic(const DiffusionType& diffusion_tensor)
    : diffusion_factor_(new DiffusionFactorType(1.))
    , diffusion_tensor_(diffusion_tensor)
  {}

  /// \name Required by LocalEvaluation::Codim0Interface< ..., 2 >
  /// \{

  LocalfunctionTupleType localFunctions(const EntityType& entity) const
  {
    return std::make_tuple(diffusion_factor_.access().local_function(entity),
                           diffusion_tensor_.access().local_function(entity));
  }

  /**
   * \brief extracts the local functions and calls the correct order() method
   */
  template< class R, size_t rT, size_t rCT, size_t rA, size_t rCA >
  size_t order(const LocalfunctionTupleType& local_functions_tuple,
               const Stuff::LocalfunctionSetInterface< E, D, d, R, rT, rCT >& test_base,
               const Stuff::LocalfunctionSetInterface< E, D, d, R, rA, rCA >& ansatz_base) const
  {
    const auto local_diffusion_factor = std::get< 0 >(local_functions_tuple);
    const auto local_diffusion_tensor = std::get< 1 >(local_functions_tuple);
    return order(*local_diffusion_factor, *local_diffusion_tensor, test_base, ansatz_base);
  }

  /**
   * \brief extracts the local functions and calls the correct evaluate() method
   */
  template< class R, size_t rT, size_t rCT, size_t rA, size_t rCA >
  void evaluate(const LocalfunctionTupleType& local_functions_tuple,
                const Stuff::LocalfunctionSetInterface< E, D, d, R, rT, rCT >& test_base,
                const Stuff::LocalfunctionSetInterface< E, D, d, R, rA, rCA >& ansatz_base,
                const Dune::FieldVector< D, d >& localPoint,
                Dune::DynamicMatrix< R >& ret) const
  {
    const auto local_diffusion_factor = std::get< 0 >(local_functions_tuple);
    const auto local_diffusion_tensor = std::get< 1 >(local_functions_tuple);
    evaluate(*local_diffusion_factor, *local_diffusion_tensor, test_base, ansatz_base, localPoint, ret);
  }

  /// \}
  /// \name Actual implementations of order
  /// \{

  template< class R, size_t rDF, size_t rCDF, size_t rDT, size_t rCDT, size_t rT, size_t rCT, size_t rA, size_t rCA >
  size_t order(const Stuff::LocalfunctionInterface   < E, D, d, R, rDF, rCDF >& local_diffusion_factor,
               const Stuff::LocalfunctionInterface   < E, D, d, R, rDT, rCDT >& local_diffusion_tensor,
               const Stuff::LocalfunctionSetInterface< E, D, d, R, rT,  rCT >& test_base,
               const Stuff::LocalfunctionSetInterface< E, D, d, R, rA,  rCA >& ansatz_base) const
  {
    return local_diffusion_factor.order()
         + local_diffusion_tensor.order()
         + std::max(ssize_t(test_base.order())   - 1, ssize_t(0))
         + std::max(ssize_t(ansatz_base.order()) - 1, ssize_t(0));
  }

  /// \}
  /// \name Actual implementations of evaluate
  /// \{

  template< class R, size_t r >
  void evaluate(const Stuff::LocalfunctionInterface   < E, D, d, R, 1, 1 >& local_diffusion_factor,
                const Stuff::LocalfunctionInterface   < E, D, d, R, d, d >& local_diffusion_tensor,
                const Stuff::LocalfunctionSetInterface< E, D, d, R, r, 1 >& test_base,
                const Stuff::LocalfunctionSetInterface< E, D, d, R, r, 1 >& ansatz_base,
                const Dune::FieldVector< D, d >& localPoint,
                Dune::DynamicMatrix< R >& ret) const
  {
    typedef Stuff::Common::FieldMatrix< R, d, d > TensorType;
    // evaluate local functions
    const auto       diffusion_factor_value = local_diffusion_factor.evaluate(localPoint);
    const TensorType diffusion_tensor_value = local_diffusion_tensor.evaluate(localPoint);
    const auto       diffusion_value        = diffusion_tensor_value * diffusion_factor_value;
    // evaluate bases
    const auto testGradients   = test_base.jacobian(localPoint);
    const auto ansatzGradients = ansatz_base.jacobian(localPoint);
    // compute elliptic evaluation
    const size_t rows = test_base.size();
    const size_t cols = ansatz_base.size();
    assert(ret.rows() >= rows);
    assert(ret.cols() >= cols);
    for (size_t ii = 0; ii < rows; ++ii) {
      auto& retRow = ret[ii];
      for (size_t jj = 0; jj < cols; ++jj)
        retRow[jj] = (diffusion_value * ansatzGradients[jj][0]) * testGradients[ii][0];
    }
  } // ... evaluate(...)

  /// \}

private:
  const DiffusionFactorProvider diffusion_factor_;
  const DiffusionTensorProvider diffusion_tensor_;
}; // class Elliptic

} // namespace LocalEvaluation
} // namespace GDT
} // namespace Dune

#endif // DUNE_GDT_EVALUATION_ELLIPTIC_HH


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<double, Eigen::Dynamic, 2> test_double_D_2(const Eigen::Matrix<double, Eigen::Dynamic, 2> & M)
{
	return M;
}
void export_double_D_2()
{
	boost::python::def("test_double_D_2",test_double_D_2);
}


/**
 * @file
 * @brief Observation of a generic n-joint robot.
 * @copyright 2020, Max Planck Gesellschaft. All rights reserved.
 * @license BSD 3-clause
 */
#pragma once

#include <string>
#include <vector>

#include <Eigen/Eigen>
#include <serialization_utils/cereal_eigen.hpp>

#include <robot_interfaces/loggable.hpp>

namespace robot_interfaces
{
/**
 * @brief Basic observation for a generic n-joint robot.
 *
 * Simple observation type with position, velocity and torque for each joint.
 *
 * @tparam N Number of joints.
 */
template <size_t N>
struct NJointObservation : public Loggable
{
    //! @brief Number of joints.
    static constexpr size_t num_joints = N;

    typedef Eigen::Matrix<double, N, 1> Vector;

    Vector position = Vector::Zero();
    Vector velocity = Vector::Zero();
    Vector torque = Vector::Zero();

    template <class Archive>
    void serialize(Archive& archive)
    {
        archive(position, velocity, torque);
    }

    std::vector<std::string> get_name() override
    {
        return {"position", "velocity", "torque"};
    }

    std::vector<std::vector<double>> get_data() override
    {
        typedef std::vector<double> vecd;

        std::vector<vecd> result = {
            vecd(position.size()), vecd(velocity.size()), vecd(torque.size())};

        Vector::Map(&result[0][0], position.size()) = position;
        Vector::Map(&result[1][0], velocity.size()) = velocity;
        Vector::Map(&result[2][0], torque.size()) = torque;

        return result;
    }
};

}  // namespace robot_interfaces


#include "flow.hpp"
#include "variant.hpp"
#include "indices.hpp"

#include <Eigen/Core>
#include <Eigen/Sparse>

#include <cassert>

#include <cstdlib>
#include <memory>
#include <cxxabi.h>

std::string demangle(const char* name) {

  int status;

  // enable c++11 by passing the flag -std=c++11 to g++
  std::unique_ptr<char, void(*)(void*)> res = {
    abi::__cxa_demangle(name, NULL, NULL, &status),
    std::free
  };

  return (status==0) ? res.get() : name ;
}

using real = double;

template<int N = Eigen::Dynamic, class U = real>
using vector = Eigen::Matrix<U, N, 1>;

using vec1 = vector<1>;
using vec2 = vector<2>;
using vec3 = vector<3>;
using vec4 = vector<4>;
using vec6 = vector<6>;

template<class G> struct dofs;

template<class G> struct traits;

template<class G>
using deriv = typename traits<G>::deriv;

template<class G>
using scalar = typename traits<G>::scalar;

template<int N, class U>
struct traits< vector<N, U> > {

  using scalar = typename traits<U>::scalar;
  using deriv = vector<N, typename traits<U>::deriv>;

  static const std::size_t dim = N * traits<U>::dim;

  static scalar dot(const vector<N, U>& x,
                    const vector<N, U>& y) {
    return x.dot(y);
  }

  static scalar& coord(std::size_t i, vector<N, U>& v) {
    return reinterpret_cast<scalar*>(v.data())[i];
  }

  static const scalar& coord(std::size_t i, const vector<N, U>& v) {
    return reinterpret_cast<scalar*>(v.data())[i];
  }

};

template<>
struct traits<real> {

  using deriv = real;
  using scalar = real;

  static const std::size_t dim = 1;

  static scalar dot(real x, real y) { return x * y; }

  static const scalar& coord(std::size_t, const real& value) { return value; }
  static scalar& coord(std::size_t, real& value) { return value; }
};

using triplet = Eigen::Triplet<real>;
using triplet_iterator = std::back_insert_iterator< std::vector<triplet> >;

// metrics
template<class G>
struct metric;

enum class metric_kind {
  mass,
    damping,
    stiffness,
    compliance
    };

struct metric_base {
  virtual ~metric_base() { }

  using cast_type = variant<
    metric<real>,
    metric<vec1>,
    metric<vec2>,
    metric<vec3>,
    metric<vec4>,
    metric<vec6>>;

  virtual cast_type cast() = 0;

  const metric_kind kind;

  metric_base(metric_kind kind) : kind(kind) { };
};

template<class G>
struct metric : metric_base, std::enable_shared_from_this< metric<G> > {

  using metric_base::metric_base;

  typename metric::cast_type cast() { return this->shared_from_this(); }
  virtual void tensor(triplet_iterator out, const G& at) const = 0;

};

template<metric_kind kind, class G>
struct uniform : metric<G> {

  uniform(real value = 1.0)
    : metric<G>(kind),
    value(value) {

  }

  real value;

  virtual void tensor(triplet_iterator out, const G& at) const {
    for(int i = 0, n = traits<G>::dim; i < n; ++i) {
      *out++ = {i, i, value};
    }
  }

};

// dofs
struct dofs_base {
  virtual ~dofs_base() { }

  using cast_type = variant<
    dofs<real>,
    dofs<vec1>,
    dofs<vec2>,
    dofs<vec3>,
    dofs<vec4>,
    dofs<vec6>>;

  virtual cast_type cast() = 0;

};

template<class G>
struct dofs : public dofs_base,
              public std::enable_shared_from_this< dofs<G> > {
  G pos;

  dofs_base::cast_type cast() {
    return this->shared_from_this();
  }

  std::size_t size() const {
    return 1;
  }

};

// functions
template<class T>
struct func;

struct func_base {
  virtual ~func_base() { }

  using cast_type = variant<
    func<real (real) >,
    func<vec1 (vec3)>,
    func<real (vec3, vec2)>
    >;

  virtual cast_type cast() = 0;

};

template<class, class T>
using repeat = T;

template<class To, class ... From>
struct func< To (From...) > : public func_base,
                              public std::enable_shared_from_this< func< To (From...) > >{

  func_base::cast_type cast() {
    return this->shared_from_this();
  }

  // output size from inputs
  virtual std::size_t size(const From&... from) const = 0;

  // apply function
  virtual void apply(To& to, const From&... from) const = 0;

  // sparse jacobian
  virtual void jacobian(repeat<From, triplet_iterator> ... out,
                        const From& ... from) const = 0;

  // sparse hessian
  virtual void hessian(triplet_iterator out,
                       const deriv<To>& dto,
                       const From& ... from) const { }
};

template<int M, int N, class U = real>
struct sum : func< U (vector<M, U>, vector<N, U> ) > {

  virtual std::size_t size(const vector<M, U>& lhs, const vector<N, U>& rhs) const {
    return 1;
  }

  virtual void apply(U& to, const vector<M, U>& lhs, const vector<N, U>& rhs) const {
    to = lhs.sum() + rhs.sum();
  }

  virtual void jacobian(triplet_iterator lhs_block, triplet_iterator rhs_block,
                        const vector<M, U>& lhs, const vector<N, U>& rhs) const {

    for(int i = 0, n = lhs.size(); i < n; ++i) {
      *lhs_block++ = {0, i, 1.0};
    }

    for(int i = 0, n = rhs.size(); i < n; ++i) {
      *rhs_block++ = {0, i, 1.0};
    }

  }

};

template<class U>
struct norm2 : func< scalar<U> ( U ) > {

  virtual std::size_t size(const U& ) const {
    return 1;
  }

  virtual void apply(scalar<U>& to, const U& from) const {
    to = traits<U>::dot(from, from) / 2.0;
  }

  virtual void jacobian(triplet_iterator block, const U& from) const {
    for(int i = 0, n = traits< deriv<U> >::dim; i < n; ++i) {
      *block++ = {0, i, traits<U>::coord(i, from)};
    }
  }

  virtual void hessian(triplet_iterator block, const scalar<U>& lambda, const U& from) const {
    for(int i = 0, n = traits< deriv<U> >::dim; i < n; ++i) {
      *block++ = {i, i, lambda};
    }
  }

};

// use this for constraints
template<class U>
struct pairing : func< scalar<U>(U, U) > {
  virtual std::size_t size(const U& ) const {
    return 1;
  }

  virtual void apply(scalar<U>& to, const U& lhs, const U& rhs) const {
    to = traits<U>::dot(lhs, rhs);
  }

  virtual void jacobian(triplet_iterator lhs_block, triplet_iterator rhs_block,
                        const U& lhs, const U& rhs) const {
    for(unsigned i = 0, n = traits< deriv<U> >::dim; i < n; ++i) {
      *lhs_block++ = {0, i, traits<U>::coord(i, rhs)};
      *rhs_block++ = {0, i, traits<U>::coord(i, lhs)};
    }
  }

  virtual void hessian(triplet_iterator block, const scalar<U>& lambda,
                       const U& lhs, const U& rhs) const {
    for(int i = 0, n = traits< deriv<U> >::dim; i < n; ++i) {
      *block++ = {i, n + i, lambda};
      *block++ = {n + i, i, lambda};
    }

  }

};

using vertex = variant<dofs_base, func_base, metric_base>;
struct edge {};

struct graph : dependency_graph<vertex, edge> {

  std::map< void*, unsigned > vertex;

  template<class T>
  struct node : T {
	std::size_t vertex;

	using T::T;
  };

  template<class T, class ... Args>
  std::shared_ptr< node<T> > add(Args&& ... args) {

	auto ptr = std::make_shared<node<T>>(std::forward<Args>(args)...);
    ptr->vertex = add_vertex( std::shared_ptr<T>(ptr), *this);

	return ptr;
  }

  range<graph::vertex_iterator> vertices() {
    return make_range( boost::vertices(*this) );
  }

};

struct typecheck {

  void operator()(dofs_base* self, unsigned v, const graph& g) const {
    if( out_degree(v, g) > 0 ) {
      throw std::runtime_error("dofs must be independent (zero out-edge)");
    }
  }

  void operator()(metric_base* self, unsigned v, const graph& g) const {

    if( out_degree(v, g) != 1 ) {
      throw std::runtime_error("metric must be dependent (single out-edge)");
    }

    self->cast().apply(*this, v, g);
  }

  template<class G>
  void operator()(metric<G>* self, unsigned v, const graph& g) const {

    auto p = *adjacent_vertices(v, g).first;

    g[p].apply(expected_check<G>());
  }

  // dispatch
  void operator()(func_base* self, unsigned v, const graph& g) const {
    self->cast().apply(*this, v, g);
  }

  struct type_error : std::runtime_error {
    using std::runtime_error::runtime_error;
  };

  struct edge_error : std::runtime_error {
    using std::runtime_error::runtime_error;
  };

  template<class Expected>
  struct expected_check {

    template<class T>
    void operator()(T* self) const {
      throw std::logic_error("should not happen");
    }

    // dispatch
    void operator()(dofs_base* self) const {
      self->cast().apply(*this);
    }

    void operator()(func_base* self) const {
      self->cast().apply(*this);
    }

    // ok cases
    void operator()(dofs<Expected>* ) const { };

    template<class ... From>
    void operator()(func<Expected (From...) >* ) const { };

    // error cases
    template<class T>
    void operator()(dofs<T>* ) const {
      throw std::runtime_error("type error: "+ demangle( typeid(Expected (T) ).name() ));
    }

    template<class T, class ... From>
    void operator()(func<T (From...) >* ) const {
      throw std::runtime_error("type error: " + demangle( typeid(Expected (T) ).name() ));
    }

  };

  template<class Expected, class Iterator, class G>
  static void check_type(Iterator first, Iterator last, const G& g) {
    if(first == last) {
      throw std::runtime_error("expected more out-edges");
    }

    const unsigned v = *first;

    g[v].apply(expected_check<Expected>());
  }

  // typecheck from types
  template<class To, class ... From>
  void operator()(func<To (From...)>* self, unsigned v, const graph& g) const {

    auto out = adjacent_vertices(v, g);
    try {
      const int expand[] = { (check_type<From>(out.first++, out.second, g), 0)...};

      // TODO this could be ok though
      if(out.first != out.second) {
        throw std::runtime_error("expected less out-edges");
      }
    } catch( std::runtime_error& e ){
      throw std::runtime_error(e.what()
                               + std::string(" for vertex: ")
                               + std::to_string(v) );
    }

  }

};

// apply itself on the same variant after calling ->cast()
template<class Derived>
struct dispatch {

  const Derived& derived() const { return static_cast<const Derived&>(*this); }

  // note: we don't want perfect forwarding here since it may preempt
  // derived classes operator()
  template<class T, class ... Args>
  void operator()(T* self, const Args& ... args) {
    self->cast().apply( derived(), args...);
  }

};

class stack {
  std::vector<char> storage;
  std::size_t sp;

  // TODO not sure we need num here
  template<class G>
  std::size_t aligned_sp(std::size_t num) {
    std::size_t space = -1;
    void* buf = storage.data() + sp;
    char* aligned = (char*)std::align( alignof(G), num * sizeof(G), buf, space);
    return aligned - storage.data();
  }

public:

  struct frame {
    // stack pointer, buffer size, element count
    std::size_t sp, size, count;
  };

  struct overflow : std::runtime_error {
    stack& who;
    const std::size_t required;

    overflow(stack& who, std::size_t required)
      : std::runtime_error("stack overflow"),
        who(who),
        required(required) { }

  };

  stack(std::size_t size = 0)
    : storage(size),
      sp(0) {

  }

  template<class G>
  G* allocate(frame& f, std::size_t count = 1) {
    assert(count > 0 && "zero-size allocation");

    const std::size_t size = count * sizeof(G);

    if(!storage.capacity()) {
      reserve(size);
    }

    // align
    sp = aligned_sp<G>(count);
    const std::size_t required = sp + size;

    if( required > storage.capacity() ) {
      throw overflow(*this, required);
    }

    // allocate space
    storage.resize( required );

    G* res = reinterpret_cast<G*>(&storage[sp]);

    // record frame
    f = {sp, size, count};
    sp += size;

    return res;
  }

  template<class G>
  G& get(const frame& f) {
    return reinterpret_cast<G&>(storage[f.sp]);
  }

  template<class G>
  const G& get(const frame& f) const {
    return reinterpret_cast<const G&>(storage[f.sp]);
  }

  void reset() {
    sp = 0;
  }

  std::size_t capacity() const { return storage.capacity(); }
  std::size_t size() const { return storage.size(); }

  void reserve(std::size_t size) {
    std::clog << "stack reserve: " << capacity()
              << " -> " << size << std::endl;

    storage.reserve(size);
  }

  void grow(std::size_t required = 0) {
    const std::size_t c = capacity();
    assert( c );

    const std::size_t min = std::max(required, c+1);
    reserve( std::max(min, c + c / 2) );
  }
};

// TODO we probably want a tag phantom type here to get stricter
// typechecking
class graph_data {
  stack storage;
  std::vector<stack::frame> frame;
public:
  graph_data(std::size_t num_vertices,
             std::size_t capacity = 0)
    : storage(capacity),
      frame(num_vertices) {

  }

  template<class G>
  G* allocate(unsigned v, std::size_t count = 1) {
    return storage.allocate<G>(frame[v], count);
  }

  template<class G>
  G& get(unsigned v) {
    return storage.get<G>(frame[v]);
  }

  template<class G>
  const G& get(unsigned v) const {
    return storage.get<G>(frame[v]);
  }

  std::size_t count(unsigned v) const {
    return frame[v].count;
  }

  // TODO reserve, size, etc
  void grow() {
    storage.grow();
  }

  void reset() {
    storage.reset();
  }

};

// push positions
struct push : dispatch<push> {

  graph_data& pos;

  push(graph_data& pos) : pos(pos) { }

  using dispatch::operator();

  template<class G>
  void operator()(metric<G>* self, unsigned v, const graph& g) const {

  }

  // push dofs data to the stack
  template<class G>
  void operator()(dofs<G>* self, unsigned v, const graph& g) const {
    const std::size_t count = self->size();

    // allocate
    G* data = pos.allocate<G>(v, count);

    // TODO static_assert G is pod ?

    // copy
    *data = self->pos;
  }

  template<class To, class ... From, std::size_t ... I>
  void operator()(func<To (From...) >* self, unsigned v, const graph& g) const {
    operator()(self, v, g, indices_for<From...>() );
  }

  template<class To, class ... From, std::size_t ... I>
  void operator()(func<To (From...) >* self, unsigned v, const graph& g,
                  indices<I...> idx) const {

    // note: we need random access parent iterator for this
    auto parents = adjacent_vertices(v, g).first;

    // result size
    const std::size_t count =
      self->size( pos.get<const From>(parents[I])... );

    // allocate result
    To* to = pos.allocate<To>(v, count);

    self->apply(*to, pos.get<const From>(parents[I])...);

    std::clog << "mapped: " << *to << std::endl;
  }

};

// deriv data numbering
struct chunk {
  std::size_t start, size;
};

struct numbering : dispatch<numbering> {

  using chunks_type = std::vector<chunk>;
  numbering(chunks_type& chunks,
            std::size_t& offset,
            const graph_data& pos)
    : chunks(chunks),
      offset(offset),
      pos(pos) {

  }

  using dispatch::operator();

  chunks_type& chunks;
  std::size_t& offset;
  const graph_data& pos;

  template<class G>
  void operator()(metric<G>* self, unsigned v) const {
    // TODO don't dispatch
  }

  template<class G>
  void operator()(dofs<G>* self, unsigned v) const {
    chunk& c = chunks[v];

    c.size = pos.count(v) * traits< deriv<G> >::dim;
    c.start = offset;

    offset += c.size;
  }

  template<class To, class ... From>
  void operator()(func<To (From...) >* self, unsigned v) const {
    chunk& c = chunks[v];

    c.size = pos.count(v) * traits< deriv<To> >::dim;
    c.start = offset;

    offset += c.size;
  }

};

using rmat = Eigen::SparseMatrix<real, Eigen::RowMajor>;

// fetch jacobians w/ masking
struct fetch : dispatch<fetch> {

  using matrix_type = std::vector<triplet>;
  matrix_type& jacobian;
  matrix_type& diagonal;

  graph_data& mask;

  using elements_type = std::vector< matrix_type >;

  // internal
  elements_type& elements;

  const numbering::chunks_type& chunks;
  const graph_data& pos;

  fetch(matrix_type& jacobian,
        matrix_type& diagonal,
        graph_data& mask,
        elements_type& elements,
        const numbering::chunks_type& chunks,
        const graph_data& pos)
    : jacobian(jacobian),
      diagonal(diagonal),
      mask(mask),
      elements(elements),
      chunks(chunks),
      pos(pos) {

  }

  real dt = 1.0;

  using dispatch::operator();

  template<class G>
  void operator()(metric<G>* self, unsigned v, const graph& g) const {

    // remember current size
    const std::size_t start = diagonal.size();

    // obtain triplets
    self->tensor(std::back_inserter(diagonal), pos.get<G>(v));

    const std::size_t end = diagonal.size();

    real factor = 0;

    switch(self->kind) {
    case metric_kind::mass: factor = 1; break;
    case metric_kind::damping: factor = dt; break;
    case metric_kind::stiffness: factor = dt * dt; break;
    case metric_kind::compliance: factor = -1.0 / (dt * dt); break;
    };

    const unsigned parent = *adjacent_vertices(v, g).first;

    const chunk& c = chunks[parent];

    // shift/scale inserted data
    for(unsigned i = start; i < end; ++i) {
      auto& it = diagonal[i];
      it = {it.row() + int(c.start),
            it.col() + int(c.start),
            factor * it.value()};
    }

  }

  template<class G>
  void operator()(dofs<G>* self, unsigned v, const graph& g) const {
    const chunk& c = chunks[v];

    for(unsigned i = c.start, n = c.start + c.size; i < n; ++i) {
      jacobian.emplace_back(i, i, 1.0);
    }
  }

  template<class To, class ... From, std::size_t ... I>
  void operator()(func<To (From...) >* self, unsigned v, const graph& g) const {
    operator()(self, v, g, indices_for<From...>() );
  }

  template<class To, class ... From, std::size_t ... I>
  void operator()(func<To (From...) >* self, unsigned v, const graph& g,
                  indices<I...> idx) const {

    auto parents = adjacent_vertices(v, g).first;

    // get dofs count
    const std::size_t count = pos.count(v);
    const std::size_t parent_count[] = { pos.count(parents[I])... };

    const std::size_t rows = count * traits< deriv<To> >::dim;
    const std::size_t cols[] = { parent_count[I] * traits< deriv<From> >::dim... };

    // no child: allocate mask
    if( in_degree(v, g) == 0 ) {
      mask.allocate<char>(v, count);
    }

    const std::size_t n = sizeof...(From);

    if( elements.size() < n) elements.resize(n);

    for(unsigned p = 0; p < n; ++p) {
      // alloc parent masks
      const unsigned vp = parents[p];
      mask.allocate<char>(vp, parent_count[p]);

      // TODO make sure this does not dealloc
      elements[p].clear();
    }

    // fetch jacobian data
    elements.clear();
    self->jacobian(std::back_inserter(elements[I])...,
                   pos.get<const From>(parents[I])...);

    // TODO set parent masks
    const chunk& curr_chunk = chunks[v];

    for(unsigned p = 0; p < n; ++p) {
      const unsigned vp = parents[p];
      const chunk& parent_chunk = chunks[vp];

      // shift elements
      for(triplet& it : elements[p]) {
        // TODO filter by mask
        jacobian.emplace_back(it.row() + curr_chunk.start,
                              it.col() + parent_chunk.start,
                              it.value());
      }
    }

  }

};

// TODO compute forces + hessians

struct concatenate {
  rmat& jacobian;
  const numbering::chunks_type& chunks;

  concatenate(rmat& jacobian,
              const numbering::chunks_type& chunks)
    : jacobian(jacobian),
      chunks(chunks) {

  }

  void operator()(dofs_base* self, unsigned v) const { }

  void operator()(func_base* self, unsigned v) const {
    const chunk& c = chunks[v];

    // noalias should be safe here (diagonal is zero)
    jacobian.middleRows(c.start, c.size) =
      jacobian.middleRows(c.start, c.size) * jacobian;

  }

};

template<class F>
static void with_auto_stack( const F& f ) {
  while (true) {
    try{
      f();
      return;
    } catch(stack::overflow& e) {
      e.who.grow();
      e.who.reset();
    }
  }
}

static void concatenate(rmat& res, const rmat& src) {
  res.resize(src.rows(), src.cols());

  std::vector<bool> mask(src.cols());
  std::vector<real> values(src.cols());
  std::vector<int> indices(src.cols());

  // TODO better estimate?
  const int estimated_nnz = src.nonZeros() + src.nonZeros() / 2;
  res.reserve(estimated_nnz);

  std::fill(mask.begin(), mask.end(), false);

  for(unsigned i = 0, n = src.rows(); i < n; ++i) {
    unsigned nnz = 0;

    for(rmat::InnerIterator src_it(src, i); src_it; ++src_it) {
      const real y = src_it.value();
      const unsigned k = src_it.col();

      // take row from previously computed if possible
      for(rmat::InnerIterator res_it(k < i ? res : src, k); res_it; ++res_it) {
        const unsigned j = res_it.col();
        const real x = res_it.value();

        if(!mask[j]) {
          mask[j] = true;
          values[j] = x * y;
          indices[nnz] = j;
          ++nnz;
        } else {
          values[j] += x * y;
        }
      }
    }

    std::sort(indices.data(), indices.data() + nnz);

    res.startVec(i);
    for(unsigned k = 0; k < nnz; ++k) {
      const unsigned j = indices[k];
      res.insertBack(i, j) = values[j];
      mask[j] = false;
    }

  }

  res.finalize();
}

int main(int, char**) {

  graph g;

  // independent dofs
  auto point3 = g.add< dofs<vec3> >();
  auto point2 = g.add< dofs<vec2> >();

  auto mass3 = g.add< uniform<metric_kind::mass, vec3> >(2);
  auto mass2 = g.add< uniform<metric_kind::mass, vec2> >();

  // mapped
  auto map1 = g.add< sum<3, 2> >();
  auto map2 = g.add< norm2<double> >();

  auto ff1 = g.add< uniform<metric_kind::stiffness, double> >(3);

  point3->pos = {1, 2, 3};
  point2->pos = {4, 5};

  add_edge(map1->vertex, point3->vertex, g);
  add_edge(map1->vertex, point2->vertex, g);

  add_edge(map2->vertex, map1->vertex, g);

  add_edge(mass3->vertex, point3->vertex, g);
  add_edge(mass2->vertex, point2->vertex, g);

  add_edge(ff1->vertex, map2->vertex, g);

  std::vector<unsigned> order;
  g.sort(order);

  for(unsigned v : order) {
    g[v].apply( typecheck(), v, g );
  }

  const std::size_t n = num_vertices(g);

  graph_data pos(n);
  std::vector<chunk> chunks(n);

  // propagate positions and number dofs
  std::size_t offset;

  with_auto_stack([&] {
      offset = 0;
      for(unsigned v : order) {
        g[v].apply( push(pos), v, g);
        g[v].apply( numbering(chunks, offset, pos), v);
      }
    });

  const std::size_t total_dim = offset;

  graph_data mask(n);

  std::vector<triplet> jacobian, diagonal;
  std::vector< std::vector<triplet> > elements;

  const real dt = 0.01;

  // compute masks/jacobians
  with_auto_stack([&] {
      jacobian.clear();
      diagonal.clear();

      fetch vis(jacobian, diagonal, mask, elements, chunks, pos);
      vis.dt = dt;

      for(unsigned v : reverse(order)) {
        g[v].apply( vis, v, g);
      }
    });

  rmat J(total_dim, total_dim), H(total_dim, total_dim);

  J.setFromTriplets(jacobian.begin(), jacobian.end());
  H.setFromTriplets(diagonal.begin(), diagonal.end());

  std::cout << "diagonal: " << H << std::endl;

  rmat concat;
  concatenate(concat, J);

  std::cout << "after concatenation: " << std::endl;
  std::cout << concat << std::endl;

  rmat full = concat.transpose() * H * concat;
  std::cout << "H: " << full << std::endl;

  return 0;
}


// Copyright (c) Dietmar Wolz.
//
// This source code is licensed under the MIT license found in the
// LICENSE file in the root directory.

// Eigen based implementation of differential evolution using the DE/best/1 strategy.
// Uses three deviations from the standard DE algorithm:
// a) temporal locality introduced in
// https://www.researchgate.net/publication/309179699_Differential_evolution_for_protein_folding_optimization_based_on_a_three-dimensional_AB_off-lattice_model
// b) reinitialization of individuals based on their age.
// c) oscillating CR/F parameters.
// requires https://github.com/imneme/pcg-cpp

#include <Eigen/Core>
#include <iostream>
#include <float.h>
#include <stdint.h>
#include <ctime>
#include <random>
#include <queue>
#include <tuple>
#include "pcg_random.hpp"

using namespace std;

typedef Eigen::Matrix<double, Eigen::Dynamic, 1> vec;
typedef Eigen::Matrix<int, Eigen::Dynamic, 1> ivec;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> mat;

typedef double (*callback_type)(int, double[]);

namespace differential_evolution {

static uniform_real_distribution<> distr_01 = std::uniform_real_distribution<>(
        0, 1);

static normal_distribution<> gauss_01 = std::normal_distribution<>(0, 1);

static vec zeros(int n) {
    return Eigen::MatrixXd::Zero(n, 1);
}

static Eigen::MatrixXd uniform(int dx, int dy, pcg64 &rs) {
    return Eigen::MatrixXd::NullaryExpr(dx, dy, [&]() {
        return distr_01(rs);
    });
}

static Eigen::MatrixXd uniformVec(int dim, pcg64 &rs) {
    return Eigen::MatrixXd::NullaryExpr(dim, 1, [&]() {
        return distr_01(rs);
    });
}

static Eigen::MatrixXd normalVec(int dim, pcg64 &rs) {
    return Eigen::MatrixXd::NullaryExpr(dim, 1, [&]() {
        return gauss_01(rs);
    });
}

static int index_min(vec &v) {
    double minv = DBL_MAX;
    int mi = -1;
    for (int i = 0; i < v.size(); i++) {
        if (v[i] < minv) {
            mi = i;
            minv = v[i];
        }
    }
    return mi;
}

// wrapper around the fitness function, scales according to boundaries

class Fitness {

public:

    Fitness(callback_type pfunc, int dimension, const vec &lower_limit,
            const vec &upper_limit) {
        func = pfunc;
        dim = dimension;
        lower = lower_limit;
        upper = upper_limit;
        evaluationCounter = 0;
        if (lower.size() > 0) // bounds defined
            scale = (upper - lower);
    }

    double eval(const vec &X) {
        int n = X.size();
        double parg[n];
        for (int i = 0; i < n; i++)
            parg[i] = X(i);
        double res = func(n, parg);
        evaluationCounter++;
        return res;
    }

    void values(const mat &popX, int popsize, vec &ys) {
        for (int p = 0; p < popsize; p++)
            ys[p] = eval(popX.col(p));
    }

    vec getClosestFeasible(const vec &X) const {
        if (lower.size() > 0)
            return X.cwiseMin(upper).cwiseMax(lower);
        else
            return X;
    }

    bool feasible(int i, double x) {
        return lower.size() == 0 || (x >= lower[i] && x <= upper[i]);
    }

    vec sample(pcg64 &rs) {
        if (lower.size() > 0) {
            vec rv = uniformVec(dim, rs);
            return (rv.array() * scale.array()).matrix() + lower;
        } else
            return normalVec(dim, rs);
    }

    double sample_i(int i, pcg64 &rs) {
        if (lower.size() > 0)
            return lower[i] + scale[i] * distr_01(rs);
        else
            return gauss_01(rs);
    }

    int getEvaluations() {
        return evaluationCounter;
    }

private:
    callback_type func;
    int dim;
    vec lower;
    vec upper;
    long evaluationCounter;
    vec scale;
};

class DeOptimizer {

public:

    DeOptimizer(long runid_, Fitness *fitfun_, int dim_, int seed_,
            int popsize_, int maxEvaluations_, double keep_,
            double stopfitness_, double F_, double CR_) {
        // runid used to identify a specific run
        runid = runid_;
        // fitness function to minimize
        fitfun = fitfun_;
        // Number of objective variables/problem dimension
        dim = dim_;
        // Population size
        popsize = popsize_ > 0 ? popsize_ : 15 * dim;
        // maximal number of evaluations allowed.
        maxEvaluations = maxEvaluations_ > 0 ? maxEvaluations_ : 50000;
        // keep best young after each iteration.
        keep = keep_ > 0 ? keep_ : 30;
        // Limit for fitness value.
        stopfitness = stopfitness_;
        F = F0 = F_ > 0 ? F_ : 0.5;
        CR = CR0 = CR_ > 0 ? CR_ : 0.9;
        // Number of iterations already performed.
        iterations = 0;
        bestY = DBL_MAX;
        // stop criteria
        stop = 0;
        pos = 0;
        //std::random_device rd;
        rs = new pcg64(seed_);
        init();
    }

    ~DeOptimizer() {
        delete rs;
    }

    double rnd01() {
        return distr_01(*rs);
    }

    int rndInt(int max) {
        return (int) (max * distr_01(*rs));
    }

    vec nextX(int p, const vec &xp, const vec &xb) {
        if (p == 0) {
            iterations++;
            CR = iterations % 2 == 0 ? 0.5 * CR0 : CR0;
            F = iterations % 2 == 0 ? 0.5 * F0 : F0;
        }
        int r1, r2;
        do {
            r1 = rndInt(popsize);
        } while (r1 == p || r1 == bestI);
        do {
            r2 = rndInt(popsize);
        } while (r2 == p || r2 == bestI || r2 == r1);
        vec x1 = popX.col(r1);
        vec x2 = popX.col(r2);
        vec x = xb + (x1 - x2) * F;
        int r = rndInt(dim);
        for (int j = 0; j < dim; j++)
            if (j != r && rnd01() > CR)
                x[j] = xp[j];
        return fitfun->getClosestFeasible(x);
    }

    vec next_improve(const vec &xb, const vec &x, const vec &xi) {
        return fitfun->getClosestFeasible(xb + ((x - xi) * 0.5));
    }

    vec ask_one(int &p) {
        // ask for one new argument vector.
        if (improvesX.empty()) {
            p = pos;
            vec x = nextX(p, popX.col(p), popX.col(bestI));
            pos = (pos + 1) % popsize;
            return x;
        } else {
            p = improvesP.front();
            vec x = improvesX.front();
            improvesP.pop();
            improvesX.pop();
            return x;
        }
    }

    int tell_one(double y, const vec &x, int p) {
        //tell function value for a argument list retrieved by ask_one().
        if (isfinite(y) && y < popY[p]) {
            if (iterations > 1) {
                // temporal locality
                improvesP.push(p);
                improvesX.push(next_improve(popX.col(bestI), x, popX0.col(p)));
            }
            popX0.col(p) = popX.col(p);
            popX.col(p) = x;
            popY[p] = y;
            popIter[p] = iterations;
            if (y < popY[bestI]) {
                bestI = p;
                if (y < bestY) {
                    bestY = y;
                    bestX = x;
                    if (isfinite(stopfitness) && bestY < stopfitness)
                        stop = 1;
                }
            }
        } else {
            // reinitialize individual
            if (keep * rnd01() < iterations - popIter[p]) {
                popX.col(p) = fitfun->sample(*rs);
                popY[p] = DBL_MAX;
            }
        }
        return stop;
    }

    void doOptimize() {

        // -------------------- Generation Loop --------------------------------
        for (iterations = 1; fitfun->getEvaluations() < maxEvaluations;
                iterations++) {

            CR = iterations % 2 == 0 ? 0.5 * CR0 : CR0;
            F = iterations % 2 == 0 ? 0.5 * F0 : F0;

            for (int p = 0; p < popsize; p++) {
                vec xp = popX.col(p);
                vec xb = popX.col(bestI);

//                vec x = nextX(p, xp, xb);

                int r1, r2;
                do {
                    r1 = rndInt(popsize);
                } while (r1 == p || r1 == bestI);
                do {
                    r2 = rndInt(popsize);
                } while (r2 == p || r2 == bestI || r2 == r1);
                vec x1 = popX.col(r1);
                vec x2 = popX.col(r2);
                int r = rndInt(dim);
                vec x = vec(xp);
                for (int j = 0; j < dim; j++) {
                    if (j == r || rnd01() < CR) {
                        x[j] = xb[j] + F * (x1[j] - x2[j]);
                        if (!fitfun->feasible(j, x[j]))
                            x[j] = fitfun->sample_i(j, *rs);
                    }
                }

                double y = fitfun->eval(x);
                if (isfinite(y) && y < popY[p]) {
                    // temporal locality
                    vec x2 = next_improve(xb, x, xp);
                    double y2 = fitfun->eval(x2);
                    if (isfinite(y2) && y2 < y) {
                        y = y2;
                        x = x2;
                    }
                    popX.col(p) = x;
                    popY(p) = y;
                    popIter[p] = iterations;
                    if (y < popY[bestI]) {
                        bestI = p;
                        if (y < bestY) {
                            bestY = y;
                            bestX = x;
                            if (isfinite(stopfitness) && bestY < stopfitness) {
                                stop = 1;
                                return;
                            }
                        }
                    }
                } else {
                    // reinitialize individual
                    if (keep * rnd01() < iterations - popIter[p]) {
                        popX.col(p) = fitfun->sample(*rs);
                        popY[p] = DBL_MAX;
                    }
                }
            }
        }
    }

    void init() {
        popX = mat(dim, popsize);
        popX0 = mat(dim, popsize);
        popY = vec(popsize);
        for (int p = 0; p < popsize; p++) {
            popX0.col(p) = popX.col(p) = fitfun->sample(*rs);
            popY[p] = DBL_MAX; // compute fitness
        }
        bestI = 0;
        bestX = popX.col(bestI);
        popIter = zeros(popsize);
    }

    vec getBestX() {
        return bestX;
    }

    double getBestValue() {
        return bestY;
    }

    double getIterations() {
        return iterations;
    }

    double getStop() {
        return stop;
    }

    Fitness* getFitfun() {
        return fitfun;
    }

    int getDim() {
        return dim;
    }

private:
    long runid;
    Fitness *fitfun;
    int popsize; // population size
    int dim;
    int maxEvaluations;
    double keep;
    double stopfitness;
    int iterations;
    double bestY;
    vec bestX;
    int bestI;
    int stop;
    double F0;
    double CR0;
    double F;
    double CR;
    pcg64 *rs;
    mat popX;
    mat popX0;
    vec popY;
    vec popIter;
    queue<vec> improvesX;
    queue<int> improvesP;
    int pos;
};

// see https://cvstuff.wordpress.com/2014/11/27/wraping-c-code-with-python-ctypes-memory-and-pointers/

}

using namespace differential_evolution;

extern "C" {
double* optimizeDE_C(long runid, callback_type func, int dim, int seed,
        double *lower, double *upper, int maxEvals, double keep,
        double stopfitness, int popsize, double F, double CR) {
    int n = dim;
    double *res = new double[n + 4];
    vec lower_limit(n), upper_limit(n);
    bool useLimit = false;
    for (int i = 0; i < n; i++) {
        lower_limit[i] = lower[i];
        upper_limit[i] = upper[i];
        useLimit |= (lower[i] != 0);
        useLimit |= (upper[i] != 0);
    }
    if (useLimit == false) {
        lower_limit.resize(0);
        upper_limit.resize(0);
    }
    Fitness fitfun(func, n, lower_limit, upper_limit);
    DeOptimizer opt(runid, &fitfun, dim, seed, popsize, maxEvals, keep,
            stopfitness, F, CR);
    try {
        opt.doOptimize();
        vec bestX = opt.getBestX();
        double bestY = opt.getBestValue();
        for (int i = 0; i < n; i++)
            res[i] = bestX[i];
        res[n] = bestY;
        res[n + 1] = fitfun.getEvaluations();
        res[n + 2] = opt.getIterations();
        res[n + 3] = opt.getStop();
        return res;
    } catch (std::exception &e) {
        cout << e.what() << endl;
        return res;
    }
}
}


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

  @copyright 2012-2015 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-constant

    Generate the constant mask2frexp, a utility mask for frexp implementation.

    @par Semantic:

    @code
    as_integer<T> r = Mask2frexp<T>();
    @endcode

    @code
    if T is double
      r =  4602678819172646912ll;
    else if T is float
      r =  1056964608;
    @endcode

    @return The Mask2frexp constant for the proper type
  **/
  template<typename T> T Mask2frexp();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant
      Generate the  constant mask2frexp.

      @return The Mask2frexp constant for the proper type
    **/
    const boost::dispatch::functor<tag::mask2frexp_> mask2frexp = {};
  }
} }
#endif

#include <boost/simd/constant/definition/mask2frexp.hpp>
#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#endif


#ifndef FeaturesExtractor_hpp
#define FeaturesExtractor_hpp

#include <QScopedPointer>
#include <QAlgorithm.hpp>
#include <UMF/SignalProcessing.hpp>
#include <SFML/Audio/InputSoundFile.hpp>
#include <SFML/System/Mutex.hpp>
#include <SFML/System/Lock.hpp>
#include <TMath.h>
#include <ROOT/TSeq.hxx>
#include <TSpectrum.h>
#include <TVector.h>
#include <TH1D.h>
#include <TF1.h>
#include <alglib/fasttransforms.h>

namespace AA {
	class FeaturesExtractor;
}

/** Class to process an audio file supporting multithreading.
 This class is both a QObject and a QRunnable, so that it can be executed in QThreadPool,
 but it is also able to signal that the process has ended, and that some notable series
 has been computed, allowing them to be exploited in other threads.
 */
class AA::FeaturesExtractor : public QAlgorithm {

	Q_OBJECT

	/** Sample rate of the audio input. */
	QA_OUTPUT(double, SampleRate)
	/** Total number of records in the file. */
	QA_OUTPUT(int, TotalRecords)
	/** Record length in samples */
	QA_OUTPUT(int, RecordLength)
	/** The features computed during the process. */
	QA_OUTPUT(QVector<double>, Features)

	/** Directory pointing to the file to be read. */
	QA_PARAMETER(QString, File, QString())
	/** Record index to be evaluated. */
	QA_PARAMETER(int, SelectRecord, -1)
	/** Minimum frequency allowed */
	QA_PARAMETER(double, MinimumFrequency, 200.0)
	/** Maximum frequency allowed */
	QA_PARAMETER(double, MaximumFrequency, 4000.0)
	/** Maximum allowed spectrum leakage.
	 This parameter will affect the record length. Namely,
	 a larger value will reduce the record length, thus speeding up the algorithm
	 and, on the other hand, will increase the spectrum leakage, as described
	 in Douglas A. Lyon: “The Discrete Fourier Transform, Part 4: Spectral Leakage”.
	 */
	QA_PARAMETER(double, MaximumSpectrumLeakage, 10.0)
	/** Operation performed to reduce channles.
	 @sa UMF::ReduceChannels, UMF::ReduceChannels::operation
	 */
	QA_PARAMETER(int, ChannelsOperation, UMF::ReduceChannels::average)
	/** How channels are arranged in the input file.
	 @sa UMF::ReduceChannels, UMF::ReduceChannels::channels
	 */
	QA_PARAMETER(int, ChannelsArrangement, UMF::ReduceChannels::interleaved)
	/** Function used for windowing.
	 @sa UMF::Windowing, UMF::Windowing::function
	 */
	QA_PARAMETER(int, WindowingFunction, UMF::Windowing::hann)
	/** Border extrapolation method.
	 @sa UMF::ArrayPad, UMF::ArrayPad::border_type
	 */
	QA_PARAMETER(int, ExtrapolationMethod, UMF::ArrayPad::constant)
	/** Width of the Gaussian filter applied to the input signal.
	 @sa UMF::GaussianFilter
	 */
	QA_PARAMETER(int, GaussianFilterWidth, 5)
	/** Number of iterations for background suppression.
	 @sa UMF::SpectrumRemoveBackground
	 */
	QA_PARAMETER(int, BackIterations, 6)
	/** Direction used for background suppression.
	 @sa UMF::SpectrumRemoveBackground
	 */
	QA_PARAMETER(int, BackDirection, TSpectrum::kBackIncreasingWindow)
	/** Filter order used for background suppression.
	 @sa UMF::SpectrumRemoveBackground
	 */
	QA_PARAMETER(int, BackFilterOrder, TSpectrum::kBackOrder2)
	/** Whether applying a smoothing in background suppression.
	 @sa UMF::SpectrumRemoveBackground
	 */
	QA_PARAMETER(bool, BackSmoothing, false)
	/** Smoothing window size used for background suppression.
	 @sa UMF::SpectrumRemoveBackground
	 */
	QA_PARAMETER(int, BackSmoothWindow, TSpectrum::kBackSmoothing3)
	/** Whether computing Compton edges in background suppression.
	 @sa UMF::SpectrumRemoveBackground
	 */
	QA_PARAMETER(bool, BackCompton, false)

	QA_CTOR_INHERIT
	QA_IMPL_CREATE(FeaturesExtractor)

public:
	void run();

Q_SIGNALS:
	Q_SIGNAL void timeSeries(QVector<double>);
	Q_SIGNAL void frequencySeries(QVector<double>);
	Q_SIGNAL void pointSeries(QVector<int>);

private:
	static sf::Mutex mutex;
};

#endif /* FeaturesExtractor_hpp */


#ifndef BOOST_HASH2_FNV1A_HPP_INCLUDED
#define BOOST_HASH2_FNV1A_HPP_INCLUDED

// Copyright 2017, 2018 Peter Dimov.
// Distributed under the Boost Software License, Version 1.0.
//
// FNV-1a
//
// https://en.wikipedia.org/wiki/Fowler%E2%80%93Noll%E2%80%93Vo_hash_function

#include <boost/hash2/byte_type.hpp>
#include <boost/hash2/detail/write.hpp>
#include <boost/cstdint.hpp>
#include <boost/assert.hpp>
#include <boost/config.hpp>
#include <cstddef>

namespace boost
{
namespace hash2
{

namespace detail
{

template<class T> struct fnv1a_const;

template<> struct fnv1a_const<boost::uint32_t>
{
    BOOST_STATIC_CONSTEXPR boost::uint32_t basis = 0x811C9DC5ul;
    BOOST_STATIC_CONSTEXPR boost::uint32_t prime = 0x01000193ul;
};

template<> struct fnv1a_const<boost::uint64_t>
{
    BOOST_STATIC_CONSTEXPR boost::uint64_t basis = 0xCBF29CE484222325ull;
    BOOST_STATIC_CONSTEXPR boost::uint64_t prime = 0x00000100000001B3ull;
};

template<class T> class fnv1a
{
private:

    T st_;

public:

    typedef T result_type;
    typedef T size_type;

    fnv1a(): st_( fnv1a_const<T>::basis )
    {
    }

    explicit fnv1a( boost::uint64_t seed ): st_( fnv1a_const<T>::basis )
    {
        if( seed )
        {
            if( seed >> 32 )
            {
                byte_type tmp[ 8 ];
                detail::write64le( tmp, seed );
                update( tmp, 8 );
            }
            else
            {
                byte_type tmp[ 4 ];
                detail::write32le( tmp, static_cast<boost::uint32_t>( seed ) );
                update( tmp, 4 );
            }
        }
    }

    fnv1a( byte_type const * p, std::ptrdiff_t n ): st_( fnv1a_const<T>::basis )
    {
        BOOST_ASSERT( n >= 0 );

        if( n != 0 )
        {
            update( p, n );
        }
    }

    void update( byte_type const * p, std::ptrdiff_t n )
    {
        BOOST_ASSERT( n >= 0 );

        T h = st_;

        for( std::ptrdiff_t i = 0; i < n; ++i )
        {
            h ^= static_cast<T>( p[i] );
            h *= fnv1a_const<T>::prime;
        }

        st_ = h;
    }

    T result()
    {
        T r = st_;

        // advance as if by update( "\xFF", 1 ), to allow
        // multiple result() calls to generate a sequence
        // of distinct values

        st_ = ( st_ ^ 0xFF ) * fnv1a_const<T>::prime;

        return r;
    }
};

} // namespace detail

class fnv1a_32: public detail::fnv1a<boost::uint32_t>
{
public:

    fnv1a_32(): detail::fnv1a<boost::uint32_t>()
    {
    }

    explicit fnv1a_32( boost::uint64_t seed ): detail::fnv1a<boost::uint32_t>( seed )
    {
    }

    fnv1a_32( byte_type const * p, std::ptrdiff_t n ): detail::fnv1a<boost::uint32_t>( p, n )
    {
    }
};

class fnv1a_64: public detail::fnv1a<boost::uint64_t>
{
public:

    fnv1a_64(): detail::fnv1a<boost::uint64_t>()
    {
    }

    explicit fnv1a_64( boost::uint64_t seed ): detail::fnv1a<boost::uint64_t>( seed )
    {
    }

    fnv1a_64( byte_type const * p, std::ptrdiff_t n ): detail::fnv1a<boost::uint64_t>( p, n )
    {
    }
};

} // namespace hash2
} // namespace boost

#endif // #ifndef BOOST_HASH2_FNV1A_HPP_INCLUDED


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <limits>
#include <vector>

#include <boost/test/tools/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/make_shared.hpp>

#include "tudat/basics/testMacros.h"

#include "tudat/astro/gravitation/centralGravityModel.h"
#include "tudat/astro/gravitation/thirdBodyPerturbation.h"

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_third_body_perturbation )

//! Test if perturbational acceleration is computed correctly.
BOOST_AUTO_TEST_CASE( testComputationOfThirdBodyPerturbation )
{
    // This verifies whether the implementation has been done correctly, using simplified test
    // cases.

    // Define tolerance level for differences
    double tolerance =  std::numeric_limits< double >::epsilon( );

    // Define test cases.

    // Declare vectors for later loop-wise check.
    std::vector< Eigen::Vector3d > positionOfBodyForTestCase;
    std::vector< double > gravitationalParameterOfPerturbingBodyForTestCase;
    std::vector< Eigen::Vector3d > expectedAccelerationForTestCase;

    // Central body at origin assumed.
    // Perturbed body at 0 index.
    Eigen::Vector3d position( 4.0, 0.0, 0.0 ); // Arbitrary body.
    positionOfBodyForTestCase.push_back( position );

    gravitationalParameterOfPerturbingBodyForTestCase.push_back( 1.0 ); // Technically not a
                           // perturbing body, but for corresponding indices still added. Also,
                           // parameter of one to simplify manual computation.

    // Set expected acceleration.
    Eigen::Vector3d acceleration( 0.0, 0.0, 0.0 ); // Should be infinity, but
                                                   // shouldn't be necessary anyways.
    expectedAccelerationForTestCase.push_back( acceleration );

    // Inner planet perturbing body.
    position << 3.0, 0.0, 0.0; // Arbitrary body.
    positionOfBodyForTestCase.push_back( position );

    gravitationalParameterOfPerturbingBodyForTestCase.push_back( 1.0 ); // A parameter of one to
                                                                    // simplify manual computation.

    // Set expected acceleration.
    acceleration << -1.1111111111111111111111111111111, 0.0, 0.0; // Manually computed.
    expectedAccelerationForTestCase.push_back( acceleration );

    // Outer planet perturbing body.
    position << 5.0, 0.0, 0.0; // Arbitrary body
    positionOfBodyForTestCase.push_back( position );

    gravitationalParameterOfPerturbingBodyForTestCase.push_back( 1.0 ); // A parameter of one to
                                                                    // simplify manual computation.

    // Set expected acceleration.
    acceleration << 0.96, 0.0, 0.0; // Manually computed.
    expectedAccelerationForTestCase.push_back( acceleration );

    // Inner planet perturbing body (second position).
    position << 0.0, 3.0, 0.0; // Arbitrary body.
    positionOfBodyForTestCase.push_back( position );

    gravitationalParameterOfPerturbingBodyForTestCase.push_back( 1.0 ); // A parameter of one to
                                                                    // simplify manual computation.

    // Set expected acceleration.
    acceleration << -0.032, -0.08711111111111111111111111111111, 0.0; // Manually computed.
    expectedAccelerationForTestCase.push_back( acceleration );

    // Inner planet perturbing body (third position).
    position << -3.0, 0.0, 0.0; // Arbitrary body.
    positionOfBodyForTestCase.push_back( position );

    gravitationalParameterOfPerturbingBodyForTestCase.push_back( 1.0 ); // A parameter of one to
                                                                     // simplify manual computation.

    // Set expected acceleration.
    acceleration << 0.09070294784580498866213151927438, 0.0, 0.0; // Manually computed.
    expectedAccelerationForTestCase.push_back( acceleration );

    // Heavier outer planet perturbing body.
    position << 5.0, 0.0, 0.0; // Arbitrary body.
    positionOfBodyForTestCase.push_back( position );

    gravitationalParameterOfPerturbingBodyForTestCase.push_back( 2.0 ); // A parameter of two to
                                                                    // simplify manual computation.

    // Set expected acceleration.
    acceleration << 1.92, 0.0, 0.0; // Manually computed.
    expectedAccelerationForTestCase.push_back( acceleration );

    // Examine test cases.
    for ( unsigned int i = 1; i < positionOfBodyForTestCase.size( ); i++ )
    {

        // Create central gravity acceleration objects.
        gravitation::CentralGravitationalAccelerationModel3dPointer directAccelerationModel =
                std::make_shared< gravitation::CentralGravitationalAccelerationModel3d >(
                    [ & ]( Eigen::Vector3d& input ){ input = positionOfBodyForTestCase[ 0 ]; },
                    gravitationalParameterOfPerturbingBodyForTestCase[ i ],
                    [ & ]( Eigen::Vector3d& input ){ input = positionOfBodyForTestCase[ i ]; } );

        gravitation::CentralGravitationalAccelerationModel3dPointer
                centralBodyAccelerationModel =
                std::make_shared< gravitation::CentralGravitationalAccelerationModel3d >(
                    [ & ]( Eigen::Vector3d& input ){ input = Eigen::Vector3d::Zero( ); },
                    gravitationalParameterOfPerturbingBodyForTestCase[ i ],
                    [ & ]( Eigen::Vector3d& input ){ input = positionOfBodyForTestCase[ i ]; } );

        // Create third body gravity acceleration objects.
        std::shared_ptr<
                gravitation::ThirdBodyCentralGravityAcceleration > thirdBodyAcceleration =
                    std::make_shared< gravitation::ThirdBodyCentralGravityAcceleration >(
                        directAccelerationModel, centralBodyAccelerationModel, "centralBody" );
        thirdBodyAcceleration->updateMembers( );

        // Compute perturbational acceleration for parameters given.
        acceleration = gravitation::computeThirdBodyPerturbingAcceleration(
                    gravitationalParameterOfPerturbingBodyForTestCase[ i ],
                    positionOfBodyForTestCase[ i ],
                    positionOfBodyForTestCase[ 0 ] );

        // Check values with reference data.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedAccelerationForTestCase[ i ], acceleration,
                                           tolerance );

        // Compute from manual sum.
        acceleration = gravitation::computeGravitationalAcceleration(
                    positionOfBodyForTestCase[ 0 ],
                gravitationalParameterOfPerturbingBodyForTestCase[ i ] ,
                positionOfBodyForTestCase[ i ] ) -
                gravitation::computeGravitationalAcceleration(
                    Eigen::Vector3d::Zero( ),
                    gravitationalParameterOfPerturbingBodyForTestCase[ i ],
                    positionOfBodyForTestCase[ i ] );

        // Check values with reference data.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedAccelerationForTestCase[ i ], acceleration,
                                           tolerance );

        acceleration = thirdBodyAcceleration->getAcceleration( );
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedAccelerationForTestCase[ i ], acceleration,
                                           tolerance );
    }
}

//! Test if perturbational acceleration is physically correct.
BOOST_AUTO_TEST_CASE( testRealisticThirdBodyPerturbation )
{
    // This is mainly using more realistic orders of magnitude for values. Also, this verifies that
    // the routine handles correctly all three components.

    // Define tolerance.
    // This tolerance is set to higher than machine precision, because the unit tests otherwise
    // fail. This is most likely the case because the accelerations being tested are much smalled
    // than 1.0, which is what the values are normalized to when using a relative measure to
    // check if the computed values match the expected values.
    double tolerance = 1.0e-14;

    // Define the affected position (almost geostationary).
    Eigen::Vector3d realisticTestPosition( -40000000.0, 9000000.0, -9500000.0 );

    // Define the perturbing position (almost like the Moon).
    Eigen::Vector3d realisticPerturberPosition ( 25000000.0, -380000000.0, -55000000.0 );
    double realisticGravitationalParameterOfPerturbingBody = 4900.0e9;

    // Set the expected acceleration values (computed manually).
    Eigen::Vector3d expectedRealisticAcceleration ( 2.9394638802044552864534126789629e-6,
                                                    2.2253948636344714462843917227786e-6,
                                                    1.1680066249314399399805985890762e-6 );

    // Compute the acceleration.
    Eigen::Vector3d realisticComputedAcceleration =
            gravitation::computeThirdBodyPerturbingAcceleration(
                realisticGravitationalParameterOfPerturbingBody,
                realisticPerturberPosition,
                realisticTestPosition );

    // Compare differences.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedRealisticAcceleration, realisticComputedAcceleration,
                                       tolerance );

    // Compute from manual sum.
    realisticComputedAcceleration = gravitation::computeGravitationalAcceleration(
                realisticTestPosition,
                realisticGravitationalParameterOfPerturbingBody,
                realisticPerturberPosition ) -
            gravitation::computeGravitationalAcceleration(
                Eigen::Vector3d::Zero( ),
                realisticGravitationalParameterOfPerturbingBody,
                realisticPerturberPosition );

    // Compare differences.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedRealisticAcceleration,
                                       realisticComputedAcceleration,
                                       tolerance );

    // Redo test in barycentric frame to check whether position of central body is handled
    // correctly.
    Eigen::Vector3d barycentricEarthPosition = Eigen::Vector3d( 100E9, -110E9, 3E9 );
    realisticPerturberPosition += barycentricEarthPosition;
    realisticTestPosition += barycentricEarthPosition;

    // Compute the acceleration.
    realisticComputedAcceleration =
            gravitation::computeThirdBodyPerturbingAcceleration(
                realisticGravitationalParameterOfPerturbingBody,
                realisticPerturberPosition,
                realisticTestPosition,
                barycentricEarthPosition );

    // Compare differences.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedRealisticAcceleration,
                                       realisticComputedAcceleration,
                                       tolerance );

    // Compute from manual sum.
    realisticComputedAcceleration = gravitation::computeGravitationalAcceleration(
                realisticTestPosition,
                realisticGravitationalParameterOfPerturbingBody,
                realisticPerturberPosition ) -
            gravitation::computeGravitationalAcceleration(
                barycentricEarthPosition,
                realisticGravitationalParameterOfPerturbingBody,
                realisticPerturberPosition );

    // Compare differences.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedRealisticAcceleration,
                                       realisticComputedAcceleration,
                                       tolerance );

    // Create central gravity acceleration objects.
    gravitation::CentralGravitationalAccelerationModel3dPointer directAccelerationModel =
            std::make_shared< gravitation::CentralGravitationalAccelerationModel3d >(
                [ & ]( Eigen::Vector3d& input ){ input = realisticTestPosition; },
                realisticGravitationalParameterOfPerturbingBody,
                [ & ]( Eigen::Vector3d& input ){ input = realisticPerturberPosition; } );

    gravitation::CentralGravitationalAccelerationModel3dPointer
            centralBodyAccelerationModel =
            std::make_shared< gravitation::CentralGravitationalAccelerationModel3d >(
                [ & ]( Eigen::Vector3d& input ){ input = barycentricEarthPosition; },
                realisticGravitationalParameterOfPerturbingBody,
                [ & ]( Eigen::Vector3d& input ){ input = realisticPerturberPosition; } );

    // Create third body gravity acceleration objects.
    std::shared_ptr<
            gravitation::ThirdBodyCentralGravityAcceleration > thirdBodyAcceleration =
                std::make_shared< gravitation::ThirdBodyCentralGravityAcceleration >(
                    directAccelerationModel, centralBodyAccelerationModel, "centralBody" );
    thirdBodyAcceleration->updateMembers( );

    realisticComputedAcceleration = thirdBodyAcceleration->getAcceleration( );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedRealisticAcceleration,
                                       realisticComputedAcceleration,
                                       tolerance );
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


#include <algorithm>
#include <iostream>
#include <memory>

#include <boost/program_options.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>

#include "cmd_helpers.h"

namespace bpo = boost::program_options;

namespace
{
    bpo::options_description initializeOptionsDescription()
    {
        using namespace std;

        bpo::options_description desc("Options");
        desc.add_options()
            ("help,h", "Print help message")
            ("input", bpo::value<string>(),
                    "Filename of a matrix market vector file "
                    "containing the denoised input data set")
            ("output", bpo::value<string>()->default_value("-"),
                    "Filename of the matrix market vector file "
                    "the level data should be written to")
            ("level-distance", bpo::value<double>(),
                    "Distance between each level between "
                    "min/max value of input vector")
            ("level-number", bpo::value<size_t>(),
                    "Number of linearly spaced leves between "
                    "min/max value of input vector")
            ("debug,d", "Turn on debug output if flag is set");

        return desc;
    }

    bool tryParseProgramOptions(const bpo::options_description &desc,
                                const int argc,
                                const char *argv[],
                                bpo::variables_map &vm)
    {
        using namespace std;
        using namespace boost::program_options;

        bpo::positional_options_description ppos;
        ppos.add("input", 1);
        ppos.add("output", 2);

        try {
            bpo::store(bpo::command_line_parser(argc, argv)
                        .options(desc)
                        .positional(ppos)
                        .run(), vm);
            bpo::notify(vm);

            if (cmd::isHelpRequest(desc, vm)) {
                return cmd::SUCCESS;
            }

            bool is_valid = true;

            if (! vm.count("input")) {
                cout << "ERROR: 'input' argument is required" << endl;
                is_valid = false;
            }

            if (! vm.count("level-distance") && ! vm.count("level-number")) {
                cout << "ERROR: either 'level-distance' or 'level-number' argument is required" << endl;
                is_valid = false;
            }

            if (! vm.count("output")) {
                cout << "ERROR: 'output' argument is required" << endl;
                is_valid = false;
            }

            return is_valid;
        }
        catch (error& e)
        {
            std::cerr << "ERROR: " << e.what() << std::endl << std::endl;
            std::cerr << desc << std::endl;
            return false;
        }

        return false;
    }

    template <typename VectorType>
    std::pair<double, double> getVectorMinMax(const VectorType &v)
    {
        using namespace std;

        auto min = min_element(v.begin(), v.end());
        auto max = max_element(v.begin(), v.end());
        return make_pair(*min, *max);
    }

    template <typename VectorType>
    void populateLevels(VectorType &levels,
                        double min,
                        double distance,
                        size_t n)
    {
        levels.resize(n);

        for (size_t i = 0; i < n; i++)
            levels(i) = min + distance * i;
    }

    template <typename VectorType>
    void populateLevelsByDistance(const bpo::variables_map &vm,
                                  const VectorType &input,
                                  VectorType &levels)
    {
        using namespace std;

        double distance = vm["level-distance"].as<double>();

        double min, max;
        tie(min, max) = getVectorMinMax(input);

        size_t n = ceil((max - min) / distance);

        populateLevels(levels, min, distance, n);
    }

    template <typename VectorType>
    void populateLevelsByNumber(const bpo::variables_map &vm,
                                const VectorType &input,
                                VectorType &levels)
    {
        using namespace std;

        auto n = vm["level-number"].as<size_t>();

        double min, max;
        tie(min, max) = getVectorMinMax(input);

        double distance = (max - min) / (double)n;

        populateLevels(levels, min, distance, n);
    }

    int runProgram(const bpo::variables_map &vm)
    {
        using namespace std;
        typedef boost::numeric::ublas::vector<double> VectorType;

        VectorType input;

        if (! cmd::loadInputVector(vm, input))
            return cmd::ERROR_UNHANDLED_EXCEPTION;

        VectorType levels;
        if (vm.count("level-distance"))
            populateLevelsByDistance(vm, input, levels);
        else
            populateLevelsByNumber(vm, input, levels);

        if (! cmd::saveOutputVector(vm, levels))
            return cmd::ERROR_UNHANDLED_EXCEPTION;

        return cmd::SUCCESS;
    }
}

int main (const int argc, const char *argv[])
{
    using namespace std;

    auto desc = initializeOptionsDescription();

    try
    {
        bpo::variables_map vm;
        if (! tryParseProgramOptions(desc, argc, argv, vm)) {
            return cmd::ERROR_IN_COMMAND_LINE;
        }

        return runProgram(vm);
    }
    catch (exception &e)
    {
        cerr << "Unhandled Exception reached the top of main: "
             << e.what() << ", application will now exit" << endl;
    }
}


#define BOOST_TEST_MODULE pcraster geo count_filter_test
#include <boost/test/unit_test.hpp>
#include "geo_countfilter.h"
#include "geo_filterengine.h"

BOOST_AUTO_TEST_CASE(test)
{
  using namespace geo;

  // Create source raster.
#ifdef __x86_64__
  typedef unsigned int count_t;
#else
  typedef size_t count_t;
#endif
  SimpleRaster<count_t> source(3, 3);
  source.cell(0, 0) = 1;
  source.cell(0, 1) = 2;
  source.cell(0, 2) = 3;
  source.cell(1, 0) = 4;
  source.cell(1, 1) = 5;
  source.cell(1, 2) = 6;
  source.cell(2, 0) = 7;
  source.cell(2, 1) = 8;
  source.cell(2, 2) = 9;

  SimpleRaster<double> weights(3, 3, 1.0);
  CountFilter<count_t, double> filter(weights, 4);
  SimpleRaster<double> destination(3, 3);
  FilterEngine<count_t, double> engine(source, filter, destination);

  engine.calc();

  BOOST_CHECK(destination.cell(0, 0) == 1.0);
  BOOST_CHECK(destination.cell(0, 1) == 1.0);
  BOOST_CHECK(destination.cell(0, 2) == 0.0);
  BOOST_CHECK(destination.cell(1, 0) == 1.0);
  BOOST_CHECK(destination.cell(1, 1) == 1.0);
  BOOST_CHECK(destination.cell(1, 2) == 0.0);
  BOOST_CHECK(destination.cell(2, 0) == 1.0);
  BOOST_CHECK(destination.cell(2, 1) == 1.0);
  BOOST_CHECK(destination.cell(2, 2) == 0.0);

  // see __x86_64__
  // TODO Suse problem: 8 byte isMV template instantion for unsigned long
  bool size_tRequires8Byteonx86_64=false;
  BOOST_WARN(size_tRequires8Byteonx86_64);
}


// Copyright 2008 Gautam Sewani
// Copyright 2008 John Maddock
//
// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_DISTRIBUTIONS_HYPERGEOMETRIC_HPP
#define BOOST_MATH_DISTRIBUTIONS_HYPERGEOMETRIC_HPP

#include <boost/math/distributions/detail/common_error_handling.hpp>
#include <boost/math/distributions/complement.hpp>
#include <boost/math/distributions/detail/hypergeometric_pdf.hpp>
#include <boost/math/distributions/detail/hypergeometric_cdf.hpp>
#include <boost/math/distributions/detail/hypergeometric_quantile.hpp>
#include <boost/math/special_functions/fpclassify.hpp>

namespace boost { namespace math {

   template <class RealType = double, class Policy = policies::policy<> >
   class hypergeometric_distribution
   {
   public:
      typedef RealType value_type;
      typedef Policy policy_type;

      hypergeometric_distribution(unsigned r, unsigned n, unsigned N) // Constructor.
         : m_n(n), m_N(N), m_r(r)
      {
         static const char* function = "boost::math::hypergeometric_distribution<%1%>::hypergeometric_distribution";
         RealType ret;
         check_params(function, &ret);
      }
      // Accessor functions.
      unsigned total()const
      {
         return m_N;
      }

      unsigned defective()const
      {
         return m_r;
      }

      unsigned sample_count()const
      {
         return m_n;
      }

      bool check_params(const char* function, RealType* result)const
      {
         if(m_r > m_N)
         {
            *result = boost::math::policies::raise_domain_error<RealType>(
               function, "Parameter r out of range: must be <= N but got %1%", static_cast<RealType>(m_r), Policy());
            return false;
         }
         if(m_n > m_N)
         {
            *result = boost::math::policies::raise_domain_error<RealType>(
               function, "Parameter n out of range: must be <= N but got %1%", static_cast<RealType>(m_n), Policy());
            return false;
         }
         return true;
      }
      bool check_x(unsigned x, const char* function, RealType* result)const
      {
         if(x < static_cast<unsigned>((std::max)(0, (int)(m_n + m_r) - (int)(m_N))))
         {
            *result = boost::math::policies::raise_domain_error<RealType>(
               function, "Random variable out of range: must be > 0 and > m + r - N but got %1%", static_cast<RealType>(x), Policy());
            return false;
         }
         if(x > (std::min)(m_r, m_n))
         {
            *result = boost::math::policies::raise_domain_error<RealType>(
               function, "Random variable out of range: must be less than both n and r but got %1%", static_cast<RealType>(x), Policy());
            return false;
         }
         return true;
      }

   private:
      // Data members:
      unsigned m_n;  // number of items picked
      unsigned m_N; // number of "total" items
      unsigned m_r; // number of "defective" items

   }; // class hypergeometric_distribution

   typedef hypergeometric_distribution<double> hypergeometric;

   template <class RealType, class Policy>
   inline const std::pair<unsigned, unsigned> range(const hypergeometric_distribution<RealType, Policy>& dist)
   { // Range of permissible values for random variable x.
#ifdef BOOST_MSVC
#  pragma warning(push)
#  pragma warning(disable:4267)
#endif
      unsigned r = dist.defective();
      unsigned n = dist.sample_count();
      unsigned N = dist.total();
      unsigned l = static_cast<unsigned>((std::max)(0, (int)(n + r) - (int)(N)));
      unsigned u = (std::min)(r, n);
      return std::pair<unsigned, unsigned>(l, u);
#ifdef BOOST_MSVC
#  pragma warning(pop)
#endif
   }

   template <class RealType, class Policy>
   inline const std::pair<unsigned, unsigned> support(const hypergeometric_distribution<RealType, Policy>& d)
   {
      return range(d);
   }

   template <class RealType, class Policy>
   inline RealType pdf(const hypergeometric_distribution<RealType, Policy>& dist, const unsigned& x)
   {
      static const char* function = "boost::math::pdf(const hypergeometric_distribution<%1%>&, const %1%&)";
      RealType result = 0;
      if(!dist.check_params(function, &result))
         return result;
      if(!dist.check_x(x, function, &result))
         return result;

      return boost::math::detail::hypergeometric_pdf<RealType>(
         x, dist.defective(), dist.sample_count(), dist.total(), Policy());
   }

   template <class RealType, class Policy, class U>
   inline RealType pdf(const hypergeometric_distribution<RealType, Policy>& dist, const U& x)
   {
      BOOST_MATH_STD_USING
      static const char* function = "boost::math::pdf(const hypergeometric_distribution<%1%>&, const %1%&)";
      RealType r = static_cast<RealType>(x);
      unsigned u = itrunc(r, typename policies::normalise<Policy, policies::rounding_error<policies::ignore_error> >::type());
      if(u != r)
      {
         return boost::math::policies::raise_domain_error<RealType>(
            function, "Random variable out of range: must be an integer but got %1%", r, Policy());
      }
      return pdf(dist, u);
   }

   template <class RealType, class Policy>
   inline RealType cdf(const hypergeometric_distribution<RealType, Policy>& dist, const unsigned& x)
   {
      static const char* function = "boost::math::cdf(const hypergeometric_distribution<%1%>&, const %1%&)";
      RealType result = 0;
      if(!dist.check_params(function, &result))
         return result;
      if(!dist.check_x(x, function, &result))
         return result;

      return boost::math::detail::hypergeometric_cdf<RealType>(
         x, dist.defective(), dist.sample_count(), dist.total(), false, Policy());
   }

   template <class RealType, class Policy, class U>
   inline RealType cdf(const hypergeometric_distribution<RealType, Policy>& dist, const U& x)
   {
      BOOST_MATH_STD_USING
      static const char* function = "boost::math::cdf(const hypergeometric_distribution<%1%>&, const %1%&)";
      RealType r = static_cast<RealType>(x);
      unsigned u = itrunc(r, typename policies::normalise<Policy, policies::rounding_error<policies::ignore_error> >::type());
      if(u != r)
      {
         return boost::math::policies::raise_domain_error<RealType>(
            function, "Random variable out of range: must be an integer but got %1%", r, Policy());
      }
      return cdf(dist, u);
   }

   template <class RealType, class Policy>
   inline RealType cdf(const complemented2_type<hypergeometric_distribution<RealType, Policy>, unsigned>& c)
   {
      static const char* function = "boost::math::cdf(const hypergeometric_distribution<%1%>&, const %1%&)";
      RealType result = 0;
      if(!c.dist.check_params(function, &result))
         return result;
      if(!c.dist.check_x(c.param, function, &result))
         return result;

      return boost::math::detail::hypergeometric_cdf<RealType>(
         c.param, c.dist.defective(), c.dist.sample_count(), c.dist.total(), true, Policy());
   }

   template <class RealType, class Policy, class U>
   inline RealType cdf(const complemented2_type<hypergeometric_distribution<RealType, Policy>, U>& c)
   {
      BOOST_MATH_STD_USING
      static const char* function = "boost::math::cdf(const hypergeometric_distribution<%1%>&, const %1%&)";
      RealType r = static_cast<RealType>(c.param);
      unsigned u = itrunc(r, typename policies::normalise<Policy, policies::rounding_error<policies::ignore_error> >::type());
      if(u != r)
      {
         return boost::math::policies::raise_domain_error<RealType>(
            function, "Random variable out of range: must be an integer but got %1%", r, Policy());
      }
      return cdf(complement(c.dist, u));
   }

   template <class RealType, class Policy>
   inline RealType quantile(const hypergeometric_distribution<RealType, Policy>& dist, const RealType& p)
   {
      BOOST_MATH_STD_USING // for ADL of std functions

         // Checking function argument
         RealType result = 0;
      const char* function = "boost::math::quantile(const hypergeometric_distribution<%1%>&, %1%)";
      if (false == dist.check_params(function, &result)) return result;
      if(false == detail::check_probability(function, p, &result, Policy())) return result;

      return static_cast<RealType>(detail::hypergeometric_quantile(p, RealType(1 - p), dist.defective(), dist.sample_count(), dist.total(), Policy()));
   } // quantile

   template <class RealType, class Policy>
   inline RealType quantile(const complemented2_type<hypergeometric_distribution<RealType, Policy>, RealType>& c)
   {
      BOOST_MATH_STD_USING // for ADL of std functions

      // Checking function argument
      RealType result = 0;
      const char* function = "quantile(const complemented2_type<hypergeometric_distribution<%1%>, %1%>&)";
      if (false == c.dist.check_params(function, &result)) return result;
      if(false == detail::check_probability(function, c.param, &result, Policy())) return result;

      return static_cast<RealType>(detail::hypergeometric_quantile(RealType(1 - c.param), c.param, c.dist.defective(), c.dist.sample_count(), c.dist.total(), Policy()));
   } // quantile

   template <class RealType, class Policy>
   inline RealType mean(const hypergeometric_distribution<RealType, Policy>& dist)
   {
      return static_cast<RealType>(dist.defective() * dist.sample_count()) / dist.total();
   } // RealType mean(const hypergeometric_distribution<RealType, Policy>& dist)

   template <class RealType, class Policy>
   inline RealType variance(const hypergeometric_distribution<RealType, Policy>& dist)
   {
      RealType r = static_cast<RealType>(dist.defective());
      RealType n = static_cast<RealType>(dist.sample_count());
      RealType N = static_cast<RealType>(dist.total());
      return n * r  * (N - r) * (N - n) / (N * N * (N - 1));
   } // RealType variance(const hypergeometric_distribution<RealType, Policy>& dist)

   template <class RealType, class Policy>
   inline RealType mode(const hypergeometric_distribution<RealType, Policy>& dist)
   {
      BOOST_MATH_STD_USING
      RealType r = static_cast<RealType>(dist.defective());
      RealType n = static_cast<RealType>(dist.sample_count());
      RealType N = static_cast<RealType>(dist.total());
      return floor((r + 1) * (n + 1) / (N + 2));
   }

   template <class RealType, class Policy>
   inline RealType skewness(const hypergeometric_distribution<RealType, Policy>& dist)
   {
      BOOST_MATH_STD_USING
      RealType r = static_cast<RealType>(dist.defective());
      RealType n = static_cast<RealType>(dist.sample_count());
      RealType N = static_cast<RealType>(dist.total());
      return (N - 2 * r) * sqrt(N - 1) * (N - 2 * n) / (sqrt(n * r * (N - r) * (N - n)) * (N - 2));
   } // RealType skewness(const hypergeometric_distribution<RealType, Policy>& dist)

   template <class RealType, class Policy>
   inline RealType kurtosis_excess(const hypergeometric_distribution<RealType, Policy>& dist)
   {
      RealType r = static_cast<RealType>(dist.defective());
      RealType n = static_cast<RealType>(dist.sample_count());
      RealType N = static_cast<RealType>(dist.total());
      RealType t1 = N * N * (N - 1) / (r * (N - 2) * (N - 3) * (N - r));
      RealType t2 = (N * (N + 1) - 6 * N * (N - r)) / (n * (N - n))
         + 3 * r * (N - r) * (N + 6) / (N * N) - 6;
      return t1 * t2;
   } // RealType kurtosis_excess(const hypergeometric_distribution<RealType, Policy>& dist)

   template <class RealType, class Policy>
   inline RealType kurtosis(const hypergeometric_distribution<RealType, Policy>& dist)
   {
      return kurtosis_excess(dist) + 3;
   } // RealType kurtosis_excess(const hypergeometric_distribution<RealType, Policy>& dist)
}} // namespaces

// This include must be at the end, *after* the accessors
// for this distribution have been defined, in order to
// keep compilers that support two-phase lookup happy.
#include <boost/math/distributions/detail/derived_accessors.hpp>

#endif // include guard


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2020 Tinko Bartels, Berlin, Germany.

// Contributed and/or modified by Tinko Bartels,
//   as part of Google Summer of Code 2020 program.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_EXTENSIONS_GENERIC_ROBUST_PREDICATES_STRATEGIES_CARTESIAN_DETAIL_SEMI_STATIC_FILTER_HPP
#define BOOST_GEOMETRY_EXTENSIONS_GENERIC_ROBUST_PREDICATES_STRATEGIES_CARTESIAN_DETAIL_SEMI_STATIC_FILTER_HPP

#include <array>

#include <boost/mp11/list.hpp>
#include <boost/mp11/algorithm.hpp>
#include <boost/mp11/set.hpp>

#include <boost/geometry/extensions/generic_robust_predicates/strategies/cartesian/detail/expression_tree.hpp>
#include <boost/geometry/extensions/generic_robust_predicates/strategies/cartesian/detail/approximate.hpp>

namespace boost { namespace geometry
{

namespace detail { namespace generic_robust_predicates
{

template
<
    typename Expression,
    typename CalculationType,
    typename ErrorExpression
>
struct semi_static_filter
{
private:
    using stack = typename boost::mp11::mp_unique<post_order<Expression>>;
    using evals = typename boost::mp11::mp_remove_if<stack, is_leaf>;
    using error_eval_stack = boost::mp11::mp_unique
        <
            post_order<ErrorExpression>
        >;
    using error_eval_stack_remainder = boost::mp11::mp_set_difference
        <
            error_eval_stack,
            evals
        >;
    using all_evals = boost::mp11::mp_append
        <
            evals,
            error_eval_stack_remainder
        >;
    using ct = CalculationType;
public:
    template <typename ...Reals>
    static inline int apply(const Reals&... args)
    {
        std::array<CalculationType, sizeof...(Reals)> input
            {{ static_cast<ct>(args)... }};
        std::array<ct, boost::mp11::mp_size<all_evals>::value> results;
        approximate_interim<all_evals, all_evals, ct>(results, input);
        const ct error_bound =
            get_approx<all_evals, ErrorExpression, ct>(results, input);
        const ct det =
            get_approx<all_evals, Expression, ct>(results, input);
        if (det > error_bound)
        {
            return 1;
        }
        else if (det < -error_bound)
        {
            return -1;
        }
        else if (error_bound == 0 && det == 0)
        {
            return 0;
        }
        else
        {
            return sign_uncertain;
        }
    }
};

}} // namespace detail::generic_robust_predicates

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_GENERIC_ROBUST_PREDICATES_STRATEGIES_CARTESIAN_DETAIL_SEMI_STATIC_FILTER_HPP