// This file is part of the dune-stuff project:
//   https://github.com/wwu-numerik/dune-stuff/
// Copyright holders: Rene Milk, Felix Schindler
// License: BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)

#ifndef DUNE_STUFF_FUNCTIONS_ESV2007_HH
#define DUNE_STUFF_FUNCTIONS_ESV2007_HH

#include <cmath>

#include <type_traits>

#if HAVE_EIGEN
#include <Eigen/Eigenvalues>
#endif

#include <dune/geometry/referenceelements.hh>

#include <dune/stuff/common/configuration.hh>
#include <dune/stuff/common/debug.hh>
#include <dune/stuff/common/ranges.hh>
#include <dune/stuff/la/container/eigen.hh>

#include "interfaces.hh"

namespace Dune {
namespace Stuff {
namespace Functions {
namespace ESV2007 {

template <class E, class D, size_t d, class R, size_t r, size_t rC = 1>
class Testcase1Force : public LocalizableFunctionInterface<E, D, d, R, r, rC>
{
  Testcase1Force() { static_assert(AlwaysFalse<E>::value, "Not available for these dimensions!"); }
};

template <class EntityImp, class DomainFieldImp, class RangeFieldImp>
class Testcase1Force<EntityImp, DomainFieldImp, 2, RangeFieldImp, 1, 1>
    : public GlobalFunctionInterface<EntityImp, DomainFieldImp, 2, RangeFieldImp, 1>
{
  typedef Testcase1Force<EntityImp, DomainFieldImp, 2, RangeFieldImp, 1, 1> ThisType;
  typedef GlobalFunctionInterface<EntityImp, DomainFieldImp, 2, RangeFieldImp, 1, 1> BaseType;

public:
  using typename BaseType::DomainFieldType;
  using typename BaseType::DomainType;
  using typename BaseType::RangeType;
  using typename BaseType::JacobianRangeType;

  static const bool available = true;

  static std::string static_id() { return BaseType::static_id() + ".ESV2007.testcase1.force"; }

  static Common::Configuration default_config(const std::string sub_name = "")
  {
    Common::Configuration config;
    config["integration_order"] = "3";
    config["name"] = static_id();
    if (sub_name.empty())
      return config;
    else {
      Common::Configuration tmp;
      tmp.add(config, sub_name);
      return tmp;
    }
  } // ... default_config(...)

  static std::unique_ptr<ThisType> create(const Common::Configuration config = default_config(),
                                          const std::string sub_name = static_id())
  {
    // get correct config
    const Common::Configuration cfg         = config.has_sub(sub_name) ? config.sub(sub_name) : config;
    const Common::Configuration default_cfg = default_config();
    // create
    return Common::make_unique<ThisType>(cfg.get("integration_order", default_cfg.get<size_t>("integration_order")),
                                         cfg.get("name", default_cfg.get<std::string>("name")));
  } // ... create(...)

  Testcase1Force(const size_t ord = default_config().get<size_t>("integration_order"),
                 const std::string nm = static_id())
    : order_(ord), name_(nm)
  {
  }

  Testcase1Force(const ThisType& /*other*/) = default;

  ThisType& operator=(const ThisType& /*other*/) = delete;

  virtual std::string type() const override final { return BaseType::static_id() + ".ESV2007.testcase1.force"; }

  virtual std::string name() const override final { return name_; }

  virtual size_t order() const override final { return order_; }

  /**
   * \brief "0.5 * pi * pi * cos(0.5 * pi * x[0]) * cos(0.5 * pi * x[1])"
   */
  virtual void evaluate(const DomainType& xx, RangeType& ret) const override final
  {
    ret[0] = M_PI_2l * M_PIl * cos(M_PI_2l * xx[0]) * cos(M_PI_2l * xx[1]);
  }

  /**
   * \brief ["-0.25 * pi * pi * pi * sin(0.5 * pi * x[0]) * cos(0.5 * pi * x[1])"
   *         "-0.25 * pi * pi * pi * cos(0.5 * pi * x[0]) * sin(0.5 * pi * x[1])"]
   */
  virtual void jacobian(const DomainType& xx, JacobianRangeType& ret) const override final
  {
    const DomainFieldType pre   = -0.25 * M_PIl * M_PIl * M_PIl;
    const DomainFieldType x_arg = M_PI_2l * xx[0];
    const DomainFieldType y_arg = M_PI_2l * xx[1];
    ret[0][0]                   = pre * sin(x_arg) * cos(y_arg);
    ret[0][1]                   = pre * cos(x_arg) * sin(y_arg);
  } // ... jacobian(...)

private:
  const size_t order_;
  const std::string name_;
}; // class Testcase1Force

template <class E, class D, size_t d, class R, size_t r, size_t rC = 1>
class Testcase1ExactSolution : public LocalizableFunctionInterface<E, D, d, R, r, rC>
{
  Testcase1ExactSolution() { static_assert(AlwaysFalse<E>::value, "Not available for these dimensions!"); }
};

template <class EntityImp, class DomainFieldImp, class RangeFieldImp>
class Testcase1ExactSolution<EntityImp, DomainFieldImp, 2, RangeFieldImp, 1, 1>
    : public GlobalFunctionInterface<EntityImp, DomainFieldImp, 2, RangeFieldImp, 1, 1>
{
  typedef Testcase1ExactSolution<EntityImp, DomainFieldImp, 2, RangeFieldImp, 1, 1> ThisType;
  typedef GlobalFunctionInterface<EntityImp, DomainFieldImp, 2, RangeFieldImp, 1, 1> BaseType;

public:
  using typename BaseType::DomainFieldType;
  using typename BaseType::DomainType;
  using typename BaseType::RangeType;
  using typename BaseType::JacobianRangeType;

  static const bool available = true;

  static std::string static_id() { return BaseType::static_id() + ".ESV2007.testcase1.exactsolution"; }

  static Common::Configuration default_config(const std::string sub_name = "")
  {
    Common::Configuration config;
    config["integration_order"] = "3";
    config["name"] = static_id();
    if (sub_name.empty())
      return config;
    else {
      Common::Configuration tmp;
      tmp.add(config, sub_name);
      return tmp;
    }
  } // ... default_config(...)

  static std::unique_ptr<ThisType> create(const Common::Configuration config = default_config(),
                                          const std::string sub_name = static_id())
  {
    // get correct config
    const Common::Configuration cfg         = config.has_sub(sub_name) ? config.sub(sub_name) : config;
    const Common::Configuration default_cfg = default_config();
    // create
    return Common::make_unique<ThisType>(cfg.get("integration_order", default_cfg.get<size_t>("integration_order")),
                                         cfg.get("name", default_cfg.get<std::string>("name")));
  } // ... create(...)

  Testcase1ExactSolution(const size_t ord = default_config().get<size_t>("integration_order"),
                         const std::string nm = static_id())
    : order_(ord), name_(nm)
  {
  }

  Testcase1ExactSolution(const ThisType& /*other*/) = default;

  ThisType& operator=(const ThisType& /*other*/) = delete;

  virtual std::string type() const override final { return BaseType::static_id() + ".ESV2007.testcase1.exactsolution"; }

  virtual std::string name() const override final { return name_; }

  virtual size_t order() const override final { return order_; }

  /**
   * \brief "cos(0.5 * pi * x[0]) * cos(0.5 * pi * x[1])"
   */
  virtual void evaluate(const DomainType& xx, RangeType& ret) const override final
  {
    ret[0] = cos(M_PI_2l * xx[0]) * cos(M_PI_2l * xx[1]);
  }

  /**
   * \brief ["-0.5 * pi * sin(0.5 * pi * x[0]) * cos(0.5 * pi * x[1])"
   *         "-0.5 * pi * cos(0.5 * pi * x[0]) * sin(0.5 * pi * x[1])"]
   */
  virtual void jacobian(const DomainType& xx, JacobianRangeType& ret) const override final
  {
    const DomainFieldType pre   = -0.5 * M_PIl;
    const DomainFieldType x_arg = M_PI_2l * xx[0];
    const DomainFieldType y_arg = M_PI_2l * xx[1];
    ret[0][0]                   = pre * sin(x_arg) * cos(y_arg);
    ret[0][1]                   = pre * cos(x_arg) * sin(y_arg);
  } // ... jacobian(...)

private:
  const size_t order_;
  const std::string name_;
}; // class Testcase1ExactSolution

template <class DiffusionFactorType, class DiffusionTensorType = void>
class Cutoff;

template <class DiffusionType>
class Cutoff<DiffusionType, void>
    : public LocalizableFunctionInterface<typename DiffusionType::EntityType, typename DiffusionType::DomainFieldType,
                                          DiffusionType::dimDomain, typename DiffusionType::RangeFieldType, 1, 1>
{
  static_assert(std::is_base_of<Tags::LocalizableFunction, DiffusionType>::value,
                "DiffusionType has to be tagged as a LocalizableFunction!");
  typedef typename DiffusionType::EntityType E_;
  typedef typename DiffusionType::DomainFieldType D_;
  static const size_t d_ = DiffusionType::dimDomain;
  typedef typename DiffusionType::RangeFieldType R_;
  typedef LocalizableFunctionInterface<E_, D_, d_, R_, 1> BaseType;
  typedef Cutoff<DiffusionType> ThisType;

  class Localfunction : public LocalfunctionInterface<E_, D_, d_, R_, 1, 1>
  {
    typedef LocalfunctionInterface<E_, D_, d_, R_, 1, 1> BaseType;

  public:
    typedef typename BaseType::EntityType EntityType;

    typedef typename BaseType::DomainFieldType DomainFieldType;
    static const size_t dimDomain = BaseType::dimDomain;
    typedef typename BaseType::DomainType DomainType;

    typedef typename BaseType::RangeFieldType RangeFieldType;
    static const size_t dimRange     = BaseType::dimRange;
    static const size_t dimRangeCols = BaseType::dimRangeCols;
    typedef typename BaseType::RangeType RangeType;

    typedef typename BaseType::JacobianRangeType JacobianRangeType;

  private:
    template <class D, int r, int rR>
    struct Compute
    {
      static_assert(AlwaysFalse<D>::value, "Not implemented for these dimensions!");
    };

    template <class D>
    struct Compute<D, 1, 1>
    {
      static RangeFieldType min_eigenvalue_of(const D& diffusion, const EntityType& ent)
      {
        const auto local_diffusion = diffusion.local_function(ent);
        assert(local_diffusion->order() == 0);
        const auto& reference_element = ReferenceElements<DomainFieldType, dimDomain>::general(ent.type());
        return local_diffusion->evaluate(reference_element.position(0, 0))[0];
      } // ... min_eigenvalue_of_(...)
    };  // class Compute< ..., 1, 1 >

  public:
    Localfunction(const EntityType& ent, const DiffusionType& diffusion, const RangeFieldType poincare_constant)
      : BaseType(ent), value_(0)
    {
      const RangeFieldType min_eigen_value =
          Compute<DiffusionType, DiffusionType::dimRange, DiffusionType::dimRangeCols>::min_eigenvalue_of(diffusion,
                                                                                ent);
      assert(min_eigen_value > 0.0);
      const DomainFieldType hh = compute_diameter_of_(ent);
      value_                   = (poincare_constant * hh * hh) / min_eigen_value;
    }

    Localfunction(const Localfunction& /*other*/) = delete;

    Localfunction& operator=(const Localfunction& /*other*/) = delete;

    virtual size_t order() const override final { return 0; }

    virtual void evaluate(const DomainType& UNUSED_UNLESS_DEBUG(xx), RangeType& ret) const override final
    {
      assert(this->is_a_valid_point(xx));
      ret[0] = value_;
    }

    virtual void jacobian(const DomainType& UNUSED_UNLESS_DEBUG(xx), JacobianRangeType& ret) const override final
    {
      assert(this->is_a_valid_point(xx));
      ret *= RangeFieldType(0);
    }

  private:
    static DomainFieldType compute_diameter_of_(const EntityType& ent)
    {
      DomainFieldType ret(0);
      for (auto cc : DSC::valueRange(ent.template count<dimDomain>())) {
        const auto vertex = ent.template subEntity<dimDomain>(cc)->geometry().center();
        for (auto dd : DSC::valueRange(cc + 1, ent.template count<dimDomain>())) {
          const auto other_vertex = ent.template subEntity<dimDomain>(dd)->geometry().center();
          const auto diff         = vertex - other_vertex;
          ret                     = std::max(ret, diff.two_norm());
        }
      }
      return ret;
    } // ... compute_diameter_of_(...)

    RangeFieldType value_;
  }; // class Localfunction

public:
  typedef typename BaseType::EntityType EntityType;
  typedef typename BaseType::LocalfunctionType LocalfunctionType;
  typedef typename BaseType::RangeFieldType RangeFieldType;

  static std::string static_id() { return BaseType::static_id() + ".ESV2007.cutoff"; }

  Cutoff(const DiffusionType& diffusion, const RangeFieldType poincare_constant = 1.0 / (M_PIl * M_PIl),
         const std::string nm = static_id())
    : diffusion_(diffusion), poincare_constant_(poincare_constant), name_(nm)
  {
  }

  Cutoff(const ThisType& other) = default;

  ThisType& operator=(const ThisType& other) = delete;

  virtual std::string name() const override final { return name_; }

  virtual std::unique_ptr<LocalfunctionType> local_function(const EntityType& entity) const override final
  {
    return std::unique_ptr<Localfunction>(new Localfunction(entity, diffusion_, poincare_constant_));
  }

private:
  const DiffusionType& diffusion_;
  const RangeFieldType poincare_constant_;
  std::string name_;
}; // class Cutoff

template <class DiffusionFactorType, class DiffusionTensorType>
class Cutoff
    : public LocalizableFunctionInterface<typename DiffusionFactorType::EntityType,
                                          typename DiffusionFactorType::DomainFieldType, DiffusionFactorType::dimDomain,
                                          typename DiffusionFactorType::RangeFieldType, 1, 1>
{
  static_assert(std::is_base_of<Tags::LocalizableFunction, DiffusionFactorType>::value,
                "DiffusionFactorType has to be tagged as a LocalizableFunction!");
  static_assert(std::is_base_of<Tags::LocalizableFunction, DiffusionTensorType>::value,
                "DiffusionTensorType has to be tagged as a LocalizableFunction!");
  typedef typename DiffusionFactorType::EntityType E_;
  typedef typename DiffusionFactorType::DomainFieldType D_;
  static const size_t d_ = DiffusionFactorType::dimDomain;
  typedef typename DiffusionFactorType::RangeFieldType R_;
  typedef LocalizableFunctionInterface<E_, D_, d_, R_, 1> BaseType;
  typedef Cutoff<DiffusionFactorType, DiffusionTensorType> ThisType;

  static_assert(DiffusionFactorType::dimRange == 1, "The diffusion factor has to be scalar!");
  static_assert(DiffusionFactorType::dimRangeCols == 1, "The diffusion factor has to be scalar!");

  static_assert(std::is_same<typename DiffusionTensorType::EntityType, E_>::value, "Types do not match!");
  static_assert(std::is_same<typename DiffusionTensorType::DomainFieldType, D_>::value, "Types do not match!");
  static_assert(DiffusionTensorType::dimDomain == d_, "Dimensions do not match!");
  static_assert(std::is_same<typename DiffusionTensorType::RangeFieldType, R_>::value, "Types do not match!");

  static_assert(DiffusionTensorType::dimRange == d_, "The diffusion tensor has to be a matrix!");
  static_assert(DiffusionTensorType::dimRangeCols == d_, "The diffusion tensor has to be a matrix!");

  class Localfunction : public LocalfunctionInterface<E_, D_, d_, R_, 1, 1>
  {
    typedef LocalfunctionInterface<E_, D_, d_, R_, 1, 1> BaseType;

  public:
    typedef typename BaseType::EntityType EntityType;

    typedef typename BaseType::DomainFieldType DomainFieldType;
    static const size_t dimDomain = BaseType::dimDomain;
    typedef typename BaseType::DomainType DomainType;

    typedef typename BaseType::RangeFieldType RangeFieldType;
    static const size_t dimRange     = BaseType::dimRange;
    static const size_t dimRangeCols = BaseType::dimRangeCols;
    typedef typename BaseType::RangeType RangeType;

    typedef typename BaseType::JacobianRangeType JacobianRangeType;

  private:
    template <class DF, size_t r, size_t rR>
    struct ComputeDiffusionFactor
    {
      static_assert(AlwaysFalse<DF>::value, "Not implemented for these dimensions!");
    };

    template <class DF>
    struct ComputeDiffusionFactor<DF, 1, 1>
    {
      /**
       * We try to find the minimum of a polynomial of given order by evaluating it at the points of a quadrature that
       * would integrate this polynomial exactly.
       * \todo These are just some heuristics and should be replaced by something proper.
       */
      static RangeFieldType min_of(const DF& diffusion_factor, const EntityType& ent)
      {
        typename DF::RangeType tmp_value(0);
        RangeFieldType minimum            = std::numeric_limits<RangeFieldType>::max();
        const auto local_diffusion_factor = diffusion_factor.local_function(ent);
        const size_t ord = local_diffusion_factor->order();
        const auto& quadrature =
            QuadratureRules<DomainFieldType, dimDomain>::rule(ent.type(), boost::numeric_cast<int>(ord));
        const auto quad_point_it_end = quadrature.end();
        for (auto quad_point_it = quadrature.begin(); quad_point_it != quad_point_it_end; ++quad_point_it) {
          local_diffusion_factor->evaluate(quad_point_it->position(), tmp_value);
          minimum = std::min(minimum, tmp_value[0]);
        }
        return minimum;
      } // ... min_of(...)
    };  // class ComputeDiffusionFactor< ..., 1, 1 >

    template <class DT, size_t r, size_t rR>
    struct ComputeDiffusionTensor
    {
      static_assert(AlwaysFalse<DT>::value, "Not implemented for these dimensions!");
    };

    template <class DT, size_t d>
    struct ComputeDiffusionTensor<DT, d, d>
    {
      static RangeFieldType min_eigenvalue_of(const DT& diffusion_tensor, const EntityType& ent)
      {
#if !HAVE_EIGEN
        static_assert(AlwaysFalse<DT>::value, "You are missing eigen!");
#else
        const auto local_diffusion_tensor = diffusion_tensor.local_function(ent);
        assert(local_diffusion_tensor->order() == 0);
        const auto& reference_element = ReferenceElements<DomainFieldType, dimDomain>::general(ent.type());
        const Stuff::LA::EigenDenseMatrix<RangeFieldType> tensor =
            local_diffusion_tensor->evaluate(reference_element.position(0, 0));
        ::Eigen::EigenSolver<typename Stuff::LA::EigenDenseMatrix<RangeFieldType>::BackendType> eigen_solver(
            tensor.backend());
        assert(eigen_solver.info() == ::Eigen::Success);
        const auto eigenvalues = eigen_solver.eigenvalues(); // <- this should be an Eigen vector of std::complex
        RangeFieldType min_ev = std::numeric_limits<RangeFieldType>::max();
        for (size_t ii = 0; ii < boost::numeric_cast<size_t>(eigenvalues.size()); ++ii) {
          // assert this is real
          assert(std::abs(eigenvalues[ii].imag()) < 1e-15);
          // assert that this eigenvalue is positive
          const RangeFieldType eigenvalue = eigenvalues[ii].real();
          assert(eigenvalue > 1e-15);
          min_ev = std::min(min_ev, eigenvalue);
        }
        return min_ev;
#endif  // HAVE_EIGEN
      } // ... min_eigenvalue_of_(...)
    };  // class Compute< ..., d, d >

  public:
    Localfunction(const EntityType& ent, const DiffusionFactorType& diffusion_factor,
                  const DiffusionTensorType& diffusion_tensor, const RangeFieldType poincare_constant)
      : BaseType(ent), value_(0)
    {
      const RangeFieldType min_diffusion_factor =
          ComputeDiffusionFactor<DiffusionFactorType,
                                 DiffusionFactorType::dimRange,
                                 DiffusionFactorType::dimRangeCols>::min_of(diffusion_factor, ent);
      const RangeFieldType min_eigen_value_diffusion_tensor =
          ComputeDiffusionTensor<DiffusionTensorType,
                                 DiffusionTensorType::dimRange,
                                 DiffusionTensorType::dimRangeCols>::min_eigenvalue_of(diffusion_tensor, ent);
      assert(min_diffusion_factor > RangeFieldType(0));
      assert(min_eigen_value_diffusion_tensor > RangeFieldType(0));
      const DomainFieldType hh = compute_diameter_of_(ent);
      value_                   = (poincare_constant * hh * hh) / (min_diffusion_factor * min_eigen_value_diffusion_tensor);
    } // Localfunction(...)

    Localfunction(const Localfunction& /*other*/) = delete;

    Localfunction& operator=(const Localfunction& /*other*/) = delete;

    virtual size_t order() const override final { return 0; }

    virtual void evaluate(const DomainType& UNUSED_UNLESS_DEBUG(xx), RangeType& ret) const override final
    {
      assert(this->is_a_valid_point(xx));
      ret[0] = value_;
    }

    virtual void jacobian(const DomainType& UNUSED_UNLESS_DEBUG(xx), JacobianRangeType& ret) const override final
    {
      assert(this->is_a_valid_point(xx));
      ret *= RangeFieldType(0);
    }

  private:
    static DomainFieldType compute_diameter_of_(const EntityType& ent)
    {
      DomainFieldType ret(0);
      for (auto cc : DSC::valueRange(ent.template count<dimDomain>())) {
        const auto vertex = ent.template subEntity<dimDomain>(cc)->geometry().center();
        for (auto dd : DSC::valueRange(cc + 1, ent.template count<dimDomain>())) {
          const auto other_vertex = ent.template subEntity<dimDomain>(dd)->geometry().center();
          const auto diff         = vertex - other_vertex;
          ret                     = std::max(ret, diff.two_norm());
        }
      }
      return ret;
    } // ... compute_diameter_of_(...)

    RangeFieldType value_;
  }; // class Localfunction

public:
  typedef typename BaseType::EntityType EntityType;
  typedef typename BaseType::LocalfunctionType LocalfunctionType;

  typedef typename BaseType::DomainFieldType DomainFieldType;
  static const size_t dimDomain = BaseType::dimDomain;
  typedef typename BaseType::DomainType DomainType;

  typedef typename BaseType::RangeFieldType RangeFieldType;
  static const size_t dimRange     = BaseType::dimRange;
  static const size_t dimRangeCols = BaseType::dimRangeCols;
  typedef typename BaseType::RangeType RangeType;

  static std::string static_id() { return BaseType::static_id() + ".ESV2007.cutoff"; }

  Cutoff(const DiffusionFactorType& diffusion_factor, const DiffusionTensorType& diffusion_tensor,
         const RangeFieldType poincare_constant = 1.0 / (M_PIl * M_PIl), const std::string nm = static_id())
    : diffusion_factor_(diffusion_factor)
    , diffusion_tensor_(diffusion_tensor)
    , poincare_constant_(poincare_constant)
    , name_(nm)
  {
  }

  Cutoff(const ThisType& other) = default;

  ThisType& operator=(const ThisType& other) = delete;

  virtual std::string name() const override final { return name_; }

  virtual std::unique_ptr<LocalfunctionType> local_function(const EntityType& entity) const override final
  {
    return std::unique_ptr<Localfunction>(
        new Localfunction(entity, diffusion_factor_, diffusion_tensor_, poincare_constant_));
  }

private:
  const DiffusionFactorType& diffusion_factor_;
  const DiffusionTensorType& diffusion_tensor_;
  const RangeFieldType poincare_constant_;
  std::string name_;
}; // class Cutoff

} // namespace ESV2007
} // namespace Functions
} // namespace Stuff
} // namespace Dune

#endif // DUNE_STUFF_FUNCTIONS_ESV2007_HH


#include <boost/python/class.hpp>
#include <boost/python/def.hpp>

#include <scitbx/matrix/eigensystem.h>

namespace scitbx { namespace matrix { namespace boost_python {

  struct eigensystem_real_symmetric_wrappers
  {
    typedef eigensystem::real_symmetric<> w_t;

    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("eigensystem_real_symmetric", no_init)
        .def(init<
          af::const_ref<double, af::c_grid<2> > const&, double, double>((
            arg("m"),
            arg("relative_epsilon")=1.e-10,
            arg("absolute_epsilon")=0)))
        .def(init<
          scitbx::sym_mat3<double> const&, double, double>((
            arg("m"),
            arg("relative_epsilon")=1.e-10,
            arg("absolute_epsilon")=0)))
        .def("min_abs_pivot", &w_t::min_abs_pivot)
        .def("vectors", &w_t::vectors)
        .def("values", &w_t::values)
        .def("generalized_inverse_as_packed_u",
          &w_t::generalized_inverse_as_packed_u)
      ;
    }
  };

  // simlar to time_dsyev_*()
  vec3<double>
  time_eigensystem_real_symmetric(
    sym_mat3<double> const& m, std::size_t n_repetitions)
  {
    SCITBX_ASSERT(n_repetitions % 2 == 0);
    vec3<double> result(0,0,0);
    for(std::size_t i=0;i<n_repetitions/2;i++) {
      result += vec3<double>(
        eigensystem::real_symmetric<>(m).values().begin());
      result -= vec3<double>(
        eigensystem::real_symmetric<>(m).values().begin());
    }
    return result / static_cast<double>(n_repetitions);
  }

  void wrap_eigensystem() {
    using namespace boost::python;
    eigensystem_real_symmetric_wrappers::wrap();
    def("time_eigensystem_real_symmetric", time_eigensystem_real_symmetric);
  }

}}}


#pragma once
#include <QObject>
#include <opencv2/opencv.hpp>
#include <NTL/mat_ZZ.h>

struct Lattice {
  NTL::vec_ZZ origin;
  NTL::mat_ZZ bases;
  double total_error;
};

inline bool operator==(const Lattice& lhs, const Lattice& rhs)
{
  return lhs.origin == rhs.origin &&
         lhs.bases == rhs.bases &&
         lhs.total_error == rhs.total_error;
}

inline bool operator!=(const Lattice& lhs, const Lattice& rhs)
{
  return !(lhs == rhs);
}

class LatticeFitter : public QObject
{
  Q_OBJECT

public:
  explicit LatticeFitter(QObject* parent = 0);

  Lattice best_lattice;

signals:
  void latticeFittingStarted();
  void progressUpdated(int);
  void foundBestLattice(const Lattice&);

public slots:
  void findBestLattice(const std::vector<cv::Point2f>& points);

private:
  Lattice bestLatticeForOrigin
  (const cv::Point2f& origin, const std::vector<cv::Point2f>& points) const;

  void calculateErrorForLattice
  (Lattice& lattice, const std::vector<NTL::vec_ZZ>& points) const;

  NTL::vec_ZZ cvPoint2fToNTLVec(const cv::Point2f& point) const;
  double sq(double value) const;
};

inline NTL::vec_ZZ LatticeFitter::cvPoint2fToNTLVec(const cv::Point2f& point) const
{
  NTL::vec_ZZ result;
  result.append(NTL::to_ZZ(point.x));
  result.append(NTL::to_ZZ(point.y));
  return result;
}

inline double LatticeFitter::sq(double value) const
{
  return value * value;
}


#include "LinearMuscleConstraint.h"
#include <Eigen/SVD>
#include <Eigen/Sparse>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>
#include <iostream>
using namespace FEM;

LinearMuscleConstraint::
LinearMuscleConstraint(const double& stiffness,
		const Eigen::Vector3d& fiber_direction,
		const double& activation_level,
		int i0,int i1,int i2,int i3,
		double vol,const Eigen::Matrix3d& invDm,
		// const Eigen::Vector4d& barycentric1,const Eigen::Vector4d& barycentric2,
		double weight)
	:Constraint(stiffness),
	mi0(i0),mi1(i1),mi2(i2),mi3(i3),
	mStiffness(stiffness),mFiberDirection(fiber_direction),
	mVol(vol),mInvDm(invDm),mDs(Eigen::Matrix3d::Zero()),
	mActivationLevel(activation_level),mWeight(weight)
{
	mF.setZero();
	// mBarycentric.clear();
	// mBarycentric.push_back(barycentric1);
	// mBarycentric.push_back(barycentric2);

	mStiffness *= mWeight;
}
void
LinearMuscleConstraint::
ComputeF
(const Eigen::VectorXd& x)
{
	Eigen::Vector3d x0(x.block<3,1>(mi0*3,0));

	Eigen::Matrix3d Ds;

	Ds.block<3,1>(0,0) = x.block<3,1>(mi1*3,0)-x0;
	Ds.block<3,1>(0,1) = x.block<3,1>(mi2*3,0)-x0;
	Ds.block<3,1>(0,2) = x.block<3,1>(mi3*3,0)-x0;

	mDs = Ds;
	mF = mDs * mInvDm;
}
void
LinearMuscleConstraint::
ComputeP
(Eigen::Matrix3d& P)
{
	P = mStiffness*(mF*mFiberDirection*mFiberDirection.transpose() - mp0*mFiberDirection.transpose());
}
void
LinearMuscleConstraint::
ComputedPdF
(Tensor3333& dPdF)
{
	Tensor3333 dFdF;
	dFdF.SetIdentity();

	for(int i=0; i<3;i++)
		for(int j=0;j<3;j++)
			dPdF(i,j) = mStiffness*dFdF(i,j)*mFiberDirection*mFiberDirection.transpose();
}
void
LinearMuscleConstraint::
EvaluateJMatrix(int index, std::vector<Eigen::Triplet<double>>& J_triplets)
{
	Eigen::MatrixXd Ai(3,12);

	Eigen::Vector3d v = mInvDm*mFiberDirection;

	double a,b,c;
	a = v[0];
	b = v[1];
	c = v[2];

	Ai<<
		-(a+b+c),0,0,a,0,0,b,0,0,c,0,0,
		0,-(a+b+c),0,0,a,0,0,b,0,0,c,0,
		0,0,-(a+b+c),0,0,a,0,0,b,0,0,c;

	Eigen::MatrixXd MuAiT = mVol*mStiffness*Ai.transpose();

	J_triplets.push_back(Eigen::Triplet<double>(3*mi0+0,3*index+0,MuAiT(3*0+0,3*0+0)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi0+0,3*index+1,MuAiT(3*0+0,3*0+1)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi0+0,3*index+2,MuAiT(3*0+0,3*0+2)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi0+1,3*index+0,MuAiT(3*0+1,3*0+0)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi0+1,3*index+1,MuAiT(3*0+1,3*0+1)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi0+1,3*index+2,MuAiT(3*0+1,3*0+2)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi0+2,3*index+0,MuAiT(3*0+2,3*0+0)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi0+2,3*index+1,MuAiT(3*0+2,3*0+1)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi0+2,3*index+2,MuAiT(3*0+2,3*0+2)));

	J_triplets.push_back(Eigen::Triplet<double>(3*mi1+0,3*index+0,MuAiT(3*1+0,3*0+0)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi1+0,3*index+1,MuAiT(3*1+0,3*0+1)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi1+0,3*index+2,MuAiT(3*1+0,3*0+2)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi1+1,3*index+0,MuAiT(3*1+1,3*0+0)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi1+1,3*index+1,MuAiT(3*1+1,3*0+1)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi1+1,3*index+2,MuAiT(3*1+1,3*0+2)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi1+2,3*index+0,MuAiT(3*1+2,3*0+0)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi1+2,3*index+1,MuAiT(3*1+2,3*0+1)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi1+2,3*index+2,MuAiT(3*1+2,3*0+2)));

	J_triplets.push_back(Eigen::Triplet<double>(3*mi2+0,3*index+0,MuAiT(3*2+0,3*0+0)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi2+0,3*index+1,MuAiT(3*2+0,3*0+1)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi2+0,3*index+2,MuAiT(3*2+0,3*0+2)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi2+1,3*index+0,MuAiT(3*2+1,3*0+0)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi2+1,3*index+1,MuAiT(3*2+1,3*0+1)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi2+1,3*index+2,MuAiT(3*2+1,3*0+2)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi2+2,3*index+0,MuAiT(3*2+2,3*0+0)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi2+2,3*index+1,MuAiT(3*2+2,3*0+1)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi2+2,3*index+2,MuAiT(3*2+2,3*0+2)));

	J_triplets.push_back(Eigen::Triplet<double>(3*mi3+0,3*index+0,MuAiT(3*3+0,3*0+0)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi3+0,3*index+1,MuAiT(3*3+0,3*0+1)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi3+0,3*index+2,MuAiT(3*3+0,3*0+2)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi3+1,3*index+0,MuAiT(3*3+1,3*0+0)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi3+1,3*index+1,MuAiT(3*3+1,3*0+1)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi3+1,3*index+2,MuAiT(3*3+1,3*0+2)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi3+2,3*index+0,MuAiT(3*3+2,3*0+0)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi3+2,3*index+1,MuAiT(3*3+2,3*0+1)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi3+2,3*index+2,MuAiT(3*3+2,3*0+2)));
}
void
LinearMuscleConstraint::
EvaluateLMatrix(std::vector<Eigen::Triplet<double>>& L_triplets)
{
	Eigen::MatrixXd Ai(3,12);
	Eigen::Vector3d v = mInvDm*mFiberDirection;

	double a,b,c;
	a = v[0];
	b = v[1];
	c = v[2];

	Ai<<
		-(a+b+c),0,0,a,0,0,b,0,0,c,0,0,
		0,-(a+b+c),0,0,a,0,0,b,0,0,c,0,
		0,0,-(a+b+c),0,0,a,0,0,b,0,0,c;

	auto MuAiTAi = mVol*mStiffness*((Ai.transpose())*Ai);

	int idx[4] = {mi0,mi1,mi2,mi3};

	for(int i =0;i<4;i++)
	{
		for(int j=0;j<4;j++)
		{
			L_triplets.push_back(Eigen::Triplet<double>(3*idx[i]+0,3*idx[j]+0,MuAiTAi(3*i+0,3*j+0)));
			L_triplets.push_back(Eigen::Triplet<double>(3*idx[i]+0,3*idx[j]+1,MuAiTAi(3*i+0,3*j+1)));
			L_triplets.push_back(Eigen::Triplet<double>(3*idx[i]+0,3*idx[j]+2,MuAiTAi(3*i+0,3*j+2)));
			L_triplets.push_back(Eigen::Triplet<double>(3*idx[i]+1,3*idx[j]+0,MuAiTAi(3*i+1,3*j+0)));
			L_triplets.push_back(Eigen::Triplet<double>(3*idx[i]+1,3*idx[j]+1,MuAiTAi(3*i+1,3*j+1)));
			L_triplets.push_back(Eigen::Triplet<double>(3*idx[i]+1,3*idx[j]+2,MuAiTAi(3*i+1,3*j+2)));
			L_triplets.push_back(Eigen::Triplet<double>(3*idx[i]+2,3*idx[j]+0,MuAiTAi(3*i+2,3*j+0)));
			L_triplets.push_back(Eigen::Triplet<double>(3*idx[i]+2,3*idx[j]+1,MuAiTAi(3*i+2,3*j+1)));
			L_triplets.push_back(Eigen::Triplet<double>(3*idx[i]+2,3*idx[j]+2,MuAiTAi(3*i+2,3*j+2)));
		}
	}
}
void
LinearMuscleConstraint::
EvaluateDVector(const Eigen::VectorXd& x)
{
	ComputeF(x);
	Computep0();
}
void
LinearMuscleConstraint::
GetDVector(int& index,Eigen::VectorXd& d)
{
	d.block<3,1>(3*index,0) = mp0;
	index++;
}
void
LinearMuscleConstraint::
Computep0()
{
	mp0 = (1.0-mActivationLevel)*mF*mFiberDirection;
}
int
LinearMuscleConstraint::
GetDof()
{
	return 1;
}
ConstraintType
LinearMuscleConstraint::
GetType()
{
	return ConstraintType::LINEAR_MUSCLE;
}
void
LinearMuscleConstraint::
SetActivationLevel(const double& a)
{
	mActivationLevel = a;
}
const double&
LinearMuscleConstraint::
GetActivationLevel()
{
	return mActivationLevel;
}
const Eigen::Vector3d&
LinearMuscleConstraint::
GetFiberDirection()
{
	return mFiberDirection;
}
void
LinearMuscleConstraint::
SetActivationIndex(const int& i)
{
	mActivationIndex = i;
}


//#include <boost/multiprecision/cpp_int.hpp>

//using namespace boost::multiprecision;

//int main() {
//boost::uint64_t i = (std::numeric_limits<boost::uint64_t>::max)();
//boost::uint64_t j = 1;

//uint128_t ui128, x;
//uint256_t ui256;
////
//// Start by performing arithmetic on 64-bit integers to yield 128-bit results:
////
//x = add(ui128, i, j);
//std::cout << std::hex << std::showbase << i << std::endl;
//std::cout << std::hex << std::showbase << add(ui128, i, j) << std::endl;
//std::cout << std::hex << std::showbase << multiply(ui128, i, i) << std::endl;
//std::cout << "\n" << x << std::endl;
////
//// The try squaring a 128-bit integer to yield a 256-bit result:
////
//ui128 = (std::numeric_limits<uint128_t>::max)();
//std::cout << std::hex << std::showbase << multiply(ui256, ui128, ui128) << std::endl;
//return 0;
//}

//// independent_bits_engine constructor
//#include <iostream>
//#include <chrono>
//#include <cstdint>
//#include <random>

//int main ()
//{
  //typedef std::independent_bits_engine<std::mt19937,64,std::uint_fast64_t> generator_type;

  //generator_type g1;

  //generator_type g2(g1.base());

  //std::mt19937 temp;
  //generator_type g3(std::move(temp));

  //unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
  //generator_type g4(seed);

  //std::seed_seq sseq ({2,16,77});
  //generator_type g5(sseq);

  //std::cout << "g1(): " << g1() << std::endl;
  //std::cout << "g2(): " << g2() << std::endl;
  //std::cout << "g3(): " << g3() << std::endl;
  //std::cout << "g4(): " << g4() << std::endl;
  //std::cout << "g5(): " << g5() << std::endl;

  //return 0;
//}

#include <stdlib.h>
#include <iostream>
#include <fstream>
#include <cmath>
#include <ctime>
#include <chrono>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/random.hpp>
#include <gmp.h>

using namespace boost::multiprecision;
using namespace boost::random;
using namespace std;

uint256_t rand_gen(){
	typedef independent_bits_engine<mt19937, 256, uint256_t> generator_type;
    unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
    //srand(time(0));
    generator_type gen(seed);
    return gen();
}

int main()
{

   uint256_t x = rand_gen();

   //std::cout << x << std::endl;

   ofstream keyfile;
   keyfile.open("privatekeys.csv");

   for(int i = 0; i < 10; i++)
        keyfile << rand_gen() << "\n" ;
   keyfile.close();
   cout << sizeof(mpz_t);
  //// Read the data file
  //vector<uint256_t> row;
  //vector<uint256_t> keys;
  //uint256_t line;
  ////string line, word, temp;
  //ifstream fin("privatekeys.csv");
  ////int k = 0;
  //while(!fin.eof())
	//{
		//row.clear();
		//fin >> line;
		////stringstream s(line);
		//cout << line << "\n";
		//keys.push_back(line);
     //}

	//cout << "\n" << "The private keys are as follows\n";
	//for(int i=0; i < 10; i++)
		//cout << keys[i] << "\n";
   //

   // Declare our random number generator type, the underlying generator
   // is the Mersenne twister mt19937 engine, and we'll generate 256 bit
   // random values, independent_bits_engine will make multiple calls
   // to the underlying engine until we have the requested number of bits:
   //
   //
   // Alternatively if we wish to generate random values in a fixed-precision
   // type, then we must use an unsigned type in order to adhere to the
   // conceptual requirements of the generator:
   //
   //typedef independent_bits_engine<mt19937, 512, uint512_t> generator512_type;
   //generator512_type gen512;
   //
   // Generate some 1024-bit unsigned values:
   //
   //std::cout << std::hex << std::showbase;
   //for(unsigned i = 0; i < 10; ++i)
    //  std::cout << gen512() << std::endl;
      return 0;
}


//
// Copyright (c) 2018 Sergey Panov.
// Copyright (c) 2018 Second Star Algonumerix LLC
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//

#include <complex>
#include <iostream>

#include <vector>
#include <iterator>
#include <numeric>
#include <fstream>
#include <cmath>

#include <boost/lexical_cast.hpp>

#include "octio.hpp"

using boost::lexical_cast;
using namespace ssan::octio;

template<typename T>
void print_matrix(const std::vector<std::vector<T>> &matrix,
        const std::string & name) {
    if (!matrix.empty()) {
        std::cout << name << "["
                  << matrix.size() << ","
                  << matrix[0].size() << "]: [" << std::endl;
        for (const auto & row : matrix) {
            for (auto element : row) {
                std::cout << " " << lexical_cast<std::string>(element);
            }
            std::cout << std::endl;
        }
        std::cout << "]" << std::endl;
    }
}

template<typename T>
void print_vector(const std::vector<T> &vector, const std::string & name) {
    if (!vector.empty()) {
        std::cout << name << "["
                  << vector.size() << "]: [";
        for (auto element : vector) {
                std::cout << " " << lexical_cast<std::string>(element);
        }
        std::cout << "]" << std::endl;
    }
}

int main() {
    int int_var = -1;
    float float_var(nanf(""));
    double double_var(nan(""));
    long double ldouble_var(nanl(""));
    std::complex<float> cfloat_var(nanf(""), nanf(""));
    std::complex<double> cdouble_var(nan(""), nan(""));
    std::complex<long double> cldouble_var(nanl(""), nanl(""));
    std::vector<int8_t> int8_vect;
    std::vector<int16_t> int16_vect;
    std::vector<int32_t> int32_vect;
    std::vector<int64_t> int64_vect;
    std::vector<uint8_t> uint8_vect;
    std::vector<uint16_t> uint16_vect;
    std::vector<uint32_t> uint32_vect;
    std::vector<uint64_t> uint64_vect;
    std::vector<float> float_vect;
    std::vector<double> double_vect;
    std::vector<long double> ldouble_vect;
    std::vector<int> int_covect;
    std::vector<float> float_covect;
    std::vector<double> double_covect;
    std::vector<long double> ldouble_covect;
    std::vector<std::complex<int>> cint_vect;
    std::vector<std::complex<float>> cfloat_vect;
    std::vector<std::complex<double>> cdouble_vect;
    std::vector<std::complex<long double>> cldouble_vect;
    std::vector<std::complex<float>> cfloat_covect;
    std::vector<std::complex<double>> cdouble_covect;
    std::vector<std::complex<long double>> cldouble_covect;
    std::vector<std::vector<int>> int_mat;
    std::vector<std::vector<float>> float_mat;
    std::vector<std::vector<double>> double_mat;
    std::vector<std::vector<long double>> ldouble_mat;
    std::vector<std::vector<std::complex<float>>> cfloat_mat;
    std::vector<std::vector<std::complex<double>>> cdouble_mat;
    std::vector<std::vector<std::complex<long double>>> cldouble_mat;

    const std::string file_name = "test_octio.mat";
    const std::string file_name_out = "test_octio_out.mat";

    std::ifstream file(file_name);
    reader input(&file);
    std::ofstream ofile(file_name_out);
    writer output(&ofile, "Test file");

    std::cout << input.title() << std::endl;
    do {
        bool consumed = false;
        // std::cout << input.next_name() << std::endl;
        // std::cout << input.next_type() << std::endl;
        if (input.next_type() == SCALAR) {
            if (input.next_name() == "int_var") {
                consumed = input.read(int_var);
                output.write(int_var, "int_var");
            } else if (input.next_name() == "float_var") {
                consumed = input.read(float_var);
                output.write(float_var, "float_var");
            } else if (input.next_name() == "double_var") {
                consumed = input.read(double_var);
                output.write(double_var, "double_var");
            } else if (input.next_name() == "ldouble_var") {
                consumed = input.read(ldouble_var);
                output.write(ldouble_var, "ldouble_var");
            }
        } else if (input.next_type() == COMPLEX_SCALAR) {
            if (input.next_name() == "cfloat_var") {
                consumed = input.read(cfloat_var);
                output.write(cfloat_var, "cfloat_var");
            } else if (input.next_name() == "cdouble_var") {
                consumed = input.read(cdouble_var);
                output.write(cdouble_var, "cdouble_var");
            } else if (input.next_name() == "cldouble_var") {
                consumed = input.read(cldouble_var);
                output.write(cldouble_var, "cldouble_var");
            }
        } else if (input.next_type() == VECTOR
                   || input.next_type() == COVECTOR) {
            if (input.next_name() == "int8_vect") {
                consumed = input.read(int8_vect);
                output.write(int8_vect, "int8_vect");
            } else if (input.next_name() == "int16_vect") {
                consumed = input.read(int16_vect);
                output.write(int16_vect, "int16_vect");
            } else if (input.next_name() == "int32_vect") {
                consumed = input.read(int32_vect);
                output.write(int32_vect, "int32_vect");
            } else if (input.next_name() == "int64_vect") {
                consumed = input.read(int64_vect);
                output.write(int64_vect, "int64_vect");
            } else if (input.next_name() == "uint8_vect") {
                consumed = input.read(uint8_vect);
                output.write(uint8_vect, "uint8_vect");
            } else if (input.next_name() == "uint16_vect") {
                consumed = input.read(uint16_vect);
                output.write(uint16_vect, "uint16_vect");
            } else if (input.next_name() == "uint32_vect") {
                consumed = input.read(int32_vect);
                output.write(uint32_vect, "uint32_vect");
            } else if (input.next_name() == "uint64_vect") {
                consumed = input.read(uint64_vect);
                output.write(uint64_vect, "uint64_vect");
            } else if (input.next_name() == "float_vect") {
                consumed = input.read(float_vect);
                output.write(float_vect, "float_vect");
            } else if (input.next_name() == "double_vect") {
                consumed = input.read(double_vect);
            } else if (input.next_name() == "ldouble_vect") {
                consumed = input.read(ldouble_vect);
            } else if (input.next_name() == "int_covect") {
                consumed = input.read(int_covect);
            } else if (input.next_name() == "float_covect") {
                consumed = input.read(float_covect);
                output.write_covec(float_covect, "float_covect");
            } else if (input.next_name() == "double_covect") {
                consumed = input.read(double_covect);
            } else if (input.next_name() == "ldouble_covect") {
                consumed = input.read(ldouble_covect);
            }
        } else if (input.next_type() == COMPLEX_VECTOR
                   || input.next_type() == COMPLEX_COVECTOR) {
            if (input.next_name() == "cint_vect") {
                consumed = input.read(cint_vect);
                output.write(cint_vect, "cint_vect");
            } else if (input.next_name() == "cfloat_vect") {
                consumed = input.read(cfloat_vect);
                output.write(cfloat_vect, "cfloat_vect");
            } else if (input.next_name() == "cdouble_vect") {
                consumed = input.read(cdouble_vect);
            } else if (input.next_name() == "cldouble_vect") {
                consumed = input.read(cldouble_vect);
            } else if (input.next_name() == "cfloat_covect") {
                consumed = input.read(cfloat_covect);
                output.write_covec(cfloat_covect, "cfloat_covect");
            } else if (input.next_name() == "cdouble_covect") {
                consumed = input.read(cdouble_covect);
            } else if (input.next_name() == "cldouble_covect") {
                consumed = input.read(cldouble_covect);
            }
        } else if (input.next_type() == MATRIX) {
            if (input.next_name() == "int_mat") {
                consumed = input.read(int_mat);
            } else if (input.next_name() == "float_mat") {
                consumed = input.read(float_mat);
                output.write(float_mat, "float_mat");
            } else if (input.next_name() == "double_mat") {
                consumed = input.read(double_var);
            } else if (input.next_name() == "ldouble_mat") {
                consumed = input.read(ldouble_var);
            }
        } else if (input.next_type() == COMPLEX_MATRIX) {
            if (input.next_name() == "cfloat_mat") {
                consumed = input.read(cfloat_mat);
                output.write(cfloat_mat, "cfloat_mat");
            } else if (input.next_name() == "cdouble_mat") {
                consumed = input.read(cdouble_mat);
            } else if (input.next_name() == "cldouble_mat") {
                consumed = input.read(cldouble_mat);
            }
        }

        if (!consumed) {
            input.skip_one();
        }

    } while (input.next_type() != INVALID);

    std::cout << "int_var: " << int_var << std::endl;
    std::cout << "float_var: " << lexical_cast<std::string>(float_var)
              << std::endl;
    std::cout << "double_var: " << lexical_cast<std::string>(double_var)
              << std::endl;
    std::cout << "ldouble_var: " << lexical_cast<std::string>(ldouble_var)
              << std::endl;
    std::cout << "cfloat_var: ("
              << lexical_cast<std::string>(cfloat_var.real()) << ","
              << lexical_cast<std::string>(cfloat_var.imag()) << ")"
              << std::endl;
    std::cout << "cdouble_var: ("
              << lexical_cast<std::string>(cdouble_var.real()) << ","
              << lexical_cast<std::string>(cdouble_var.imag()) << ")"
              << std::endl;
    std::cout << "cldouble_var: ("
              << lexical_cast<std::string>(cldouble_var.real()) << ","
              << lexical_cast<std::string>(cldouble_var.imag()) << ")"
              << std::endl;
    print_vector(float_vect, "float_vect");
    print_vector(float_covect, "float_covect");
    print_vector(cint_vect, "cint_vect");
    print_vector(cfloat_vect, "cfloat_vect");
    print_vector(cfloat_covect, "cfloat_covect");
    print_matrix(float_mat, "float_mat");
    print_matrix(cfloat_mat, "cfloat_mat");

    return 0;
}


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

// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.
// Copyright (c) 2017 Adam Wulkiewicz, Lodz, Poland.

// 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)

#include <geometry_test_common.hpp>

#include <boost/core/ignore_unused.hpp>

#include <boost/geometry/arithmetic/cross_product.hpp>

#include <boost/geometry/algorithms/assign.hpp>

#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/adapted/c_array.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
#include <test_common/test_point.hpp>

template <typename P>
void test_2d()
{
    P p1;
    bg::assign_values(p1, 20, 30);
    P p2;
    bg::assign_values(p2, 45, 70);
    P c = bg::cross_product(p1, p2);

    typedef typename bg::coordinate_type<P>::type scalar_type;
    BOOST_CHECK_EQUAL(bg::get<0>(c), scalar_type(50));
}

template <typename P>
void test_3d()
{
    P p1;
    bg::assign_values(p1, 20, 30, 10);
    P p2;
    bg::assign_values(p2, 45, 70, 20);
    P c = bg::cross_product(p1, p2);

    typedef typename bg::coordinate_type<P>::type scalar_type;
    BOOST_CHECK_EQUAL(bg::get<0>(c), scalar_type(-100));
    BOOST_CHECK_EQUAL(bg::get<1>(c), scalar_type(50));
    BOOST_CHECK_EQUAL(bg::get<2>(c), scalar_type(50));
}

#ifdef TEST_FAIL_CROSS_PRODUCT
template <typename P>
void test_4d()
{
    P p1;
    bg::assign_values(p1, 20, 30, 10);
    bg::set<3>(p1, 15);
    P p2;
    bg::assign_values(p2, 45, 70, 20);
    bg::set<3>(p2, 35);
    P c = bg::cross_product(p1, p2);
    boost::ignore_unused(c);
}
#endif

int test_main(int, char* [])
{
    test_2d<bg::model::point<int, 2, bg::cs::cartesian> >();
    test_2d<bg::model::point<float, 2, bg::cs::cartesian> >();
    test_2d<bg::model::point<double, 2, bg::cs::cartesian> >();

    test_3d<bg::model::point<int, 3, bg::cs::cartesian> >();
    test_3d<bg::model::point<float, 3, bg::cs::cartesian> >();
    test_3d<bg::model::point<double, 3, bg::cs::cartesian> >();

#ifdef TEST_FAIL_CROSS_PRODUCT
    test_4d<bg::model::point<int, 4, bg::cs::cartesian> >();
    test_4d<bg::model::point<float, 4, bg::cs::cartesian> >();
    test_4d<bg::model::point<double, 4, bg::cs::cartesian> >();
#endif

    return 0;
}


#include <iostream>
#include <vector>
#include <random>
#include <fstream>
#include <omp.h>
#include <boost/numeric/odeint.hpp>
#include "cahnhilliard_thermal_nodiffusion.h"
#include "utils_ch.h"

  /*
  Cahn-Hilliard:

  dc/dt = laplacian( u*c^3 - b*c ) - eps_2*biharm(c) - sigma*(c - m) + sigma_noise * N(0,1^2)

  expanding out RHS into individual differentials:
  D*laplacian( u*c^3 - b*c) - D*eps_2*biharm(c)
  assuming constant eps_2.

  need a d^4 and a d^2 operator.
  */

CahnHilliard2DRHS_thermal_nodiffusion::CahnHilliard2DRHS_thermal_nodiffusion(CHparamsScalar& chp , SimInfo& info)
  : noise_dist_(0.0,1.0) , info_(info)
  {
    chpV_.eps_2    = std::vector<double>( info_.nx*info_.ny , chp.eps_2     );
    chpV_.b        = std::vector<double>( info_.nx*info_.ny , chp.b         );
    chpV_.u        = std::vector<double>( info_.nx*info_.ny , chp.u         );
    chpV_.sigma    = std::vector<double>( info_.nx*info_.ny , chp.sigma     );
    chpV_.m        = std::vector<double>( info_.nx*info_.ny , chp.m  );
    chpV_.DT       = std::vector<double>( info_.nx*info_.ny , chp.DT  );
    chpV_.f_T      = std::vector<double>( info_.nx*info_.ny , chp.f_T  );
    chpV_.sigma_noise    = chp.sigma_noise;
    chpV_.T_const        = std::vector<double>( info_.nx*info_.ny , chp.T_const  );

    if ( info.bc.compare("dirichlet") == 0) {
      ch_rhs_ = &compute_ch_nonlocal_stationary_boundaries;
      std::cout << "Initialized Cahn-Hilliard equation: scalar parameters, dirichlet BCs, thermal coefficient dependence, no thermal diffusion" << std::endl;
    }
    else if ( info.bc.compare("neumann") == 0) {
      ch_rhs_ = &compute_ch_nonlocal_neumannBC;
      std::cout << "Initialized Cahn-Hilliard equation: scalar parameters, neumann BCs, thermal coefficient dependence, no thermal diffusion" << std::endl;
    }
    else {
      ch_rhs_ = &compute_ch_nonlocal;
      std::cout << "Initialized Cahn-Hilliard equation: scalar parameters, periodic BCs, thermal coefficient dependence, no thermal diffusion" << std::endl;
    }

  }

CahnHilliard2DRHS_thermal_nodiffusion::CahnHilliard2DRHS_thermal_nodiffusion(CHparamsVector& chp , SimInfo& info)
  : noise_dist_(0.0,1.0) , chpV_(chp) , info_(info)
  {

    if ( info.bc.compare("dirichlet") == 0) {
      ch_rhs_ = &compute_ch_nonlocal_stationary_boundaries;
      std::cout << "Initialized Cahn-Hilliard equation: spatial-field parameters, dirichlet BCs, thermal coefficient dependence, no thermal diffusion" << std::endl;
    }
    else if ( info.bc.compare("neumann") == 0) {
      ch_rhs_ = &compute_ch_nonlocal_neumannBC;
      std::cout << "Initialized Cahn-Hilliard equation: spatial-field parameters, neumann BCs, thermal coefficient dependence, no thermal diffusion" << std::endl;
    }
    else {
      ch_rhs_ = &compute_ch_nonlocal;
      std::cout << "Initialized Cahn-Hilliard equation: spatial-field parameters, periodic BCs, thermal coefficient dependence, no thermal diffusion" << std::endl;
    }

  }

CahnHilliard2DRHS_thermal_nodiffusion::~CahnHilliard2DRHS_thermal_nodiffusion() { };

void CahnHilliard2DRHS_thermal_nodiffusion::rhs(const std::vector<double> &c, std::vector<double> &dcdt, const double t)
  {
    dcdt.resize(info_.nx*info_.ny);

    // evaluate CH parameter dependencies on temperature
    //chpV_ = compute_chparams_using_temperature( chpV_ , info_ , chpV_.T_const );
    chpV_ = compute_eps2_and_sigma_from_polymer_params( chpV_ , info_ , chpV_.T_const );

    // evaluate deterministic nonlocal dynamics
    compute_ch_nonlocal(c, dcdt, t, chpV_, info_);

  }

void CahnHilliard2DRHS_thermal_nodiffusion::setInitialConditions(std::vector<double> &x)
  {
    x.resize(info_.nx * info_.ny);

    std::default_random_engine generator;
    std::uniform_real_distribution<double> distribution(-1.0,1.0);

    for (int i = 0; i < info_.ny; ++i) {
      for (int j = 0; j < info_.nx; ++j) {
        x[info_.idx2d(i,j)]   = distribution(generator) * 0.005;
      }
    }

    // Set BCs if needed
    if ( info_.bc.compare("dirichlet") == 0) {
      x = apply_dirichlet_bc( x , info_ );
    }
    else if ( info_.bc.compare("neumann") == 0 ) {
      x = apply_neumann_bc( x , info_ );
    }

  }

void CahnHilliard2DRHS_thermal_nodiffusion::write_state(const std::vector<double> &x , const int idx , const int nx , const int ny , std::string& outdir)
{
  if ( outdir.back() != '/' )
    outdir += '/';
  std::ofstream outC;
  outC.open( outdir + "C_" + std::to_string(idx) + ".out" );
  outC.precision(16);

  for (int i = 0; i < ny; ++i){
    for (int j = 0; j < nx; ++j){
      outC << x[i * ny + j] << " ";
    }
  }

  outC.close();
};


#include <boost/math/quaternion.hpp>


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

  @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-constant
    @brief Generate a constant from a static rational number representation.

    @tparam Type        Type of the generated constant
    @tparam Numerator   Numerator of the generated constant
    @tparam Denominator Denominator of the generated constant
  **/
  template<typename Type, std::uintmax_t Numerator, std::uintmax_t Denumerator> auto Ratio();
} }
#endif

#include <boost/simd/constant/scalar/ratio.hpp>
#include <boost/simd/constant/simd/ratio.hpp>

#endif


#include "problemes.h"
#include "arithmetique.h"

#include <boost/date_time/date_defs.hpp>
#include <boost/date_time/gregorian/gregorian.hpp>

typedef unsigned long long nombre;

ENREGISTRER_PROBLEME(19, "Counting Sundays") {
    // You are given the following information, but you may prefer to do some research for yourself.
    //
    // 1 Jan 1900 was a Monday.
    // Thirty days has September,
    // April, June and November.
    // All the rest have thirty-one,
    // Saving February alone,
    // Which has twenty-eight, rain or shine.
    // And on leap years, twenty-nine.
    // A leap year occurs on any year evenly divisible by 4, but not on a century unless it is divisible by 400.
    //
    // How many Sundays fell on the first of the month during the twentieth century (1 Jan 1901 to 31 Dec 2000)?
    boost::gregorian::month_iterator start(boost::gregorian::date(1901, 1, 1), 1);
    boost::gregorian::date end(2000, 12, 31);
    nombre resultat = 0;
    while (start <= end) {
        if (start->day_of_week() == boost::date_time::Sunday) {
            // std::cout << *start << std::endl;
            ++resultat;
        }
        ++start;
    }
    return std::to_string(resultat);
}


//  Boost static_min_max.hpp test program  -----------------------------------//

//  (C) Copyright Daryle Walker 2001.
//  Distributed under the Boost Software License, Version 1.0. (See
//  accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)

//  See http://www.boost.org for most recent version including documentation.

//  Revision History
//  23 Sep 2001  Initial version (Daryle Walker)

#define  BOOST_INCLUDE_MAIN
#include <boost/test/test_tools.hpp>  // for main, BOOST_CHECK

#include <boost/cstdlib.hpp>                 // for boost::exit_success
#include <boost/integer/static_min_max.hpp>  // for boost::static_signed_min, etc.

#include <iostream>  // for std::cout (std::endl indirectly)

// Main testing function
int
test_main
(
    int         ,   // "argc" is unused
    char *      []  // "argv" is unused
)
{
    using std::cout;
    using std::endl;
    using boost::static_signed_min;
    using boost::static_signed_max;
    using boost::static_unsigned_min;
    using boost::static_unsigned_max;

    // Two positives
    cout << "Doing tests with two positive values." << endl;

    BOOST_CHECK( (static_signed_min< 9, 14>::value) ==  9 );
    BOOST_CHECK( (static_signed_max< 9, 14>::value) == 14 );
    BOOST_CHECK( (static_signed_min<14,  9>::value) ==  9 );
    BOOST_CHECK( (static_signed_max<14,  9>::value) == 14 );

    BOOST_CHECK( (static_unsigned_min< 9, 14>::value) ==  9 );
    BOOST_CHECK( (static_unsigned_max< 9, 14>::value) == 14 );
    BOOST_CHECK( (static_unsigned_min<14,  9>::value) ==  9 );
    BOOST_CHECK( (static_unsigned_max<14,  9>::value) == 14 );

    // Two negatives
    cout << "Doing tests with two negative values." << endl;

    BOOST_CHECK( (static_signed_min<  -8, -101>::value) == -101 );
    BOOST_CHECK( (static_signed_max<  -8, -101>::value) ==   -8 );
    BOOST_CHECK( (static_signed_min<-101,   -8>::value) == -101 );
    BOOST_CHECK( (static_signed_max<-101,   -8>::value) ==   -8 );

    // With zero
    cout << "Doing tests with zero and a positive or negative value." << endl;

    BOOST_CHECK( (static_signed_min< 0, 14>::value) ==  0 );
    BOOST_CHECK( (static_signed_max< 0, 14>::value) == 14 );
    BOOST_CHECK( (static_signed_min<14,  0>::value) ==  0 );
    BOOST_CHECK( (static_signed_max<14,  0>::value) == 14 );

    BOOST_CHECK( (static_unsigned_min< 0, 14>::value) ==  0 );
    BOOST_CHECK( (static_unsigned_max< 0, 14>::value) == 14 );
    BOOST_CHECK( (static_unsigned_min<14,  0>::value) ==  0 );
    BOOST_CHECK( (static_unsigned_max<14,  0>::value) == 14 );

    BOOST_CHECK( (static_signed_min<   0, -101>::value) == -101 );
    BOOST_CHECK( (static_signed_max<   0, -101>::value) ==    0 );
    BOOST_CHECK( (static_signed_min<-101,    0>::value) == -101 );
    BOOST_CHECK( (static_signed_max<-101,    0>::value) ==    0 );

    // With identical
    cout << "Doing tests with two identical values." << endl;

    BOOST_CHECK( (static_signed_min<0, 0>::value) == 0 );
    BOOST_CHECK( (static_signed_max<0, 0>::value) == 0 );
    BOOST_CHECK( (static_unsigned_min<0, 0>::value) == 0 );
    BOOST_CHECK( (static_unsigned_max<0, 0>::value) == 0 );

    BOOST_CHECK( (static_signed_min<14, 14>::value) == 14 );
    BOOST_CHECK( (static_signed_max<14, 14>::value) == 14 );
    BOOST_CHECK( (static_unsigned_min<14, 14>::value) == 14 );
    BOOST_CHECK( (static_unsigned_max<14, 14>::value) == 14 );

    BOOST_CHECK( (static_signed_min< -101, -101>::value) == -101 );
    BOOST_CHECK( (static_signed_max< -101, -101>::value) == -101 );

    return boost::exit_success;
}


#ifndef HOOKE_LINEAR_ELASTICITY_HPP
#define HOOKE_LINEAR_ELASTICITY_HPP

#include <polyfem/Common.hpp>
#include <polyfem/ElasticityUtils.hpp>

#include <polyfem/ElementAssemblyValues.hpp>
#include <polyfem/ElementBases.hpp>

#include <polyfem/AutodiffTypes.hpp>

#include <Eigen/Dense>
#include <array>

//local assembler for HookeLinearElasticity C : (F+F^T)/2, see linear elasticity
namespace polyfem
{
	class HookeLinearElasticity
	{
	public:
		HookeLinearElasticity();

		// res is R^{dim²}
		Eigen::Matrix<double, Eigen::Dynamic, 1, 0, 9, 1>
		assemble(const ElementAssemblyValues &vals, const int i, const int j, const QuadratureVector &da) const;

		Eigen::Matrix<double, Eigen::Dynamic, 1, 0, 3, 1>
		compute_rhs(const AutodiffHessianPt &pt) const;

		void compute_von_mises_stresses(const int el_id, const ElementBases &bs, const ElementBases &gbs, const Eigen::MatrixXd &local_pts, const Eigen::MatrixXd &displacement, Eigen::MatrixXd &stresses) const;
		void compute_stress_tensor(const int el_id, const ElementBases &bs, const ElementBases &gbs, const Eigen::MatrixXd &local_pts, const Eigen::MatrixXd &displacement, Eigen::MatrixXd &tensor) const;

		inline int size() const { return size_; }

		void set_size(const int size);

		//sets the elasticty tensor
		void set_parameters(const json &params);

	private:
		int size_ = 2;

		ElasticityTensor elasticity_tensor_;

		void assign_stress_tensor(const int el_id, const ElementBases &bs, const ElementBases &gbs, const Eigen::MatrixXd &local_pts, const Eigen::MatrixXd &displacement, const int all_size, Eigen::MatrixXd &all, const std::function<Eigen::MatrixXd(const Eigen::MatrixXd &)> &fun) const;
	};
} // namespace polyfem

#endif //HOOKE_LINEAR_ELASTICITY_HPP


// Copyright (c) 2015-2018, CNRS
// Authors: Justin Carpentier <jcarpent@laas.fr>

#ifndef __multicontact_api_geometry_ellipsoid_hpp__
#define __multicontact_api_geometry_ellipsoid_hpp__

#include <Eigen/Dense>
#include <iostream>

#include "multicontact-api/geometry/fwd.hpp"

namespace multicontact_api {
  namespace geometry
  {
    template<typename _Scalar, int _dim, int _Options>
    struct Ellipsoid
    {
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
      typedef _Scalar Scalar;
      enum { dim = _dim };
      enum { Options = _Options };

      typedef Eigen::Matrix<Scalar,dim,dim,Options> Matrix;
      typedef Eigen::Matrix<Scalar,dim,1,Options> Vector;

      Ellipsoid(const Matrix & A, const Vector & center)
      : m_A(A)
      , m_center(center)
      {}

      Scalar lhsValue(const Vector & point) const { return (m_A*(point-m_center)).norm(); }

      const Matrix & A() const { return m_A; }
      Matrix & A() { return m_A; }
      const Vector & center() const { return m_center; }
      Vector & center() { return m_center; }

      void disp(std::ostream & os) const
      {
        os
        << "A:\n" << m_A << std::endl
        << "center: " << m_center.transpose() << std::endl;
      }

      friend std::ostream & operator << (std::ostream & os,const Ellipsoid & E)
      {
        E.disp(os);
        return os;
      }

    protected:
      /// \brief
      Matrix m_A;

      /// \brief Center of the ellipsoid expressed in the global frame.
      Vector m_center;
    };
  }
}

#endif // ifndef __multicontact_api_geometry_ellipsoid_hpp__


// Copyright PinaPL
//
// functions.cpp
// PinaPL
//
#include <stdlib.h>
#include <math.h>
#include <Eigen/Dense>
#include <string>
#include <sstream>
#include <iostream>
#include <map>

double sigmoid(double x) {
    return (1/(1+exp(-x)));
}

double sigmoid_derivative(double x) {
    return x*(1-x);
}

double tanh_derivative(double x) {
    return 1-tanh(x)*tanh(x);
}

double tanhyp(double x) {
    return tanh(x);
}


#ifndef FAST_GICP_FAST_VGICP_VOXEL_HPP
#define FAST_GICP_FAST_VGICP_VOXEL_HPP

#include <unordered_map>
#include <boost/functional/hash.hpp>
#include <fast_gicp/gicp/gicp_settings.hpp>

namespace fast_gicp {

static std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i>> neighbor_offsets(NeighborSearchMethod search_method) {
  switch(search_method) {
      // clang-format off
    default:
      std::cerr << "unsupported neighbor search method" << std::endl;
      abort();
    case NeighborSearchMethod::DIRECT1:
      return std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i>>{
        Eigen::Vector3i(0, 0, 0)
      };
    case NeighborSearchMethod::DIRECT7:
      return std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i>>{
        Eigen::Vector3i(0, 0, 0),
        Eigen::Vector3i(1, 0, 0),
        Eigen::Vector3i(-1, 0, 0),
        Eigen::Vector3i(0, 1, 0),
        Eigen::Vector3i(0, -1, 0),
        Eigen::Vector3i(0, 0, 1),
        Eigen::Vector3i(0, 0, -1)
      };
    case NeighborSearchMethod::DIRECT27:
      break;
      // clang-format on
  }

  std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i>> offsets27;
  for(int i = 0; i < 3; i++) {
    for(int j = 0; j < 3; j++) {
      for(int k = 0; k < 3; k++) {
        offsets27.push_back(Eigen::Vector3i(i - 1, j - 1, k - 1));
      }
    }
  }
  return offsets27;
}

class Vector3iHash {
public:
  size_t operator()(const Eigen::Vector3i& x) const {
    size_t seed = 0;
    boost::hash_combine(seed, x[0]);
    boost::hash_combine(seed, x[1]);
    boost::hash_combine(seed, x[2]);
    return seed;
  }
};

struct GaussianVoxel {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  using Ptr = std::shared_ptr<GaussianVoxel>;

  GaussianVoxel() {
    num_points = 0;
    mean.setZero();
    cov.setZero();
  }
  virtual ~GaussianVoxel() {}

  virtual void append(const Eigen::Vector4d& mean_, const Eigen::Matrix4d& cov_) = 0;

  virtual void finalize() = 0;

public:
  int num_points;
  Eigen::Vector4d mean;
  Eigen::Matrix4d cov;
};

struct MultiplicativeGaussianVoxel : GaussianVoxel {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  MultiplicativeGaussianVoxel() : GaussianVoxel() {}
  virtual ~MultiplicativeGaussianVoxel() {}

  virtual void append(const Eigen::Vector4d& mean_, const Eigen::Matrix4d& cov_) override {
    num_points++;
    Eigen::Matrix4d cov_inv = cov_;
    cov_inv(3, 3) = 1;
    cov_inv = cov_inv.inverse().eval();

    cov += cov_inv;
    mean += cov_inv * mean_;
  }

  virtual void finalize() override {
    cov(3, 3) = 1;
    mean[3] = 1;

    cov = cov.inverse().eval();
    mean = (cov * mean).eval();
  }
};

struct AdditiveGaussianVoxel : GaussianVoxel {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  AdditiveGaussianVoxel() : GaussianVoxel() {}
  virtual ~AdditiveGaussianVoxel() {}

  virtual void append(const Eigen::Vector4d& mean_, const Eigen::Matrix4d& cov_) override {
    num_points++;
    mean += mean_;
    cov += cov_;
  }

  virtual void finalize() override {
    mean /= num_points;
    cov /= num_points;
  }
};

template<typename PointT>
class GaussianVoxelMap {
public:
  GaussianVoxelMap(double resolution, VoxelAccumulationMode mode) : voxel_resolution_(resolution), voxel_mode_(mode) {}

  void create_voxelmap(const pcl::PointCloud<PointT>& cloud, const std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>>& covs) {
    voxels_.clear();
    for(int i = 0; i < cloud.size(); i++) {
      Eigen::Vector3i coord = voxel_coord(cloud.at(i).getVector4fMap().template cast<double>());

      auto found = voxels_.find(coord);
      if(found == voxels_.end()) {
        GaussianVoxel::Ptr voxel;
        switch(voxel_mode_) {
          case VoxelAccumulationMode::ADDITIVE:
          case VoxelAccumulationMode::ADDITIVE_WEIGHTED:
            voxel = std::shared_ptr<AdditiveGaussianVoxel>(new AdditiveGaussianVoxel);
            break;
          case VoxelAccumulationMode::MULTIPLICATIVE:
            voxel = std::shared_ptr<MultiplicativeGaussianVoxel>(new MultiplicativeGaussianVoxel);
            break;
        }
        found = voxels_.insert(found, std::make_pair(coord, voxel));
      }

      auto& voxel = found->second;
      voxel->append(cloud.at(i).getVector4fMap().template cast<double>(), covs[i]);
    }

    for(auto& voxel : voxels_) {
      voxel.second->finalize();
    }
  }

  Eigen::Vector3i voxel_coord(const Eigen::Vector4d& x) const {
    return (x.array() / voxel_resolution_ - 0.5).floor().template cast<int>().template head<3>();
  }

  Eigen::Vector4d voxel_origin(const Eigen::Vector3i& coord) const {
    Eigen::Vector3d origin = (coord.template cast<double>().array() + 0.5) * voxel_resolution_;
    return Eigen::Vector4d(origin[0], origin[1], origin[2], 1.0f);
  }

  GaussianVoxel::Ptr lookup_voxel(const Eigen::Vector3i& coord) const {
    auto found = voxels_.find(coord);
    if(found == voxels_.end()) {
      return nullptr;
    }

    return found->second;
  }

private:
  double voxel_resolution_;
  VoxelAccumulationMode voxel_mode_;

  using VoxelMap = std::unordered_map<Eigen::Vector3i, GaussianVoxel::Ptr, Vector3iHash, std::equal_to<Eigen::Vector3i>, Eigen::aligned_allocator<std::pair<const Eigen::Vector3i, GaussianVoxel::Ptr>>>;
  VoxelMap voxels_;
};

}  // namespace fast_gicp

#endif


#include <iostream>
using namespace std;

#include <mtl/mtl.h>
#include <mtl/matrix.h>
#include <mtl/dense1D.h>
#include <mtl/utils.h>

namespace mtl {

template <class MatA>
inline void
simple_print(const MatA& A)
{
  typedef typename matrix_traits<MatA>::size_type Int;
  typename MatA::const_iterator A_k;
  typename MatA::OneD::const_iterator A_ki;

  A_k = A.begin();
  while (not_at(A_k, A.end())) {
    const typename MatA::OneDRef A_k_ = *A_k;
    A_ki = A_k_.begin();
    while (not_at(A_ki, A_k_.end())) {
      Int k = A_ki.column();
      Int i = A_ki.row();
      std::cout << "A(" << i << "," << k << ")" << *A_ki << std::endl;
      ++A_ki;
    }
    ++A_k;
  }
}

}

int
main()
{
  typedef mtl::matrix<double,
                  mtl::diagonal<>,
                  mtl::banded<mtl::external>,
                   mtl::column_major>::type DiagMatE;

  typedef mtl::matrix<double,
                 mtl::rectangle<>,
                 mtl::dense<mtl::external>,
                 mtl::column_major>::type MatrixE;

  typedef mtl::matrix<double,
                 mtl::rectangle<>,
                 mtl::dense<mtl::internal>,
                 mtl::column_major>::type MatrixI;

  int N = 3;
  double da [] = { 1, 3, 2, 2, 1, 2, 2, 2, 1 };
  double dc [] = { 1, 3, 2};

  MatrixE A(da, N, N);
  DiagMatE C(dc, N, N, 0, 0);
  MatrixI AxC(N,N);

  cout << "Full A:" << endl;
  mtl::print_all_matrix(A);
  cout << "Diag C:" << endl;
  mtl::print_all_banded(C,0,0);
  mtl::simple_print(C);
  cout << "Output AxC:" << endl;

  //put C as the first parameter in mult
  //since C is a banded matrix
  mtl::mult(C,A,AxC);
  mtl::print_all_matrix(AxC);

  return 0;
}


// Copyright (c) 2021 Graphcore Ltd. All rights reserved.
#include "poplin/Cholesky.hpp"
#include "poplin/MatMul.hpp"
#include "poplin/TriangularSolve.hpp"
#include <boost/assign/list_of.hpp>
#include <boost/optional.hpp>
#include <boost/optional/optional_io.hpp>
#include <boost/program_options.hpp>
#include <boost/random.hpp>
#include <boost/version.hpp>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <poplibs_support/TestDevice.hpp>
#include <poplibs_support/VectorUtils.hpp>
#include <poplibs_test/GeneralMatrixMultiply.hpp>
#include <poplibs_test/Util.hpp>
#include <poplin/MatMul.hpp>
#include <poplin/codelets.hpp>
#include <popops/codelets.hpp>
#include <poputil/TileMapping.hpp>
#include <sstream>

using namespace poplibs_support;

void printArray(std::string name, boost::multi_array<double, 3> a) {
  std::cout << name << ": " << std::endl;
  std::size_t ng = a.shape()[0];
  std::size_t nr = a.shape()[1];
  std::size_t nc = a.shape()[2];

  for (std::size_t g = 0; g < ng; g++) {
    std::cout << g << std::endl;
    for (std::size_t r = 0; r < nr; r++) {
      std::cout << " ";
      for (std::size_t c = 0; c < nc; c++) {
        std::cout << std::setw(10) << a[g][r][c];
      }
      std::cout << std::endl;
    }
    std::cout << std::endl;
  }
}

boost::multi_array<double, 3> createPositiveDefiniteMatrix(std::size_t batches,
                                                           std::size_t rank) {
  boost::multi_array<double, 3> l(boost::extents[batches][rank][rank]);
  boost::multi_array<double, 3> pd(boost::extents[batches][rank][rank]);

  std::mt19937 randomEngine;
  boost::random::uniform_real_distribution<> dist(0.01, 1.0);

  for (std::size_t b = 0; b < batches; b++) {
    for (std::size_t r = 0; r < rank; r++) {
      for (std::size_t c = 0; c < rank; c++) {
        double v = dist(randomEngine);
        l[b][r][c] = v;
      }
    }
  }

  poplibs_test::gemm::generalGroupedMatrixMultiply(l, l, pd, false, true);

  for (std::size_t b = 0; b < batches; b++) {
    for (std::size_t r = 0; r < rank; r++) {
      pd[b][r][r] += rank;
    }
  }

  return pd;
}

const boost::multi_array<double, 3>
maskTriangularMatrix(const boost::multi_array<double, 3> &m,
                     bool lower = true) {
  std::size_t batches = m.shape()[0];
  std::size_t rank = m.shape()[1];
  boost::multi_array<double, 3> tm(boost::extents[batches][rank][rank]);

  for (std::size_t b = 0; b < batches; b++) {
    for (std::size_t r = 0; r < rank; r++) {
      for (std::size_t c = 0; c < rank; c++) {
        if ((lower && c <= r) || (!lower && c >= r))
          tm[b][r][c] = m[b][r][c];
        else
          tm[b][r][c] = 0;
      }
    }
  }

  return tm;
}

int main(int argc, char **argv) try {
  namespace po = boost::program_options;

  DeviceType deviceType;
  boost::optional<unsigned> tilesPerIPU;

  po::options_description desc("Options");

  unsigned numBatches = 1;
  unsigned aRank;
  unsigned bRank = -1;
  bool leftSide = true;
  poplar::Type dataType;
  boost::optional<unsigned> blockSizeParam;
  bool lower = true;
  bool unitDiagonal = true;
  boost::optional<std::string> profileDir;

  // clang-format off
  desc.add_options()
    ("help,h", "produce help message")
    ("compile-only", "Stop after compilation; don't run the program")
    ("device-type",
      po::value<DeviceType>(&deviceType)->default_value(DeviceType::IpuModel2),
      deviceTypeHelp)
    ("profile", "Output profiling report to standard output")
    ("profile-dir",
     po::value<decltype(profileDir)>(&profileDir)
      ->default_value(boost::none),
     "Write profile files to the specified directory.")
    ("cholesky", "Run cholesky solver")
    ("ignore-data", "Don't upload and download the results from the device. "
     "Note that this means the result is not validated against the model.")
    ("tiles-per-ipu", po::value(&tilesPerIPU), "Number of tiles per IPU")
    ("data-type",
     po::value(&dataType)->required(),
     "Data Type")
    ("a-rank",
     po::value(&aRank)->required(),
     "Rank of the A matrix.")
    ("b-rank",
     po::value(&bRank),
     "Rank of the B matrix.")
    ("batches",
     po::value(&numBatches)->default_value(numBatches),
     "Number of batch dimensions.")
    ("left-side",
     po::value(&leftSide)->default_value(leftSide),
     "Left side - solve AX = B, XA = B overwise.")
    ("lower",
     po::value(&lower)->default_value(lower),
     "Generate lower A, upper A overwise.")
    ("unit-diagonal",
     po::value(&unitDiagonal)->default_value(unitDiagonal),
     "Assume A has unit diagonal.")
    ("block-size",
     po::value(&blockSizeParam),
     "Solver block size if specified, no block solver overwise.")
    ;
  // clang-format on

  po::variables_map vm;
  try {
    const po::positional_options_description p;
    po::store(
        po::command_line_parser(argc, argv).options(desc).positional(p).run(),
        vm);
    po::notify(vm);

    if (vm.count("help")) {
      std::cout << desc << "\n";
      return 1;
    }
  } catch (const boost::program_options::error &e) {
    std::cerr << e.what() << std::endl;
    return 1;
  }

  poplar::OptionFlags engineOptions;
  if (vm.count("profile") || profileDir) {
    engineOptions.set("debug.instrumentCompute", "true");
    if (profileDir) {
      engineOptions.set("autoReport.all", "true");
      engineOptions.set("autoReport.directory", *profileDir);
    }
  }

  const bool runCholesky = vm.count("cholesky");

  const bool ignoreData = vm.count("ignore-data");

  const unsigned numIPUs = 1;
  const bool compileIPUCode = true;
  auto device =
      tilesPerIPU
          ? createTestDevice(deviceType, numIPUs, *tilesPerIPU, compileIPUCode)
          : createTestDeviceFullSize(deviceType, numIPUs, compileIPUCode);

  const auto &target = device.getTarget();
  poplar::Graph graph(target);
  poplin::addCodelets(graph);
  popops::addCodelets(graph);

  poplin::matmul::PlanningCache cache;
  poplar::program::Sequence uploadProg, prog, downloadProg;
  poplar::OptionFlags options;

  poplar::DebugContext debugContext;

  if (blockSizeParam) {
    options.set("blockSize", std::to_string(*blockSizeParam));
  }

  if (runCholesky) {
    bRank = 0;
    unitDiagonal = false;

    if (!leftSide)
      throw poplar::poplar_error(
          "left-side must be true when using the cholesky solver.");
  } else if (bRank < 0) {
    throw poplar::poplar_error(
        "--b-rank is mandatory option for triangular solver");
  }

  std::vector<std::size_t> inputAShape{numBatches, aRank, aRank};
  std::vector<std::size_t> inputBShape{numBatches, leftSide ? aRank : bRank,
                                       leftSide ? bRank : aRank};

  std::vector<std::pair<poplin::MatMulParams, poplar::OptionFlags>>
      matmulOptPairs;
  if (runCholesky) {
    matmulOptPairs = poplin::getCholeskyMatMulPrePlanParameters(
        dataType, inputAShape, lower, options);
  } else {
    matmulOptPairs = poplin::getTriangularSolveMatMulPrePlanParameters(
        dataType, dataType, inputAShape, inputBShape, leftSide, lower, options);
  }

  std::set<poplin::MatMulPlanParams> params;
  for (auto &pair : matmulOptPairs)
    params.emplace(&target, pair.first, &pair.second);
  preplanMatMuls(params, cache);

  poplar::Tensor inputA;
  if (runCholesky) {
    inputA = poplin::createCholeskyInput(graph, dataType, inputAShape, lower,
                                         debugContext, options, &cache);
  } else {
    inputA = poplin::createTriangularSolveInputLHS(
        graph, dataType, dataType, inputAShape, inputBShape, leftSide,
        debugContext, options, &cache);
  }

  poplar::Tensor inputB;
  if (!runCholesky) {
    inputB = poplin::createTriangularSolveInputRHS(
        graph, dataType, dataType, inputAShape, inputBShape, leftSide,
        debugContext, options, &cache);
  }

  poplar::Tensor out;
  if (runCholesky) {
    poplin::choleskyInPlace(graph, inputA, lower, prog, debugContext, options,
                            &cache);
    out = inputA;
  } else {
    out = poplin::triangularSolve(graph, inputA, inputB, leftSide, lower,
                                  unitDiagonal, prog, debugContext, options,
                                  &cache);
  }

  std::vector<std::pair<std::string, char *>> tmap;
  std::unique_ptr<char[]> rawHostInputA, rawHostInputB, rawHostOutput,
      rawHostOutputT;
  if (!ignoreData) {
    rawHostInputA = poplibs_test::util::allocateHostMemoryForTensor(
        inputA, "A", graph, uploadProg, boost::none, tmap);

    if (!runCholesky) {
      rawHostInputB = poplibs_test::util::allocateHostMemoryForTensor(
          inputB, "B", graph, uploadProg, boost::none, tmap);
    }

    rawHostOutput = poplibs_test::util::allocateHostMemoryForTensor(
        out, "X", graph, boost::none, downloadProg, tmap);

    rawHostOutputT = poplibs_test::util::allocateHostMemoryForTensor(
        poplin::transposeGroupedMatrix(out), "XT", graph, boost::none,
        downloadProg, tmap);
  }

  poplar::Engine engine(graph, {uploadProg, prog, downloadProg}, engineOptions);

  if (vm.count("compile-only"))
    return 0;

  boost::multi_array<double, 3> hostInputA;
  boost::multi_array<double, 3> hostInputAFilled;
  boost::multi_array<double, 3> hostInputB;
  if (!ignoreData) {
    std::mt19937 randomEngine;
    boost::random::uniform_real_distribution<> dist(0.01, 1.0);
    boost::random::uniform_real_distribution<> diagonalDist(0.95, 1.05);

    poplibs_test::util::attachStreams(engine, tmap);

    if (runCholesky) {
      hostInputAFilled.resize(boost::extents[numBatches][aRank][aRank]);
      hostInputA.resize(boost::extents[numBatches][aRank][aRank]);

      hostInputAFilled = createPositiveDefiniteMatrix(numBatches, aRank);
      hostInputA = maskTriangularMatrix(hostInputAFilled, lower);
    } else {

      hostInputA.resize(boost::extents[numBatches][aRank][aRank]);
      for (std::size_t g = 0; g < numBatches; ++g) {
        auto matrix = hostInputA[g];
        for (std::size_t i = 0; i < aRank; ++i) {
          auto rows = matrix[i];
          for (std::size_t j = 0; j < aRank; ++j) {
            double value;
            if (i == j) {
              value = unitDiagonal ? 1.0 : diagonalDist(randomEngine);
            } else if ((i <= j && !lower) || (j <= i && lower)) {
              value = dist(randomEngine);
            } else {
              value = 0.0;
            }
            rows[j] = value;
          }
        }
      }
    }
    poplibs_test::util::copy(target, hostInputA, dataType, rawHostInputA.get());

    if (!runCholesky) {
      hostInputB.resize(
          boost::extents[numBatches][inputBShape[1]][inputBShape[2]]);
      poplibs_test::util::writeRandomValues(target, dataType, hostInputB, -1.0,
                                            1.0, randomEngine);
      poplibs_test::util::copy(target, hostInputB, dataType,
                               rawHostInputB.get());
    }
  }

  device.bind([&](const poplar::Device &d) {
    engine.load(d);
    if (!ignoreData) {
      // upload
      engine.run(0);
    }

    // convolve
    engine.run(1);

    if (!ignoreData) {
      // download
      engine.run(2);
    }
  });

  bool matchesModel = true;
  if (!ignoreData) {
    uint64_t dim1, dim2;
    if (runCholesky) {
      dim1 = inputAShape[1];
      dim2 = inputAShape[2];
    } else {
      dim1 = inputBShape[1];
      dim2 = inputBShape[2];
    }

    boost::multi_array<double, 3> hostOutput(
        boost::extents[numBatches][dim1][dim2]);
    poplibs_test::util::copy(target, dataType, rawHostOutput.get(), hostOutput);

    boost::multi_array<double, 3> hostOutputT(
        boost::extents[numBatches][dim1][dim2]);
    poplibs_test::util::copy(target, dataType, rawHostOutputT.get(),
                             hostOutputT);

    boost::multi_array<double, 3> modelOutput(
        boost::extents[numBatches][dim1][dim2]);

    boost::multi_array<double, 3> *testOutput;

    if (runCholesky) {
      testOutput = &hostInputAFilled;

      if (lower)
        poplibs_test::gemm::generalGroupedMatrixMultiply(
            hostOutput, hostOutputT, modelOutput);
      else
        poplibs_test::gemm::generalGroupedMatrixMultiply(
            hostOutputT, hostOutput, modelOutput);
    } else {
      testOutput = &hostInputB;

      if (leftSide) {
        poplibs_test::gemm::generalGroupedMatrixMultiply(hostInputA, hostOutput,
                                                         modelOutput);
      } else {
        poplibs_test::gemm::generalGroupedMatrixMultiply(hostOutput, hostInputA,
                                                         modelOutput);
      }
    }

    const auto tolerance = dataType == poplar::HALF ? 0.3 : 0.001;
    matchesModel = poplibs_test::util::checkIsClose(
        "AX vs B: ", *testOutput, modelOutput, tolerance, tolerance);
  }

  if (deviceType != DeviceType::Cpu && vm.count("profile")) {
    engine.printProfileSummary(
        std::cout, poplar::OptionFlags{{"showExecutionSteps", "true"}});
  }

  if (!matchesModel) {
    std::cerr << "Validation failed\n";
    return 1;
  }

  return 0;
} catch (const poplar::graph_memory_allocation_error &e) {
  std::cerr << e.what() << std::endl;

  // this exit code has been marked as a "skip" for ctest.
  return 77;
}


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

#include <Eigen/Cholesky>

#include "lama/nlls/gauss_newton.h"

lama::GaussNewton::Options::Options()
{
    eps1 = 1e-4;
    eps2 = 1e-4;
}

lama::GaussNewton::GaussNewton(const Options& options)
    : opt_(options)
{}

void lama::GaussNewton::reset()
{
    stop_ = false;
}

Eigen::VectorXd lama::GaussNewton::step(const VectorXd& residuals, const MatrixXd& J)
{
    VectorXd g = J.transpose() * residuals;
    chi2_ = residuals.squaredNorm();

    double max_abs_g = g.lpNorm<Eigen::Infinity>();
    if (max_abs_g < opt_.eps1){
        stop_ = true;
        return VectorXd::Zero(J.cols());
    }

    MatrixXd A = J.transpose() * J;
    // Solve the system
    VectorXd h = A.selfadjointView<Eigen::Lower>().ldlt().solve(-g);

    double max_abs_h = h.lpNorm<Eigen::Infinity>();
    if (max_abs_h < opt_.eps2)
        stop_ = true;

    return h;
}

bool lama::GaussNewton::valid(const VectorXd& residuals)
{
    if (stop_) return true;

    double dF  = chi2_ - residuals.squaredNorm();
    if (dF > 0){
        return true;
    }

    stop_ = true;
    return false;
}

bool lama::GaussNewton::stop()
{
    return stop_;
}


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 2018 - 2020 by the deal.II authors
 *
 * This file is part of the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE.md at
 * the top level directory of deal.II.
 *
 * ---------------------------------------------------------------------
 */

// Solve Laplacian using SIPG + mesh_loop + ScratchData

#include <deal.II/numerics/data_out.h>
#include <deal.II/base/function_parser.h>
#include <deal.II/base/patterns.h>
#include <deal.II/base/thread_management.h>

#include <deal.II/dofs/dof_tools.h>

#include <deal.II/fe/fe_dgq.h>

#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_out.h>
#include <deal.II/grid/grid_tools.h>
#include <deal.II/grid/grid_tools_cache.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>

#include <deal.II/lac/affine_constraints.h>
#include <deal.II/lac/sparse_direct.h>

#include <deal.II/meshworker/copy_data.h>
#include <deal.II/meshworker/mesh_loop.h>
#include <deal.II/meshworker/scratch_data.h>

#include <deal.II/numerics/data_out.h>
#include <deal.II/numerics/vector_tools.h>

#include <cmath>
#include <fstream>
#include <iostream>
#include <unordered_map>

using namespace dealii;
using namespace MeshWorker;

template <int dim, int spacedim>
void test()
{
  Triangulation<dim, spacedim> tria;
  FE_DGQ<dim, spacedim> fe(1);
  DoFHandler<dim, spacedim> dh(tria);

  FunctionParser<spacedim> rhs_function("1");
  FunctionParser<spacedim> boundary_function("0");

  AffineConstraints<double> constraints;
  constraints.close();

  GridGenerator::hyper_cube(tria);
  tria.refine_global(4);
  tria.execute_coarsening_and_refinement();
  dh.distribute_dofs(fe);

  SparsityPattern sparsity;

  {
    DynamicSparsityPattern dsp(dh.n_dofs(), dh.n_dofs());
    DoFTools::make_flux_sparsity_pattern(dh, dsp);
    sparsity.copy_from(dsp);
  }

  SparseMatrix<double> matrix;
  matrix.reinit(sparsity);

  Vector<double> solution(dh.n_dofs());
  Vector<double> rhs(dh.n_dofs());

  QGauss<dim> quad(3);
  QGauss<dim - 1> face_quad(3);

  UpdateFlags cell_flags = update_values | update_gradients |
                           update_quadrature_points | update_JxW_values;
  UpdateFlags face_flags = update_values | update_gradients |
                           update_quadrature_points |
                           update_face_normal_vectors | update_JxW_values;

  // Stabilization for SIPG
  double gamma = 1;

  using ScratchData = MeshWorker::ScratchData<dim, spacedim>;
  using CopyData = MeshWorker::CopyData<1 + GeometryInfo<dim>::faces_per_cell,
                                        1,
                                        1 + GeometryInfo<dim>::faces_per_cell>;

  ScratchData scratch(fe, quad, cell_flags, face_quad, face_flags);
  CopyData copy(fe.dofs_per_cell);

  auto cell = dh.begin_active();
  auto endc = dh.end();

  typedef decltype(cell) Iterator;

  auto cell_worker =
      [&rhs_function](const Iterator &cell, ScratchData &s, CopyData &c) {
        const auto &fev = s.reinit(cell);
        const auto &JxW = s.get_JxW_values();
        const auto &p = s.get_quadrature_points();

        c.local_dof_indices[0] = s.get_local_dof_indices();

        for (unsigned int q = 0; q < p.size(); ++q)
          for (unsigned int i = 0; i < fev.dofs_per_cell; ++i)
          {
            for (unsigned int j = 0; j < fev.dofs_per_cell; ++j)
            {
              c.matrices[0](i, j) +=
                  fev.shape_grad(i, q) * fev.shape_grad(j, q) * JxW[q];
            }
            c.vectors[0](i) +=
                fev.shape_value(i, q) * rhs_function.value(p[q]) * JxW[q];
          }
      };

  auto boundary_worker = [gamma, &boundary_function](const Iterator &cell,
                                                     const unsigned int &f,
                                                     ScratchData &s,
                                                     CopyData &c) {
    const auto &fev = s.reinit(cell, f);
    const auto &JxW = s.get_JxW_values();
    const auto &p = s.get_quadrature_points();
    const auto &n = s.get_normal_vectors();

    for (unsigned int q = 0; q < p.size(); ++q)
      for (unsigned int i = 0; i < fev.dofs_per_cell; ++i)
      {
        for (unsigned int j = 0; j < fev.dofs_per_cell; ++j)
        {
          c.matrices[0](i, j) +=
              (-fev.shape_grad(i, q) * n[q] * fev.shape_value(j, q) +
               -fev.shape_grad(j, q) * n[q] * fev.shape_value(i, q) +
               gamma / cell->face(f)->diameter() * fev.shape_value(i, q) *
                   fev.shape_value(j, q)) *
              JxW[q];
        }
        c.vectors[0](i) +=
            ((gamma / cell->face(f)->diameter() * fev.shape_value(i, q) -
              fev.shape_grad(i, q) * n[q]) *
             boundary_function.value(p[q])) *
            JxW[q];
      }
  };

  auto face_worker = [gamma](const Iterator &cell,
                             const unsigned int &f,
                             const unsigned int &sf,
                             const Iterator &ncell,
                             const unsigned int &nf,
                             const unsigned int &nsf,
                             ScratchData &s,
                             CopyData &c) {
    const auto &fev = s.reinit(cell, f, sf);
    const auto &JxW = s.get_JxW_values();
    const auto &nfev = s.reinit_neighbor(ncell, nf, nsf);

    c.local_dof_indices[f + 1] = s.get_neighbor_dof_indices();

    const auto &p = s.get_quadrature_points();
    const auto &n = s.get_normal_vectors();
    const auto &nn = s.get_neighbor_normal_vectors();

    const double gh = gamma / cell->face(f)->diameter();

    for (unsigned int q = 0; q < p.size(); ++q)
      for (unsigned int i = 0; i < fev.dofs_per_cell; ++i)
        for (unsigned int j = 0; j < fev.dofs_per_cell; ++j)
        {
          c.matrices[0](i, j) +=
              (-.5 * fev.shape_grad(i, q) * n[q] * fev.shape_value(j, q) +
               -.5 * fev.shape_value(i, q) * n[q] * fev.shape_grad(j, q) +
               gh * fev.shape_value(i, q) * fev.shape_value(j, q)) *
              JxW[q];

          c.matrices[f + 1](i, j) +=
              (-.5 * fev.shape_grad(i, q) * nn[q] * nfev.shape_value(j, q) +
               -.5 * fev.shape_value(i, q) * n[q] * nfev.shape_grad(j, q) -
               gh * fev.shape_value(i, q) * nfev.shape_value(j, q)) *
              JxW[q];
        }
  };

  auto copier = [&constraints, &matrix, &rhs](const CopyData &c) {
    constraints.distribute_local_to_global(
        c.matrices[0], c.vectors[0], c.local_dof_indices[0], matrix, rhs);

    for (unsigned int f = 0; f < GeometryInfo<dim>::faces_per_cell; ++f)
      constraints.distribute_local_to_global(c.matrices[1 + f],
                                             c.local_dof_indices[0],
                                             c.local_dof_indices[1 + f],
                                             matrix);
  };

  mesh_loop(cell,
            endc,
            cell_worker,
            copier,
            scratch,
            copy,
            assemble_own_cells | assemble_boundary_faces |
                assemble_own_interior_faces_both,
            boundary_worker,
            face_worker);

  SparseDirectUMFPACK inv;
  inv.initialize(matrix);

  inv.vmult(solution, rhs);
  constraints.distribute(solution);

  {
    DataOut<dim> data_out;
    data_out.attach_dof_handler(dh);
    data_out.add_data_vector(solution, "solution");
    data_out.build_patches();
    std::ofstream output("solution.vtu");
    data_out.write_vtu(output);
  }
  deallog << "Linfty norm of solution " << solution.linfty_norm() << std::endl;
}

int main()
{
  test<2, 2>();
}