// Copyright 2019 Intel Corporation.

#include "tile/lang/ast/jacobian.h"

#include <map>
#include <memory>
#include <stack>
#include <unordered_set>

#include <boost/format.hpp>

#include "tile/lang/ast/gradient.h"
#include "tile/lang/ast/traversal.h"

namespace vertexai {
namespace tile {
namespace lang {
namespace ast {

namespace {

struct UseInfo {
  ExprPtr expr;
  size_t idx;
};

class ComputeUses : public AstVisitor<void> {
 public:
  explicit ComputeUses(const ExprPtr& src) {
    stack_.push(src);
    while (stack_.size()) {
      auto expr = stack_.top();
      stack_.pop();
      if (!seen_.count(expr.get())) {
        seen_.insert(expr.get());
        expr->Accept(this);
      }
    }
  }

  const std::vector<UseInfo>& uses(const Expr* expr) const { return uses_.at(expr); }

 private:
  void Visit(const CallExpr& expr) final {
    for (size_t i = 0; i < expr.args.size(); i++) {
      Push(expr, expr.args[i], i);
    }
  }

  void Visit(const ContractionExpr& expr) final {
    for (size_t i = 0; i < expr.srcs.size(); i++) {
      Push(expr, expr.srcs[i]->ref, i);
    }
    if (expr.use_default) {
      Push(expr, expr.use_default, expr.srcs.size());
    }
  }

  void Visit(const GradOverrideExpr& expr) final {
    for (size_t i = 0; i < expr.ins.size(); i++) {
      Push(expr, expr.ins[i], i);
    }
  }

  void Visit(const DimExprExpr& expr) final {}
  void Visit(const FloatConst& expr) final {}
  void Visit(const IntConst& expr) final {}
  void Visit(const ParamExpr& expr) final {}

 private:
  void Push(const Expr& user, const ExprPtr& used, size_t idx) {
    IVLOG(2, "ComputeUses::Push> user: " << &user << ", used: " << used << ", idx: " << idx);
    auto ptr = std::const_pointer_cast<Expr>(user.as_ptr());
    uses_[used.get()].emplace_back(UseInfo{ptr, idx});
    stack_.push(used);
  }

 private:
  std::stack<ExprPtr> stack_;
  std::unordered_set<const Expr*> seen_;
  std::unordered_map<const Expr*, std::vector<UseInfo>> uses_;
};

class Jacobian {
 public:
  explicit Jacobian(const ExprPtr& err) : uses_(err) {
    IVLOG(2, "Jacobian::Jacobian> err: " << err);

    // Create identity matrix to represent d(err)/d(err)
    auto dims = err->shape.dims_as_exprs();
    orank_ = dims.size();

    auto Ji = std::make_shared<ContractionExpr>();
    Ji->agg_op = AggregationOp::ASSIGN;
    Ji->combo_op = CombinationOp::NONE;

    auto src = std::make_shared<FloatConst>(1.0);
    Ji->srcs.push_back(std::make_shared<IndexMapExpr>(src, std::vector<PolyExprPtr>{}));

    std::vector<PolyExprPtr> Jidxs;
    std::vector<DimExprPtr> Jdims;

    for (size_t io = 0; io < 2; io++) {
      for (size_t i = 0; i < orank_; i++) {
        Jidxs.push_back(std::make_shared<PolyIndex>(i));
        Jdims.push_back(dims[i]);
      }
    }

    Ji->sink_idxs = std::make_shared<IndexMapExpr>(nullptr, Jidxs);
    Ji->sink_dims = std::make_shared<SizeMapExpr>(Jdims);
    Ji->ComputeShape("");

    seen_[err.get()] = Ji;
  }

  ExprPtr GetDerivative(const ExprPtr& expr) {
    IVLOG(2, "Jacobian::GetDerivative> " << expr);
    auto it = seen_.find(expr.get());
    if (it != seen_.end()) {
      IVLOG(2, "  returning: " << it->second);
      return it->second;
    }
    ExprPtr total;
    for (const auto& use : uses_.uses(expr.get())) {
      ExprPtr dop;
      auto dout = GetDerivative(use.expr);
      if (auto grad_override_expr = std::dynamic_pointer_cast<GradOverrideExpr>(use.expr)) {
        // A gradient override replaces all the derivatives, so set total and exit the loop
        total = DeriveOverride(dout, grad_override_expr, use.idx);
        break;
      }
      if (auto call_expr = std::dynamic_pointer_cast<CallExpr>(use.expr)) {
        dop = DeriveCall(dout, call_expr, use.idx);
      } else if (auto cion_expr = std::dynamic_pointer_cast<ContractionExpr>(use.expr)) {
        dop = DeriveContraction(dout, cion_expr, use.idx);
      } else {
        throw std::runtime_error("Invalid operation type in Gradient::GetDerivative");
      }
      if (!total) {
        total = dop;
      } else {
        total = MakeCall("add", {total, dop});
      }
    }
    if (!total) {
      total = std::make_shared<FloatConst>(0.0);
    }
    IVLOG(2, "  Gradient::GetDerivative, final result -> " << total);
    seen_.emplace(expr.get(), total);
    return total;
  }

 private:
  ExprPtr DeriveContraction(const ExprPtr& dout, const std::shared_ptr<ContractionExpr>& expr, size_t idx) {
    if (expr->use_default && idx == expr->srcs.size()) {
      return dout;
    }
    if (expr->combo_op == CombinationOp::EQ) {
      return std::make_shared<IntConst>(0);
    }
    if (expr->agg_op == AggregationOp::SUM || expr->agg_op == AggregationOp::ASSIGN) {
      return DeriveSum(dout, expr, idx);
    }
    if (expr->agg_op == AggregationOp::MIN || expr->agg_op == AggregationOp::MAX) {
      return DeriveExtreme(dout, expr, idx);
    }
    if (expr->agg_op == AggregationOp::PROD) {
      throw std::runtime_error("PROD AggregationOp not implemented for Jacobian");
    }
    throw std::runtime_error("Invalid ContractionExpr in DeriveContraction");
  }

  // Each of these must compute single-layer Jacobian of op, then combine it with prior Jacobian dout to
  // produce new total Jacobian

  ExprPtr DeriveCall(const ExprPtr& dout, const std::shared_ptr<CallExpr>& op, size_t idx) {
    if (op->fn == "reshape") {
      throw std::runtime_error("Jacobian not implemented for reshape");
    }
    auto deriv = DerivRegistry::Instance()->Resolve(op->fn);  // Returns elementwise derivative data
    // Autobroadcasting handles J i/o dims correctly
    return deriv.fn(op, dout, op->args, deriv.user_fn, deriv.user_ctx)[idx];
  }

  ExprPtr DeriveSum(const ExprPtr& dout, const std::shared_ptr<ContractionExpr>& op, size_t idx) {
    // Compute Jacobian for SumContraction Op
    auto Ji = std::make_shared<ContractionExpr>();
    Ji->agg_op = AggregationOp::ASSIGN;
    Ji->constraints = op->constraints;

    std::vector<PolyExprPtr> Jidxs;  // Indexes of new total Jacobian
    std::vector<DimExprPtr> Jdims;   // Dimensions of new total Jacobian

    // Add dimensions to Ji corresponding to op output dimensions
    auto odims = op->shape.dims_as_exprs();
    for (size_t i = 0; i < odims.size(); i++) {
      Jidxs.push_back(op->sink_idxs->idxs[i]);
      Jdims.push_back(odims[i]);
    }

    for (size_t j = 0; j < op->srcs.size(); j++) {
      if (j == idx) {
        // Add dimensions to Ji corresponding to op w.r.t. input dimensions
        auto idims = op->srcs[j]->ref->shape.dims_as_exprs();
        for (size_t i = 0; i < idims.size(); i++) {
          Jidxs.push_back(op->srcs[j]->idxs[i]);
          Jdims.push_back(idims[i]);
        }
      } else {
        switch (op->combo_op) {
          case CombinationOp::MULTIPLY:
            // Add non-w.r.t. op inputs as inputs to Ji, set combo op for Ji
            Ji->srcs.push_back(op->srcs[j]);
            Ji->combo_op = CombinationOp::MULTIPLY;
            break;
          case CombinationOp::PLUS:
            // Jacobian is identity matrix, broadcast through non-wrt dimensions
            Ji->srcs.push_back(
                std::make_shared<IndexMapExpr>(std::make_shared<FloatConst>(1.0), std::vector<PolyExprPtr>{}));
            Ji->combo_op = CombinationOp::NONE;
            break;
          default:
            throw std::runtime_error("Combination Op receieved by DeriveSum in Jacobian not implemented");
        }
      }
    }

    Ji->sink_idxs = std::make_shared<IndexMapExpr>(nullptr, Jidxs);
    Ji->sink_dims = std::make_shared<SizeMapExpr>(Jdims);
    Ji->ComputeShape("");

    return ChainRule(dout, Ji);
  }

  ExprPtr DeriveOverride(const ExprPtr& dout, const std::shared_ptr<GradOverrideExpr>& op, size_t idx) {
    throw std::runtime_error("DeriveOverride not implemented for Jacobian");
  }

  ExprPtr DeriveExtreme(const ExprPtr& dout, const std::shared_ptr<ContractionExpr>& op, size_t idx) {
    throw std::runtime_error("DeriveExtreme not implemented for Jacobian");
  }

  ExprPtr ChainRule(const ExprPtr& Jprev, const ExprPtr& Ji) {
    // Combine current Jacobian with total. Output dimensions are
    // [first {orank_} dimensions of Jprev, last {Ji_rank - orank} dimensions of Ji]
    auto Jnew = std::make_shared<ContractionExpr>();
    Jnew->agg_op = AggregationOp::SUM;
    Jnew->combo_op = CombinationOp::MULTIPLY;

    std::vector<PolyExprPtr> iidxs;  // Indices for op Jacobian
    std::vector<PolyExprPtr> pidxs;  // Indices for previous total Jacobian
    std::vector<PolyExprPtr> nidxs;  // Indices for new total Jacobian

    auto idims = Ji->shape.dims_as_exprs();     // Dimensions for op Jacobian
    auto pdims = Jprev->shape.dims_as_exprs();  // Dimensions for previous total Jacobian
    std::vector<DimExprPtr> ndims;              // Dimensions for new total Jacobian

    size_t Jirank = idims.size();
    size_t Jprank = pdims.size();

    // Add dims/idxs corresponding to output dimensions
    for (size_t i = 0; i < orank_; i++) {
      auto idx = std::make_shared<PolyIndex>(i);
      pidxs.push_back(idx);
      nidxs.push_back(idx);
      ndims.push_back(pdims[i]);
    }

    // Add "overlapping" idxs
    for (size_t i = orank_; i < Jprank; i++) {
      auto idx = std::make_shared<PolyIndex>(i);
      pidxs.push_back(idx);
      iidxs.push_back(idx);
    }

    // Add dim/idxs corresponding to new input dimensions
    for (size_t i = Jprank - orank_; i < Jirank; i++) {
      auto idx = std::make_shared<PolyIndex>(i + orank_);
      iidxs.push_back(idx);
      nidxs.push_back(idx);
      ndims.push_back(idims[i]);
    }

    // Bind sources
    Jnew->srcs.push_back(std::make_shared<IndexMapExpr>(Jprev, pidxs));
    Jnew->srcs.push_back(std::make_shared<IndexMapExpr>(Ji, iidxs));

    // Bind output dims/idxs
    Jnew->sink_idxs = std::make_shared<IndexMapExpr>(nullptr, nidxs);
    Jnew->sink_dims = std::make_shared<SizeMapExpr>(ndims);
    Jnew->ComputeShape("");

    return Jnew;
  }

 private:
  ComputeUses uses_;
  std::map<const Expr*, ExprPtr> seen_;
  size_t orank_;
};

}  // namespace

std::vector<ExprPtr> ComputeJacobian(const std::vector<ExprPtr>& wrts, const ExprPtr& loss) {
  ExprPtr value = loss;
  Jacobian grad(value);
  std::vector<ExprPtr> ret(wrts.size());
  for (size_t i = 0; i < wrts.size(); i++) {
    auto wrt = wrts[i];
    ret[i] = grad.GetDerivative(wrts[i]);
  }
  return ret;
}

}  // namespace ast
}  // namespace lang
}  // namespace tile
}  // namespace vertexai


#ifndef SDM_UTILS_HPP_
#define SDM_UTILS_HPP_

#include "fmath.hpp"
#include "alias.hpp"

#include <Eigen/Core>
#include <Eigen/SparseCore>

#include <algorithm>
#include <cstdio>
#include <cstdlib>
#include <cmath>
#include <cstring>
#include <cstdarg>
#include <cstddef>
#include <fstream>
#include <functional>
#include <iomanip>
#include <iostream>
#include <limits>
#include <numeric>
#include <random>
#include <sstream>
#include <string>
#include <vector>

namespace sdm {

static inline void
get_cross_validation_index(const int num_fold, const int num_ins,
                           std::vector<std::vector<int>> &train_indexs,
                           std::vector<std::vector<int>> &valid_indexs,
                           const bool random_flag = false,
                           const bool seed_flag = false) {
  train_indexs.clear();
  train_indexs.resize(num_fold);
  valid_indexs.clear();
  valid_indexs.resize(num_fold);
  std::vector<int> whole_index(num_ins);
  std::iota(std::begin(whole_index), std::end(whole_index), 0);

  if (random_flag) {
    std::mt19937 engine;
    if (seed_flag) {
      std::random_device rnd;
      std::vector<std::uint_least32_t> v(10);
      std::generate(std::begin(v), std::end(v), std::ref(rnd));
      std::seed_seq seed(std::begin(v), std::end(v));
      engine.seed(seed);
    }
    std::shuffle(std::begin(whole_index), std::end(whole_index), engine);
  }

  int each_fold_num_ins = num_ins / num_fold;
  std::vector<int> each_fold_num_ins_vec(num_fold, each_fold_num_ins);
  int rest = num_ins % num_fold;
  for (int i = 0; i < rest; ++i)
    ++each_fold_num_ins_vec[i];

  auto start_it = std::begin(whole_index);
  auto end_it = std::end(whole_index);
  for (int i = 0; i < num_fold; ++i) {
    valid_indexs[i].reserve(each_fold_num_ins_vec[i]);
    end_it = start_it + each_fold_num_ins_vec[i];
    valid_indexs[i].insert(std::end(valid_indexs[i]), start_it, end_it);
    start_it = end_it;
  }

  for (int i = 0; i < num_fold; ++i) {
    std::sort(std::begin(valid_indexs[i]), std::end(valid_indexs[i]));
    train_indexs[i].reserve(num_ins - each_fold_num_ins_vec[i]);
  }

  for (int i = 0; i < num_fold; ++i)
    for (int j = 0; j < num_fold; ++j)
      if (i != j)
        train_indexs[j].insert(std::end(train_indexs[j]),
                               std::begin(valid_indexs[i]),
                               std::end(valid_indexs[i]));

  for (int i = 0; i < num_fold; ++i)
    std::sort(std::begin(train_indexs[i]), std::end(train_indexs[i]));
}

// misc
static inline std::vector<std::string> split_string(const std::string &str,
                                                    const std::string &delim) {
  std::vector<std::string> res;
  std::string::size_type current = 0, found, delimlen = delim.size();
  while ((found = str.find(delim, current)) != std::string::npos) {
    res.push_back(std::string(str, current, found - current));
    current = found + delimlen;
  }
  res.push_back(std::string(str, current, str.size() - current));
  return res;
}

static inline int count_lines(const std::string file_name) {
  int num_lines = 0;
  std::ifstream data_file;
  std::string line;
  data_file.open(file_name);
  if (data_file.fail()) {
    std::cerr << "Unable to open " << file_name << '\n';
    exit(8);
  }
  while (std::getline(data_file, line))
    ++num_lines;
  data_file.close();

  return num_lines;
}

// for cygwin(GCC) can't use std::stoi
static inline int str2int(const std::string &s) {
  int r = 0;
  bool neg = false;
  int index = 0;
  while (s[index] == ' ')
    ++index;
  if (s[index] == '-') {
    neg = true;
    ++index;
  } else if (s[index] == '+') {
    ++index;
  }
  while (s[index] >= '0' && s[index] <= '9') {
    r = (r * 10.0) + (s[index] - '0');
    ++index;
  }
  if (neg) {
    r = -r;
  }
  return r;
}

template <typename ValueType> ValueType naive_atot(const char *p) {
  ValueType r = 0.0;
  bool neg = false;
  while (*p == ' ')
    ++p;
  if (*p == '-') {
    neg = true;
    ++p;
  } else if (*p == '+') {
    ++p;
  }
  while (*p >= '0' && *p <= '9') {
    r = (r * 10.0) + (*p - '0');
    ++p;
  }
  if (*p == '.') {
    ValueType f = 0.0;
    int n = 0;
    ++p;
    while (*p >= '0' && *p <= '9') {
      f = (f * 10.0) + (*p - '0');
      ++p;
      ++n;
    }
    r += f / std::pow(10.0, n);
  }
  if (neg) {
    r = -r;
  }
  return r;
}

template <typename ValueType> ValueType naive_atot(const std::string &s) {
  ValueType r = 0.0;
  bool neg = false;
  auto it = s.begin();
  while (*it == ' ')
    ++it;
  if (*it == '-') {
    neg = true;
    ++it;
  } else if (*it == '-') {
    ++it;
  }
  while (*it >= '0' && *it <= '9') {
    r = (r * 10.0) + (*it - '0');
    ++it;
  }
  if (*it == '.') {
    ++it;
    ValueType f = 0.0;
    int n = 0;
    while (*it >= '0' && *it <= '9') {
      f = (f * 10.0) + (*it - '0');
      ++it;
      ++n;
    }
    r += f / std::pow(10.0, n);
  }
  if (neg)
    r = -r;
  return r;
}

template <typename RealType1, typename RealType2>
inline bool lessPair(const std::pair<RealType1, RealType2> &l,
                     const std::pair<RealType1, RealType2> &r) {
  return l.second < r.second;
}

template <typename RealType1, typename RealType2>
inline bool greaterPair(const std::pair<RealType1, RealType2> &l,
                        const std::pair<RealType1, RealType2> &r) {
  return l.second > r.second;
}

template <typename RealType> inline double norm_cdf(const RealType x) {
  constexpr double a1 = 0.254829592;
  constexpr double a2 = -0.284496736;
  constexpr double a3 = 1.421413741;
  constexpr double a4 = -1.453152027;
  constexpr double a5 = 1.061405429;
  constexpr double p = 0.3275911;

  int sign = 1;
  if (x < static_cast<RealType>(0.0))
    sign = -1;
  double tmp = fabs(x) / sqrt(2.0);

  // A&S formula 7.1.26
  double t = 1.0 / (1.0 + p * tmp);
  double y = 1.0 -
             (((((a5 * t + a4) * t) + a3) * t + a2) * t + a1) * t *
                 fmath::exp(-tmp * tmp);

  return 0.5 * (1.0 + sign * y);
}

template <typename RealType> inline double normsinv(const RealType pos) {

  constexpr double A1 = (-3.969683028665376e+01);
  constexpr double A2 = 2.209460984245205e+02;
  constexpr double A3 = (-2.759285104469687e+02);
  constexpr double A4 = 1.383577518672690e+02;
  constexpr double A5 = (-3.066479806614716e+01);
  constexpr double A6 = 2.506628277459239e+00;

  constexpr double B1 = (-5.447609879822406e+01);
  constexpr double B2 = 1.615858368580409e+02;
  constexpr double B3 = (-1.556989798598866e+02);
  constexpr double B4 = 6.680131188771972e+01;
  constexpr double B5 = (-1.328068155288572e+01);

  constexpr double C1 = (-7.784894002430293e-03);
  constexpr double C2 = (-3.223964580411365e-01);
  constexpr double C3 = (-2.400758277161838e+00);
  constexpr double C4 = (-2.549732539343734e+00);
  constexpr double C5 = 4.374664141464968e+00;
  constexpr double C6 = 2.938163982698783e+00;

  constexpr double D1 = 7.784695709041462e-03;
  constexpr double D2 = 3.224671290700398e-01;
  constexpr double D3 = 2.445134137142996e+00;
  constexpr double D4 = 3.754408661907416e+00;

  constexpr double P_LOW = 0.02425;
  /* P_high = 1 - p_low*/
  constexpr double P_HIGH = 0.97575;

  double x = 0.0;
  double q, r;
  if ((0 < pos) && (pos < P_LOW)) {
    q = sqrt(-2 * fmath::log(pos));
    x = (((((C1 * q + C2) * q + C3) * q + C4) * q + C5) * q + C6) /
        ((((D1 * q + D2) * q + D3) * q + D4) * q + 1);
  } else {
    if ((P_LOW <= pos) && (pos <= P_HIGH)) {
      q = pos - 0.5;
      r = q * q;
      x = (((((A1 * r + A2) * r + A3) * r + A4) * r + A5) * r + A6) * q /
          (((((B1 * r + B2) * r + B3) * r + B4) * r + B5) * r + 1);
    } else {
      if ((P_HIGH < pos) && (pos < 1)) {
        q = sqrt(-2 * fmath::log(1 - pos));
        x = -(((((C1 * q + C2) * q + C3) * q + C4) * q + C5) * q + C6) /
            ((((D1 * q + D2) * q + D3) * q + D4) * q + 1);
      } else {
        std::cout << "CDF position > 1 : " << pos << std::endl;
        x = 1.0;
      }
    }
  }

  /*
  // If you are composiling this under UNIX OR LINUX, you may uncomment this
  // block for better accuracy.
  if(( 0 < pos)&&(pos < 1)){
    e = 0.5 * erfc(-x/sqrt(2)) - pos;
    u = e * sqrt(2*M_PI) * exp(x*x/2);
    x = x - u/(1 + x*u/2);
  }
  */

  return x;
}

template <typename ValueType>
bool compare(const ValueType &lhs, const ValueType &rhs,
             const ValueType eps = 1e-12) {
  return (std::abs(lhs - rhs) <= std::max(lhs, rhs) * eps ||
          std::abs(lhs - rhs) <= eps);
}

template <typename ValueType, int _Rows, int _Cols, int _Option>
ValueType compare(const Eigen::Matrix<ValueType, _Rows, _Cols, _Option> &mat,
                  const ValueType value, const ValueType eps = 1e-12) {
  ValueType v;
  int num_eq = 0;
  if (_Option == Eigen::RowMajor) {
    for (int i = 0; i < mat.rows(); ++i) {
      for (int j = 0; j < mat.cols(); ++j) {
        v = mat.coeffRef(i, j);
        if (compare(v, value, eps))
          ++num_eq;
      }
    }
  } else {
    for (int j = 0; j < mat.cols(); ++j) {
      for (int i = 0; i < mat.rows(); ++i) {
        v = mat.coeffRef(i, j);
        if (compare(v, value, eps))
          ++num_eq;
      }
    }
  }
  return num_eq;
}

} // namespace sdm

#endif


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

// Copyright (c) 2007-2015 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2015 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2015 Mateusz Loskot, London, UK.

// This file was modified by Oracle on 2018-2022.
// Modifications copyright (c) 2018-2022 Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_SIMPLIFY_HPP
#define BOOST_GEOMETRY_ALGORITHMS_SIMPLIFY_HPP

#include <cstddef>
#ifdef BOOST_GEOMETRY_DEBUG_DOUGLAS_PEUCKER
#include <iostream>
#endif
#include <set>
#include <vector>

#include <boost/core/addressof.hpp>
#include <boost/core/ignore_unused.hpp>
#include <boost/range/begin.hpp>
#include <boost/range/end.hpp>
#include <boost/range/size.hpp>
#include <boost/range/value_type.hpp>

#include <boost/geometry/algorithms/area.hpp>
#include <boost/geometry/algorithms/clear.hpp>
#include <boost/geometry/algorithms/convert.hpp>
#include <boost/geometry/algorithms/detail/dummy_geometries.hpp>
#include <boost/geometry/algorithms/detail/equals/point_point.hpp>
#include <boost/geometry/algorithms/detail/visit.hpp>
#include <boost/geometry/algorithms/not_implemented.hpp>
#include <boost/geometry/algorithms/is_empty.hpp>
#include <boost/geometry/algorithms/perimeter.hpp>

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/closure.hpp>
#include <boost/geometry/core/exterior_ring.hpp>
#include <boost/geometry/core/interior_rings.hpp>
#include <boost/geometry/core/mutable_range.hpp>
#include <boost/geometry/core/tags.hpp>
#include <boost/geometry/core/visit.hpp>

#include <boost/geometry/geometries/adapted/boost_variant.hpp> // For backward compatibility
#include <boost/geometry/geometries/concepts/check.hpp>

#include <boost/geometry/strategies/concepts/simplify_concept.hpp>
#include <boost/geometry/strategies/default_strategy.hpp>
#include <boost/geometry/strategies/detail.hpp>
#include <boost/geometry/strategies/distance/comparable.hpp>
#include <boost/geometry/strategies/simplify/cartesian.hpp>
#include <boost/geometry/strategies/simplify/geographic.hpp>
#include <boost/geometry/strategies/simplify/spherical.hpp>

#include <boost/geometry/util/type_traits_std.hpp>

#ifdef BOOST_GEOMETRY_DEBUG_DOUGLAS_PEUCKER
#include <boost/geometry/io/dsv/write.hpp>
#endif

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace simplify
{

/*!
\brief Small wrapper around a point, with an extra member "included"
\details
    It has a const-reference to the original point (so no copy here)
\tparam the enclosed point type
*/
template <typename Point>
struct douglas_peucker_point
{
    typedef Point point_type;

    Point const* p;
    bool included;

    inline douglas_peucker_point(Point const& ap)
        : p(boost::addressof(ap))
        , included(false)
    {}
};

/*!
\brief Implements the simplify algorithm.
\details The douglas_peucker policy simplifies a linestring, ring or
    vector of points using the well-known Douglas-Peucker algorithm.
\tparam Point the point type
\tparam PointDistanceStrategy point-segment distance strategy to be used
\note This strategy uses itself a point-segment-distance strategy which
    can be specified
\author Barend and Maarten, 1995/1996
\author Barend, revised for Generic Geometry Library, 2008
*/

/*
For the algorithm, see for example:
 - http://en.wikipedia.org/wiki/Ramer-Douglas-Peucker_algorithm
 - http://www2.dcs.hull.ac.uk/CISRG/projects/Royal-Inst/demos/dp.html
*/
class douglas_peucker
{
    template <typename Iterator, typename Distance, typename PSDistanceStrategy>
    static inline void consider(Iterator begin,
                                Iterator end,
                                Distance const& max_dist,
                                int& n,
                                PSDistanceStrategy const& ps_distance_strategy)
    {
        typedef typename std::iterator_traits<Iterator>::value_type::point_type point_type;
        typedef decltype(ps_distance_strategy.apply(std::declval<point_type>(),
                            std::declval<point_type>(), std::declval<point_type>())) distance_type;

        std::size_t size = end - begin;

        // size must be at least 3
        // because we want to consider a candidate point in between
        if (size <= 2)
        {
#ifdef BOOST_GEOMETRY_DEBUG_DOUGLAS_PEUCKER
            if (begin != end)
            {
                std::cout << "ignore between " << dsv(*(begin->p))
                    << " and " << dsv(*((end - 1)->p))
                    << " size=" << size << std::endl;
            }
            std::cout << "return because size=" << size << std::endl;
#endif
            return;
        }

        Iterator last = end - 1;

#ifdef BOOST_GEOMETRY_DEBUG_DOUGLAS_PEUCKER
        std::cout << "find between " << dsv(*(begin->p))
            << " and " << dsv(*(last->p))
            << " size=" << size << std::endl;
#endif

        // Find most far point, compare to the current segment
        //geometry::segment<Point const> s(begin->p, last->p);
        distance_type md(-1.0); // any value < 0
        Iterator candidate = end;
        for (Iterator it = begin + 1; it != last; ++it)
        {
            distance_type dist = ps_distance_strategy.apply(*(it->p), *(begin->p), *(last->p));

#ifdef BOOST_GEOMETRY_DEBUG_DOUGLAS_PEUCKER
            std::cout << "consider " << dsv(*(it->p))
                << " at " << double(dist)
                << ((dist > max_dist) ? " maybe" : " no")
                << std::endl;

#endif
            if (md < dist)
            {
                md = dist;
                candidate = it;
            }
        }

        // If a point is found, set the include flag
        // and handle segments in between recursively
        if (max_dist < md && candidate != end)
        {
#ifdef BOOST_GEOMETRY_DEBUG_DOUGLAS_PEUCKER
            std::cout << "use " << dsv(candidate->p) << std::endl;
#endif

            candidate->included = true;
            n++;

            consider(begin, candidate + 1, max_dist, n, ps_distance_strategy);
            consider(candidate, end, max_dist, n, ps_distance_strategy);
        }
    }

    template
    <
        typename Range, typename OutputIterator, typename Distance,
        typename PSDistanceStrategy
    >
    static inline OutputIterator apply_(Range const& range,
                                        OutputIterator out,
                                        Distance const& max_distance,
                                        PSDistanceStrategy const& ps_distance_strategy)
    {
#ifdef BOOST_GEOMETRY_DEBUG_DOUGLAS_PEUCKER
            std::cout << "max distance: " << max_distance
                      << std::endl << std::endl;
#endif

        typedef typename boost::range_value<Range>::type point_type;
        typedef douglas_peucker_point<point_type> dp_point_type;

        // Copy coordinates, a vector of references to all points
        std::vector<dp_point_type> ref_candidates(boost::begin(range),
                                                  boost::end(range));

        // Include first and last point of line,
        // they are always part of the line
        int n = 2;
        ref_candidates.front().included = true;
        ref_candidates.back().included = true;

        // Get points, recursively, including them if they are further away
        // than the specified distance
        consider(boost::begin(ref_candidates), boost::end(ref_candidates), max_distance, n,
                 ps_distance_strategy);

        // Copy included elements to the output
        for (auto it = boost::begin(ref_candidates); it != boost::end(ref_candidates); ++it)
        {
            if (it->included)
            {
                // copy-coordinates does not work because OutputIterator
                // does not model Point (??)
                //geometry::convert(*(it->p), *out);
                *out = *(it->p);
                ++out;
            }
        }
        return out;
    }

public:
    template <typename Range, typename OutputIterator, typename Distance, typename Strategies>
    static inline OutputIterator apply(Range const& range,
                                       OutputIterator out,
                                       Distance const& max_distance,
                                       Strategies const& strategies)
    {
        typedef typename boost::range_value<Range>::type point_type;
        typedef decltype(strategies.distance(detail::dummy_point(), detail::dummy_segment())) distance_strategy_type;

        typedef typename strategy::distance::services::comparable_type
            <
                distance_strategy_type
            >::type comparable_distance_strategy_type;

        comparable_distance_strategy_type cstrategy = strategy::distance::services::get_comparable
            <
                distance_strategy_type
            >::apply(strategies.distance(detail::dummy_point(), detail::dummy_segment()));

        return apply_(range, out,
                      strategy::distance::services::result_from_distance
                          <
                              comparable_distance_strategy_type, point_type, point_type
                          >::apply(cstrategy, max_distance),
                      cstrategy);
    }
};

template <typename Range, typename Strategies>
inline bool is_degenerate(Range const& range, Strategies const& strategies)
{
    return boost::size(range) == 2
        && detail::equals::equals_point_point(geometry::range::front(range),
                                              geometry::range::back(range),
                                              strategies);
}

struct simplify_range_insert
{
    template
    <
        typename Range, typename OutputIterator, typename Distance,
        typename Impl, typename Strategies
    >
    static inline void apply(Range const& range, OutputIterator out,
                             Distance const& max_distance,
                             Impl const& impl,
                             Strategies const& strategies)
    {
        if (is_degenerate(range, strategies))
        {
            std::copy(boost::begin(range), boost::begin(range) + 1, out);
        }
        else if (boost::size(range) <= 2 || max_distance < 0)
        {
            std::copy(boost::begin(range), boost::end(range), out);
        }
        else
        {
            impl.apply(range, out, max_distance, strategies);
        }
    }
};

struct simplify_copy_assign
{
    template
    <
        typename In, typename Out, typename Distance,
        typename Impl, typename Strategies
    >
    static inline void apply(In const& in, Out& out,
                             Distance const& ,
                             Impl const& ,
                             Strategies const& )
    {
        out = in;
    }
};

struct simplify_copy
{
    template
    <
        typename RangeIn, typename RangeOut, typename Distance,
        typename Impl, typename Strategies
    >
    static inline void apply(RangeIn const& range, RangeOut& out,
                             Distance const& ,
                             Impl const& ,
                             Strategies const& )
    {
        std::copy(boost::begin(range), boost::end(range),
                  geometry::range::back_inserter(out));
    }
};

template <std::size_t MinimumToUseStrategy>
struct simplify_range
{
    template
    <
        typename RangeIn, typename RangeOut, typename Distance,
        typename Impl, typename Strategies
    >
    static inline void apply(RangeIn const& range, RangeOut& out,
                             Distance const& max_distance,
                             Impl const& impl,
                             Strategies const& strategies)
    {
        // For a RING:
        // Note that, especially if max_distance is too large,
        // the output ring might be self intersecting while the input ring is
        // not, although chances are low in normal polygons

        if (boost::size(range) <= MinimumToUseStrategy || max_distance < 0)
        {
            simplify_copy::apply(range, out, max_distance, impl, strategies);
        }
        else
        {
            simplify_range_insert::apply(range, geometry::range::back_inserter(out),
                                         max_distance, impl, strategies);
        }

        // Verify the two remaining points are equal. If so, remove one of them.
        // This can cause the output being under the minimum size
        if (is_degenerate(out, strategies))
        {
            range::resize(out, 1);
        }
    }
};

struct simplify_ring
{
private :
    template <typename Area>
    static inline int area_sign(Area const& area)
    {
        return area > 0 ? 1 : area < 0 ? -1 : 0;
    }

    template <typename Ring, typename Strategies>
    static std::size_t get_opposite(std::size_t index, Ring const& ring,
                                    Strategies const& strategies)
    {
        // TODO: Instead of calling the strategy call geometry::comparable_distance() ?

        auto const cdistance_strategy = strategies::distance::detail::make_comparable(strategies)
            .distance(detail::dummy_point(), detail::dummy_point());

        using point_type = typename geometry::point_type<Ring>::type;
        using cdistance_type = decltype(cdistance_strategy.apply(
            std::declval<point_type>(), std::declval<point_type>()));

        // Verify if it is NOT the case that all points are less than the
        // simplifying distance. If so, output is empty.
        cdistance_type max_cdistance(-1);

        point_type const& point = range::at(ring, index);
        std::size_t i = 0;
        for (auto it = boost::begin(ring); it != boost::end(ring); ++it, ++i)
        {
            cdistance_type const cdistance = cdistance_strategy.apply(*it, point);
            if (cdistance > max_cdistance)
            {
                max_cdistance = cdistance;
                index = i;
            }
        }
        return index;
    }

public :
    template <typename Ring, typename Distance, typename Impl, typename Strategies>
    static inline void apply(Ring const& ring, Ring& out, Distance const& max_distance,
                             Impl const& impl, Strategies const& strategies)
    {
        std::size_t const size = boost::size(ring);
        if (size == 0)
        {
            return;
        }

        bool const is_closed = closure<Ring>::value == closed;

        // TODO: instead of area() use calculate_point_order() ?

        int const input_sign = area_sign(geometry::area(ring, strategies));

        std::set<std::size_t> visited_indexes;

        // Rotate it into a copied vector
        // (vector, because source type might not support rotation)
        // (duplicate end point will be simplified away)
        typedef typename geometry::point_type<Ring>::type point_type;

        std::vector<point_type> rotated;
        rotated.reserve(size + 1); // 1 because open rings are closed

        // Closing point (but it will not start here)
        std::size_t index = 0;

        // Iterate (usually one iteration is enough)
        for (std::size_t iteration = 0; iteration < 4u; iteration++)
        {
            // Always take the opposite. Opposite guarantees that no point
            // "halfway" is chosen, creating an artefact (very narrow triangle)
            // Iteration 0: opposite to closing point (1/2, = on convex hull)
            //              (this will start simplification with that point
            //               and its opposite ~0)
            // Iteration 1: move a quarter on that ring, then opposite to 1/4
            //              (with its opposite 3/4)
            // Iteration 2: move an eight on that ring, then opposite (1/8)
            // Iteration 3: again move a quarter, then opposite (7/8)
            // So finally 8 "sides" of the ring have been examined (if it were
            // a semi-circle). Most probably, there are only 0 or 1 iterations.
            switch (iteration)
            {
                case 1 : index = (index + size / 4) % size; break;
                case 2 : index = (index + size / 8) % size; break;
                case 3 : index = (index + size / 4) % size; break;
            }
            index = get_opposite(index, ring, strategies);

            if (visited_indexes.count(index) > 0)
            {
                // Avoid trying the same starting point more than once
                continue;
            }

            // Do not duplicate the closing point
            auto rot_end = boost::end(ring);
            std::size_t rot_index = index;
            if (is_closed && size > 1)
            {
                --rot_end;
                if (rot_index == size - 1) { rot_index = 0; }
            }

            std::rotate_copy(boost::begin(ring), range::pos(ring, rot_index),
                             rot_end, std::back_inserter(rotated));

            // Close the rotated copy
            rotated.push_back(range::at(ring, rot_index));

            simplify_range<0>::apply(rotated, out, max_distance, impl, strategies);

            // Open output if needed
            if (! is_closed && boost::size(out) > 1)
            {
                range::pop_back(out);
            }

            // TODO: instead of area() use calculate_point_order() ?

            // Verify that what was positive, stays positive (or goes to 0)
            // and what was negative stays negative (or goes to 0)
            int const output_sign = area_sign(geometry::area(out, strategies));
            if (output_sign == input_sign)
            {
                // Result is considered as satisfactory (usually this is the
                // first iteration - only for small rings, having a scale
                // similar to simplify_distance, next iterations are tried
                return;
            }

            // Original is simplified away. Possibly there is a solution
            // when another starting point is used
            geometry::clear(out);

            if (iteration == 0
                && geometry::perimeter(ring, strategies) < 3 * max_distance)
            {
                // Check if it is useful to iterate. A minimal triangle has a
                // perimeter of a bit more than 3 times the simplify distance
                return;
            }

            // Prepare next try
            visited_indexes.insert(index);
            rotated.clear();
        }
    }
};

struct simplify_polygon
{
private:

    template
    <
        typename IteratorIn,
        typename InteriorRingsOut,
        typename Distance,
        typename Impl,
        typename Strategies
    >
    static inline void iterate(IteratorIn begin, IteratorIn end,
                               InteriorRingsOut& interior_rings_out,
                               Distance const& max_distance,
                               Impl const& impl, Strategies const& strategies)
    {
        typedef typename boost::range_value<InteriorRingsOut>::type single_type;
        for (IteratorIn it = begin; it != end; ++it)
        {
            single_type out;
            simplify_ring::apply(*it, out, max_distance, impl, strategies);
            if (! geometry::is_empty(out))
            {
                range::push_back(interior_rings_out, std::move(out));
            }
        }
    }

    template
    <
        typename InteriorRingsIn,
        typename InteriorRingsOut,
        typename Distance,
        typename Impl,
        typename Strategies
    >
    static inline void apply_interior_rings(InteriorRingsIn const& interior_rings_in,
                                            InteriorRingsOut& interior_rings_out,
                                            Distance const& max_distance,
                                            Impl const& impl, Strategies const& strategies)
    {
        range::clear(interior_rings_out);

        iterate(boost::begin(interior_rings_in), boost::end(interior_rings_in),
                interior_rings_out,
                max_distance,
                impl, strategies);
    }

public:
    template <typename Polygon, typename Distance, typename Impl, typename Strategies>
    static inline void apply(Polygon const& poly_in, Polygon& poly_out,
                             Distance const& max_distance,
                             Impl const& impl, Strategies const& strategies)
    {
        // Note that if there are inner rings, and distance is too large,
        // they might intersect with the outer ring in the output,
        // while it didn't in the input.
        simplify_ring::apply(exterior_ring(poly_in), exterior_ring(poly_out),
                             max_distance, impl, strategies);

        apply_interior_rings(interior_rings(poly_in), interior_rings(poly_out),
                             max_distance, impl, strategies);
    }
};

template<typename Policy>
struct simplify_multi
{
    template <typename MultiGeometry, typename Distance, typename Impl, typename Strategies>
    static inline void apply(MultiGeometry const& multi, MultiGeometry& out,
                             Distance const& max_distance,
                             Impl const& impl, Strategies const& strategies)
    {
        range::clear(out);

        typedef typename boost::range_value<MultiGeometry>::type single_type;

        for (typename boost::range_iterator<MultiGeometry const>::type
                it = boost::begin(multi); it != boost::end(multi); ++it)
        {
            single_type single_out;
            Policy::apply(*it, single_out, max_distance, impl, strategies);
            if (! geometry::is_empty(single_out))
            {
                range::push_back(out, std::move(single_out));
            }
        }
    }
};

}} // namespace detail::simplify
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template
<
    typename Geometry,
    typename Tag = typename tag<Geometry>::type
>
struct simplify: not_implemented<Tag>
{};

template <typename Point>
struct simplify<Point, point_tag>
{
    template <typename Distance, typename Impl, typename Strategy>
    static inline void apply(Point const& point, Point& out, Distance const& ,
                             Impl const& , Strategy const& )
    {
        geometry::convert(point, out);
    }
};

template <typename Segment>
struct simplify<Segment, segment_tag>
    : detail::simplify::simplify_copy_assign
{};

template <typename Box>
struct simplify<Box, box_tag>
    : detail::simplify::simplify_copy_assign
{};

// Linestring, keep 2 points (unless those points are the same)
template <typename Linestring>
struct simplify<Linestring, linestring_tag>
    : detail::simplify::simplify_range<2>
{};

template <typename Ring>
struct simplify<Ring, ring_tag>
    : detail::simplify::simplify_ring
{};

template <typename Polygon>
struct simplify<Polygon, polygon_tag>
    : detail::simplify::simplify_polygon
{};

template
<
    typename Geometry,
    typename Tag = typename tag<Geometry>::type
>
struct simplify_insert: not_implemented<Tag>
{};

template <typename Linestring>
struct simplify_insert<Linestring, linestring_tag>
    : detail::simplify::simplify_range_insert
{};

template <typename Ring>
struct simplify_insert<Ring, ring_tag>
    : detail::simplify::simplify_range_insert
{};

template <typename MultiPoint>
struct simplify<MultiPoint, multi_point_tag>
    : detail::simplify::simplify_copy
{};

template <typename MultiLinestring>
struct simplify<MultiLinestring, multi_linestring_tag>
    : detail::simplify::simplify_multi<detail::simplify::simplify_range<2> >
{};

template <typename MultiPolygon>
struct simplify<MultiPolygon, multi_polygon_tag>
    : detail::simplify::simplify_multi<detail::simplify::simplify_polygon>
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

namespace resolve_strategy
{

template
<
    typename Strategies,
    bool IsUmbrella = strategies::detail::is_umbrella_strategy<Strategies>::value
>
struct simplify
{
    template <typename Geometry, typename Distance>
    static inline void apply(Geometry const& geometry,
                             Geometry& out,
                             Distance const& max_distance,
                             Strategies const& strategies)
    {
        dispatch::simplify
            <
                Geometry
            >::apply(geometry, out, max_distance,
                     detail::simplify::douglas_peucker(),
                     strategies);
    }
};

template <typename Strategy>
struct simplify<Strategy, false>
{
    template <typename Geometry, typename Distance>
    static inline void apply(Geometry const& geometry,
                             Geometry& out,
                             Distance const& max_distance,
                             Strategy const& strategy)
    {
        using strategies::simplify::services::strategy_converter;

        simplify
            <
                decltype(strategy_converter<Strategy>::get(strategy))
            >::apply(geometry, out, max_distance,
                     strategy_converter<Strategy>::get(strategy));
    }
};

template <>
struct simplify<default_strategy, false>
{
    template <typename Geometry, typename Distance>
    static inline void apply(Geometry const& geometry,
                             Geometry& out,
                             Distance const& max_distance,
                             default_strategy)
    {
        typedef typename strategies::simplify::services::default_strategy
            <
                Geometry
            >::type strategy_type;

        simplify
            <
                strategy_type
            >::apply(geometry, out, max_distance, strategy_type());
    }
};

template
<
    typename Strategies,
    bool IsUmbrella = strategies::detail::is_umbrella_strategy<Strategies>::value
>
struct simplify_insert
{
    template<typename Geometry, typename OutputIterator, typename Distance>
    static inline void apply(Geometry const& geometry,
                             OutputIterator& out,
                             Distance const& max_distance,
                             Strategies const& strategies)
    {
        dispatch::simplify_insert
            <
                Geometry
            >::apply(geometry, out, max_distance,
                     detail::simplify::douglas_peucker(),
                     strategies);
    }
};

template <typename Strategy>
struct simplify_insert<Strategy, false>
{
    template<typename Geometry, typename OutputIterator, typename Distance>
    static inline void apply(Geometry const& geometry,
                             OutputIterator& out,
                             Distance const& max_distance,
                             Strategy const& strategy)
    {
        using strategies::simplify::services::strategy_converter;

        simplify_insert
            <
                decltype(strategy_converter<Strategy>::get(strategy))
            >::apply(geometry, out, max_distance,
                     strategy_converter<Strategy>::get(strategy));
    }
};

template <>
struct simplify_insert<default_strategy, false>
{
    template <typename Geometry, typename OutputIterator, typename Distance>
    static inline void apply(Geometry const& geometry,
                             OutputIterator& out,
                             Distance const& max_distance,
                             default_strategy)
    {
        typedef typename strategies::simplify::services::default_strategy
            <
                Geometry
            >::type strategy_type;

        simplify_insert
            <
                strategy_type
            >::apply(geometry, out, max_distance, strategy_type());
    }
};

} // namespace resolve_strategy

namespace resolve_dynamic {

template <typename Geometry, typename Tag = typename tag<Geometry>::type>
struct simplify
{
    template <typename Distance, typename Strategy>
    static inline void apply(Geometry const& geometry,
                             Geometry& out,
                             Distance const& max_distance,
                             Strategy const& strategy)
    {
        resolve_strategy::simplify<Strategy>::apply(geometry, out, max_distance, strategy);
    }
};

template <typename Geometry>
struct simplify<Geometry, dynamic_geometry_tag>
{
    template <typename Distance, typename Strategy>
    static inline void apply(Geometry const& geometry,
                             Geometry& out,
                             Distance const& max_distance,
                             Strategy const& strategy)
    {
        traits::visit<Geometry>::apply([&](auto const& g)
        {
            using geom_t = util::remove_cref_t<decltype(g)>;
            geom_t o;
            simplify<geom_t>::apply(g, o, max_distance, strategy);
            out = std::move(o);
        }, geometry);
    }
};

template <typename Geometry>
struct simplify<Geometry, geometry_collection_tag>
{
    template <typename Distance, typename Strategy>
    static inline void apply(Geometry const& geometry,
                             Geometry& out,
                             Distance const& max_distance,
                             Strategy const& strategy)
    {
        detail::visit_breadth_first([&](auto const& g)
        {
            using geom_t = util::remove_cref_t<decltype(g)>;
            geom_t o;
            simplify<geom_t>::apply(g, o, max_distance, strategy);
            traits::emplace_back<Geometry>::apply(out, std::move(o));
            return true;
        }, geometry);
    }
};

} // namespace resolve_dynamic

/*!
\brief Simplify a geometry using a specified strategy
\ingroup simplify
\tparam Geometry \tparam_geometry
\tparam Distance A numerical distance measure
\tparam Strategy A type fulfilling a SimplifyStrategy concept
\param strategy A strategy to calculate simplification
\param geometry input geometry, to be simplified
\param out output geometry, simplified version of the input geometry
\param max_distance distance (in units of input coordinates) of a vertex
    to other segments to be removed
\param strategy simplify strategy to be used for simplification, might
    include point-distance strategy

\image html svg_simplify_country.png "The image below presents the simplified country"
\qbk{distinguish,with strategy}
*/
template<typename Geometry, typename Distance, typename Strategy>
inline void simplify(Geometry const& geometry, Geometry& out,
                     Distance const& max_distance, Strategy const& strategy)
{
    concepts::check<Geometry>();

    geometry::clear(out);

    resolve_dynamic::simplify<Geometry>::apply(geometry, out, max_distance, strategy);
}

/*!
\brief Simplify a geometry
\ingroup simplify
\tparam Geometry \tparam_geometry
\tparam Distance \tparam_numeric
\note This version of simplify simplifies a geometry using the default
    strategy (Douglas Peucker),
\param geometry input geometry, to be simplified
\param out output geometry, simplified version of the input geometry
\param max_distance distance (in units of input coordinates) of a vertex
    to other segments to be removed

\qbk{[include reference/algorithms/simplify.qbk]}
 */
template<typename Geometry, typename Distance>
inline void simplify(Geometry const& geometry, Geometry& out,
                     Distance const& max_distance)
{
    concepts::check<Geometry>();

    geometry::simplify(geometry, out, max_distance, default_strategy());
}

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace simplify
{

/*!
\brief Simplify a geometry, using an output iterator
    and a specified strategy
\ingroup simplify
\tparam Geometry \tparam_geometry
\param geometry input geometry, to be simplified
\param out output iterator, outputs all simplified points
\param max_distance distance (in units of input coordinates) of a vertex
    to other segments to be removed
\param strategy simplify strategy to be used for simplification,
    might include point-distance strategy

\qbk{distinguish,with strategy}
\qbk{[include reference/algorithms/simplify.qbk]}
*/
template<typename Geometry, typename OutputIterator, typename Distance, typename Strategy>
inline void simplify_insert(Geometry const& geometry, OutputIterator out,
                            Distance const& max_distance, Strategy const& strategy)
{
    concepts::check<Geometry const>();

    resolve_strategy::simplify_insert<Strategy>::apply(geometry, out, max_distance, strategy);
}

/*!
\brief Simplify a geometry, using an output iterator
\ingroup simplify
\tparam Geometry \tparam_geometry
\param geometry input geometry, to be simplified
\param out output iterator, outputs all simplified points
\param max_distance distance (in units of input coordinates) of a vertex
    to other segments to be removed

\qbk{[include reference/algorithms/simplify_insert.qbk]}
 */
template<typename Geometry, typename OutputIterator, typename Distance>
inline void simplify_insert(Geometry const& geometry, OutputIterator out,
                            Distance const& max_distance)
{
    // Concept: output point type = point type of input geometry
    concepts::check<Geometry const>();
    concepts::check<typename point_type<Geometry>::type>();

    simplify_insert(geometry, out, max_distance, default_strategy());
}

}} // namespace detail::simplify
#endif // DOXYGEN_NO_DETAIL

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_SIMPLIFY_HPP


// Distributed under the MIT License.
// See LICENSE.txt for details.

#pragma once

#include "Evolution/DiscontinuousGalerkin/Limiters/Minmod.hpp"

#include <array>
#include <boost/functional/hash.hpp>
#include <cstdlib>
#include <iterator>
#include <limits>
#include <memory>
#include <pup.h>
#include <type_traits>
#include <unordered_map>
#include <utility>

#include "DataStructures/DataVector.hpp"
#include "DataStructures/Tags.hpp"
#include "DataStructures/Tensor/Tensor.hpp"
#include "Domain/Structure/Direction.hpp"
#include "Domain/Structure/DirectionMap.hpp"
#include "Domain/Structure/Element.hpp"
#include "Domain/Structure/ElementId.hpp"
#include "Domain/Structure/OrientationMap.hpp"
#include "Utilities/ErrorHandling/Assert.hpp"
#include "Evolution/DiscontinuousGalerkin/Limiters/MinmodHelpers.hpp"
#include "Evolution/DiscontinuousGalerkin/Limiters/MinmodType.hpp"
#include "NumericalAlgorithms/LinearOperators/MeanValue.hpp"
#include "NumericalAlgorithms/Spectral/Mesh.hpp"
#include "Utilities/Gsl.hpp"
#include "Utilities/Literals.hpp"
#include "Utilities/Numeric.hpp"
#include "Utilities/TMPL.hpp"

namespace Limiters {
namespace Minmod_detail {

// Implements the minmod limiter for one Tensor<DataVector> at a time.
template <size_t VolumeDim, typename Tag, typename PackagedData>
bool limit_one_tensor(
    const gsl::not_null<DataVector*> u_lin_buffer,
    const gsl::not_null<BufferWrapper<VolumeDim>*> buffer,
    const gsl::not_null<typename Tag::type*> tensor,
    const Limiters::MinmodType minmod_type, const double tvb_constant,
    const Mesh<VolumeDim>& mesh, const Element<VolumeDim>& element,
    const tnsr::I<DataVector, VolumeDim, Frame::Logical>& logical_coords,
    const std::array<double, VolumeDim>& element_size,
    const std::unordered_map<
        std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>, PackagedData,
        boost::hash<std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>>>&
        neighbor_data) noexcept {
  // True if the mesh is linear-order in every direction
  const bool mesh_is_linear = (mesh.extents() == Index<VolumeDim>(2));
  const bool minmod_type_is_linear =
      (minmod_type != Limiters::MinmodType::LambdaPiN);
  const bool using_linear_limiter_on_non_linear_mesh =
      minmod_type_is_linear and not mesh_is_linear;

  // In each direction, average the size of all different neighbors in that
  // direction. Note that only the component of neighor_size that is normal
  // to the face is needed (and, therefore, computed). Note that this average
  // does not depend on the solution on the neighboring elements, so could be
  // precomputed outside of `limit_one_tensor`. Changing the code to
  // precompute the average may or may not be a measurable optimization.
  const auto effective_neighbor_sizes =
      compute_effective_neighbor_sizes(element, neighbor_data);

  bool some_component_was_limited = false;
  for (size_t i = 0; i < tensor->size(); ++i) {
    // In each direction, average the mean of the i'th tensor component over
    // all different neighbors in that direction. This produces one effective
    // neighbor per direction.
    const auto effective_neighbor_means =
        compute_effective_neighbor_means<Tag>(i, element, neighbor_data);

    DataVector& u = (*tensor)[i];
    double u_mean = std::numeric_limits<double>::signaling_NaN();
    std::array<double, VolumeDim> u_limited_slopes{};
    const bool reduce_slopes = minmod_limited_slopes(
        u_lin_buffer, buffer, make_not_null(&u_mean),
        make_not_null(&u_limited_slopes), minmod_type, tvb_constant, u, mesh,
        element, element_size, effective_neighbor_means,
        effective_neighbor_sizes);

    if (reduce_slopes or using_linear_limiter_on_non_linear_mesh) {
      u = u_mean;
      for (size_t d = 0; d < VolumeDim; ++d) {
        u += logical_coords.get(d) * gsl::at(u_limited_slopes, d);
      }
      some_component_was_limited = true;
    }
  }  // end for loop over tensor components

  return some_component_was_limited;
}

}  // namespace Minmod_detail

template <size_t VolumeDim, typename... Tags>
Minmod<VolumeDim, tmpl::list<Tags...>>::Minmod(
    const MinmodType minmod_type, const double tvb_constant,
    const bool disable_for_debugging) noexcept
    : minmod_type_(minmod_type),
      tvb_constant_(tvb_constant),
      disable_for_debugging_(disable_for_debugging) {
  ASSERT(tvb_constant >= 0.0, "The TVB constant must be non-negative.");
}

template <size_t VolumeDim, typename... Tags>
void Minmod<VolumeDim, tmpl::list<Tags...>>::pup(PUP::er& p) noexcept {
  p | minmod_type_;
  p | tvb_constant_;
  p | disable_for_debugging_;
}

template <size_t VolumeDim, typename... Tags>
void Minmod<VolumeDim, tmpl::list<Tags...>>::package_data(
    const gsl::not_null<PackagedData*> packaged_data,
    const typename Tags::type&... tensors, const Mesh<VolumeDim>& mesh,
    const std::array<double, VolumeDim>& element_size,
    const OrientationMap<VolumeDim>& orientation_map) const noexcept {
  if (UNLIKELY(disable_for_debugging_)) {
    // Do not initialize packaged_data
    return;
  }

  const auto wrap_compute_means = [&mesh, &packaged_data](
                                      auto tag, const auto tensor) noexcept {
    for (size_t i = 0; i < tensor.size(); ++i) {
      // Compute the mean using the local orientation of the tensor and mesh:
      // this avoids the work of reorienting the tensor while giving the same
      // result.
      get<::Tags::Mean<decltype(tag)>>(packaged_data->means)[i] =
          mean_value(tensor[i], mesh);
    }
    return '0';
  };
  expand_pack(wrap_compute_means(Tags{}, tensors)...);
  packaged_data->element_size =
      orientation_map.permute_from_neighbor(element_size);
}

template <size_t VolumeDim, typename... Tags>
bool Minmod<VolumeDim, tmpl::list<Tags...>>::operator()(
    const gsl::not_null<std::add_pointer_t<typename Tags::type>>... tensors,
    const Mesh<VolumeDim>& mesh, const Element<VolumeDim>& element,
    const tnsr::I<DataVector, VolumeDim, Frame::Logical>& logical_coords,
    const std::array<double, VolumeDim>& element_size,
    const std::unordered_map<
        std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>, PackagedData,
        boost::hash<std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>>>&
        neighbor_data) const noexcept {
  if (UNLIKELY(disable_for_debugging_)) {
    // Do not modify input tensors
    return false;
  }

  DataVector u_lin_buffer(mesh.number_of_grid_points());
  Minmod_detail::BufferWrapper<VolumeDim> buffer(mesh);

  bool limiter_activated = false;
  const auto wrap_limit_one_tensor = [this, &limiter_activated, &element, &mesh,
                                      &logical_coords, &element_size,
                                      &neighbor_data, &u_lin_buffer, &buffer](
                                         auto tag, const auto tensor) noexcept {
    limiter_activated =
        Minmod_detail::limit_one_tensor<VolumeDim, decltype(tag)>(
            &u_lin_buffer, &buffer, tensor, minmod_type_, tvb_constant_, mesh,
            element, logical_coords, element_size, neighbor_data) or
        limiter_activated;
    return '0';
  };
  expand_pack(wrap_limit_one_tensor(Tags{}, tensors)...);
  return limiter_activated;
}

template <size_t LocalDim, typename LocalTagList>
bool operator==(const Minmod<LocalDim, LocalTagList>& lhs,
                const Minmod<LocalDim, LocalTagList>& rhs) noexcept {
  return lhs.minmod_type_ == rhs.minmod_type_ and
         lhs.tvb_constant_ == rhs.tvb_constant_ and
         lhs.disable_for_debugging_ == rhs.disable_for_debugging_;
}

template <size_t VolumeDim, typename TagList>
bool operator!=(const Minmod<VolumeDim, TagList>& lhs,
                const Minmod<VolumeDim, TagList>& rhs) noexcept {
  return not(lhs == rhs);
}

}  // namespace Limiters


/******************************************************************************
 *
 * AMDiS - Adaptive multidimensional simulations
 *
 * Copyright (C) 2013 Dresden University of Technology. All Rights Reserved.
 * Web: https://fusionforge.zih.tu-dresden.de/projects/amdis
 *
 * Authors:
 * Simon Vey, Thomas Witkowski, Andreas Naumann, Simon Praetorius, et al.
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 *
 * This file is part of AMDiS
 *
 * See also license.opensource.txt in the distribution.
 *
 ******************************************************************************/

/** \file FeSpaceMapping.h */

#ifndef AMDIS_FE_SPACE_MAPPING_H
#define AMDIS_FE_SPACE_MAPPING_H

#include <mpi.h>
#include <vector>
#include <map>
#include <set>
#include <boost/container/flat_map.hpp>
#include <boost/container/flat_set.hpp>

#ifndef HAVE_PARALLEL_MTL4
#include <petsc.h>
#include <petscis.h>
#endif

#include "AMDiS_fwd.h"
#include "parallel/DofComm.hpp"
#include "parallel/MpiHelper.hpp"
#include "parallel/ParallelTypes.hpp"
#include "parallel/StdMpi.hpp"

namespace AMDiS
{
  namespace Parallel
  {

    /** \brief
     * Is used to store matrix indices to all DOFs in rank's subdomain. Thus, the
     * class defines a mapping from component number and DOF index to a global
     * matrix index. This class does not calculate the indices by itself!
     */
    class DofToMatIndex
    {
    public:
      typedef boost::container::flat_map<DegreeOfFreedom, int> MapType;

      DofToMatIndex() {}

      /// Reset the data structure.
      inline void clear()
      {
        data.clear();
      }

      inline void clear(int component)
      {
        data[component].clear();
      }

      /** Add a new mapping for a given DOF.
       *
       * \param[in]   component       Component number for which the mapping
       *                              is defined.
       * \param[in]   dof             DOF index
       * \param[in]   globalMatIndex  Global matrix index.
       */
      inline void add(int component, DegreeOfFreedom dof, int globalMatIndex)
      {
        data[component][dof] = globalMatIndex;
      }

      /// Maps a global DOF index to the global matrix index for a specific
      /// system component number.
      inline int get(int component, DegreeOfFreedom dof) const
      {
        FUNCNAME_DBG("DofToMatIndex::get()");

        std::map<int, MapType>::const_iterator it_component = data.find(component);
        TEST_EXIT_DBG(it_component != data.end())
        ("No mapping data for component %d available!\n", component);

        MapType::const_iterator it_dof = (it_component->second).find(dof);
        TEST_EXIT_DBG(it_dof != (it_component->second).end())
        ("Mapping for DOF %d in component %d does not exist!\n",
         dof, component);

        return it_dof->second;
      }

      /// Returns the number of DOF mappings in one component
      inline int getSize(int component) const
      {
        FUNCNAME_DBG("DofToMatIndex::getSize()");

        std::map<int, MapType>::const_iterator it_component = data.find(component);
        TEST_EXIT_DBG(it_component != data.end())
        ("No mapping data for component %d available!\n", component);
        return (it_component->second).size();
      }

      /// Returns the whole mapping for one component
      inline MapType& getData(int component)
      {
        FUNCNAME_DBG("DofToMatIndex::getData()");

        std::map<int, MapType>::iterator it_component = data.find(component);
        TEST_EXIT_DBG(it_component != data.end())
        ("No mapping data for component %d available!\n", component);
        return it_component->second;
      }

      /// Returns for a given matrix index the component and (local or global) DOF
      /// index. As the data structure is not made for this kind of reverse
      /// search, this is very slow and should be only used for debugging.
      void getReverse(int rowIndex, int& component, int& dofIndex) const;

    private:
      /// The mapping data. For each system component there is a specific map that
      /// maps global DOF indices to global matrix indices.
      std::map<int, MapType> data;
    };

    /// This class defines the parallel mapping of DOFs for one FE space. It is
    /// used by the class \ref ParallelDofMapping to specifiy the mapping for a
    /// set of  FE spaces.
    class ComponentDofMap
    {
    public:
      /// Constructor.
      ComponentDofMap();

      /// Clears all data of the mapping.
      void clear();

      /// Maps a DOF index to both, the local and global index of the mapping. The
      /// global index must not be set.
      MultiIndex& operator[](DegreeOfFreedom d)
      {
        TEST_EXIT_DBG(dofMap.count(d))("DOF %d is not in map!\n", d);

        return dofMap[d];
      }

      /** \brief
       * Searchs the map for a given DOF. It does not fail, if the DOF is not
       * mapped by this mapping. In this case, it returns false. If the DOF is
       * mapped, the result is stored and the function returns true.
       *
       * \param[in]    dof     DOF index for which a mapping is searched.
       * \param[out]   index   In the case that the DOF is mapped, the result
       *                       is stored here.
       */
      inline bool find(DegreeOfFreedom dof, MultiIndex& index)
      {
        DofMap::iterator it = dofMap.find(dof);
        if (it == dofMap.end())
          return false;

        index = it->second;
        return true;
      }

      /// Inserts a new DOF to rank's mapping. The DOF is assumed to be owend by
      /// the rank.
      inline void insertRankDof(DegreeOfFreedom dof0,
                                DegreeOfFreedom dof1 = -1)
      {
        FUNCNAME("ComponentDofMap::insertRankDof()");

        TEST_EXIT_DBG(dofMap.count(dof0) == 0)
        ("DOF %d is already defined in mapping!\n", dof0);

        dofMap[dof0].local = dof1;
        nLocalDofs++;
        if (dof1 != -1)
          nRankDofs++;
      }

      /// Inserts a new DOF to rank's mapping. The DOF exists in rank's subdomain
      /// but is owned by a different rank, thus it is part of an interior boundary.
      inline void insertNonRankDof(DegreeOfFreedom dof0,
                                   DegreeOfFreedom dof1 = -1)
      {
        FUNCNAME("ComponentDofMap::insertNonRankDof()");

        TEST_EXIT_DBG(dofMap.count(dof0) == 0)
        ("DOF %d is already in mapping!\n", dof0);

        dofMap[dof0].local = dof1;
        nLocalDofs++;
        nonRankDofs.insert(dof0);
      }

      /// Checks if a given DOF is in the DOF mapping.
      bool isSet(DegreeOfFreedom dof)
      {
        return static_cast<bool>(dofMap.count(dof));
      }

      /// Checks if a given DOF is a rank owned DOF of the DOF mapping. The DOF
      /// must be a DOF of the mapping (this is not checked here), otherwise the
      /// result is meaningsless.
      bool isRankDof(DegreeOfFreedom dof)
      {
        return !(static_cast<bool>(nonRankDofs.count(dof)));
      }

      bool isRankGlobalDof(int dof)
      {
        return (dof >= rStartDofs && dof < rStartDofs + nRankDofs);
      }

      /// Returns number of DOFs in the mapping.
      unsigned int size()
      {
        return dofMap.size();
      }

      /// Returns the raw data of the mapping.
      DofMap& getMap()
      {
        return dofMap;
      }

      const FiniteElemSpace* getFeSpace() const
      {
        return feSpace;
      }

      const Mesh* getMesh() const
      {
        return mesh;
      }

      DofComm& getDofComm()
      {
        FUNCNAME("ComponentDofMap::getDofComm()");

        TEST_EXIT_DBG(dofComm)("No DOF communicator object defined!\n");

        return *dofComm;
      }

      DofMap::iterator begin()
      {
        return dofMap.begin();
      }

      DofMap::iterator end()
      {
        return dofMap.end();
      }

      /// Recomputes the mapping.
      void update();

      /// Sets the FE space this mapping corresponds to.
      void setFeSpace(const FiniteElemSpace* fe)
      {
        feSpace = fe;
      }

      void setMesh(Mesh* mesh_)
      {
        mesh = mesh_;
      }

      /// Informs the mapping whether a global index must be computed.
      void setGlobalMapping(bool b)
      {
        globalMapping = b;
      }

      /// Sets the DOF communicator.
      void setDofComm(DofComm& dc)
      {
        dofComm = &dc;
      }

      void setMpiComm(MPI::Intracomm& m)
      {
        mpiComm = m;
      }

    private:
      /// Computes a global mapping from the local one.
      void computeGlobalMapping();

      /// Computes the global indices of all DOFs in the mapping that are not owned
      /// by the rank.
      void computeNonLocalIndices();

    private:
      /// DOF communicator for all DOFs on interior boundaries.
      DofComm* dofComm;

      MPI::Intracomm mpiComm;

      /// The FE space this mapping belongs to. This is used only the get the
      /// correct DOF communicator in \ref dofComm.
      const FiniteElemSpace* feSpace;

      const Mesh* mesh;

      /// Mapping data from DOF indices to local and global indices.
      DofMap dofMap;

      /// Set of all DOFs that are in mapping but are not owned by the rank.
      boost::container::flat_set<DegreeOfFreedom> nonRankDofs;

      /// If true, a global index mapping will be computed for all DOFs.
      bool globalMapping;

    public:
      ///
      int nRankDofs, nLocalDofs, nOverallDofs, rStartDofs;
    };

    /// Abstract iterator interface to access containrs containing values of
    /// type \ref ComponentDofMap.
    class ComponentIterator
    {
    public:
      virtual ComponentDofMap& operator*() = 0;

      virtual ComponentDofMap* operator->() = 0;

      virtual bool end() = 0;

      virtual void next() = 0;

      virtual void reset() = 0;
    };

    /// Abstract interface to acces DOF mapping data for each component. Allows
    /// to hide specific implementations, which allow, e.g., to efficiently map
    /// all components having the same FE space to the same DOF mapping.
    class ComponentDataInterface
    {
    public:
      virtual ~ComponentDataInterface() {};
      /// Access via component number
      virtual ComponentDofMap& operator[](int compNumber) = 0;

      /// Acess via FE space pointer
      virtual ComponentDofMap& operator[](const FiniteElemSpace* feSpace) = 0;

      /// Checks whether the DOF mapping is defined for a specific
      /// component number.
      virtual bool isDefinedFor(int compNumber) const = 0;

      /// Returns iterator which iterates over all DOF mappings.
      virtual ComponentIterator& getIteratorData() = 0;

      /// Returns iterator which iterates over the DOF mappings of all
      /// components. If the data is defined for each FE space and more than
      /// one commponent is defined on the same FE space, the iterative will
      /// also iterative multple times over the same DOF mapping object.
      virtual ComponentIterator& getIteratorComponent() = 0;

      /** \brief
       * Initialization of the object.
       *
       * \param[in]   componentSpaces   Set of the FE spaces for each component.
       * \param[in]   feSpaces          Set of all different FE spaces.
       * \param[in]   globalMapping     Mapping is parallel (true) or only
       *                                local (false).
       */
      virtual void init(std::vector<const FiniteElemSpace*>& componentSpaces,
                        std::vector<const FiniteElemSpace*>& feSpaces,
                        bool globalMapping) = 0;

    protected:
      /// The FE spaces for all components.
      std::vector<const FiniteElemSpace*> componentSpaces;

      /// The set of all FE spaces. It uniquly contains all different FE spaces
      /// from \ref feSpaces.
      std::vector<const FiniteElemSpace*> feSpaces;
    };

    /// This class concretizes the interface class \ref ComponentDataInterface. A
    /// DOF mapping is implemented for each component.
    class ComponentData : public ComponentDataInterface
    {
    public:
      ComponentData()
        : iter(this)
      {}

      /// Returns DOF mapping for a given component number.
      ComponentDofMap& operator[](int compNumber)
      {
        TEST_EXIT_DBG(componentData.count(compNumber))
        ("No data for component %d!\n", compNumber);

        return componentData.find(compNumber)->second;
      }

      /// Just to implement the corresponding virtual function in \ref
      /// ComponentDataInterface. Of course it does not work as we have data for
      /// each component. Thus there may be different mappings for the same
      /// FE space.
      ComponentDofMap& operator[](const FiniteElemSpace* feSpace)
      {
        ERROR_EXIT("FE Space access is not possible for component wise defined DOF mappings\n");
        return componentData.find(0)->second;
      }

      /// Return data iterator.
      ComponentIterator& getIteratorData()
      {
        iter.reset();
        return iter;
      }

      /// Return component iterator.
      ComponentIterator& getIteratorComponent()
      {
        iter.reset();
        return iter;
      }

      /// Checks whether the DOF mapping is defined for a specific
      /// component number.
      bool isDefinedFor(int compNumber) const
      {
        return (static_cast<unsigned int>(compNumber) < componentData.size());
      }

      /// Initialization
      void init(std::vector<const FiniteElemSpace*>& f0,
                std::vector<const FiniteElemSpace*>& f1,
                bool globalMapping);

    protected:

      /// Iterator class to iterate over all parallel DOF mappings.
      class Iterator : public ComponentIterator
      {
      public:
        Iterator(ComponentData* d)
          : data(d),
            componentCounter(-1)
        {}

        ComponentDofMap& operator*()
        {
          return (*data)[componentCounter];
        }

        ComponentDofMap* operator->()
        {
          return &((*data)[componentCounter]);
        }

        bool end()
        {
          return (it == data->componentSpaces.end());
        }

        void next()
        {
          ++it;
          ++componentCounter;

          if (it == data->componentSpaces.end())
            componentCounter = -1;
        }

        void reset()
        {
          it = data->componentSpaces.begin();
          componentCounter = 0;
        }

      protected:
        /// Pointer to data class over which the iterator must iterate.
        ComponentData* data;

        /// Internal iterator of the internal data from \ref ComponentData.
        std::vector<const FiniteElemSpace*>::iterator it;

        /// Component number of current iteration.
        int componentCounter;
      };

      /// Data mapping from component numbers to DOF mapping objects.
      std::map<unsigned int, ComponentDofMap> componentData;

      /// Iterator object.
      Iterator iter;

      friend class Iterator;
    };

    /// This class concretizes the interface class \ref ComponentDataInterface. A
    /// DOF mapping is implemented for each finite element space. Thus, different
    /// components sharing the same FE space are handled by the same DOF mapping.
    class FeSpaceData : public ComponentDataInterface
    {
    public:
      FeSpaceData()
        : iterData(this),
          iterComponent(this)
      {}

      /// Returns DOF mapping for a given component number.
      ComponentDofMap& operator[](int compNumber)
      {
        const FiniteElemSpace* feSpace = componentSpaces[compNumber];
        return componentData.find(feSpace)->second;
      }

      /// Returns DOF mapping for a given FE space.
      ComponentDofMap& operator[](const FiniteElemSpace* feSpace)
      {
        TEST_EXIT_DBG(componentData.count(feSpace))("No data for FE space!\n");;

        return componentData.find(feSpace)->second;
      }

      /// Checks whether the DOF mapping is defined for a specific
      /// component number.
      bool isDefinedFor(int compNumber) const
      {
        const FiniteElemSpace* feSpace = componentSpaces[compNumber];
        return static_cast<bool>(componentData.count(feSpace));
      }

      /// Return data iterator.
      ComponentIterator& getIteratorData()
      {
        iterData.reset();
        return iterData;
      }

      /// Return component iterator.
      ComponentIterator& getIteratorComponent()
      {
        iterComponent.reset();
        return iterComponent;
      }

      /// Initialization
      void init(std::vector<const FiniteElemSpace*>& f0,
                std::vector<const FiniteElemSpace*>& f1,
                bool globalMapping);

    protected:

      /// Iterator class to iterate over all parallel DOF mappings.
      class IteratorData : public ComponentIterator
      {
      public:
        IteratorData(FeSpaceData* d)
          : data(d)
        {}

        ComponentDofMap& operator*()
        {
          return (*data)[*it];
        }

        ComponentDofMap* operator->()
        {
          return &((*data)[*it]);
        }

        bool end()
        {
          return (it == data->feSpaces.end());
        }

        void next()
        {
          ++it;
        }

        void reset()
        {
          it = data->feSpaces.begin();
        }

      protected:
        FeSpaceData* data;

        std::vector<const FiniteElemSpace*>::iterator it;
      };

      /// Iterator class to iterate over all component DOF mappings.
      class IteratorComponent : public ComponentIterator
      {
      public:
        IteratorComponent(FeSpaceData* d)
          : data(d)
        {}

        ComponentDofMap& operator*()
        {
          return (*data)[*it];
        }

        ComponentDofMap* operator->()
        {
          return &((*data)[*it]);
        }

        bool end()
        {
          return (it == data->componentSpaces.end());
        }

        void next()
        {
          ++it;
        }

        void reset()
        {
          it = data->componentSpaces.begin();
        }

      protected:
        FeSpaceData* data;

        std::vector<const FiniteElemSpace*>::iterator it;
      };

      std::map<const FiniteElemSpace*, ComponentDofMap> componentData;

      IteratorData iterData;

      IteratorComponent iterComponent;

      friend class IteratorData;

      friend class IteratorComponent;
    };

    /// Used to specify whether a parallel DOF mapping is defined for each
    /// specific component or for each FE space.
    enum DofMappingMode
    {
      COMPONENT_WISE,
      FESPACE_WISE
    };

    /**
     * Implements the mapping from sets of distributed DOF indices to local and
     * global indices. The mapping works for a given set of FE spaces. Furthermore,
     * this class may compute the matrix indices of the set of DOF indices.
     */
    class ParallelDofMapping
    {
    public:
      /** \brief
       * Constructur for parallel DOF mapping.
       *
       * \param[in]  mode            Defines if DOF mapping is defined either per
       *                             component or per FE space.
       * \param[in]  matIndexGlobal  If true, the mat index is defined on global
       *                             DOF indices, otherwise on local ones.
       */
      ParallelDofMapping(DofMappingMode mode,
                         bool matIndexFromGlobal = false);

      ~ParallelDofMapping()
      {
        if (data)
          delete data;
        data = NULL;
      }

      /** \brief
       * Initialize the parallel DOF mapping.
       *
       * \param[in]  componentSpaces    The FE spaces of all components of the
       *                                PDE to be solved.
       * \param[in]  feSpaces           Unique list of FE spaces. Thus, two
       *                                arbitrary elements of this list are always
       *                                different.
       * \param[in]  globalMapping      If true, at least one rank's mapping con-
       *                                taines DOFs that are not owend by the rank.
       */
      void init(std::vector<const FiniteElemSpace*>& componentSpaces,
                std::vector<const FiniteElemSpace*>& feSpaces,
                bool globalMapping = true);

      /// In the case of having only one FE space, this init function can be used.
      void init(const FiniteElemSpace* feSpace,
                bool globalMapping = true)
      {
        std::vector<const FiniteElemSpace*> feSpaces;
        feSpaces.push_back(feSpace);
        init(feSpaces, feSpaces, globalMapping);
      }

      void setMpiComm(MPI::Intracomm& m);

      inline MPI::Intracomm& getMpiComm()
      {
        return mpiComm;
      }

      /// Clear all data.
      void clear(Mesh* mesh = NULL);

      /// Set the DOF communicator objects that are required to exchange information
      /// about DOFs that are on interior boundaries.

      void setDofComms(std::map<Mesh*, MultiLevelDofComm>& dofComms, int level);

      /// Returns the DOF communicator.
      DofComm& getDofComm(const FiniteElemSpace* feSpace)
      {
        return (*data)[feSpace].getDofComm();
      }

      inline bool isMatIndexFromGlobal()
      {
        return needMatIndexFromGlobal;
      }

      /// Access the DOF mapping for a given component number.
      inline ComponentDofMap& operator[](int compNumber)
      {
        return (*data)[compNumber];
      }

      /// Access the DOF mapping for a given FE space
      inline ComponentDofMap& operator[](const FiniteElemSpace* feSpace)
      {
        return (*data)[feSpace];
      }

      /// Checks whether the DOF mapping is defined for a specific
      /// component number.
      inline bool isDefinedFor(int compNumber) const
      {
        return data->isDefinedFor(compNumber);
      }

      /// Returns the number of solution components the mapping is defined on.
      inline int getNumberOfComponents() const
      {
        return static_cast<int>(componentSpaces.size());
      }

      /// Returns \ref nRankDofs, thus the number of DOFs owned by the rank.
      inline int getRankDofs() const
      {
        TEST_EXIT_DBG(nRankDofs >= 0)("Should not happen!\n");

        return nRankDofs;
      }

      inline int getRankDofs(int component) const
      {
        int nDofs = (*data)[component].nRankDofs;
        TEST_EXIT_DBG(nDofs >= 0)("Should not happen!\n");

        return nDofs;
      }

      /// Returns \ref nLocalDofs, thus the number of DOFs in ranks subdomain.
      inline int getLocalDofs() const
      {
        TEST_EXIT_DBG(nLocalDofs >= 0)("Should not happen!\n");

        return nLocalDofs;
      }

      inline int getLocalDofs(int component) const
      {
        int nDofs = (*data)[component].nLocalDofs;
        TEST_EXIT_DBG(nDofs >= 0)("Should not happen!\n");

        return nDofs;
      }

      /// Returns \ref nOverallDofs, thus the number of all DOFs in this mapping.
      inline int getOverallDofs() const
      {
        TEST_EXIT_DBG(nOverallDofs >= 0)("Should not happen!\n");

        return nOverallDofs;
      }

      inline int getOverallDofs(int component) const
      {
        int nDofs = (*data)[component].nOverallDofs;
        TEST_EXIT_DBG(nDofs >= 0)("Should not happen!\n");

        return nDofs;
      }

      /// Returns \ref rStartDofs, thus the smallest global index of a DOF that is
      /// owned by the rank.
      inline int getStartDofs() const
      {
        TEST_EXIT_DBG(rStartDofs >= 0)("Should not happen!\n");

        return rStartDofs;
      }

      inline int getStartDofs(int component) const
      {
        int nDofs = (*data)[component].rStartDofs;
        TEST_EXIT_DBG(nDofs >= 0)("Should not happen!\n");

        return nDofs;
      }

      /// Update the mapping.
      void update(Mesh* mesh = NULL);

      /// Updates only the DOF to matrix index mapping
      void updateMatIndex();

      inline DofToMatIndex::MapType& getMatData(int component)
      {
        return dofToMatIndex.getData(component);
      }

      /// Returns the global matrix index of a given DOF for a given
      /// component number.
      inline int getMatIndex(int ithComponent, DegreeOfFreedom d) const
      {
        return dofToMatIndex.get(ithComponent, d);
      }

      /// Returns the component number and local/global DOF index for a given
      /// matrix row index. Should be used for debugging only!
      inline void getReverseMatIndex(int index, int& component, int& dofIndex) const
      {
        dofToMatIndex.getReverse(index, component, dofIndex);
      }

      /// Returns the local matrix index of a given DOF for a given
      /// component number.
      inline int getLocalMatIndex(int ithComponent, DegreeOfFreedom d) const
      {
        return dofToMatIndex.get(ithComponent, d) - rStartDofs;
      }

      /// Returns the set of unique FE spaces.
      std::vector<const FiniteElemSpace*> getFeSpaces(Mesh* mesh = NULL)
      {
        if (!mesh)
          return feSpaces;

        std::vector<const FiniteElemSpace*> thisMeshSpaces;
        for (size_t i = 0; i < feSpaces.size(); i++)
          if (feSpaces[i]->getMesh() == mesh)
            thisMeshSpaces.push_back(feSpaces[i]);
        return thisMeshSpaces;
      }

      bool hasFeSpace(const FiniteElemSpace* feSpace)
      {
        return (std::find(feSpaces.begin(), feSpaces.end(), feSpace)
                != feSpaces.end());
      }

      // Writes all data of this object to an output stream.
      void serialize(std::ostream& out)
      {
        ERROR_EXIT("MUST BE IMPLEMENTED!\n");
      }

      // Reads the object data from an input stream.
      void deserialize(std::istream& in)
      {
        ERROR_EXIT("MUST BE IMPLEMENTED!\n");
      }

      /// Compute local and global matrix indices.
      void computeMatIndex(bool globalIndex);

#ifndef HAVE_PARALLEL_MTL4
      void createIndexSet(IS& is,
                          int firstComponent,
                          int nComponents);
#endif

      /// Prints out some information about the mapping. May be used during
      /// debugging or parallel solver creation.
      void printInfo();

    protected:
      /// Compute \ref nRankDofs.
      int computeRankDofs();

      /// Compute \ref nLocalDofs.
      int computeLocalDofs();

      /// Compute \ref nOverallDofs.
      int computeOverallDofs();

      /// Compute \ref rStartDofs.
      int computeStartDofs();

    private:
      MPI::Intracomm mpiComm;

      /// Is true if there are DOFs in at least one subdomain that are not owned
      /// by the rank. If the value is false, each rank contains only DOFs that
      /// are also owned by this rank.
      bool globalMapping;

      /// If matrix indices should be computed, this variable defines if the
      /// mapping from DOF indices to matrix row indices is defined on local
      /// or global DOF indices. If true, the mapping is to specify and to use
      /// on global ones, otherwise on local DOF indices.
      /// In most scenarios the mapping stored on local DOF indices is what we
      /// want to have. Only when periodic boundary conditions are used together
      /// with some global matrix approach, the matrix indices must be stored
      /// also for DOFs that are not part of the local subdomain. Thus, the
      /// mapping will be stored on global DOF indices.
      bool needMatIndexFromGlobal;

      /// Maps from components to DOF mappings.
      ComponentDataInterface* data;

      /// Number of DOFs owned by rank.
      int nRankDofs;

      /// Number of DOFs in rank's subdomain.
      int nLocalDofs;

      /// Number of global DOFs (this value is thus the same on all ranks).
      int nOverallDofs;

      /// Smallest global index of a DOF owned by the rank.
      int rStartDofs;

      /// Mapping from global DOF indices to global matrix indices under
      /// consideration of possibly multiple components.
      DofToMatIndex dofToMatIndex;

      /// FE spaces of all components.
      std::vector<const FiniteElemSpace*> componentSpaces;

      /// Set of unique FE spaces.
      std::vector<const FiniteElemSpace*> feSpaces;

      /// Defines the DOF mapping either. The mapping may be defined either for
      /// FE spaces or for component numbers.
      DofMappingMode mode;
    };
  }
}

#endif


#pragma once
#include <Eigen/Core>
#include <Eigen/Dense>
#include <array>
#include <chrono>
#include <iostream>
#include <memory>
#include <tuple>

// This is a base class for turning a bilinear form into an Eigen matvec.
namespace spacetime {
template <typename DblVecIn, typename DblVecOut = DblVecIn>
class BilinearFormBase;
}

// Define a necessary Eigen trait.
namespace Eigen {
namespace internal {
template <typename DblVecIn, typename DblVecOut>
struct traits<spacetime::BilinearFormBase<DblVecIn, DblVecOut>>
    : public Eigen::internal::traits<Eigen::SparseMatrix<double>> {};
}  // namespace internal
}  // namespace Eigen

namespace spacetime {
template <typename DblVecInType, typename DblVecOutType>
class BilinearFormBase
    : public Eigen::EigenBase<BilinearFormBase<DblVecInType, DblVecOutType>> {
 public:
  using DblVecIn = DblVecInType;
  using DblVecOut = DblVecOutType;
  virtual ~BilinearFormBase() {}

  // These are the BilinearForm functions that must be implemented.
  virtual Eigen::VectorXd Apply(const Eigen::VectorXd &v) { assert(false); }
  virtual DblVecIn *vec_in() const { assert(false); }
  virtual DblVecOut *vec_out() const { assert(false); }

  // These are the functions that must be implemented for Eigen to work.
  Eigen::Index rows() const { return vec_out()->container().size(); }
  Eigen::Index cols() const { return vec_in()->container().size(); }

  // Eigen related stuff.
  using Scalar = double;
  using RealScalar = double;
  using StorageIndex = int;
  enum {
    ColsAtCompileTime = Eigen::Dynamic,
    MaxColsAtCompileTime = Eigen::Dynamic,
    IsRowMajor = false
  };

  template <typename Rhs>
  Eigen::VectorXd operator*(const Eigen::MatrixBase<Rhs> &x) const {
    return const_cast<BilinearFormBase *>(this)->Apply(x);
  }

  double TimeApply() const { return time_apply_.count(); };
  double TimePerApply() const {
    if (num_apply_ == 0) return 0;
    return time_apply_.count() / num_apply_;
  };
  double TimeConstruct() const { return time_construct_.count(); }

 protected:
  // Timing debug information.
  std::chrono::duration<double> time_construct_{0};
  std::chrono::duration<double> time_apply_{0};
  size_t num_apply_ = 0;
};

// This class represents the adjoint of a bilinear form.
template <typename BilForm>
class TransposeBilinearForm
    : public BilinearFormBase<typename BilForm::DblVecOut,
                              typename BilForm::DblVecIn> {
 public:
  using DblVecIn = typename BilForm::DblVecOut;
  using DblVecOut = typename BilForm::DblVecIn;

  TransposeBilinearForm(std::shared_ptr<BilForm> bil_form)
      : bil_form_(bil_form) {}

  Eigen::VectorXd Apply(const Eigen::VectorXd &v) final {
    // Debug information.
    auto time_start = std::chrono::steady_clock::now();
    num_apply_++;

    Eigen::VectorXd result = bil_form_->ApplyTranspose(v);

    // Store timing results.
    time_apply_ += std::chrono::duration<double>(
        std::chrono::steady_clock::now() - time_start);

    return result;
  }

  DblVecIn *vec_in() const final { return bil_form_->vec_out(); }
  DblVecOut *vec_out() const final { return bil_form_->vec_in(); }

  auto Transpose() { return bil_form_; }

 protected:
  std::shared_ptr<BilForm> bil_form_;

  using BilinearFormBase<DblVecIn, DblVecOut>::time_apply_;
  using BilinearFormBase<DblVecIn, DblVecOut>::num_apply_;
};

// This class represents the sum of two bilinear forms.
template <typename BilFormA, typename BilFormB>
class SumBilinearForm : public BilinearFormBase<typename BilFormA::DblVecIn,
                                                typename BilFormA::DblVecOut> {
 public:
  using DblVecIn = typename BilFormA::DblVecIn;
  using DblVecOut = typename BilFormA::DblVecOut;

  SumBilinearForm(std::shared_ptr<BilFormA> a, std::shared_ptr<BilFormB> b)
      : a_(a), b_(b) {
    assert(a->vec_in() == b->vec_in());
    assert(a->vec_out() == b->vec_out());
  }

  Eigen::VectorXd Apply(const Eigen::VectorXd &v) final {
    // Debug information.
    auto time_start = std::chrono::steady_clock::now();
    num_apply_++;

    Eigen::VectorXd result = a_->Apply(v) + b_->Apply(v);

    // Store timing results.
    time_apply_ += std::chrono::duration<double>(
        std::chrono::steady_clock::now() - time_start);

    return result;
  }
  DblVecIn *vec_in() const final { return a_->vec_in(); }
  DblVecOut *vec_out() const final { return a_->vec_out(); }
  auto sigma() { return a_->sigma(); }
  auto theta() { return a_->theta(); }

  auto Transpose() {
    auto a_t = a_->Transpose();
    auto b_t = b_->Transpose();
    return std::make_shared<
        SumBilinearForm<typename decltype(a_t)::element_type,
                        typename decltype(b_t)::element_type>>(a_t, b_t);
  }

 protected:
  std::shared_ptr<BilFormA> a_;
  std::shared_ptr<BilFormB> b_;

  using BilinearFormBase<DblVecIn, DblVecOut>::time_apply_;
  using BilinearFormBase<DblVecIn, DblVecOut>::num_apply_;
};

// This class represents a negative bilinear form (-BilForm).
template <typename BilForm>
class NegativeBilinearForm
    : public BilinearFormBase<typename BilForm::DblVecIn,
                              typename BilForm::DblVecOut> {
 public:
  using DblVecIn = typename BilForm::DblVecIn;
  using DblVecOut = typename BilForm::DblVecOut;
  NegativeBilinearForm(std::shared_ptr<BilForm> bil_form)
      : bil_form_(bil_form) {}

  Eigen::VectorXd Apply(const Eigen::VectorXd &v) final {
    return -bil_form_->Apply(v);
  }
  DblVecIn *vec_in() const final { return bil_form_->vec_in(); }
  DblVecOut *vec_out() const final { return bil_form_->vec_out(); }

  auto Transpose() {
    auto b_t = bil_form_->Transpose();
    return std::make_shared<
        NegativeBilinearForm<typename decltype(b_t)::element_type>>(bil_form_);
  }

 protected:
  std::shared_ptr<BilForm> bil_form_;
};

/**
 * The Schur complement operator for the matrix [A B; B^t G].
 * x \mapsto (B.T A^{-1} B + G) x.
 */
template <typename Ainv, typename B, typename BT, typename G>
class SchurBilinearForm
    : public BilinearFormBase<typename B::DblVecIn, typename BT::DblVecOut> {
 public:
  using DblVecIn = typename B::DblVecIn;
  using DblVecOut = typename BT::DblVecOut;
  SchurBilinearForm(std::shared_ptr<Ainv> a_inv, std::shared_ptr<B> b,
                    std::shared_ptr<BT> bt, std::shared_ptr<G> g)
      : a_inv_(a_inv), b_(b), bt_(bt), g_(g) {
    assert(b_->vec_in() == g_->vec_in());
    assert(a_inv_->vec_in() == b_->vec_out());
    assert(bt_->vec_in() == a_inv_->vec_out());
    assert(bt_->vec_out() == g_->vec_out());
  }

  Eigen::VectorXd Apply(const Eigen::VectorXd &v) final {
    // Debug information.
    auto time_start = std::chrono::steady_clock::now();
    num_apply_++;

    Eigen::VectorXd result;
    result = b_->Apply(v);
    result = a_inv_->Apply(result);
    result = bt_->Apply(result);

    result += g_->Apply(v);

    // Store timing results.
    time_apply_ += std::chrono::duration<double>(
        std::chrono::steady_clock::now() - time_start);

    return result;
  }

  DblVecIn *vec_in() const final { return b_->vec_in(); }
  DblVecOut *vec_out() const final { return bt_->vec_out(); }

 protected:
  std::shared_ptr<Ainv> a_inv_;
  std::shared_ptr<B> b_;
  std::shared_ptr<BT> bt_;
  std::shared_ptr<G> g_;

  using BilinearFormBase<DblVecIn, DblVecOut>::time_apply_;
  using BilinearFormBase<DblVecIn, DblVecOut>::num_apply_;
};
}  // namespace spacetime