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

#ifndef DUNE_GDT_OPERATORS_FV_REALIZABILITY_HH
#define DUNE_GDT_OPERATORS_FV_REALIZABILITY_HH

#include "config.h"

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

#include <dune/geometry/quadraturerules.hh>

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

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

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

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

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

namespace Dune {
namespace GDT {

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

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

  virtual ~LocalRealizabilityLimiterBase()
  {
  }

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

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

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

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

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

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

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

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

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

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

public:
  using typename BaseType::EntityType;

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

#if HAVE_QHULL

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

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

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

  ~ConvexHullLocalRealizabilityLimiter()
  {
    is_instantiated_ = false;
  }

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

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

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

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

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

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

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

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

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

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

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

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

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

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

  ~DgConvexHullLocalRealizabilityLimiter()
  {
    is_instantiated_ = false;
  }

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

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

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

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

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

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

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

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

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

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

#else // HAVE_QHULL

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

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

#endif // HAVE_QHULL

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

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

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

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

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

    RangeFieldType theta_entity(0.);

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

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

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

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

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

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

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

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

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

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

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

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

    return theta;
  }

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

#else // HAVE_CLP

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

#endif // HAVE_CLP

template <class LocalRealizabilityLimiterImp, class Traits>
class RealizabilityLimiter;

namespace internal {

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

} // namespace internal

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

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

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

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

} // namespace GDT
} // namespace Dune

#endif // DUNE_GDT_OPERATORS_FV_REALIZABILITY_HH


#include "self_adjoint_angular_flux.h"
#include <deal.II/dofs/dof_accessor.h>

template <int dim>
SelfAdjointAngularFlux<dim>::SelfAdjointAngularFlux(
    const std::string equation_name,
    const dealii::ParameterHandler &prm,
    std::shared_ptr<FundamentalData<dim>> &data_ptr)
    : EquationBase<dim>(equation_name, prm, data_ptr) {}

/*
 * =============================================================================
 * PUBLIC FUNCTIONS
 * =============================================================================
 */

template <int dim>
void SelfAdjointAngularFlux<dim>::AssembleLinearForms (const int &g) {
  if (have_reflective_bc_) {
    for (int dir = 0; dir < n_dir_; ++dir) {
      global_angular_flux_[this->GetCompInd(g, dir)] =
          *(mat_vec_->sys_flxes[equ_name_][this->GetCompInd(g, dir)]);
    }
  }
  EquationBase<dim>::AssembleLinearForms(g);
}

template<int dim>
void SelfAdjointAngularFlux<dim>::IntegrateBoundaryBilinearForm (
      typename dealii::DoFHandler<dim>::active_cell_iterator &,
      const int &,
      dealii::FullMatrix<double> &cell_matrix,
      const int &,
      const int &dir) {

  // Get the boundary ID and the normal vector
  const dealii::Tensor<1, dim> normal_vector = fvf_->normal_vector(0);

  double normal_dot_omega = normal_vector * omega_[dir];

  if (normal_dot_omega > 0) {
    // Integrate bilinear term
    for (int q = 0; q < n_qf_; ++q) {
      for (int i = 0; i < dofs_per_cell_; ++i) {
        for (int j = 0; j < dofs_per_cell_; ++j) {
          cell_matrix(i,j) += normal_dot_omega *
                              fvf_->shape_value(i, q) *
                              fvf_->shape_value(j, q) *
                              fvf_->JxW(q);
        }
      }
    }
  }
}

template<int dim>
void SelfAdjointAngularFlux<dim>::IntegrateBoundaryLinearForm (
      typename dealii::DoFHandler<dim>::active_cell_iterator &cell,
      const int &fn,/*face number*/
      dealii::Vector<double> &cell_rhs,
      const int &g,
      const int &dir) {

  // Get boundary id to determine if we have reflective BCs
  int boundary_id = cell->face(fn)->boundary_id();

  if (have_reflective_bc_ && is_reflective_bc_.at(boundary_id)) {

    // Get normal vector and dot product with omega
    const dealii::Tensor<1, dim> normal_vector = fvf_->normal_vector(0);
    double normal_dot_omega = normal_vector * omega_[dir];

    if (normal_dot_omega < 0) {
      // Retrieve previous iteration angular flux and get local values for the
      // reflected angle
      int reflected_dir = this->ref_dir_ind_[std::make_pair(boundary_id, dir)];

      std::vector<double> angular_flux(n_qf_);
      fvf_->get_function_values(
          global_angular_flux_[this->GetCompInd(g, reflected_dir)],
          angular_flux);

      for (int q = 0; q < n_qf_; ++q) {
        for (int i = 0; i < dofs_per_cell_; ++i) {
          cell_rhs(i) -= normal_dot_omega *
                         fvf_->shape_value(i, q) *
                         angular_flux[q] *
                         fvf_->JxW(q);
        }
      }
    }
  }
}

template<int dim>
void SelfAdjointAngularFlux<dim>::IntegrateCellBilinearForm (
      typename dealii::DoFHandler<dim>::active_cell_iterator &cell,
      dealii::FullMatrix<double> &cell_matrix,
      const int &g,
      const int &dir) {
  // Get material id for the given cell and cross-sections
  int material_id = cell->material_id();
  auto sigma_t = xsec_->sigt.at(material_id)[g];
  auto inv_sigma_t = xsec_->inv_sigt.at(material_id)[g];

  // Integrate and add both bilinear terms using precomputed values
  for (int q = 0; q < n_q_; ++q) {
    for (int i = 0; i < dofs_per_cell_; ++i) {
      for (int j = 0; j < dofs_per_cell_; ++j) {

        cell_matrix(i, j) += (pre_streaming_[{dir, q}](i, j) * inv_sigma_t
                              +
                              pre_collision_[q](i, j) * sigma_t) * fv_->JxW(q);
      }
    }
  }
}

template<int dim>
void SelfAdjointAngularFlux<dim>::IntegrateCellFixedLinearForm (
    typename dealii::DoFHandler<dim>::active_cell_iterator &cell,
    dealii::Vector<double> &cell_rhs,
    const int &g,
    const int &dir) {
  // get material id for the given cell
  int material_id = cell->material_id();

  // SAAF has two fixed source terms, the first is proportional to q/4pi, the
  // second contains an additional division by sigma_t. These two vectors hold
  // these values, respectively, for each quadrature point.
  std::vector<double> cell_q(n_q_);
  std::vector<double> cell_q_over_total(n_q_);

  if (!is_eigen_problem_) {

    // Fill the two vectors with the appropriate q/4pi and q/4pi*sigma_t values
    auto q_per_ster = xsec_->q_per_ster.at(material_id)[g];
    std::fill(cell_q.begin(), cell_q.end(),
              q_per_ster);
    std::fill(cell_q_over_total.begin(), cell_q_over_total.end(),
              q_per_ster * xsec_->inv_sigt.at(material_id)[g]);

  } else if (xsec_->is_material_fissile.at(material_id)) {

    // Fill the two vectors with the appropriate fission sources, summed
    // over all groups
    for (int group_in = 0; group_in < n_group_; ++group_in) {
      // Retrieve cell scalar flux, inverse sigma t, fission terms
      std::vector<double> group_cell_scalar_flux(n_q_);
      this->GetGroupCellScalarFlux(group_cell_scalar_flux, group_in);
      auto inv_sigma_t = xsec_->inv_sigt.at(material_id)[g];
      auto scaled_fission_transfer =
          scaled_fiss_transfer_.at(material_id)(group_in, g);

      for (int q = 0; q < n_q_; ++q) {
        cell_q[q] +=
            scaled_fission_transfer * group_cell_scalar_flux[q];
        cell_q_over_total[q] +=
            cell_q[q] * inv_sigma_t;
      }
    }
  }

  //Integrate and add both source terms
  for (int q = 0; q < n_q_; ++q) {

    cell_q[q] *= fv_->JxW(q);
    cell_q_over_total[q] *= fv_->JxW(q);

    for (int i = 0; i < dofs_per_cell_; ++i) {
      // First scattering term
      cell_rhs(i) += fv_->shape_value(i, q) * cell_q[q];
      // Second scattering term
      cell_rhs(i) +=
          omega_[dir] * fv_->shape_grad(i, q) * cell_q_over_total[q];
    }
  }
}

template<int dim>
void SelfAdjointAngularFlux<dim>::IntegrateScatteringLinearForm (
      typename dealii::DoFHandler<dim>::active_cell_iterator &cell,
      dealii::Vector<double> &cell_rhs,
      const int &g,
      const int &dir) {
  // Get material id for the given cell:
  int material_id = cell->material_id();

  // SAAF has two scattering terms, the first is proportional to scalar flux
  // time sigma_s over 4pi, the second has an additional division by sigma_t.
  // These two vectors hold those values, respectively,  at each quadrature
  // point:
  std::vector<double> cell_scatter_flux(n_q_);
  std::vector<double> cell_scatter_over_total_flux(n_q_);

  // Iterate over groups to populate cell_scatter_flux
  for (int group_in = 0; group_in < n_group_; ++group_in) {
    std::vector<double> group_cell_scalar_flux(n_q_);
    this->GetGroupCellScalarFlux(group_cell_scalar_flux, group_in);

    // Get needed cross-sections:
    auto sigma_s_per_ster = xsec_->sigs_per_ster.at(material_id)(group_in, g);
    auto inv_sigma_t = xsec_->inv_sigt.at(material_id)[g];

    // Fold group cell scalar flux into total cell scalar flux and multiply by
    // appropriate cross-sections:
    for (int q = 0; q < n_q_; ++q) {
      cell_scatter_flux[q] += sigma_s_per_ster * group_cell_scalar_flux[q];
      cell_scatter_over_total_flux[q] = cell_scatter_flux[q] * inv_sigma_t;
    }
  }

  // Integrate and add both scattering terms
  for (int q = 0; q < n_q_; ++q) {
    cell_scatter_flux[q] *= fv_->JxW(q);
    cell_scatter_over_total_flux[q] *= fv_->JxW(q);
    for (int i = 0; i < dofs_per_cell_; ++i) {
      // First scattering term
      cell_rhs(i) += fv_->shape_value(i, q) * cell_scatter_flux[q];
      // Second scattering term
      cell_rhs(i) +=
          omega_[dir] * fv_->shape_grad(i, q) * cell_scatter_over_total_flux[q];
    }
  }
}

template <int dim>
void SelfAdjointAngularFlux<dim>::PreassembleCellMatrices () {
  // Reinitialize FEM values to an arbitrary cell (the first one) in the list of
  // local cells.
  fv_->reinit(dat_ptr_->local_cells[0]);

  // For each quadrature angle, generate the Collision and Streaming matrices
  for (int q = 0; q < n_q_; ++q) {

    dealii::FullMatrix<double> temp_matrix(dofs_per_cell_, dofs_per_cell_);

    for (int i = 0; i < dofs_per_cell_; ++i) {
      for (int j = 0; j < dofs_per_cell_; ++j) {
        temp_matrix(i,j) =
            (fv_->shape_value(i,q) * fv_->shape_value(j,q));
      }
    }

    pre_collision_[q] = temp_matrix;

    temp_matrix = 0;

    // Streaming terms also depend on direction
    for (int dir = 0; dir < n_dir_; ++dir) {
      for (int i = 0; i < dofs_per_cell_; ++i) {
        for (int j = 0; j < dofs_per_cell_; ++j) {
          temp_matrix(i,j) =
              (fv_->shape_grad(i,q) * omega_[dir])
              *
              (fv_->shape_grad(j,q) * omega_[dir]);
        }
      }
      pre_streaming_[{dir, q}] = temp_matrix;
    }
  }
}

/*
 * =============================================================================
 * PROTECTED FUNCTIONS
 * =============================================================================
 */

template <int dim>
void SelfAdjointAngularFlux<dim>::GetGroupCellScalarFlux
(std::vector<double> &to_fill, int group) {

  // Get the global scalar flux for the current group
  auto & group_global_scalar_flux =
      mat_vec_->moments[equ_name_][std::make_tuple(group, 0, 0)];
  // Evaluate the global scalar flux at the quadrature points of the current
  // cell and store in return_vector (dealii function in FEValuesBase)
  fv_->get_function_values(group_global_scalar_flux, to_fill);

}

template class SelfAdjointAngularFlux<1>;
template class SelfAdjointAngularFlux<2>;
template class SelfAdjointAngularFlux<3>;


/*
 Copyright (C) 2016 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <boost/algorithm/string.hpp>
#include <ored/utilities/dategrid.hpp>
#include <ored/utilities/log.hpp>
#include <ored/utilities/parsers.hpp>
#include <ql/settings.hpp>
#include <ql/time/daycounters/actualactual.hpp>

using namespace QuantLib;
using namespace std;

namespace ore {
namespace data {

DateGrid::DateGrid()
    : dates_(1, Settings::instance().evaluationDate()), tenors_(1, 0 * Days), times_(1, 0.0),
      timeGrid_(times_.begin(), times_.end()), isValuationDate_(1, true), isCloseOutDate_(1, false) {}

DateGrid::DateGrid(const string& grid, const QuantLib::Calendar& gridCalendar, const QuantLib::DayCounter& dayCounter)
    : calendar_(gridCalendar), dayCounter_(dayCounter) {

    if (grid == "ALPHA") {
        // ALPHA is
        // quarterly up to 10Y,
        // annual up to 30Y,
        // quinquennial up to 100Y
        for (Size i = 1; i < 40; i++) { // 3M up to 39*3M = 117M = 9Y9M
            Period p(i * 3, Months);
            p.normalize();
            tenors_.push_back(p);
        }
        for (Size i = 10; i < 30; i++) // 10Y up to 29Y
            tenors_.push_back(Period(i, Years));
        for (Size i = 30; i < 105; i += 5) // 30Y up to 100Y
            tenors_.push_back(Period(i, Years));
    } else if (grid == "BETA") {
        // BETA is
        // monthly up to 10Y,
        // quarterly up to 20Y,
        // annually up to 50Y,
        // quinquennial up to 100Y
        for (Size i = 1; i < 119; i++) {
            Period p = i * Months;
            p.normalize();
            tenors_.push_back(p);
        }
        for (Size i = 40; i < 80; i++) {
            Period p = i * 3 * Months;
            p.normalize();
            tenors_.push_back(p);
        }
        for (Size i = 20; i < 50; i++)
            tenors_.push_back(i * Years);
        for (Size i = 50; i <= 100; i += 5)
            tenors_.push_back(i * Years);
    } else { // uniform grid of format "numPillars,spacing" (e.g. 40,1M)
        vector<string> tokens;
        boost::split(tokens, grid, boost::is_any_of(","));
        if (tokens.size() <= 2) {
            // uniform grid of format "numPillars,spacing" (e.g. 40,1M)
            Period gridTenor = 1 * Years; // default
            Size gridSize = atoi(tokens[0].c_str());
            QL_REQUIRE(gridSize > 0, "Invalid DateGrid string " << grid);
            if (tokens.size() == 2)
                gridTenor = data::parsePeriod(tokens[1]);
            if (gridTenor == Period(1, Days)) {
                // we have a daily grid. Period and Calendar are not consistant with
                // working & actual days, so we set the tenor grid
                Date today = Settings::instance().evaluationDate();
                Date d = today;
                for (Size i = 0; i < gridSize; i++) {
                    d = gridCalendar.advance(d, Period(1, Days), Following); // next working day
                    Size n = d - today;
                    tenors_.push_back(Period(n, Days));
                }
            } else {
                for (Size i = 0; i < gridSize; i++)
                    tenors_.push_back((i + 1) * gridTenor);
            }
        } else {
            // New style : 1D,2D,1W,2W,3Y,5Y,....
            for (Size i = 0; i < tokens.size(); i++)
                tenors_.push_back(data::parsePeriod(tokens[i]));
        }
    }
    buildDates(gridCalendar, dayCounter);
}

DateGrid::DateGrid(const vector<Period>& tenors, const QuantLib::Calendar& gridCalendar,
                   const QuantLib::DayCounter& dayCounter)
    : calendar_(gridCalendar), dayCounter_(dayCounter), tenors_(tenors) {
    QL_REQUIRE(!tenors_.empty(), "DateGrid requires a non-empty vector of tenors");
    QL_REQUIRE(is_sorted(tenors_.begin(), tenors_.end()),
               "Construction of DateGrid requires a sorted vector of unique tenors");
    buildDates(gridCalendar, dayCounter);
}

DateGrid::DateGrid(const vector<Date>& dates, const QuantLib::Calendar& cal, const DayCounter& dayCounter)
    : calendar_(cal), dayCounter_(dayCounter), dates_(dates) {
    QL_REQUIRE(!dates_.empty(), "Construction of DateGrid requires a non-empty vector of dates");
    QL_REQUIRE(is_sorted(dates_.begin(), dates_.end()),
               "Construction of DateGrid requires a sorted vector of unique dates");
    Date today = Settings::instance().evaluationDate();
    QL_REQUIRE(today < dates_.front(),
               "Construction of DateGrid requires first element to be strictly greater than today");

    // Populate the tenors, times and timegrid
    tenors_.resize(dates_.size());
    times_.resize(dates_.size());
    for (Size i = 0; i < dates_.size(); i++) {
        tenors_[i] = (dates_[i] - today) * Days;
        times_[i] = dayCounter.yearFraction(today, dates_[i]);
    }
    timeGrid_ = TimeGrid(times_.begin(), times_.end());
    isValuationDate_ = std::vector<bool>(dates_.size(), true);
    isCloseOutDate_ = std::vector<bool>(dates_.size(), false);

    // Log the date grid
    log();
}

void DateGrid::buildDates(const QuantLib::Calendar& cal, const QuantLib::DayCounter& dc) {
    // build dates from tenors
    // this is called by both constructors
    dates_.resize(tenors_.size());
    Date today = Settings::instance().evaluationDate();
    for (Size i = 0; i < tenors_.size(); i++) {
        if (tenors_[i].units() == Days)
            dates_[i] = cal.adjust(today + tenors_[i]);
        else
            dates_[i] = cal.advance(today, tenors_[i], Following, false);
        if (i > 0) {
            QL_REQUIRE(dates_[i] >= dates_[i - 1], "DateGrid::buildDates(): tenors must be monotonic");
            if (dates_[i] == dates_[i - 1]) {
                dates_.erase(std::next(dates_.begin(), i));
                tenors_.erase(std::next(tenors_.begin(), i));
                --i;
            }
        }
    }

    // Build times
    times_.resize(dates_.size());
    for (Size i = 0; i < dates_.size(); i++)
        times_[i] = dc.yearFraction(today, dates_[i]);

    timeGrid_ = TimeGrid(times_.begin(), times_.end());
    isValuationDate_ = std::vector<bool>(dates_.size(), true);
    isCloseOutDate_ = std::vector<bool>(dates_.size(), false);

    // Log the date grid
    log();
}

void DateGrid::log() {
    DLOG("DateGrid constructed, size = " << size());
    for (Size i = 0; i < tenors_.size(); i++)
        DLOG("[" << setw(2) << i << "] Tenor:" << tenors_[i] << ", Date:" << io::iso_date(dates_[i])
                 << ", Valuation:" << isValuationDate_[i] << ", CloseOut:" << isCloseOutDate_[i]);
}

void DateGrid::truncate(const Date& d, bool overrun) {
    if (d >= dates_.back())
        return; // no need for any truncation
    DLOG("Truncating DateGrid beyond " << QuantLib::io::iso_date(d));
    vector<Date>::iterator it = std::upper_bound(dates_.begin(), dates_.end(), d);
    if (overrun)
        ++it;
    dates_.erase(it, dates_.end());
    tenors_.resize(dates_.size());
    times_.resize(dates_.size());
    timeGrid_ = TimeGrid(times_.begin(), times_.end());
    DLOG("DateGrid size now " << dates_.size());
}

void DateGrid::truncate(Size len) {
    // Truncate grid up length len
    if (dates_.size() > len) {
        DLOG("Truncating DateGrid, removing elements " << dates_[len] << " to " << dates_.back());
        dates_.resize(len);
        tenors_.resize(len);
        times_.resize(len);
        timeGrid_ = TimeGrid(times_.begin(), times_.end());
        isValuationDate_.resize(len);
        isCloseOutDate_.resize(len);
        DLOG("DateGrid size now " << dates_.size());
    }
}

void DateGrid::addCloseOutDates(const QuantLib::Period& p) {
    if (p == QuantLib::Period(0, QuantLib::Days)) {
        for (Size i = 0; i < dates_.size(); ++i) {
            if (i == 0) {
                isCloseOutDate_.front() = false;
                isValuationDate_.front() = true;
            } else if (i == dates_.size() - 1) {
                isCloseOutDate_.back() = true;
                isValuationDate_.back() = false;
            } else {
                isCloseOutDate_[i] = true;
                isValuationDate_[i] = true;
            }
        }
    } else {
        std::vector<Date> tmpDates;
        std::vector<bool> tmpIsCloseOutDate, tmpIsValuationDate;
        for (Size i = 0; i < dates_.size(); ++i) {
            Date c;
            if (p.units() == Days)
                c = calendar_.adjust(dates_[i] + p);
            else
                c = calendar_.advance(dates_[i], p, Following, false);
            if (i < dates_.size() - 1) {
                // adjust the grid to ensure no overlap in valuation and closeout dates
                if (c >= dates_[i + 1]) {
                    dates_[i + 1] = calendar_.advance(c, QuantLib::Period(1, QuantLib::Days));
                    std::cout << QuantLib::io::iso_date(dates_[i + 1]) << std::endl;
                    // check that the grid is still monotonic
                    if ((i + 2) < dates_.size()) {
                        QL_REQUIRE(dates_[i + 1] < dates_[i + 2],
                                   "date grid is no longer monotonic: " << dates_[i + 1] << ", " << dates_[i + 2]);
                    }
                }
                QL_REQUIRE(c < dates_[i + 1],
                           "close out date " << c << " does not lie before next grid date " << dates_[i + 1]);
            }
            tmpDates.push_back(dates_[i]);
            tmpDates.push_back(c);
            tmpIsCloseOutDate.push_back(false);
            tmpIsCloseOutDate.push_back(true);
            tmpIsValuationDate.push_back(true);
            tmpIsValuationDate.push_back(false);
        }
        dates_ = tmpDates;
        isCloseOutDate_ = tmpIsCloseOutDate;
        isValuationDate_ = tmpIsValuationDate;
        // FIXME ... (is that needed anywhere ?)
        tenors_ = std::vector<QuantLib::Period>(dates_.size(), 0 * Days);
        times_.resize(dates_.size());
        Date today = Settings::instance().evaluationDate();
        for (Size i = 0; i < dates_.size(); i++)
            times_[i] = dayCounter_.yearFraction(today, dates_[i]);
        timeGrid_ = TimeGrid(times_.begin(), times_.end());
    }
    // Log Grid
    DLOG("Added Close Out Dates to DateGrid , size = " << size());
    log();
}

std::vector<QuantLib::Date> DateGrid::valuationDates() const {
    std::vector<Date> res;
    for (Size i = 0; i < dates_.size(); ++i) {
        if (isValuationDate_[i])
            res.push_back(dates_[i]);
    }
    return res;
}

std::vector<QuantLib::Date> DateGrid::closeOutDates() const {
    std::vector<Date> res;
    for (Size i = 0; i < dates_.size(); ++i) {
        if (isCloseOutDate_[i])
            res.push_back(dates_[i]);
    }
    return res;
}

QuantLib::TimeGrid DateGrid::valuationTimeGrid() const {
    std::vector<Real> times;
    Date today = Settings::instance().evaluationDate();
    for (Size i = 0; i < dates_.size(); ++i) {
        if (isValuationDate_[i])
            times.push_back(dayCounter_.yearFraction(today, dates_[i]));
    }
    return TimeGrid(times.begin(), times.end());
}

QuantLib::TimeGrid DateGrid::closeOutTimeGrid() const {
    std::vector<Real> times;
    Date today = Settings::instance().evaluationDate();
    for (Size i = 0; i < dates_.size(); ++i) {
        if (isCloseOutDate_[i])
            times.push_back(dayCounter_.yearFraction(today, dates_[i]));
    }
    return TimeGrid(times.begin(), times.end());
}

boost::shared_ptr<DateGrid> generateShiftedDateGrid(const boost::shared_ptr<DateGrid>& dg,
                                                    const QuantLib::Period& shift) {
    DLOG("Building shifted date grid with shift of " << shift);
    vector<Date> defaultDates = dg->dates();
    vector<Date> closeOutDates;
    for (auto d : defaultDates) {
        Date closeOut = dg->calendar().adjust(d + shift);
        closeOutDates.push_back(closeOut);
    }
    boost::shared_ptr<DateGrid> newDg = boost::make_shared<DateGrid>(closeOutDates, dg->calendar(), dg->dayCounter());
    return newDg;
}

boost::shared_ptr<DateGrid> combineDateGrids(const boost::shared_ptr<DateGrid>& dg1,
                                             const boost::shared_ptr<DateGrid>& dg2) {
    DLOG("Combining date grids");
    vector<Date> combinedVec;
    vector<Date> dates1 = dg1->dates();
    vector<Date> dates2 = dg2->dates();
    combinedVec.reserve(dates1.size() + dates2.size());
    combinedVec.insert(combinedVec.end(), dates1.begin(), dates1.end());
    combinedVec.insert(combinedVec.end(), dates2.begin(), dates2.end());
    std::sort(combinedVec.begin(), combinedVec.end());
    auto last = std::unique(combinedVec.begin(), combinedVec.end());
    combinedVec.erase(last, combinedVec.end());
    // FIXME: Check that grid calendars and day counters match?
    boost::shared_ptr<DateGrid> newDg = boost::make_shared<DateGrid>(combinedVec, dg1->calendar(), dg1->dayCounter());
    return newDg;
}

} // namespace data
} // namespace ore


#pragma once

#include <mtao/geometry/volume.h>
#include <Eigen/Sparse>
#include <mtao/geometry/mesh/compactify.hpp>
#include <mtao/eigen/stack.h>
#include <vector>
#include "mandoline/cutface.hpp"
#include "cutmesh.pb.h"
namespace mandoline {
struct CutCell : public std::map<int, bool> {

    using coord_type = std::array<int, 3>;
    int index = -1;
    int region = -1;
    coord_type grid_cell;

    operator std::string() const;
    void serialize(protobuf::CutCell &) const;
    static CutCell from_proto(const protobuf::CutCell &);

    std::vector<Eigen::Triplet<double>> boundary_triplets() const;
    mtao::ColVecs3i triangulated(const std::vector<CutFace<3>> &Fs) const;
    std::tuple<mtao::ColVecs3d, mtao::ColVecs3i> triangulated_with_additional_vertices(const std::vector<CutFace<3>> &Fs, int vertex_offset) const;

    /*
           coord_type grid_cell(const std::vector<CutFace<3>>& F) const;
           */

    template<typename Derived>
    double volume(const Eigen::MatrixBase<Derived> &V, const mtao::vector<CutFace<3>> &Fs) const;
    double volume(const mtao::VecXd &face_brep_vols) const;
    template<typename Derived>
    mtao::Vec3d centroid(const Eigen::MatrixBase<Derived> &V, const mtao::vector<CutFace<3>> &Fs) const;
    mtao::Vec3d moment(const mtao::ColVecs3d &face_brep_cents) const;
    template<typename Derived>
    std::tuple<mtao::ColVecs3d, mtao::ColVecs3i> get_mesh(const Eigen::MatrixBase<Derived> &V, const mtao::vector<CutFace<3>> &Fs) const;

    template<typename Derived, typename VecType>
    bool contains(const Eigen::MatrixBase<Derived> &V, const mtao::vector<CutFace<3>> &Fs, const Eigen::MatrixBase<VecType> &v) const;

    template<typename Derived, typename VecType>
    double solid_angle(const Eigen::MatrixBase<Derived> &V, const mtao::vector<CutFace<3>> &Fs, const Eigen::MatrixBase<VecType> &v) const;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
};

template<typename Derived>
double CutCell::volume(const Eigen::MatrixBase<Derived> &V, const mtao::vector<CutFace<3>> &Fs) const {

    double vol = 0;

    for (auto &&[f, b] : *this) {
        auto &&F = Fs[f];
        double sign = b ? 1 : -1;
        vol += sign * Fs[f].brep_volume(V);
    }
    return vol;
}
template<typename Derived>
mtao::Vec3d CutCell::centroid(const Eigen::MatrixBase<Derived> &V, const mtao::vector<CutFace<3>> &Fs) const {

    double vol = 0;
    mtao::Vec3d mom = mtao::Vec3d::Zero();

    for (auto &&[f, b] : *this) {
        auto &&F = Fs[f];
        double sign = b ? 1 : -1;
        vol += sign * Fs[f].brep_volume(V);
        mom += sign * Fs[f].brep_centroid(V);
    }
    mom /= vol;
    return mom;
}
template<typename Derived>
std::tuple<mtao::ColVecs3d, mtao::ColVecs3i> CutCell::get_mesh(const Eigen::MatrixBase<Derived> &V, const mtao::vector<CutFace<3>> &Fs) const {
    std::vector<mtao::ColVecs3i> FF;
    for (auto &&[fidx, s] : *this) {
        auto &&f = Fs[fidx];
        assert(bool(f.triangulation));
        FF.push_back(*f.triangulation);
    }
    auto F = mtao::eigen::hstack_iter(FF.begin(), FF.end());
    return mtao::geometry::mesh::compactify(V, F);
}

template<typename Derived, typename VecType>
bool CutCell::contains(const Eigen::MatrixBase<Derived> &V, const mtao::vector<CutFace<3>> &Fs, const Eigen::MatrixBase<VecType> &v) const {
    //> 4 * M_PI, but with some slack
    return solid_angle(V, Fs, v) > .5;
}
template<typename Derived, typename VecType>
double CutCell::solid_angle(const Eigen::MatrixBase<Derived> &V, const mtao::vector<CutFace<3>> &Fs, const Eigen::MatrixBase<VecType> &v) const {
    double sa = 0;
    for (auto &&[fid, sgn] : *this) {
        sa += (sgn ? -1 : 1) * Fs[fid].solid_angle(V, v);
    }
    return sa;
}
}// namespace mandoline


#include <df/optimization/deformationGraphRegularization.h>

#include <vector>
#include <Eigen/Core>

#include <df/util/dualQuaternion.h> // TODO
#include <df/util/typeList.h>

namespace df {

template <typename Scalar, typename ScalarOpt, int Options, template <typename,int...> class TransformT>
inline void computeEdgeResidual(const Eigen::Matrix<Scalar,3,1,Eigen::DontAlign> & vertexA, const TransformT<Scalar,Options> & transformA,
                                const Eigen::Matrix<Scalar,3,1,Eigen::DontAlign> & vertexB, const TransformT<Scalar,Options> & transformB,
                                Eigen::Matrix<ScalarOpt,3,1,Eigen::DontAlign> & residual,
                                Eigen::Matrix<Scalar,3,1,Eigen::DontAlign> & vertexADisplacementByTransformA,
                                Eigen::Matrix<Scalar,3,1,Eigen::DontAlign> & vertexADisplacementByTransformB,
                                Eigen::Matrix<Scalar,3,1,Eigen::DontAlign> & vertexARelativeToVertexB) {

    typedef Eigen::Matrix<Scalar,3,1,Eigen::DontAlign> Vec3;

    vertexADisplacementByTransformA = transformA.translation();

    vertexARelativeToVertexB = vertexA - vertexB;

    vertexADisplacementByTransformB = transformB * vertexARelativeToVertexB;

    const Vec3 vertexAWarpedByTransformA = vertexA + vertexADisplacementByTransformA;

    const Vec3 vertexAWarpedByTransformB = vertexB + vertexADisplacementByTransformB;

    residual = (vertexAWarpedByTransformA - vertexAWarpedByTransformB).template cast<ScalarOpt>();

}

template <typename Scalar, typename ScalarOpt, template <typename,int...> class TransformT>
inline void fillEdgeJacobianAndResidual(const Eigen::Matrix<Scalar,3,1,Eigen::DontAlign> & vertexA, const TransformT<Scalar> & transformA,
                                        const Eigen::Matrix<Scalar,3,1,Eigen::DontAlign> & vertexB, const TransformT<Scalar> & transformB,
                                        internal::JacobianAndResidual<ScalarOpt,3,12> & jacobianAndResidual,
                                        const IntToType<internal::TransformUpdateLeftMultiply> ) {

    typedef Eigen::Matrix<Scalar,3,1,Eigen::DontAlign> Vec3;

    Vec3 vertexADisplacementByTransformA, vertexADisplacementByTransformB, vertexARelativeToVertexB;

    computeEdgeResidual(vertexA,transformA,vertexB,transformB,jacobianAndResidual.r,
                        vertexADisplacementByTransformA, vertexADisplacementByTransformB,
                        vertexARelativeToVertexB);

    // the first block is for vertex A
    jacobianAndResidual.J.template block<3,6>(0,0) << 1, 0, 0,  0, vertexADisplacementByTransformA(2), -vertexADisplacementByTransformA(1),
                                                      0, 1, 0,  -vertexADisplacementByTransformA(2), 0, vertexADisplacementByTransformA(0),
                                                      0, 0, 1,  vertexADisplacementByTransformA(1), -vertexADisplacementByTransformA(0), 0;

    // the second block is for vertex B
    jacobianAndResidual.J.template block<3,6>(0,6) << -1,  0,  0,  0, -vertexADisplacementByTransformB(2), vertexADisplacementByTransformB(1),
                                                       0, -1,  0,  vertexADisplacementByTransformB(2), 0, -vertexADisplacementByTransformB(0),
                                                       0,  0, -1,  -vertexADisplacementByTransformB(1), vertexADisplacementByTransformB(0), 0;

}

template <typename Scalar, typename ScalarOpt, template <typename,int...> class TransformT>
inline void fillEdgeJacobianAndResidual(const Eigen::Matrix<Scalar,3,1,Eigen::DontAlign> & vertexA, const TransformT<Scalar> & transformA,
                                        const Eigen::Matrix<Scalar,3,1,Eigen::DontAlign> & vertexB, const TransformT<Scalar> & transformB,
                                        internal::JacobianAndResidual<ScalarOpt,3,12> & jacobianAndResidual,
                                        const IntToType<internal::TransformUpdateRightMultiply> ) {

    typedef Eigen::Matrix<Scalar,3,1,Eigen::DontAlign> Vec3;
    typedef Eigen::Matrix<ScalarOpt,3,3,Eigen::DontAlign> Mat3;

    Vec3 vertexADisplacementByTransformA, vertexADisplacementByTransformB, vertexARelativeToVertexB;

    computeEdgeResidual(vertexA,transformA,vertexB,transformB,jacobianAndResidual.r,
                        vertexADisplacementByTransformA, vertexADisplacementByTransformB,
                        vertexARelativeToVertexB);

    // the first block is for vertex A
    jacobianAndResidual.J.template block<3,3>(0,0) = transformA.rotationMatrix().template cast<ScalarOpt>();
    jacobianAndResidual.J.template block<3,3>(0,3) = Mat3::Zero();

    // the second block is for vertex B
    jacobianAndResidual.J.template block<3,3>(0,6) = -transformB.rotationMatrix().template cast<ScalarOpt>();

    Mat3 partial;
    partial << 0, vertexARelativeToVertexB(2), -vertexARelativeToVertexB(1),
               -vertexARelativeToVertexB(2), 0, vertexARelativeToVertexB(0),
               vertexARelativeToVertexB(1), -vertexARelativeToVertexB(0), 0;

    jacobianAndResidual.J.template block<3,3>(0,9) = -transformB.rotationMatrix().template cast<ScalarOpt>()*partial;

}

template <typename Scalar, uint Rows>
struct JacobianFillColUnroller {

    typedef Eigen::Triplet<Scalar> Triplet;

    inline static void fill(std::vector<Triplet> & globalJacobianTriplets,
                            const Eigen::Matrix<Scalar,Rows,1,Eigen::DontAlign> & localJacobianColumn,
                            const uint firstRowIndex, const uint columnIndex) {

//        std::cout << "colFill " << firstRowIndex << ", " << columnIndex << ":" << std::endl << localJacobianColumn << std::endl << std::endl;

        const Scalar & localVal = localJacobianColumn(0);

        if (localVal != Scalar(0)) {

            globalJacobianTriplets.push_back(Triplet(firstRowIndex,columnIndex,localVal));

        }

        JacobianFillColUnroller<Scalar,Rows-1>::fill(globalJacobianTriplets,
                                                     localJacobianColumn.template tail<Rows-1>(),
                                                     firstRowIndex+1,columnIndex);

    }

};

template <typename Scalar>
struct JacobianFillColUnroller<Scalar,0> {

    typedef Eigen::Triplet<Scalar> Triplet;

    inline static void fill(std::vector<Triplet> & /*globalJacobianTriplets*/,
                            const Eigen::Matrix<Scalar,0,1,Eigen::DontAlign> & /*localJacobianColumn*/,
                            const uint /*firstRowIndex*/, const uint /*columnIndex*/) { }

};

template <typename Scalar, uint Rows, uint Cols>
struct JacobianFillUnroller {

    typedef Eigen::Triplet<Scalar> Triplet;

    inline static void fill(std::vector<Triplet> & globalJacobianTriplets,
                            const Eigen::Matrix<Scalar,Rows,Cols,Eigen::DontAlign> & localJacobian,
                            const uint firstRowIndex, const uint firstColumnIndex) {

//        std::cout << "matFill " << firstRowIndex << ", " << firstColumnIndex << ":" << std::endl << localJacobian << std::endl << std::endl;

        JacobianFillColUnroller<Scalar,Rows>::fill(globalJacobianTriplets,
                                                   localJacobian.template block<Rows,1>(0,0),
                                                   firstRowIndex, firstColumnIndex);

        JacobianFillUnroller<Scalar,Rows,Cols-1>::fill(globalJacobianTriplets,
                                                       localJacobian.template block<Rows,Cols-1>(0,1),
                                                       firstRowIndex, firstColumnIndex + 1);

    }

};

template <typename Scalar, uint Rows>
struct JacobianFillUnroller<Scalar,Rows,0> {

    typedef Eigen::Triplet<Scalar> Triplet;

    inline static void fill(std::vector<Triplet> & /*globalJacobianTriplets*/,
                            const Eigen::Matrix<Scalar,Rows,0,Eigen::DontAlign> & /*localJacobian*/,
                            const uint /*firstRowIndex*/, const uint /*firstColumnIndex*/) { }

};

template <typename Scalar, typename ScalarOpt, template <typename,int...> class TransformT, internal::TransformUpdateMethod U>
void fillRegularizationResidualAndJacobianTripletList(std::vector<Eigen::Triplet<ScalarOpt> > & globalJacobianTripletList,
                                                      Eigen::Matrix<ScalarOpt,Eigen::Dynamic,1> & globalResidual,
                                                      const NonrigidTransformer<Scalar,TransformT> & transformer) {

    typedef typename NonrigidTransformer<Scalar,TransformT>::Transform Transform;
    typedef Eigen::Matrix<Scalar,3,1,Eigen::DontAlign> Vec3;

    static constexpr uint ResidualDim = 3;
    static constexpr uint BlockDim = 6;

    uint runningRowCounter = 0;

    uint lowerLevelParameterStart = 0;
    uint higherLevelParameterStart;

    for (uint level = 0; level < transformer.numRegularizationTreeLevels() - 1; ++level) {

        const uint numLowerLevelVertices = transformer.numVerticesAtLevel(level);

        higherLevelParameterStart = lowerLevelParameterStart + numLowerLevelVertices*BlockDim;

        for (uint lowerLevelIndex = 0; lowerLevelIndex < numLowerLevelVertices; ++lowerLevelIndex) {

            const Vec3 & lowerLevelVertex = transformer.deformationGraphVertices(level)[lowerLevelIndex];

            const Transform & lowerLevelTransform = transformer.transforms(level)[lowerLevelIndex];

            const uint numNeighbors = transformer.numHigherLevelNeighbors(level,lowerLevelIndex);

            for (uint k = 0; k < numNeighbors; ++k) {

                const uint higherLevelIndex = transformer.higherLevelNeighbors(level,lowerLevelIndex)[k];

                const Vec3 & higherLevelVertex = transformer.deformationGraphVertices(level+1)[higherLevelIndex];

                const Transform & higherLevelTransform = transformer.transforms(level+1)[higherLevelIndex];

                internal::JacobianAndResidual<ScalarOpt,ResidualDim,2*BlockDim> jacobianAndResidual;

                fillEdgeJacobianAndResidual(lowerLevelVertex,lowerLevelTransform,
                                            higherLevelVertex,higherLevelTransform,
                                            jacobianAndResidual,IntToType<U>());

//                std::cout << level << ", " << lowerLevelIndex << ", " << k << ": " << std::endl << jacobianAndResidual.J << std::endl << std::endl;

//                std::vector<Eigen::Triplet<ScalarOpt> > localTripletList;
//                JacobianFillUnroller<ScalarOpt,ResidualDim,BlockDim>::fill(localTripletList,jacobianAndResidual.J.template block<ResidualDim,BlockDim>(0,0),
//                                                                           0, lowerLevelParameterStart + lowerLevelIndex*BlockDim);

//                JacobianFillUnroller<ScalarOpt,ResidualDim,BlockDim>::fill(localTripletList,jacobianAndResidual.J.template block<ResidualDim,BlockDim>(0,BlockDim),
//                                                                           0, higherLevelParameterStart + higherLevelIndex*BlockDim);
//                Eigen::SparseMatrix<ScalarOpt> localJacobian(3,transformer.numVerticesTotal()*BlockDim);
//                localJacobian.setFromTriplets(localTripletList.begin(),localTripletList.end());
//                localJacobian.finalize();
//                std::cout << localJacobian.toDense() << std::endl << std::endl;

                JacobianFillUnroller<ScalarOpt,ResidualDim,BlockDim>::fill(globalJacobianTripletList,jacobianAndResidual.J.template block<ResidualDim,BlockDim>(0,0),
                                                                           runningRowCounter, lowerLevelParameterStart + lowerLevelIndex*BlockDim);

                JacobianFillUnroller<ScalarOpt,ResidualDim,BlockDim>::fill(globalJacobianTripletList,jacobianAndResidual.J.template block<ResidualDim,BlockDim>(0,BlockDim),
                                                                           runningRowCounter, higherLevelParameterStart + higherLevelIndex*BlockDim);

//                std::cout << "(" << runningRowCounter << " - " << (runningRowCounter + ResidualDim) << ") / " << globalResidual.rows() << std::endl;
                globalResidual.template block<ResidualDim,1>(runningRowCounter,0) = jacobianAndResidual.r;

                runningRowCounter += ResidualDim;

                fillEdgeJacobianAndResidual(higherLevelVertex,higherLevelTransform,
                                            lowerLevelVertex,lowerLevelTransform,
                                            jacobianAndResidual,IntToType<U>());

//                std::cout << level << ", " << lowerLevelIndex << ", " << k << ": " << std::endl << jacobianAndResidual.J << std::endl << std::endl;

                JacobianFillUnroller<ScalarOpt,ResidualDim,BlockDim>::fill(globalJacobianTripletList,jacobianAndResidual.J.template block<ResidualDim,BlockDim>(0,BlockDim),
                                                                           runningRowCounter, lowerLevelParameterStart + lowerLevelIndex*BlockDim);

                JacobianFillUnroller<ScalarOpt,ResidualDim,BlockDim>::fill(globalJacobianTripletList,jacobianAndResidual.J.template block<ResidualDim,BlockDim>(0,0),
                                                                           runningRowCounter, higherLevelParameterStart + higherLevelIndex*BlockDim);

//                std::cout << "(" << runningRowCounter << " - " << (runningRowCounter + ResidualDim) << ") / " << globalResidual.rows() << std::endl;
                globalResidual.template block<ResidualDim,1>(runningRowCounter,0) = jacobianAndResidual.r;

                runningRowCounter += ResidualDim;

            }

        }

        lowerLevelParameterStart = higherLevelParameterStart;

    }

}

template <typename Scalar, typename ScalarOpt, template <typename,int...> class TransformT, internal::TransformUpdateMethod U>
void computeRegularizerNormalEquations(const NonrigidTransformer<Scalar,TransformT> & transformer,
                                       Eigen::SparseMatrix<ScalarOpt> & JTJ,
                                       Eigen::Matrix<ScalarOpt,Eigen::Dynamic,1> & JTr) {

    // TODO: come up with a nice way to compute only the upper triangle of JTJ,
    // as that is the only part used in the solver anyway

    typedef Eigen::Triplet<ScalarOpt> Triplet;
    typedef Eigen::SparseMatrix<ScalarOpt> SparseMatX;
    typedef Eigen::Matrix<ScalarOpt,Eigen::Dynamic,1> VecX;

    static constexpr uint ResidualDim = 3;
    static constexpr uint BlockDim = 6;
    // TODO TODO TODO:
    static constexpr uint NumNeighbors = 4;

    const uint numTotalDeformationGraphVertices = transformer.numVerticesTotal();

    const uint numTopLevelVertices = transformer.numVerticesAtLevel(transformer.numRegularizationTreeLevels()-1);

    const uint totalResidualRows = 2 * (numTotalDeformationGraphVertices - numTopLevelVertices) * ResidualDim * NumNeighbors;

    std::vector<Triplet> globalJacobianTriplets;
    globalJacobianTriplets.reserve(totalResidualRows*2*BlockDim);

    VecX globalResidual(totalResidualRows);

    fillRegularizationResidualAndJacobianTripletList<Scalar,ScalarOpt,TransformT,U>(globalJacobianTriplets,globalResidual,transformer);

    const uint totalModelDimension = numTotalDeformationGraphVertices * BlockDim;

    SparseMatX globalJacobian(totalResidualRows,totalModelDimension);

    globalJacobian.setFromTriplets(globalJacobianTriplets.begin(),globalJacobianTriplets.end());

    globalJacobian.finalize();

    JTJ = globalJacobian.transpose()*globalJacobian;

    JTr = globalJacobian.transpose()*globalResidual;
}

template void computeRegularizerNormalEquations<float,double,DualQuaternion,internal::TransformUpdateLeftMultiply>
                                                     (const NonrigidTransformer<float,DualQuaternion> &,
                                                      Eigen::SparseMatrix<double> &,
                                                      Eigen::VectorXd &);

template void computeRegularizerNormalEquations<float,double,DualQuaternion,internal::TransformUpdateRightMultiply>
                                                     (const NonrigidTransformer<float,DualQuaternion> &,
                                                      Eigen::SparseMatrix<double> &,
                                                      Eigen::VectorXd &);

template void computeRegularizerNormalEquations<float,double,Sophus::SE3,internal::TransformUpdateLeftMultiply>
                                                     (const NonrigidTransformer<float,Sophus::SE3> &,
                                                      Eigen::SparseMatrix<double> &,
                                                      Eigen::VectorXd &);

template void computeRegularizerNormalEquations<float,double,Sophus::SE3,internal::TransformUpdateRightMultiply>
                                                     (const NonrigidTransformer<float,Sophus::SE3> &,
                                                      Eigen::SparseMatrix<double> &,
                                                      Eigen::VectorXd &);

} // namespace df


/******************************************************************************
 * Author:   Laurent Kneip                                                    *
 * Contact:  kneip.laurent@gmail.com                                          *
 * License:  Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved.      *
 *                                                                            *
 * Redistribution and use in source and binary forms, with or without         *
 * modification, are permitted provided that the following conditions         *
 * are met:                                                                   *
 * * Redistributions of source code must retain the above copyright           *
 *   notice, this list of conditions and the following disclaimer.            *
 * * Redistributions in binary form must reproduce the above copyright        *
 *   notice, this list of conditions and the following disclaimer in the      *
 *   documentation and/or other materials provided with the distribution.     *
 * * Neither the name of ANU 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 ANU OR THE 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 <stdlib.h>
#include <stdio.h>
#include <iostream>
#include <iomanip>
#include <opengv/absolute_pose/methods.hpp>
#include <opengv/absolute_pose/CentralAbsoluteAdapter.hpp>
#include <opengv/math/cayley.hpp>
#include <sstream>
#include <fstream>

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

#include "random_generators.hpp"
#include "experiment_helpers.hpp"
#include "time_measurement.hpp"

using namespace std;
using namespace Eigen;
using namespace opengv;

int main(int argc, char **argv)
{
  //initialize random seed
  initializeRandomSeed();

  //set experiment parameters
  double noise = 0.0;
  double outlierFraction = 0.0;
  size_t numberPoints = 10;

  //create a random viewpoint pose
  translation_t position = generateRandomTranslation(2.0);
  rotation_t rotation = generateRandomRotation(0.5);
  // 正規化されている

  //non-existent covariances
  cov3_mat_t test_cov;
  cov3_mats_t emptyCovariances;
  // NOTE　特徴点の数だけ、分散を加味する
  for (int i{0}; i < 10; ++i)
    emptyCovariances.push_back(test_cov.Identity() * 1.2);
  std::cout << test_cov.Identity() * 2 << std::endl;

  //create a fake central camera
  translations_t camOffsets;
  rotations_t camRotations;
  generateCentralCameraSystem(camOffsets, camRotations);

  //derive correspondences based on random point-cloud
  bearingVectors_t bearingVectors;
  points_t points;
  std::vector<int> camCorrespondences; //unused in the central case!
  Eigen::MatrixXd gt(3, numberPoints);
  generateRandom2D3DCorrespondences(
      position, rotation, camOffsets, camRotations, numberPoints, noise, outlierFraction,
      bearingVectors, points, camCorrespondences, gt);
  //print the experiment characteristics
  printExperimentCharacteristics(
      position, rotation, noise, outlierFraction);

  //create a central absolute adapter
  absolute_pose::CentralAbsoluteAdapter adapter(
      bearingVectors,
      points);

  absolute_pose::CentralAbsoluteAdapter adapter1(
      bearingVectors,
      points,
      emptyCovariances);
  // NOTE   /** Reference to the covariance-matrix related to the bearing vector */

  //timer
  struct timeval tic;
  struct timeval toc;
  size_t iterations = 50;

  //run the experiments
  std::cout << "running Kneip's P2P (first two correspondences)" << std::endl;
  translation_t p2p_translation;
  gettimeofday(&tic, 0);
  for (size_t i = 0; i < iterations; i++)
    p2p_translation = absolute_pose::p2p(adapter);
  gettimeofday(&toc, 0);
  double p2p_time = TIMETODOUBLE(timeval_minus(toc, tic)) / iterations;

  std::cout << "running Kneip's P3P (first three correspondences)" << std::endl;
  transformations_t p3p_kneip_transformations;
  gettimeofday(&tic, 0);
  for (size_t i = 0; i < iterations; i++)
    p3p_kneip_transformations = absolute_pose::p3p_kneip(adapter);
  gettimeofday(&toc, 0);
  double p3p_kneip_time = TIMETODOUBLE(timeval_minus(toc, tic)) / iterations;

  std::cout << "running Gao's P3P (first three correspondences)" << std::endl;
  transformations_t p3p_gao_transformations;
  gettimeofday(&tic, 0);
  for (size_t i = 0; i < iterations; i++)
    p3p_gao_transformations = absolute_pose::p3p_gao(adapter);
  gettimeofday(&toc, 0);
  double p3p_gao_time = TIMETODOUBLE(timeval_minus(toc, tic)) / iterations;

  std::cout << "running epnp (all correspondences)" << std::endl;
  transformation_t epnp_transformation;
  gettimeofday(&tic, 0);
  for (size_t i = 0; i < iterations; i++)
    epnp_transformation = absolute_pose::epnp(adapter);
  gettimeofday(&toc, 0);
  double epnp_time = TIMETODOUBLE(timeval_minus(toc, tic)) / iterations;

  std::cout << "running MLPnP (all correspondences w/o covariance)" << std::endl;
  transformation_t mlpnp_transformation;
  transformation_t mlpnp_transformation_;
  gettimeofday(&tic, 0);

  Eigen::MatrixXd cov_xx;
  Eigen::MatrixXd cov_ldld;
  Eigen::MatrixXd cov_xx_;
  Eigen::MatrixXd cov_ldld_;

  for (size_t i = 0; i < iterations; i++)
  {
    mlpnp_transformation = absolute_pose::mlpnp(adapter, cov_xx, cov_ldld);
    mlpnp_transformation_ = absolute_pose::mlpnp(adapter1, cov_xx_, cov_ldld_);
  }
  cout << mlpnp_transformation << endl;
  cout << "with cov is ..." << std::endl;
  cout << mlpnp_transformation_ << endl;
  cout << "cov_xx is " << std::endl;
  std::cout << cov_xx << endl;
  cout << "cov_xx_ is " << std::endl;
  std::cout << cov_xx_ << endl;
  // cout<<"cov_xx; "<<cov_xx<<endl;
  // cout<<"cov_ldld; "<<std::endl;
  // std::cout<<cov_ldld<<endl;
  gettimeofday(&toc, 0);
  double mlpnp_time = TIMETODOUBLE(timeval_minus(toc, tic)) / iterations;

  std::cout << "running epnp with 6 correspondences" << std::endl;
  std::vector<int> indices6 = getNindices(6);
  transformation_t epnp_transformation_6 =
      absolute_pose::epnp(adapter, indices6);

  // std::cout << "running mlpnp with 6 correspondences" << std::endl;
  // transformation_t mlpnp_transformation_6 =
  //   absolute_pose::mlpnp(adapter, indices6);

  std::cout << "running upnp with all correspondences" << std::endl;
  transformations_t upnp_transformations;
  gettimeofday(&tic, 0);
  for (size_t i = 0; i < iterations; i++)
    upnp_transformations = absolute_pose::upnp(adapter);
  gettimeofday(&toc, 0);
  double upnp_time = TIMETODOUBLE(timeval_minus(toc, tic)) / iterations;

  std::cout << "running upnp with 3 correspondences" << std::endl;
  std::vector<int> indices3 = getNindices(3);
  transformations_t upnp_transformations_3 =
      absolute_pose::upnp(adapter, indices3);

  std::cout << "setting perturbed pose";
  std::cout << "and performing nonlinear optimization" << std::endl;
  //add a small perturbation to the pose
  translation_t t_perturbed;
  rotation_t R_perturbed;
  getPerturbedPose(position, rotation, t_perturbed, R_perturbed, 0.1);
  transformation_t nonlinear_transformation;
  gettimeofday(&tic, 0);
  for (size_t i = 0; i < iterations; i++)
  {
    adapter.sett(t_perturbed);
    adapter.setR(R_perturbed);
    nonlinear_transformation = absolute_pose::optimize_nonlinear(adapter);
  }
  gettimeofday(&toc, 0);
  double nonlinear_time = TIMETODOUBLE(timeval_minus(toc, tic)) / iterations;

  std::cout << "setting perturbed pose ";
  std::cout << "and performing nonlinear optimization with 10 correspondences";
  std::cout << std::endl;
  std::vector<int> indices10 = getNindices(10);
  //add a small perturbation to the pose
  getPerturbedPose(position, rotation, t_perturbed, R_perturbed, 0.1);
  adapter.sett(t_perturbed);
  adapter.setR(R_perturbed);
  transformation_t nonlinear_transformation_10 =
      absolute_pose::optimize_nonlinear(adapter, indices10);
}