#ifndef PYBNESIAN_LEARNING_INDEPENDENCES_CONTINUOUS_RCOT_HPP
#define PYBNESIAN_LEARNING_INDEPENDENCES_CONTINUOUS_RCOT_HPP

#include <random>
#include <Eigen/Eigenvalues>
#include <learning/independences/independence.hpp>
#include <util/math_constants.hpp>
#include <util/basic_eigen_ops.hpp>
#include <util/chisquaresum.hpp>

using learning::independences::IndependenceTest;

namespace learning::independences::continuous {

template <typename MatrixType>
typename MatrixType::Scalar rf_sigma_impl(MatrixType& m) {
    using Scalar = typename MatrixType::Scalar;
    using VectorType = Matrix<Scalar, Dynamic, 1>;
    auto r = std::min(static_cast<decltype(m.rows())>(500), m.rows());
    VectorType distances(r * (r - 1) / 2);

    for (int i = r - 1, j = 0; i > 0; --i, j += i) {
        distances.segment(j, i) = (m.topRows(i).rowwise() - m.row(i)).matrix().rowwise().norm();
    }

    // Compute median
    int median_index = distances.rows() / 2;

    double median;
    if (distances.rows() % 2 == 1) {
        std::nth_element(distances.data(), distances.data() + median_index, distances.data() + distances.rows());
        median = distances[median_index];
    } else {
        std::sort(distances.data(), distances.data() + distances.rows());
        median = 0.5 * (distances[median_index - 1] + distances[median_index]);
    }

    if (median == 0) median = 1;

    return median;
}

class RCoT : public IndependenceTest {
public:
    RCoT(const DataFrame& df, int random_fourier_xy = 5, int random_fourier_z = 100)
        : m_df(df.normalize()),
          m_num_random_fourier_xy(random_fourier_xy),
          m_num_random_fourier_z(random_fourier_z),
          m_dfourier_x(),
          m_dfourier_y(),
          m_dfourier_z(),
          m_dsigma(),
          m_ffourier_x(),
          m_ffourier_y(),
          m_ffourier_z(),
          m_fsigma() {
        auto continuous_indices = df.continuous_columns();

        if (continuous_indices.size() < 2) {
            throw std::invalid_argument("DataFrame does not contain enough continuous columns.");
        }

        auto type = m_df.same_type(continuous_indices);

        switch (type->id()) {
            case Type::DOUBLE: {
                m_dfourier_x = MatrixXd(df.num_rows(), m_num_random_fourier_xy);
                m_dfourier_y = MatrixXd(df.num_rows(), m_num_random_fourier_xy);
                m_dfourier_z = MatrixXd(df.num_rows(), m_num_random_fourier_z);
                m_tmp_dcov = MatrixXd(df.num_rows(), m_num_random_fourier_xy * m_num_random_fourier_xy);
                m_dsigma = VectorXd(df->num_columns());

                for (auto c : continuous_indices) {
                    if (m_df.null_count(c) == 0) {
                        auto x_vec = m_df.to_eigen<false, arrow::DoubleType, false>(c);
                        m_dsigma(c) = rf_sigma_impl(*x_vec);
                    }
                }

                break;
            }
            case Type::FLOAT: {
                m_ffourier_x = MatrixXf(df.num_rows(), m_num_random_fourier_xy);
                m_ffourier_y = MatrixXf(df.num_rows(), m_num_random_fourier_xy);
                m_ffourier_z = MatrixXf(df.num_rows(), m_num_random_fourier_z);
                m_tmp_fcov = MatrixXf(df.num_rows(), m_num_random_fourier_xy * m_num_random_fourier_xy);
                m_fsigma = VectorXf(df->num_columns());

                for (auto c : continuous_indices) {
                    if (m_df.null_count(c) == 0) {
                        auto x_vec = m_df.to_eigen<false, arrow::FloatType, false>(c);
                        m_fsigma(c) = rf_sigma_impl(*x_vec);
                    }
                }

                break;
            }
            default:
                throw std::runtime_error("[RCoT] Unreachable code");
        }
    }

    double pvalue(const std::string& x, const std::string& y) const override;
    template <typename ArrowType>
    double pvalue(const std::string& x, const std::string& y) const;

    double pvalue(const std::string& x, const std::string& y, const std::string& z) const override;
    template <typename ArrowType>
    double pvalue(const std::string& x, const std::string& y, const std::string& z) const;

    double pvalue(const std::string& x, const std::string& y, const std::vector<std::string>& z) const override;
    template <typename ArrowType>
    double pvalue(const std::string& x, const std::string& y, const std::vector<std::string>& z) const;

    int num_variables() const override { return m_df->num_columns(); }

    std::vector<std::string> variable_names() const override { return m_df.column_names(); }

    const std::string& name(int i) const override { return m_df.name(i); }

    bool has_variables(const std::string& name) const override { return m_df.has_columns(name); }

    bool has_variables(const std::vector<std::string>& cols) const override { return m_df.has_columns(cols); }

private:
    template <typename Scalar>
    Scalar rf_sigma(int index) const {
        if constexpr (std::is_same_v<Scalar, double>)
            return m_dsigma(index);
        else
            return m_fsigma(index);
    }

    template <typename Scalar>
    Matrix<Scalar, Dynamic, Dynamic>& fourier_x() const {
        if constexpr (std::is_same_v<Scalar, double>)
            return m_dfourier_x;
        else
            return m_ffourier_x;
    }

    template <typename Scalar>
    Matrix<Scalar, Dynamic, Dynamic>& fourier_y() const {
        if constexpr (std::is_same_v<Scalar, double>)
            return m_dfourier_y;
        else
            return m_ffourier_y;
    }

    template <typename Scalar>
    Matrix<Scalar, Dynamic, Dynamic>& fourier_z() const {
        if constexpr (std::is_same_v<Scalar, double>)
            return m_dfourier_z;
        else
            return m_ffourier_z;
    }

    template <typename Scalar>
    Matrix<Scalar, Dynamic, Dynamic>& tmp_cov() const {
        if constexpr (std::is_same_v<Scalar, double>)
            return m_tmp_dcov;
        else
            return m_tmp_fcov;
    }

    template <typename Mat>
    Matrix<typename Mat::Scalar, Dynamic, 1> eigenvalues_covariance(Mat& fourier_x, Mat& fourier_y) const;

    template <typename VectorType, typename FeatureType>
    double RIT_impl(
        VectorType& x, VectorType& y, FeatureType& feat_x, FeatureType& feat_y, double sigma_x, double sigma_y) const;
    template <bool contains_null, typename VectorType>
    double RIT(int x_index, int y_index, VectorType& x, VectorType& y) const;

    template <typename VectorType, typename MatType, typename FeatureType>
    double TestWithZ_impl(VectorType& x,
                          VectorType& y,
                          MatType& z,
                          FeatureType& feat_x,
                          FeatureType& feat_y,
                          FeatureType& feat_z,
                          double sigma_x,
                          double sigma_y,
                          double sigma_z) const;

    template <bool contains_null, typename VectorType>
    double RSingleZ(int x_index, int y_index, int z_index, VectorType& x, VectorType& y, VectorType& z) const;

    template <bool contains_null, typename VectorType, typename MatType>
    double RMultiZ(int x_index, int y_index, VectorType& x, VectorType& y, MatType& z) const;

    DataFrame m_df;
    int m_num_random_fourier_xy;
    int m_num_random_fourier_z;
    // Cache fourier matrices and sigmas (double or float).
    mutable MatrixXd m_dfourier_x;
    mutable MatrixXd m_dfourier_y;
    mutable MatrixXd m_dfourier_z;
    mutable MatrixXd m_tmp_dcov;
    VectorXd m_dsigma;
    mutable MatrixXf m_ffourier_x;
    mutable MatrixXf m_ffourier_y;
    mutable MatrixXf m_ffourier_z;
    mutable MatrixXf m_tmp_fcov;
    VectorXf m_fsigma;
};

template <typename InputMatrix, typename OutputMatrix>
void random_fourier_features(InputMatrix& m,
                             typename InputMatrix::Scalar sigma,
                             int num_features,
                             OutputMatrix& fourier_features) {
    static_assert(std::is_same_v<typename InputMatrix::Scalar, typename OutputMatrix::Scalar>,
                  "Input/Output matrices must have the same type");

    using Scalar = typename InputMatrix::Scalar;
    using MatrixType = Matrix<Scalar, Dynamic, Dynamic>;
    using VectorType = Matrix<Scalar, Dynamic, 1>;

    MatrixType W(m.cols(), num_features);
    VectorType b(num_features);

    std::mt19937 rng(std::random_device{}());
    std::normal_distribution<Scalar> normal;
    for (auto j = 0; j < W.cols(); ++j) {
        for (auto i = 0; i < W.rows(); ++i) {
            W(i, j) = normal(rng);
        }
    }
    W *= (1 / sigma);

    std::uniform_real_distribution<Scalar> unif;
    for (auto i = 0; i < num_features; ++i) {
        b(i) = unif(rng);
    }
    b *= 2 * util::pi<Scalar>;

    fourier_features.noalias() = (m * W).rowwise() + b.transpose();
    fourier_features = fourier_features.array().cos().matrix();
    fourier_features = fourier_features * util::root_two<Scalar>;
}

template <typename Mat, typename TmpMat>
Matrix<typename Mat::Scalar, Dynamic, 1> eigenvalues_covariance_impl(Mat& fourier_x, Mat& fourier_y, TmpMat& tmp_mat) {
    using Scalar = typename Mat::Scalar;
    using MatrixType = Matrix<Scalar, Dynamic, Dynamic>;

    for (int i = 0; i < fourier_x.cols(); ++i) {
        tmp_mat.block(0, i * fourier_y.cols(), tmp_mat.rows(), fourier_y.cols()) =
            fourier_y.array().colwise() * fourier_x.col(i).array();
    }
    auto cov = util::sse_mat(tmp_mat) * (1 / static_cast<Scalar>(fourier_x.rows()));
    auto eigen_solver = Eigen::SelfAdjointEigenSolver<MatrixType>(cov, Eigen::DecompositionOptions::EigenvaluesOnly);
    return eigen_solver.eigenvalues();
}

template <typename Mat>
Matrix<typename Mat::Scalar, Dynamic, 1> RCoT::eigenvalues_covariance(Mat& fourier_x, Mat& fourier_y) const {
    using Scalar = typename Mat::Scalar;
    auto& cc = tmp_cov<Scalar>();

    if (fourier_x.rows() != cc.rows()) {
        auto tmp = cc.topRows(fourier_x.rows());
        return eigenvalues_covariance_impl(fourier_x, fourier_y, tmp);
    } else {
        return eigenvalues_covariance_impl(fourier_x, fourier_y, cc);
    }
}

template <typename VectorType>
Matrix<typename VectorType::Scalar, Dynamic, 1> filter_positive_elements(const VectorType& v) {
    using Scalar = typename VectorType::Scalar;
    using NewVectorType = Matrix<Scalar, Dynamic, 1>;
    std::vector<Scalar> positive;
    for (int i = 0; i < v.rows(); ++i) {
        if (v(i) > 0) positive.push_back(v(i));
    }

    NewVectorType res(positive.size());
    for (size_t i = 0; i < positive.size(); ++i) {
        res(i) = positive[i];
    }

    return res;
}

template <typename VectorType, typename FeatureType>
double RCoT::RIT_impl(
    VectorType& x, VectorType& y, FeatureType& feat_x, FeatureType& feat_y, double sigma_x, double sigma_y) const {
    random_fourier_features(x, sigma_x, m_num_random_fourier_xy, feat_x);
    random_fourier_features(y, sigma_y, m_num_random_fourier_xy, feat_y);

    util::normalize_cols(feat_x);
    util::normalize_cols(feat_y);

    auto Cxy = util::cov(feat_x, feat_y);
    auto sta = x.rows() * Cxy.squaredNorm();
    auto eigs = eigenvalues_covariance(feat_x, feat_y);
    auto pos_eigs = filter_positive_elements(eigs);

    if (pos_eigs.rows() < 4) {
        auto pvalue = util::hbe_complement(pos_eigs, sta);
        if (pvalue < 0) return 0;
        return pvalue;
    }

    try {
        auto pvalue = util::lpb4_complement(pos_eigs, sta);
        if (pvalue < 0) return 0;
        return pvalue;
    } catch (std::exception&) {
        auto pvalue = util::hbe_complement(pos_eigs, sta);
        if (pvalue < 0) return 0;
        return pvalue;
    }
}

template <bool contains_null, typename VectorType>
double RCoT::RIT(int x_index, int y_index, VectorType& x, VectorType& y) const {
    using Scalar = typename VectorType::Scalar;

    if constexpr (contains_null) {
        Scalar sigma_x = rf_sigma_impl(x);
        Scalar sigma_y = rf_sigma_impl(y);

        auto feat_x = fourier_x<Scalar>().topRows(x.rows());
        auto feat_y = fourier_y<Scalar>().topRows(y.rows());

        return RIT_impl(x, y, feat_x, feat_y, sigma_x, sigma_y);
    } else {
        Scalar sigma_x = rf_sigma<Scalar>(x_index);
        Scalar sigma_y = rf_sigma<Scalar>(y_index);

        auto& feat_x = fourier_x<Scalar>();
        auto& feat_y = fourier_y<Scalar>();

        return RIT_impl(x, y, feat_x, feat_y, sigma_x, sigma_y);
    }
}

template <typename VectorType, typename MatType, typename FeatureType>
double RCoT::TestWithZ_impl(VectorType& x,
                            VectorType& y,
                            MatType& z,
                            FeatureType& feat_x,
                            FeatureType& feat_y,
                            FeatureType& feat_z,
                            double sigma_x,
                            double sigma_y,
                            double sigma_z) const {
    random_fourier_features(x, sigma_x, m_num_random_fourier_xy, feat_x);
    random_fourier_features(y, sigma_y, m_num_random_fourier_xy, feat_y);
    random_fourier_features(z, sigma_z, m_num_random_fourier_z, feat_z);

    util::normalize_cols(feat_x);
    util::normalize_cols(feat_y);
    util::normalize_cols(feat_z);

    auto Cxy = util::cov(feat_x, feat_y);

    auto Czz = util::cov(feat_z);
    Czz.diagonal().array() += 1e-10;

    auto i_Czz = Czz.inverse();

    auto Cxz = util::cov(feat_x, feat_z);
    auto Czy = util::cov(feat_z, feat_y);

    auto z_i_Czz = feat_z * i_Czz;
    feat_x = feat_x - z_i_Czz * Cxz.transpose();
    feat_y = feat_y - z_i_Czz * Czy;

    auto Cxy_z = Cxy - Cxz * i_Czz * Czy;

    auto sta = x.rows() * Cxy_z.squaredNorm();
    auto eigs = eigenvalues_covariance(feat_x, feat_y);
    auto pos_eigs = filter_positive_elements(eigs);

    if (m_num_random_fourier_z == 1 || pos_eigs.rows() < 4) {
        auto pvalue = util::hbe_complement(pos_eigs, sta);
        if (pvalue < 0) return 0;
        return pvalue;
    }

    try {
        auto pvalue = util::lpb4_complement(pos_eigs, sta);
        if (pvalue < 0) return 0;
        return pvalue;
    } catch (std::exception&) {
        auto pvalue = util::hbe_complement(pos_eigs, sta);
        if (pvalue < 0) return 0;
        return pvalue;
    }
}

template <bool contains_null, typename VectorType>
double RCoT::RSingleZ(int x_index, int y_index, int z_index, VectorType& x, VectorType& y, VectorType& z) const {
    using Scalar = typename VectorType::Scalar;

    if constexpr (contains_null) {
        Scalar sigma_x = rf_sigma_impl(x);
        Scalar sigma_y = rf_sigma_impl(y);
        Scalar sigma_z = rf_sigma_impl(z);

        auto feat_x = fourier_x<Scalar>().topRows(x.rows());
        auto feat_y = fourier_y<Scalar>().topRows(y.rows());
        auto feat_z = fourier_z<Scalar>().topRows(z.rows());

        return TestWithZ_impl(x, y, z, feat_x, feat_y, feat_z, sigma_x, sigma_y, sigma_z);
    } else {
        Scalar sigma_x = rf_sigma<Scalar>(x_index);
        Scalar sigma_y = rf_sigma<Scalar>(y_index);
        Scalar sigma_z = rf_sigma<Scalar>(z_index);

        auto& feat_x = fourier_x<Scalar>();
        auto& feat_y = fourier_y<Scalar>();
        auto& feat_z = fourier_z<Scalar>();

        return TestWithZ_impl(x, y, z, feat_x, feat_y, feat_z, sigma_x, sigma_y, sigma_z);
    }
}

template <bool contains_null, typename VectorType, typename MatType>
double RCoT::RMultiZ(int x_index, int y_index, VectorType& x, VectorType& y, MatType& z) const {
    using Scalar = typename VectorType::Scalar;

    if constexpr (contains_null) {
        Scalar sigma_x = rf_sigma_impl(x);
        Scalar sigma_y = rf_sigma_impl(y);
        Scalar sigma_z = rf_sigma_impl(z);

        auto feat_x = fourier_x<Scalar>().topRows(x.rows());
        auto feat_y = fourier_y<Scalar>().topRows(y.rows());
        auto feat_z = fourier_z<Scalar>().topRows(z.rows());

        return TestWithZ_impl(x, y, z, feat_x, feat_y, feat_z, sigma_x, sigma_y, sigma_z);
    } else {
        Scalar sigma_x = rf_sigma<Scalar>(x_index);
        Scalar sigma_y = rf_sigma<Scalar>(y_index);
        Scalar sigma_z = rf_sigma_impl(z);

        auto& feat_x = fourier_x<Scalar>();
        auto& feat_y = fourier_y<Scalar>();
        auto& feat_z = fourier_z<Scalar>();

        return TestWithZ_impl(x, y, z, feat_x, feat_y, feat_z, sigma_x, sigma_y, sigma_z);
    }
}

using DynamicRCoT = DynamicIndependenceTestAdaptator<RCoT>;

}  // namespace learning::independences::continuous

#endif  // PYBNESIAN_LEARNING_INDEPENDENCES_CONTINUOUS_RCOT_HPP


#include <armadillo>
#include "../Array.hpp"
#include "../Function.hpp"
#include "../NodePool.hpp"
#include "../Operator.hpp"
#include "../IO.hpp"
#include "gtest/gtest.h"
#include <boost/filesystem.hpp>
#include <unistd.h>

namespace gt=::testing;

#define MRange gt::Range(1, 4)
#define NRange gt::Range(1, 4)
#define PRange gt::Range(1, 4)

class MPITest : public gt::TestWithParam
<std::tr1::tuple<MPI_Comm, int, int, int> > {
protected:
  virtual void SetUp() {

    comm = std::tr1::get<0>(GetParam());

    m  = std::tr1::get<1>(GetParam());
    n  = std::tr1::get<2>(GetParam());
    p  = std::tr1::get<3>(GetParam());

    MPI_Comm_size(comm, &size);
    MPI_Comm_rank(comm, &rank);
  }

  virtual void TearDown() {

  }

  MPI_Comm comm;
  int size;
  int rank;

  int m;
  int n;
  int p;
};

template<class T>
arma::Cube<T> make_seqs(int m, int n, int p){
  arma::Cube<T> v1(m, n, p);

  int cnt = 0;
  for(int k = 0; k < p; ++k){
    for(int j = 0; j < n; ++j){
      for(int i = 0; i < m; ++i){
        v1(i, j, k) = cnt;
        cnt++;
      }
    }
  }

  return v1;
}

///:set dtypes = ['int', 'float', 'double']
///:set fdtypes = ['float', 'double']

using namespace oa::funcs;

namespace{
  TEST(Array, Basic){
    int rank, size;
    MPI_Comm_rank(MPI_COMM_WORLD, &rank);
    MPI_Comm_size(MPI_COMM_WORLD, &size);

    ArrayPtr A1 = ones(MPI_COMM_SELF, {10,10,10}, 1, DATA_INT);
    if(rank == 0){
      EXPECT_FALSE(A1->is_scalar());
      EXPECT_FALSE(A1->is_seqs_scalar());
      EXPECT_TRUE(A1->is_seqs());
      EXPECT_EQ(A1->shape(), Shape({10,10,10}));
    }

    ArrayPtr A2 = ones(MPI_COMM_WORLD, {1,1,1}, 1, DATA_INT);
    if(rank == 0){
      EXPECT_TRUE(A2->is_scalar());
      EXPECT_FALSE(A2->is_seqs_scalar());
      EXPECT_FALSE(A2->is_seqs());
      EXPECT_EQ(A2->shape(), Shape({1,1,1}));
    }

    ArrayPtr A3 = ones(MPI_COMM_WORLD, {1,1,1}, 1, DATA_INT);
    if(rank == 0){
      EXPECT_TRUE(A3->is_scalar());
      EXPECT_FALSE(A3->is_seqs_scalar());
      EXPECT_FALSE(A3->is_seqs());
      EXPECT_EQ(A3->shape(), Shape({1,1,1}));
    }

    ArrayPtr A4 = ones(MPI_COMM_SELF, {1,1,1}, 1, DATA_INT);
    if(rank == 0){
      EXPECT_TRUE(A4->is_scalar());
      EXPECT_TRUE(A4->is_seqs_scalar());
      EXPECT_TRUE(A4->is_seqs());
      EXPECT_EQ(A4->shape(), Shape({1,1,1}));
    }

  }

  //test save and load function
  TEST_P(MPITest, InputAndOutput){
    ArrayPtr A = oa::funcs::seqs(MPI_COMM_WORLD, {m,n,p}, 1);
    oa::io::save(A, "/tmp/A.nc", "data");

    // const boost::filesystem::path fileName("/tmp/A.nc");

    // bool b = boost::filesystem::exists(boost::filesystem::status(fileName));
    // std::cout<<b<<std::endl;

    if(rank == 0){
      std::ifstream infile("/tmp/A.nc");
      EXPECT_TRUE(infile.good());
    }

    ArrayPtr B = oa::io::load("/tmp/A.nc", "data", MPI_COMM_WORLD);

    ArrayPtr A1 = to_rank0(A);
    ArrayPtr B1 = to_rank0(B);

    if(rank == 0){
      EXPECT_TRUE(oa::funcs::is_equal(A1, B1));
    }
  }

  TEST_P(MPITest, ArrayCreation){
    ///:for t in dtypes
    {
      ArrayPtr A1 = oa::funcs::to_rank0(oa::funcs::seqs(comm, {m, n, p}, 1,
                      oa::utils::dtype<${t}$>::type));
      arma::Cube<${t}$> B1 = make_seqs<${t}$>(m, n, p);

      ArrayPtr A2 =
        oa::funcs::to_rank0(oa::funcs::consts<${t}$>(comm, {m, n, p},
                        ${t}$(2),
                        oa::utils::dtype<${t}$>::type));
      arma::Cube<${t}$> B2(m, n, p);
      B2.fill(${t}$(2));

      if(rank == 0){
        EXPECT_TRUE(oa::funcs::is_equal(A1, B1));
        EXPECT_TRUE(oa::funcs::is_equal(A2, B2));
      }
    }
    ///:endfor
  }

  TEST_P(MPITest, BasicMath_Arrray_Array){
    ///:for t1 in dtypes
    ///:for t2 in dtypes
    {
      DataType dt1  = oa::utils::dtype<${t1}$>::type;
      DataType dt2  = oa::utils::dtype<${t2}$>::type;

      NodePtr N1 = oa::ops::new_node(oa::funcs::seqs(comm, {m, n, p}, 0, dt1));
      NodePtr N2 = oa::ops::new_node(oa::funcs::consts(comm,
                      {m, n, p},
                      ${t2}$(2.0), 0));

      typedef otype<${t1}$, ${t2}$>::value result_type;
      arma::Cube<result_type> C3;
      arma::Cube<result_type> C1 = make_seqs<result_type>(m, n, p);
      arma::Cube<result_type> C2(m,n,p);
      C2.fill(result_type(2));

      ///:for o in [['+','PLUS'], ['-', 'MINUS'], ['%','MULT'], ['/', 'DIVD']]
      {
        NodePtr N3 = oa::ops::new_node(TYPE_${o[1]}$, N1, N2);
        ArrayPtr A3 = oa::funcs::to_rank0(oa::ops::eval(N3));

        C3 = C1 ${o[0]}$ C2;

        // N1->get_data()->display("A1");
        // N2->get_data()->display("A2");

        if(rank == 0){
          // A3->display("A3");
          // std::cout<<"C3"<<std::endl<<C3<<std::endl;
          // std::cout<<"Operation:${o[1]}$"<<std::endl;

          EXPECT_TRUE(oa::funcs::is_equal(A3, C3));
        }
      }
      ///:endfor
    }
    ///:endfor
    ///:endfor
  }

  TEST_P(MPITest, BasicMath_Arrray_Scalar){
    ///:for t1 in dtypes
    ///:for t2 in dtypes
    {
      DataType dt1  = oa::utils::dtype<${t1}$>::type;
      DataType dt2  = oa::utils::dtype<${t2}$>::type;

      NodePtr N1 = oa::ops::new_node(oa::funcs::consts(comm, {m, n, p},
                      ${t1}$(3.0), 0));
      NodePtr N2 = oa::ops::new_node(oa::funcs::consts(MPI_COMM_SELF,
                      {1, 1, 1}, ${t2}$(2), 0));

      typedef otype<${t1}$, ${t2}$>::value result_type;
      arma::Cube<result_type> C3, C4;
      arma::Cube<result_type> C1 = arma::ones<arma::Cube<result_type> >(m, n, p) * 3.0;
      result_type  C2 = result_type(2);

      ///:for o in [['+','PLUS'], ['-', 'MINUS'], ['*','MULT'], ['/', 'DIVD']]
      {
      	NodePtr N3 = oa::ops::new_node(TYPE_${o[1]}$, N1, N2);
      	NodePtr N4 = oa::ops::new_node(TYPE_${o[1]}$, N2, N1);
	ArrayPtr A3 = oa::funcs::to_rank0(oa::ops::eval(N3));
	ArrayPtr A4 = oa::funcs::to_rank0(oa::ops::eval(N4));

	C3 = C1 ${o[0]}$ C2;
        C4 = C2 ${o[0]}$ C1;

	if(rank == 0){
          // std::cout<<"${o[1]}$"<<"    ${o[0]}$"<<std::endl;
          // std::cout<<C3<<std::endl;
          // A4->display("A4");
          // MPI_Barrier(A4->get_partition()->get_comm());

	  EXPECT_TRUE(oa::funcs::is_equal(A3, C3));
	  EXPECT_TRUE(oa::funcs::is_equal(A4, C4));
	}
      }
      ///:endfor
    }
    ///:endfor
    ///:endfor
  }

  TEST_P(MPITest, GhostUpdate){
    ///:for t in dtypes
    {
      ArrayPtr A1 =
        oa::funcs::seqs(comm,{m*5, n*5, p*5}, 1, oa::utils::dtype<${t}$>::type);

      // ArrayPtr A2 =
      //   oa::funcs::seqs(comm,{m*5, n*5, p*5}, 2, oa::utils::dtype<${t}$>::type);

      std::vector<MPI_Request> reqs;
      update_ghost_start(A1, reqs, -1);
      update_ghost_end(reqs);
      reqs.clear();

      // update_ghost_start(A2, reqs, -1);
      // update_ghost_end(reqs);
      // reqs.clear();

      // update_ghost_start(A3, reqs, -1);
      // update_ghost_end(reqs);
      // reqs.clear();

      // if(A1->local_size() > 0){
      //   arma::Cube<${t}$> C1 = oa::utils::make_cube<${t}$>(A1->buffer_shape(),
      //                                                      A1->get_buffer());
      // }
    }
    ///:endfor
  }

  TEST_P(MPITest, MinMax){

    ///:for t in dtypes
    {
      int sw = NO_STENCIL;
      DataType dt = oa::utils::dtype<${t}$>::type;
      ArrayPtr A = oa::funcs::rands(comm, {m,n,p}, sw, dt);
      NodePtr NA = oa::ops::new_node(A);
      //NA->display("NA");

      NodePtr N1 = oa::ops::new_node(TYPE_MAX, NA);
      NodePtr N2 = oa::ops::new_node(TYPE_MIN, NA);
      NodePtr N3 = oa::ops::new_node(TYPE_ABS_MAX, NA);
      NodePtr N4 = oa::ops::new_node(TYPE_ABS_MIN, NA);

      NodePtr N5 = oa::ops::new_node(TYPE_MAX_AT, NA);
      NodePtr N6 = oa::ops::new_node(TYPE_MIN_AT, NA);
      NodePtr N7 = oa::ops::new_node(TYPE_ABS_MAX_AT, NA);
      NodePtr N8 = oa::ops::new_node(TYPE_ABS_MIN_AT, NA);

      ArrayPtr V1 = oa::funcs::to_rank0(oa::ops::eval(N1));
      ArrayPtr V2 = oa::funcs::to_rank0(oa::ops::eval(N2));
      ArrayPtr V3 = oa::funcs::to_rank0(oa::ops::eval(N3));
      ArrayPtr V4 = oa::funcs::to_rank0(oa::ops::eval(N4));
      ArrayPtr V5 = oa::funcs::to_rank0(oa::ops::eval(N5));
      ArrayPtr V6 = oa::funcs::to_rank0(oa::ops::eval(N6));
      ArrayPtr V7 = oa::funcs::to_rank0(oa::ops::eval(N7));
      ArrayPtr V8 = oa::funcs::to_rank0(oa::ops::eval(N8));

      // NodePtr NSA = oa::ops::new_node(TYPE_MIN, NA);

      //A->display("A");

      ArrayPtr A1 = oa::funcs::to_rank0(A);
      Shape s = A1->buffer_shape();
      arma::Cube<${t}$> C = oa::utils::make_cube<${t}$>(
          s, A1->get_buffer())(
              arma::span(sw, s[0] - sw - 1),
              arma::span(sw, s[1] - sw - 1),
              arma::span(sw, s[2] - sw - 1));

      // ArrayPtr V2 = oa::ops::eval(NSA);

      if(rank == 0){

        EXPECT_TRUE(oa::funcs::is_equal(V1, C.max()));
        EXPECT_TRUE(oa::funcs::is_equal(V2, C.min()));
        EXPECT_TRUE(oa::funcs::is_equal(V3, arma::abs(C).max()));
        EXPECT_TRUE(oa::funcs::is_equal(V4, arma::abs(C).min()));

        // V1->display("V1");
        // std::cout<<"C.max():"<<C.max()<<std::endl;
        // V2->display("V2");
        // std::cout<<"C.min():"<<C.min()<<std::endl;
        // V1->display("V3");
        // std::cout<<"abs(C).max():"<<arma::abs(C).max()<<std::endl;
        // V2->display("V4");
        // std::cout<<"abs(C).min():"<<arma::abs(C).min()<<std::endl;

        arma::uvec VI;
        VI = ind2sub(arma::size(C), C.index_max());

        EXPECT_TRUE(oa::funcs::is_equal(V5, VI.memptr()));

        VI = ind2sub(arma::size(C), C.index_min());

        // V6->display("V6");
        // std::cout<<VI<<std::endl;

        EXPECT_TRUE(oa::funcs::is_equal(V6, VI.memptr()));

        VI = ind2sub(arma::size(C), abs(C).index_max());
        EXPECT_TRUE(oa::funcs::is_equal(V7, VI.memptr()));

        VI = ind2sub(arma::size(C), abs(C).index_min());
        EXPECT_TRUE(oa::funcs::is_equal(V8, VI.memptr()));

        // V2->display("V2");
        //EXPECT_TRUE(V1->is_seqs());
        //EXPECT_TRUE(V1->shape() == SCALAR_SHAPE);
      }
    }
    ///:endfor
  }

  TEST_P(MPITest, SUM_scalar_CSUM_scalar){

    ///:for t in dtypes
    {
      int sw = NO_STENCIL;
      DataType dt = oa::utils::dtype<${t}$>::type;

      double x = 0;
      ArrayPtr A = oa::funcs::seqs(comm, {m,n,p}, sw, dt);
      NodePtr NA = oa::ops::new_node(A);

      NodePtr type0 = oa::ops::new_seqs_scalar_node(MPI_COMM_SELF, 0);//c=0 scalar, c=1 sum to x, c=2 sum to y, c=3 sum to z
      NodePtr N0 = oa::ops::new_node(TYPE_CSUM, NA, type0);
      ArrayPtr RA0 = oa::ops::eval(N0);
      ${t}$* res = (${t}$*) RA0->get_buffer();

      NodePtr N1 = oa::ops::new_node(TYPE_SUM, NA, type0);
      ArrayPtr RA1 = oa::ops::eval(N1);
      ${t}$* res1 = (${t}$*) RA1->get_buffer();

      ArrayPtr rank0A = oa::funcs::to_rank0(A);
      if(rank == 0){
        arma::Cube<${t}$> C = oa::utils::make_cube<${t}$>(rank0A->buffer_shape(), rank0A->get_buffer());
        x = accu(C);
        EXPECT_TRUE(res[0] == x);
        EXPECT_TRUE(res1[0] == x);
      }

      MPI_Barrier(comm);
    }
    ///:endfor
  }

  TEST_P(MPITest, CSUM_x){
    ///:for t in dtypes
    {
      int sw = NO_STENCIL;
      DataType dt = oa::utils::dtype<${t}$>::type;

      double x = 0;
      ArrayPtr A = oa::funcs::seqs(comm, {m,n,p}, sw, dt);
      NodePtr NA = oa::ops::new_node(A);

      NodePtr type0 = oa::ops::new_seqs_scalar_node(MPI_COMM_SELF, 1);//c=0 scalar, c=1 sum to x, c=2 sum to y, c=3 sum to z
      NodePtr N0 = oa::ops::new_node(TYPE_CSUM, NA, type0);
      ArrayPtr RA0 = oa::ops::eval(N0);

      ArrayPtr rank0A = oa::funcs::to_rank0(A);
      ArrayPtr result = oa::funcs::to_rank0(RA0);
      if(rank == 0){
        arma::Cube<${t}$> C = oa::utils::make_cube<${t}$>(rank0A->buffer_shape(), rank0A->get_buffer());
        for(int i = 0; i <= m; i++){
          if(i-1 >= 0 && i <= m-1){
            C.subcube( i, 0, 0, i, n-1, p-1 ) += C.subcube( i-1, 0, 0, i-1, n-1, p-1 );
          }
        }
        EXPECT_TRUE(oa::funcs::is_equal(result, C));
      }

      MPI_Barrier(comm);
    }
    ///:endfor
  }

  TEST_P(MPITest, CSUM_y){
    ///:for t in dtypes
    {
      int sw = NO_STENCIL;
      DataType dt = oa::utils::dtype<${t}$>::type;

      double x = 0;
      ArrayPtr A = oa::funcs::seqs(comm, {m,n,p}, sw, dt);
      NodePtr NA = oa::ops::new_node(A);

      NodePtr type0 = oa::ops::new_seqs_scalar_node(MPI_COMM_SELF, 2);//c=0 scalar, c=1 sum to x, c=2 sum to y, c=3 sum to z
      NodePtr N0 = oa::ops::new_node(TYPE_CSUM, NA, type0);
      ArrayPtr RA0 = oa::ops::eval(N0);

      ArrayPtr rank0A = oa::funcs::to_rank0(A);
      ArrayPtr result = oa::funcs::to_rank0(RA0);
      if(rank == 0){
        arma::Cube<${t}$> C = oa::utils::make_cube<${t}$>(rank0A->buffer_shape(), rank0A->get_buffer());
        for(int i = 0; i <= n ; i++){
          if(i-1 >= 0 && i <= n-1){
            C.subcube( 0, i, 0, m-1, i, p-1 ) += C.subcube( 0, i-1, 0, m-1, i-1, p-1 );
          }
        }
        EXPECT_TRUE(oa::funcs::is_equal(result, C));
      }

      MPI_Barrier(comm);
    }
    ///:endfor
  }

  TEST_P(MPITest, CSUM_z){
    ///:for t in dtypes
    {
      int sw = NO_STENCIL;
      DataType dt = oa::utils::dtype<${t}$>::type;

      double x = 0;
      ArrayPtr A = oa::funcs::seqs(comm, {m,n,p}, sw, dt);
      NodePtr NA = oa::ops::new_node(A);

      NodePtr type0 = oa::ops::new_seqs_scalar_node(MPI_COMM_SELF, 3);//c=0 scalar, c=1 sum to x, c=2 sum to y, c=3 sum to z
      NodePtr N0 = oa::ops::new_node(TYPE_CSUM, NA, type0);
      ArrayPtr RA0 = oa::ops::eval(N0);

      ArrayPtr rank0A = oa::funcs::to_rank0(A);
      ArrayPtr result = oa::funcs::to_rank0(RA0);
      if(rank == 0){
        arma::Cube<${t}$> C = oa::utils::make_cube<${t}$>(rank0A->buffer_shape(), rank0A->get_buffer());
        for(int i = 0; i <=p; i++){
          if(i-1 >= 0 && i <= p-1){
            C.subcube( 0, 0, i, m-1, n-1, i ) += C.subcube( 0, 0, i-1, m-1, n-1, i-1 );
          }
        }
        EXPECT_TRUE(oa::funcs::is_equal(result, C));
      }

      MPI_Barrier(comm);
    }
    ///:endfor
  }

  TEST_P(MPITest, SUM_x){
    ///:for t in dtypes
    {
      int sw = NO_STENCIL;
      DataType dt = oa::utils::dtype<${t}$>::type;

      double x = 0;
      ArrayPtr A = oa::funcs::seqs(comm, {m,n,p}, sw, dt);
      NodePtr NA = oa::ops::new_node(A);

      NodePtr type0 = oa::ops::new_seqs_scalar_node(MPI_COMM_SELF, 1);//c=0 scalar, c=1 sum to x, c=2 sum to y, c=3 sum to z
      NodePtr N0 = oa::ops::new_node(TYPE_SUM, NA, type0);
      ArrayPtr RA0 = oa::ops::eval(N0);

      ArrayPtr rank0A = oa::funcs::to_rank0(A);
      ArrayPtr result = oa::funcs::to_rank0(RA0);
      if(rank == 0){
        arma::Cube<${t}$> C = oa::utils::make_cube<${t}$>(rank0A->buffer_shape(), rank0A->get_buffer());
        for(int i = m; i >= 0; i--){
          if(i-1 >= 0 && i <= m-1){
            C.subcube( 0, 0, 0, 0, n-1, p-1 ) += C.subcube( i, 0, 0, i, n-1, p-1 );
          }
        }
        arma::Cube<${t}$> Cr = C.subcube( 0, 0, 0, 0, n-1, p-1 );
        EXPECT_TRUE(oa::funcs::is_equal(result, Cr));
      }

      MPI_Barrier(comm);
    }
    ///:endfor
  }

  TEST_P(MPITest, SUM_y){
    ///:for t in dtypes
    {
      int sw = NO_STENCIL;
      DataType dt = oa::utils::dtype<${t}$>::type;

      double x = 0;
      ArrayPtr A = oa::funcs::seqs(comm, {m,n,p}, sw, dt);
      NodePtr NA = oa::ops::new_node(A);

      NodePtr type0 = oa::ops::new_seqs_scalar_node(MPI_COMM_SELF, 2);//c=0 scalar, c=1 sum to x, c=2 sum to y, c=3 sum to z
      NodePtr N0 = oa::ops::new_node(TYPE_SUM, NA, type0);
      ArrayPtr RA0 = oa::ops::eval(N0);

      ArrayPtr rank0A = oa::funcs::to_rank0(A);
      ArrayPtr result = oa::funcs::to_rank0(RA0);
      if(rank == 0){
        arma::Cube<${t}$> C = oa::utils::make_cube<${t}$>(rank0A->buffer_shape(), rank0A->get_buffer());
        for(int i = n; i >= 0; i--){
          if(i-1 >= 0 && i <= n-1){
            C.subcube( 0, 0, 0, m-1, 0, p-1 ) += C.subcube( 0, i, 0, m-1, i, p-1 );
          }
        }
        arma::Cube<${t}$> Cr = C.subcube( 0, 0, 0, m-1, 0, p-1 );
        EXPECT_TRUE(oa::funcs::is_equal(result, Cr));
      }

      MPI_Barrier(comm);
    }
    ///:endfor
  }

  TEST_P(MPITest, SUM_z){
    ///:for t in dtypes
    {
      int sw = NO_STENCIL;
      DataType dt = oa::utils::dtype<${t}$>::type;

      double x = 0;
      ArrayPtr A = oa::funcs::seqs(comm, {m,n,p}, sw, dt);
      NodePtr NA = oa::ops::new_node(A);

      NodePtr type0 = oa::ops::new_seqs_scalar_node(MPI_COMM_SELF, 3);//c=0 scalar, c=1 sum to x, c=2 sum to y, c=3 sum to z
      NodePtr N0 = oa::ops::new_node(TYPE_SUM, NA, type0);
      ArrayPtr RA0 = oa::ops::eval(N0);

      ArrayPtr rank0A = oa::funcs::to_rank0(A);
      ArrayPtr result = oa::funcs::to_rank0(RA0);
      if(rank == 0){
        arma::Cube<${t}$> C = oa::utils::make_cube<${t}$>(rank0A->buffer_shape(), rank0A->get_buffer());
        for(int i = p; i >= 0; i--){
          if(i-1 >= 0 && i <= p-1){
            C.subcube( 0, 0, 0, m-1, n-1, 0 ) += C.subcube( 0, 0, i, m-1, n-1, i );
          }
        }
        arma::Cube<${t}$> Cr = C.subcube( 0, 0, 0, m-1, n-1, 0 );
        EXPECT_TRUE(oa::funcs::is_equal(result, Cr));
      }

      MPI_Barrier(comm);
    }
    ///:endfor
  }

  TEST_P(MPITest, REP){
    ///:for t in dtypes
    {
      int sw = NO_STENCIL;
      DataType dt = oa::utils::dtype<${t}$>::type;

      int x = 2;
      int y = 2;
      int z = 2;

      ArrayPtr A = oa::funcs::seqs(comm, {m,n,p}, sw, dt);
      NodePtr NN = oa::ops::new_node(A);
      ArrayPtr lap = oa::funcs::consts(MPI_COMM_SELF, {3, 1, 1}, 2, 0);
      NodePtr NN2 = oa::ops::new_node(lap);

      NodePtr NP = oa::ops::new_node(TYPE_REP, NN, NN2);
      ArrayPtr repA = oa::ops::eval(NP);

      ArrayPtr rank0A = oa::funcs::to_rank0(A);
      ArrayPtr result = oa::funcs::to_rank0(repA);
      //if(rank == 0)result->display("result");
      if(rank == 0){
        arma::Cube<${t}$> C0 = oa::utils::make_cube<${t}$>(rank0A->buffer_shape(), rank0A->get_buffer());
        arma::Cube<${t}$> Cr(m*x, n*y, p*z);
        Cr.zeros();
        int ii,jj,kk;
        //result->display("result");
        //Cr.print("Cr");
        ii = 0;
        for(int i = 0; i < x; i++){
          jj = 0;
          for(int j = 0; j < y; j++){
            kk = 0;
            for(int k = 0; k < z; k++){
              //cout<<ii<<","<<jj<<","<<kk<<endl;
              Cr.subcube(0+ii,0+jj,0+kk,m-1+ii,n-1+jj,p-1+kk) = C0;
              kk += p;
            }
            jj += n;
          }
          ii += m;
        }
        //Cr.print("Cr");
        //result->display("result");
        //        EXPECT_TRUE(oa::funcs::is_equal(rank0A, C));
        EXPECT_TRUE(oa::funcs::is_equal(result, Cr));

      }

      MPI_Barrier(comm);
    }
    ///:endfor
  }

  TEST_P(MPITest, RAND){
    ///:for t in dtypes
    {
      int sw = NO_STENCIL;
      DataType dt = oa::utils::dtype<${t}$>::type;

      int x = 2;
      int y = 3;
      int z = 4;

      ArrayPtr A = oa::funcs::rands(comm, {m,n,p}, sw, dt);

      ArrayPtr rank0A = oa::funcs::to_rank0(A);
      //if(rank == 0)result->display("result");
      if(rank == 0){
        //rank0A->display("rand");
        ;
      }

      MPI_Barrier(comm);
    }
    ///:endfor
  }

  INSTANTIATE_TEST_CASE_P(OpenArray, MPITest,
          gt::Combine(gt::Values(MPI_COMM_WORLD),
                  MRange, NRange, PRange));

}


/**
 * Copyright (c) 2020, Román Cárdenas Rodríguez
 * ARSLab - Carleton University
 * GreenLSI - Polytechnic University of Madrid
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 * 1. Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */
#define BOOST_TEST_DYN_LINK

#include <boost/test/unit_test.hpp>
#include <cadmium/celldevs/utils/grid_utils.hpp>

using namespace cadmium::celldevs;

int moore_cells(int dimension, int range) {
    return std::pow(2 * range + 1, dimension);
}

BOOST_AUTO_TEST_CASE(moore) {
    for (unsigned int D = 1; D < 5; D++) {
        for (unsigned int r = 0; r < 4; r++) {
            cell_position shape = cell_position();
            cell_position middle = cell_position();
            for (int d = 0; d < D; d++) {
                shape.push_back(2 * r + 1);
                middle.push_back(r);
            }
            std::vector<cell_position> neighbors = grid_scenario<int, int>::biassed_moore_neighborhood(D, r);
            BOOST_CHECK_EQUAL(neighbors.size(), moore_cells(D, r));
            for (auto const &cell: neighbors) {
                int a = grid_scenario<int, int>::chebyshev_distance(middle, cell, shape, false);
                BOOST_CHECK_LE( a, r);
            }
        }
    }
}

BOOST_AUTO_TEST_CASE(von_neumann) {
    for (unsigned int D = 1; D < 5; D++) {
        for (unsigned int r = 0; r < 4; r++) {
            cell_position shape = cell_position();
            cell_position middle = cell_position();
            for (int d = 0; d < D; d++) {
                shape.push_back(2 * r + 1);
                middle.push_back(r);
            }
            std::vector<cell_position> neighbors = grid_scenario<int, int>::biassed_von_neumann_neighborhood(D, r);
            for (auto const &cell: neighbors) {
                int a = grid_scenario<int, int>::manhattan_distance(middle, cell, shape, false);
                BOOST_CHECK_LE(a, r);
            }
        }
    }
}


#include <geometry.h>
#include <tiny.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

#include <vector>

typedef tiny::MathTypes<float> MT;
typedef MT::vector3_type       V;
typedef MT::quaternion_type    Q;
typedef MT::real_type          T;
typedef MT::value_traits       VT;

class ContactInfo
{
public:

  V m_point;
  V m_normal;
  T m_distance;

};

class MyCallback
  : public geometry::ContactsCallback<V>
{
public:

  std::vector<ContactInfo> m_contacts;

public:

  void operator()(
                  V const & point
                  , V const & normal
                  , typename V::real_type const & distance
                  )
  {
    ContactInfo info;

    info.m_point = point;
    info.m_normal = normal;
    info.m_distance = distance;

    m_contacts.push_back(info);
  }

};

BOOST_AUTO_TEST_SUITE(geometry);

BOOST_AUTO_TEST_CASE(contacts_obb_obb_test)
{
  // touching bottom-top faces
  {
    V const centerA   = V::make(0.0, 0.0, 0.0);
    V const half_extA = V::make(1.0, 1.0, 1.0);
    Q const qA        = Q::identity();

    geometry::OBB<MT> obbA = geometry::make_obb<MT>(centerA, qA, half_extA);

    V const centerB   = V::make(0.0, -2.0, 0.0);
    V const half_extB = V::make(1.0, 1.0, 1.0);
    Q const qB        = Q::Ry(VT::pi_quarter());

    geometry::OBB<MT> obbB = geometry::make_obb<MT>(centerB, qB, half_extB);

    MyCallback callback1;
    MyCallback callback2;

    bool const test1 = geometry::contacts_obb_obb(obbA,obbB, 0.0, callback1);
    bool const test2 = geometry::contacts_obb_obb(obbB,obbA, 0.0, callback2);

    BOOST_CHECK(test1);
    BOOST_CHECK(test2);

    BOOST_CHECK_EQUAL(callback1.m_contacts.size(), 8u);
    BOOST_CHECK_EQUAL(callback2.m_contacts.size(), 8u);

    for(unsigned int k = 0u;k <8u;++k)
    {
      BOOST_CHECK( fabs(       callback1.m_contacts[k].m_normal(2)) < 0.00001 );
      BOOST_CHECK( fabs(       callback1.m_contacts[k].m_normal(0)) < 0.00001 );
      BOOST_CHECK( 1.0  - fabs(callback1.m_contacts[k].m_normal(1)) < 0.00001 );
      BOOST_CHECK(       fabs( callback1.m_contacts[k].m_distance)  < 0.00001 );
    }

  }
  // touching left-right faces
  {
    V const centerA   = V::make(0.0, 0.0, 0.0);
    V const half_extA = V::make(1.0, 1.0, 1.0);
    Q const qA        = Q::identity();

    geometry::OBB<MT> obbA = geometry::make_obb<MT>(centerA, qA, half_extA);

    V const centerB   = V::make(2.0, 0.0, 0.0);
    V const half_extB = V::make(1.0, 1.0, 1.0);
    Q const qB        = Q::Rx(VT::pi_quarter());

    geometry::OBB<MT> obbB = geometry::make_obb<MT>(centerB, qB, half_extB);

    MyCallback callback1;
    MyCallback callback2;

    bool const test1 = geometry::contacts_obb_obb(obbA,obbB, 0.0, callback1);
    bool const test2 = geometry::contacts_obb_obb(obbB,obbA, 0.0, callback2);

    BOOST_CHECK(test1);
    BOOST_CHECK(test2);

    BOOST_CHECK_EQUAL(callback1.m_contacts.size(), 8u);
    BOOST_CHECK_EQUAL(callback2.m_contacts.size(), 8u);

    for(unsigned int k = 0u;k <8u;++k)
    {
      BOOST_CHECK( fabs(       callback1.m_contacts[k].m_normal(2)) < 0.00001 );
      BOOST_CHECK( fabs(       callback1.m_contacts[k].m_normal(1)) < 0.00001 );
      BOOST_CHECK( 1.0  - fabs(callback1.m_contacts[k].m_normal(0)) < 0.00001 );
      BOOST_CHECK(       fabs( callback1.m_contacts[k].m_distance)  < 0.00001 );
    }

  }
  // touching front-back faces
  {
    V const centerA   = V::make(0.0, 0.0, 0.0);
    V const half_extA = V::make(1.0, 1.0, 1.0);
    Q const qA        = Q::identity();

    geometry::OBB<MT> obbA = geometry::make_obb<MT>(centerA, qA, half_extA);

    V const centerB   = V::make(0.0, 0.0, 2.0);
    V const half_extB = V::make(1.0, 1.0, 1.0);
    Q const qB        = Q::Rz(VT::pi_quarter());

    geometry::OBB<MT> obbB = geometry::make_obb<MT>(centerB, qB, half_extB);

    MyCallback callback1;
    MyCallback callback2;

    bool const test1 = geometry::contacts_obb_obb(obbA,obbB, 0.0, callback1);
    bool const test2 = geometry::contacts_obb_obb(obbB,obbA, 0.0, callback2);

    BOOST_CHECK(test1);
    BOOST_CHECK(test2);

    BOOST_CHECK_EQUAL(callback1.m_contacts.size(), 8u);
    BOOST_CHECK_EQUAL(callback2.m_contacts.size(), 8u);

    for(unsigned int k = 0u;k <8u;++k)
    {
      BOOST_CHECK( fabs(       callback1.m_contacts[k].m_normal(0)) < 0.00001 );
      BOOST_CHECK( fabs(       callback1.m_contacts[k].m_normal(1)) < 0.00001 );
      BOOST_CHECK( 1.0  - fabs(callback1.m_contacts[k].m_normal(2)) < 0.00001 );
      BOOST_CHECK(       fabs( callback1.m_contacts[k].m_distance)  < 0.00001 );
    }

  }
  // separating bottom-top faces
  {
    V const centerA   = V::make(0.0, 0.0, 0.0);
    V const half_extA = V::make(1.0, 1.0, 1.0);
    Q const qA        = Q::identity();

    geometry::OBB<MT> obbA = geometry::make_obb<MT>(centerA, qA, half_extA);

    V const centerB   = V::make(0.0, -2.1, 0.0);
    V const half_extB = V::make(1.0, 1.0, 1.0);
    Q const qB        = Q::Ry(VT::pi_quarter());

    geometry::OBB<MT> obbB = geometry::make_obb<MT>(centerB, qB, half_extB);

    MyCallback callback1;
    MyCallback callback2;

    bool const test1 = geometry::contacts_obb_obb(obbA,obbB, 0.0, callback1);
    bool const test2 = geometry::contacts_obb_obb(obbB,obbA, 0.0, callback2);

    BOOST_CHECK(!test1);
    BOOST_CHECK(!test2);

    BOOST_CHECK_EQUAL(callback1.m_contacts.size(), 0u);
    BOOST_CHECK_EQUAL(callback2.m_contacts.size(), 0u);

  }
  // separating left-right faces
  {
    V const centerA   = V::make(0.0, 0.0, 0.0);
    V const half_extA = V::make(1.0, 1.0, 1.0);
    Q const qA        = Q::identity();

    geometry::OBB<MT> obbA = geometry::make_obb<MT>(centerA, qA, half_extA);

    V const centerB   = V::make(2.1, 0.0, 0.0);
    V const half_extB = V::make(1.0, 1.0, 1.0);
    Q const qB        = Q::Rx(VT::pi_quarter());

    geometry::OBB<MT> obbB = geometry::make_obb<MT>(centerB, qB, half_extB);

    MyCallback callback1;
    MyCallback callback2;

    bool const test1 = geometry::contacts_obb_obb(obbA,obbB, 0.0, callback1);
    bool const test2 = geometry::contacts_obb_obb(obbB,obbA, 0.0, callback2);

    BOOST_CHECK(!test1);
    BOOST_CHECK(!test2);

    BOOST_CHECK_EQUAL(callback1.m_contacts.size(), 0u);
    BOOST_CHECK_EQUAL(callback2.m_contacts.size(), 0u);

  }
  // separating front-back faces
  {
    V const centerA   = V::make(0.0, 0.0, 0.0);
    V const half_extA = V::make(1.0, 1.0, 1.0);
    Q const qA        = Q::identity();

    geometry::OBB<MT> obbA = geometry::make_obb<MT>(centerA, qA, half_extA);

    V const centerB   = V::make(0.0, 0.0, 2.1);
    V const half_extB = V::make(1.0, 1.0, 1.0);
    Q const qB        = Q::Rz(VT::pi_quarter());

    geometry::OBB<MT> obbB = geometry::make_obb<MT>(centerB, qB, half_extB);

    MyCallback callback1;
    MyCallback callback2;

    bool const test1 = geometry::contacts_obb_obb(obbA,obbB, 0.0, callback1);
    bool const test2 = geometry::contacts_obb_obb(obbB,obbA, 0.0, callback2);

    BOOST_CHECK(!test1);
    BOOST_CHECK(!test2);

    BOOST_CHECK_EQUAL(callback1.m_contacts.size(), 0u);
    BOOST_CHECK_EQUAL(callback2.m_contacts.size(), 0u);

  }
  // A inside of B
  {
    V const centerA   = V::make(0.0,   0.5,  0.0 );
    V const half_extA = V::make(0.5,   0.25, 0.5);
    Q const qA        = Q::identity();

    geometry::OBB<MT> obbA = geometry::make_obb<MT>(centerA, qA, half_extA);

    V const centerB   = V::make(0.0, 0.0, 0.0);
    V const half_extB = V::make(1.0, 1.0, 1.0);
    Q const qB        = Q::identity();

    geometry::OBB<MT> obbB = geometry::make_obb<MT>(centerB, qB, half_extB);

    MyCallback callback1;
    MyCallback callback2;

    bool const test1 = geometry::contacts_obb_obb(obbA,obbB, 0.0, callback1);
    bool const test2 = geometry::contacts_obb_obb(obbB,obbA, 0.0, callback2);

    BOOST_CHECK(test1);
    BOOST_CHECK(test2);

    BOOST_CHECK_EQUAL(callback1.m_contacts.size(), 4u);
    BOOST_CHECK_EQUAL(callback2.m_contacts.size(), 4u);

    for(unsigned int k = 0u;k <4u;++k)
    {
      BOOST_CHECK( 0.5 - fabs(callback1.m_contacts[k].m_point(0))   < 0.00001 );
      BOOST_CHECK( 0.5 - fabs(callback1.m_contacts[k].m_point(1))   < 0.00001 );
      BOOST_CHECK( 0.5 - fabs(callback1.m_contacts[k].m_point(2))   < 0.00001 );
      BOOST_CHECK( fabs(       callback1.m_contacts[k].m_normal(0)) < 0.00001 );
      BOOST_CHECK( 1.0  - fabs(callback1.m_contacts[k].m_normal(1)) < 0.00001 );
      BOOST_CHECK( fabs(       callback1.m_contacts[k].m_normal(2)) < 0.00001 );
      BOOST_CHECK( 0.5 - fabs( callback1.m_contacts[k].m_distance)  < 0.00001 ); // depth is off due to technicality in method, should have been 0.75
    }

  }
  // separating edge-edge-case
  {
    V const centerA   = V::make(0.0, 0.0, 0.0);
    V const half_extA = V::make(1.0, 1.0, 1.0);
    Q const qA        = Q::identity();

    geometry::OBB<MT> obbA = geometry::make_obb<MT>(centerA, qA, half_extA);

    V const centerB   = V::make(2.01, 2.01, 0.0);
    V const half_extB = V::make(1.0, 1.0, 1.0);
    Q const qB        = Q::Ru(VT::pi_half(), centerB );

    geometry::OBB<MT> obbB = geometry::make_obb<MT>(centerB, qB, half_extB);

    MyCallback callback1;
    MyCallback callback2;

    bool const test1 = geometry::contacts_obb_obb(obbA,obbB, 0.0, callback1);
    bool const test2 = geometry::contacts_obb_obb(obbB,obbA, 0.0, callback2);

    BOOST_CHECK(!test1);
    BOOST_CHECK(!test2);

    BOOST_CHECK_EQUAL(callback1.m_contacts.size(), 0u);
    BOOST_CHECK_EQUAL(callback2.m_contacts.size(), 0u);
  }
  // touching edge-edge-case
  {
    V const centerA   = V::make(0.0, 0.0, 0.0);
    V const half_extA = V::make(1.0, 1.0, 1.0);
    Q const qA        = Q::identity();

    geometry::OBB<MT> obbA = geometry::make_obb<MT>(centerA, qA, half_extA);

    V const centerB   = V::make(1.999999, 1.999999, 0.0);
    V const half_extB = V::make(1.0, 1.0, 1.0);
    Q const qB        = Q::Ru(VT::pi_half(), centerB );

    geometry::OBB<MT> obbB = geometry::make_obb<MT>(centerB, qB, half_extB);

    MyCallback callback1;
    MyCallback callback2;

    bool const test1 = geometry::contacts_obb_obb(obbA,obbB, 0.0, callback1);
    bool const test2 = geometry::contacts_obb_obb(obbB,obbA, 0.0, callback2);

    BOOST_CHECK(test1);
    BOOST_CHECK(test2);

    BOOST_CHECK_EQUAL(callback1.m_contacts.size(), 4u);
    BOOST_CHECK_EQUAL(callback2.m_contacts.size(), 4u);

    BOOST_CHECK( fabs(1.0 - callback1.m_contacts[0].m_point(0)) < 0.00001);
    BOOST_CHECK( fabs(1.0 - callback1.m_contacts[0].m_point(1)) < 0.00001);
    BOOST_CHECK( fabs(      callback1.m_contacts[0].m_point(2)) < 0.00001);
    BOOST_CHECK( fabs(sqrt(0.5) - callback1.m_contacts[0].m_normal(0)) < 0.00001);
    BOOST_CHECK( fabs(sqrt(0.5) - callback1.m_contacts[0].m_normal(1)) < 0.00001);
    BOOST_CHECK( fabs(            callback1.m_contacts[0].m_normal(2)) < 0.00001);
    BOOST_CHECK( fabs( callback1.m_contacts[0].m_distance) < 0.00001);

    BOOST_CHECK( fabs(1.0 - callback1.m_contacts[1].m_point(0)) < 0.00001);
    BOOST_CHECK( fabs(1.0 - callback1.m_contacts[1].m_point(1)) < 0.00001);
    BOOST_CHECK( fabs(      callback1.m_contacts[1].m_point(2)) < 0.00001);
    BOOST_CHECK( fabs(sqrt(0.5) - callback1.m_contacts[1].m_normal(0)) < 0.00001);
    BOOST_CHECK( fabs(sqrt(0.5) - callback1.m_contacts[1].m_normal(1)) < 0.00001);
    BOOST_CHECK( fabs(            callback1.m_contacts[1].m_normal(2)) < 0.00001);
    BOOST_CHECK( fabs( callback1.m_contacts[1].m_distance) < 0.00001);

    BOOST_CHECK( fabs(1.0 - callback1.m_contacts[2].m_point(0)) < 0.00001);
    BOOST_CHECK( fabs(1.0 - callback1.m_contacts[2].m_point(1)) < 0.00001);
    BOOST_CHECK( fabs(      callback1.m_contacts[2].m_point(2)) < 0.00001);
    BOOST_CHECK( fabs(sqrt(0.5) - callback1.m_contacts[2].m_normal(0)) < 0.00001);
    BOOST_CHECK( fabs(sqrt(0.5) - callback1.m_contacts[2].m_normal(1)) < 0.00001);
    BOOST_CHECK( fabs(            callback1.m_contacts[2].m_normal(2)) < 0.00001);
    BOOST_CHECK( fabs( callback1.m_contacts[2].m_distance) < 0.00001);

    BOOST_CHECK( fabs(1.0 - callback1.m_contacts[3].m_point(0)) < 0.00001);
    BOOST_CHECK( fabs(1.0 - callback1.m_contacts[3].m_point(1)) < 0.00001);
    BOOST_CHECK( fabs(      callback1.m_contacts[3].m_point(2)) < 0.00001);
    BOOST_CHECK( fabs(sqrt(0.5) - callback1.m_contacts[3].m_normal(0)) < 0.00001);
    BOOST_CHECK( fabs(sqrt(0.5) - callback1.m_contacts[3].m_normal(1)) < 0.00001);
    BOOST_CHECK( fabs(            callback1.m_contacts[3].m_normal(2)) < 0.00001);
    BOOST_CHECK( fabs( callback1.m_contacts[3].m_distance) < 0.00001);

  }

}

BOOST_AUTO_TEST_SUITE_END();


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 2000 - 2018 by the deal.II authors
 *
 * This file is modification of the version in the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE.md at
 * the top level directory of deal.II.
 *
 * ---------------------------------------------------------------------
 *
 * Author: Wolfgang Bangerth, University of Heidelberg, 2000
 */
#include <deal.II/base/function.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/multithread_info.h>
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/tensor_function.h>
#include <deal.II/base/work_stream.h>
#include <deal.II/dofs/dof_accessor.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_out.h>
#include <deal.II/grid/grid_refinement.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>
#include <deal.II/lac/affine_constraints.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/solver_gmres.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/vector.h>
#include <deal.II/numerics/data_out.h>
#include <deal.II/numerics/error_estimator.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/numerics/vector_tools.h>
#include <fstream>
#include <iostream>

#include <bench_base.hpp>

DEFINE_uint32(
    num_refine_cycles, 1,
    "Number of refinement cycles for the adaptive refinement within deal.ii");
DEFINE_uint32(init_refine_level, 4,
              "Initial level for the refinement of the mesh.");
DEFINE_bool(dealii_orig, false, "Solve with dealii iterative GMRES");
DEFINE_bool(vis_sol, false, "Print the solution for visualization");

#define CHECK_HERE std::cout << "Here " << __LINE__ << std::endl;

using namespace dealii;

template <int dim>
class AdvectionField : public TensorFunction<1, dim> {
public:
    virtual Tensor<1, dim> value(const Point<dim> &p) const override;
    DeclException2(ExcDimensionMismatch, unsigned int, unsigned int,
                   << "The vector has size " << arg1 << " but should have "
                   << arg2 << " elements.");
};

template <int dim>
Tensor<1, dim> AdvectionField<dim>::value(const Point<dim> &p) const
{
    Point<dim> value;
    value[0] = 2;
    for (unsigned int i = 1; i < dim; ++i)
        value[i] = 1 + 0.8 * std::sin(8. * numbers::PI * p[0]);
    return value;
}

template <int dim>
class RightHandSide : public Function<dim> {
public:
    virtual double value(const Point<dim> &p,
                         const unsigned int component = 0) const override;

private:
    static const Point<dim> center_point;
};

template <>
const Point<1> RightHandSide<1>::center_point = Point<1>(-0.75);
template <>
const Point<2> RightHandSide<2>::center_point = Point<2>(-0.75, -0.75);
template <>
const Point<3> RightHandSide<3>::center_point = Point<3>(-0.75, -0.75, -0.75);

template <int dim>
double RightHandSide<dim>::value(const Point<dim> &p,
                                 const unsigned int component) const
{
    (void)component;
    Assert(component == 0, ExcIndexRange(component, 0, 1));
    const double diameter = 0.1;
    return ((p - center_point).norm_square() < diameter * diameter
                ? 0.1 / std::pow(diameter, dim)
                : 0.1);
}

template <int dim>
class BoundaryValues : public Function<dim> {
public:
    virtual double value(const Point<dim> &p,
                         const unsigned int component = 0) const override;
};

template <int dim>
double BoundaryValues<dim>::value(const Point<dim> &p,
                                  const unsigned int component) const
{
    (void)component;
    Assert(component == 0, ExcIndexRange(component, 0, 1));
    const double sine_term = std::sin(16. * numbers::PI * p.norm_square());
    const double weight = std::exp(5. * (1. - p.norm_square()));
    return weight * sine_term;
}

template <int dim>
class AdvectionProblem : public BenchBase<double, int> {
public:
    AdvectionProblem();
    void run();
    void run(MPI_Comm mpi_communicator);

private:
    void setup_system();
    struct AssemblyScratchData {
        AssemblyScratchData(const FiniteElement<dim> &fe);
        AssemblyScratchData(const AssemblyScratchData &scratch_data);
        FEValues<dim> fe_values;
        FEFaceValues<dim> fe_face_values;
        std::vector<double> rhs_values;
        std::vector<Tensor<1, dim>> advection_directions;
        std::vector<double> face_boundary_values;
        std::vector<Tensor<1, dim>> face_advection_directions;
        AdvectionField<dim> advection_field;
        RightHandSide<dim> right_hand_side;
        BoundaryValues<dim> boundary_values;
    };
    struct AssemblyCopyData {
        FullMatrix<double> cell_matrix;
        Vector<double> cell_rhs;
        std::vector<types::global_dof_index> local_dof_indices;
    };
    void assemble_system();
    void local_assemble_system(
        const typename DoFHandler<dim>::active_cell_iterator &cell,
        AssemblyScratchData &scratch, AssemblyCopyData &copy_data);
    void copy_local_to_global(const AssemblyCopyData &copy_data);
    void solve();
    void solve(MPI_Comm mpi_communicator);
    void refine_grid();
    void output_results(const unsigned int cycle) const;
    Triangulation<dim> triangulation;
    DoFHandler<dim> dof_handler;
    FE_Q<dim> fe;
    AffineConstraints<double> hanging_node_constraints;
    SparsityPattern sparsity_pattern;
    SparseMatrix<double> system_matrix;
    Vector<double> solution;
    Vector<double> system_rhs;
};

class GradientEstimation {
public:
    template <int dim>
    static void estimate(const DoFHandler<dim> &dof,
                         const Vector<double> &solution,
                         Vector<float> &error_per_cell);
    DeclException2(ExcInvalidVectorLength, int, int,
                   << "Vector has length " << arg1 << ", but should have "
                   << arg2);
    DeclException0(ExcInsufficientDirections);

private:
    template <int dim>
    struct EstimateScratchData {
        EstimateScratchData(const FiniteElement<dim> &fe,
                            const Vector<double> &solution,
                            Vector<float> &error_per_cell);
        EstimateScratchData(const EstimateScratchData &data);
        FEValues<dim> fe_midpoint_value;
        std::vector<typename DoFHandler<dim>::active_cell_iterator>
            active_neighbors;
        const Vector<double> &solution;
        Vector<float> &error_per_cell;
        std::vector<double> cell_midpoint_value;
        std::vector<double> neighbor_midpoint_value;
    };
    struct EstimateCopyData {};
    template <int dim>
    static void estimate_cell(
        const typename DoFHandler<dim>::active_cell_iterator &cell,
        EstimateScratchData<dim> &scratch_data,
        const EstimateCopyData &copy_data);
};

template <int dim>
AdvectionProblem<dim>::AdvectionProblem() : dof_handler(triangulation), fe(5)
{}

template <int dim>
void AdvectionProblem<dim>::setup_system()
{
    dof_handler.distribute_dofs(fe);
    hanging_node_constraints.clear();
    DoFTools::make_hanging_node_constraints(dof_handler,
                                            hanging_node_constraints);
    hanging_node_constraints.close();
    DynamicSparsityPattern dsp(dof_handler.n_dofs(), dof_handler.n_dofs());
    DoFTools::make_sparsity_pattern(dof_handler, dsp, hanging_node_constraints,
                                    /*keep_constrained_dofs =*/false);
    sparsity_pattern.copy_from(dsp);
    system_matrix.reinit(sparsity_pattern);
    solution.reinit(dof_handler.n_dofs());
    system_rhs.reinit(dof_handler.n_dofs());
}

template <int dim>
void AdvectionProblem<dim>::assemble_system()
{
    WorkStream::run(dof_handler.begin_active(), dof_handler.end(), *this,
                    &AdvectionProblem::local_assemble_system,
                    &AdvectionProblem::copy_local_to_global,
                    AssemblyScratchData(fe), AssemblyCopyData());
}

template <int dim>
AdvectionProblem<dim>::AssemblyScratchData::AssemblyScratchData(
    const FiniteElement<dim> &fe)
    : fe_values(fe, QGauss<dim>(fe.degree + 1),
                update_values | update_gradients | update_quadrature_points |
                    update_JxW_values),
      fe_face_values(fe, QGauss<dim - 1>(fe.degree + 1),
                     update_values | update_quadrature_points |
                         update_JxW_values | update_normal_vectors),
      rhs_values(fe_values.get_quadrature().size()),
      advection_directions(fe_values.get_quadrature().size()),
      face_boundary_values(fe_face_values.get_quadrature().size()),
      face_advection_directions(fe_face_values.get_quadrature().size())
{}

template <int dim>
AdvectionProblem<dim>::AssemblyScratchData::AssemblyScratchData(
    const AssemblyScratchData &scratch_data)
    : fe_values(scratch_data.fe_values.get_fe(),
                scratch_data.fe_values.get_quadrature(),
                update_values | update_gradients | update_quadrature_points |
                    update_JxW_values),
      fe_face_values(scratch_data.fe_face_values.get_fe(),
                     scratch_data.fe_face_values.get_quadrature(),
                     update_values | update_quadrature_points |
                         update_JxW_values | update_normal_vectors),
      rhs_values(scratch_data.rhs_values.size()),
      advection_directions(scratch_data.advection_directions.size()),
      face_boundary_values(scratch_data.face_boundary_values.size()),
      face_advection_directions(scratch_data.face_advection_directions.size())
{}

template <int dim>
void AdvectionProblem<dim>::local_assemble_system(
    const typename DoFHandler<dim>::active_cell_iterator &cell,
    AssemblyScratchData &scratch_data, AssemblyCopyData &copy_data)
{
    const unsigned int dofs_per_cell = fe.dofs_per_cell;
    const unsigned int n_q_points =
        scratch_data.fe_values.get_quadrature().size();
    const unsigned int n_face_q_points =
        scratch_data.fe_face_values.get_quadrature().size();
    copy_data.cell_matrix.reinit(dofs_per_cell, dofs_per_cell);
    copy_data.cell_rhs.reinit(dofs_per_cell);
    copy_data.local_dof_indices.resize(dofs_per_cell);
    scratch_data.fe_values.reinit(cell);
    scratch_data.advection_field.value_list(
        scratch_data.fe_values.get_quadrature_points(),
        scratch_data.advection_directions);
    scratch_data.right_hand_side.value_list(
        scratch_data.fe_values.get_quadrature_points(),
        scratch_data.rhs_values);
    const double delta = 0.1 * cell->diameter();
    for (unsigned int q_point = 0; q_point < n_q_points; ++q_point)
        for (unsigned int i = 0; i < dofs_per_cell; ++i) {
            const auto &sd = scratch_data;
            for (unsigned int j = 0; j < dofs_per_cell; ++j)
                copy_data.cell_matrix(i, j) +=
                    ((sd.fe_values.shape_value(i, q_point) +       // (phi_i +
                      delta * (sd.advection_directions[q_point] *  // delta beta
                               sd.fe_values.shape_grad(
                                   i, q_point))) *           // grad phi_i)
                     sd.advection_directions[q_point] *      // beta
                     sd.fe_values.shape_grad(j, q_point)) *  // grad phi_j
                    sd.fe_values.JxW(q_point);               // dx
            copy_data.cell_rhs(i) +=
                (sd.fe_values.shape_value(i, q_point) +  // (phi_i +
                 delta *
                     (sd.advection_directions[q_point] *       // delta beta
                      sd.fe_values.shape_grad(i, q_point))) *  // grad phi_i)
                sd.rhs_values[q_point] *                       // f
                sd.fe_values.JxW(q_point);                     // dx
        }
    for (const auto &face : cell->face_iterators())
        if (face->at_boundary()) {
            scratch_data.fe_face_values.reinit(cell, face);
            scratch_data.boundary_values.value_list(
                scratch_data.fe_face_values.get_quadrature_points(),
                scratch_data.face_boundary_values);
            scratch_data.advection_field.value_list(
                scratch_data.fe_face_values.get_quadrature_points(),
                scratch_data.face_advection_directions);
            for (unsigned int q_point = 0; q_point < n_face_q_points; ++q_point)
                if (scratch_data.fe_face_values.normal_vector(q_point) *
                        scratch_data.face_advection_directions[q_point] <
                    0.)
                    for (unsigned int i = 0; i < dofs_per_cell; ++i) {
                        for (unsigned int j = 0; j < dofs_per_cell; ++j)
                            copy_data.cell_matrix(i, j) -=
                                (scratch_data
                                     .face_advection_directions[q_point] *
                                 scratch_data.fe_face_values.normal_vector(
                                     q_point) *
                                 scratch_data.fe_face_values.shape_value(
                                     i, q_point) *
                                 scratch_data.fe_face_values.shape_value(
                                     j, q_point) *
                                 scratch_data.fe_face_values.JxW(q_point));
                        copy_data.cell_rhs(i) -=
                            (scratch_data.face_advection_directions[q_point] *
                             scratch_data.fe_face_values.normal_vector(
                                 q_point) *
                             scratch_data.face_boundary_values[q_point] *
                             scratch_data.fe_face_values.shape_value(i,
                                                                     q_point) *
                             scratch_data.fe_face_values.JxW(q_point));
                    }
        }
    cell->get_dof_indices(copy_data.local_dof_indices);
}

template <int dim>
void AdvectionProblem<dim>::copy_local_to_global(
    const AssemblyCopyData &copy_data)
{
    hanging_node_constraints.distribute_local_to_global(
        copy_data.cell_matrix, copy_data.cell_rhs, copy_data.local_dof_indices,
        system_matrix, system_rhs);
}

template <int dim>
void AdvectionProblem<dim>::solve(MPI_Comm mpi_communicator)
{
    using ValueType = double;
    using IndexType = int;
    schwz::Metadata<ValueType, IndexType> metadata;
    schwz::Settings settings(FLAGS_executor);

    // Set solver metadata from command line args.
    metadata.mpi_communicator = mpi_communicator;
    MPI_Comm_rank(metadata.mpi_communicator, &metadata.my_rank);
    MPI_Comm_size(metadata.mpi_communicator, &metadata.comm_size);
    metadata.tolerance = FLAGS_set_tol;
    metadata.max_iters = FLAGS_num_iters;
    metadata.num_subdomains = metadata.comm_size;
    metadata.num_threads = FLAGS_num_threads;
    metadata.oned_laplacian_size = FLAGS_set_1d_laplacian_size;

    // Generic settings
    settings.write_debug_out = FLAGS_enable_debug_write;
    settings.write_perm_data = FLAGS_write_perm_data;
    settings.write_iters_and_residuals = FLAGS_write_iters_and_residuals;
    settings.print_matrices = FLAGS_print_matrices;
    settings.shifted_iter = FLAGS_shifted_iter;

    // Set solver settings from command line args.
    // Comm settings
    settings.comm_settings.enable_onesided = FLAGS_enable_onesided;
    if (FLAGS_remote_comm_type == "put") {
        settings.comm_settings.enable_put = true;
        settings.comm_settings.enable_get = false;
    } else if (FLAGS_remote_comm_type == "get") {
        settings.comm_settings.enable_put = false;
        settings.comm_settings.enable_get = true;
    }
    settings.comm_settings.enable_one_by_one = FLAGS_enable_one_by_one;
    settings.comm_settings.stage_through_host = FLAGS_stage_through_host;
    settings.comm_settings.enable_overlap = FLAGS_enable_comm_overlap;
    if (FLAGS_flush_type == "flush-all") {
        settings.comm_settings.enable_flush_all = true;
    } else if (FLAGS_flush_type == "flush-local") {
        settings.comm_settings.enable_flush_all = false;
        settings.comm_settings.enable_flush_local = true;
    }
    if (FLAGS_lock_type == "lock-all") {
        settings.comm_settings.enable_lock_all = true;
    } else if (FLAGS_lock_type == "lock-local") {
        settings.comm_settings.enable_lock_all = false;
        settings.comm_settings.enable_lock_local = true;
    }

    // Convergence settings
    settings.convergence_settings.put_all_local_residual_norms =
        FLAGS_enable_put_all_local_residual_norms;
    settings.convergence_settings.enable_global_check_iter_offset =
        FLAGS_enable_global_check_iter_offset;
    settings.convergence_settings.enable_global_check =
        FLAGS_enable_global_check;
    if (FLAGS_global_convergence_type == "centralized-tree") {
        settings.convergence_settings.enable_global_simple_tree = true;
    } else if (FLAGS_global_convergence_type == "decentralized") {
        settings.convergence_settings.enable_decentralized_leader_election =
            true;
        settings.convergence_settings.enable_accumulate =
            FLAGS_enable_decentralized_accumulate;
    }

    // General solver settings
    metadata.local_solver_tolerance = FLAGS_local_tol;
    metadata.local_precond = FLAGS_local_precond;
    metadata.local_max_iters = FLAGS_local_max_iters;
    metadata.updated_max_iters = FLAGS_updated_max_iters;
    settings.non_symmetric_matrix = FLAGS_non_symmetric_matrix;
    settings.restart_iter = FLAGS_restart_iter;
    settings.reset_local_crit_iter = FLAGS_reset_local_crit_iter;
    settings.enable_logging = FLAGS_enable_logging;
    metadata.precond_max_block_size = FLAGS_precond_max_block_size;
    settings.matrix_filename = FLAGS_matrix_filename;
    settings.explicit_laplacian = FLAGS_explicit_laplacian;
    settings.enable_random_rhs = FLAGS_enable_random_rhs;
    settings.use_mixed_precision = FLAGS_use_mixed_precision;
    settings.overlap = FLAGS_overlap;
    settings.naturally_ordered_factor = FLAGS_factor_ordering_natural;
    settings.reorder = FLAGS_local_reordering;
    settings.factorization = FLAGS_local_factorization;
    if (FLAGS_partition == "metis") {
        settings.partition =
            schwz::Settings::partition_settings::partition_metis;
        settings.metis_objtype = FLAGS_metis_objtype;
    } else if (FLAGS_partition == "regular") {
        settings.partition =
            schwz::Settings::partition_settings::partition_regular;
    } else if (FLAGS_partition == "regular2d") {
        settings.partition =
            schwz::Settings::partition_settings::partition_regular2d;
    }
    if (FLAGS_local_solver == "iterative-ginkgo") {
        settings.local_solver =
            schwz::Settings::local_solver_settings::iterative_solver_ginkgo;
    } else if (FLAGS_local_solver == "direct-cholmod") {
        settings.local_solver =
            schwz::Settings::local_solver_settings::direct_solver_cholmod;
    } else if (FLAGS_local_solver == "direct-umfpack") {
        settings.local_solver =
            schwz::Settings::local_solver_settings::direct_solver_umfpack;
    } else if (FLAGS_local_solver == "direct-ginkgo") {
        settings.local_solver =
            schwz::Settings::local_solver_settings::direct_solver_ginkgo;
    }
    settings.debug_print = FLAGS_debug;
    int gsize = 0;
    if (metadata.my_rank == 0) {
        metadata.global_size = system_matrix.m();
        std::cout << " Running on the " << FLAGS_executor << " executor on "
                  << metadata.num_subdomains << " ranks with "
                  << FLAGS_num_threads << " threads" << std::endl;
        std::cout << " Problem Size: " << metadata.global_size
                  << " Number of non-zeros: "
                  << system_matrix.n_nonzero_elements() << std::endl;
        gsize = metadata.global_size;
    }
    MPI_Bcast(&gsize, 1, MPI_INT, 0, MPI_COMM_WORLD);
    metadata.global_size = gsize;
    if (FLAGS_print_config) {
        if (metadata.my_rank == 0) {
            this->print_config();
        }
    }
    using vec_vtype = gko::matrix::Dense<ValueType>;
    std::shared_ptr<vec_vtype> solution_vector;
    schwz::SolverRAS<ValueType, IndexType> solver(settings, metadata);
    solver.initialize(system_matrix, system_rhs);
    auto start_time = std::chrono::steady_clock::now();
    solver.run(solution_vector);
    auto elapsed_time = std::chrono::duration<double>(
        std::chrono::steady_clock::now() - start_time);
    if (metadata.my_rank == 0) {
        std::cout << "Time for solve only: " << elapsed_time.count()
                  << std::endl;
    }
    if (FLAGS_timings_file != "null") {
        std::string rank_string = std::to_string(metadata.my_rank);
        if (metadata.my_rank < 10) {
            rank_string = "0" + std::to_string(metadata.my_rank);
        }
        std::string filename = FLAGS_timings_file + "_" + rank_string + ".csv";
        this->write_timings(metadata.time_struct, filename,
                            settings.comm_settings.enable_onesided);
    }
    if (FLAGS_write_comm_data) {
        std::string rank_string = std::to_string(metadata.my_rank);
        if (metadata.my_rank < 10) {
            rank_string = "0" + std::to_string(metadata.my_rank);
        }
        std::string filename_send = "num_send_" + rank_string + ".csv";
        std::string filename_recv = "num_recv_" + rank_string + ".csv";
        this->write_comm_data(metadata.num_subdomains, metadata.my_rank,
                              metadata.comm_data_struct, filename_send,
                              filename_recv);
    }

    if (metadata.my_rank == 0) {
        std::copy(solution_vector->get_values(),
                  solution_vector->get_values() + metadata.global_size,
                  solution.begin());
        hanging_node_constraints.distribute(solution);
    }
}

template <int dim>
void AdvectionProblem<dim>::solve()
{
    SolverControl solver_control(
        std::max<std::size_t>(1000, system_rhs.size() / 10),
        1e-10 * system_rhs.l2_norm());
    SolverGMRES<> solver(solver_control);
    PreconditionJacobi<> preconditioner;
    preconditioner.initialize(system_matrix, 1.0);
    auto start_time = std::chrono::steady_clock::now();
    solver.solve(system_matrix, solution, system_rhs, preconditioner);
    auto elapsed_time = std::chrono::duration<double>(
        std::chrono::steady_clock::now() - start_time);
    std::cout << "Time for solve only: " << elapsed_time.count() << std::endl;
    Vector<double> residual(dof_handler.n_dofs());
    system_matrix.vmult(residual, solution);
    residual -= system_rhs;
    std::cout << "   Iterations required for convergence: "
              << solver_control.last_step() << '\n'
              << "   Max norm of residual:                "
              << residual.linfty_norm() << '\n';
    hanging_node_constraints.distribute(solution);
}

template <int dim>
void AdvectionProblem<dim>::refine_grid()
{
    Vector<float> estimated_error_per_cell(triangulation.n_active_cells());
    GradientEstimation::estimate(dof_handler, solution,
                                 estimated_error_per_cell);
    GridRefinement::refine_and_coarsen_fixed_number(
        triangulation, estimated_error_per_cell, 0.3, 0.03);
    triangulation.execute_coarsening_and_refinement();
}

template <int dim>
void AdvectionProblem<dim>::output_results(const unsigned int cycle) const
{
    {
        GridOut grid_out;
        std::ofstream output("grid-" + std::to_string(cycle) + ".vtu");
        grid_out.write_vtu(triangulation, output);
    }
    {
        DataOut<dim> data_out;
        data_out.attach_dof_handler(dof_handler);
        data_out.add_data_vector(solution, "solution");
        data_out.build_patches(8);
        DataOutBase::VtkFlags vtk_flags;
        vtk_flags.compression_level =
            DataOutBase::VtkFlags::ZlibCompressionLevel::best_speed;
        data_out.set_flags(vtk_flags);
        std::ofstream output("solution-" + std::to_string(cycle) + ".vtu");
        data_out.write_vtu(output);
    }
}

template <int dim>
void AdvectionProblem<dim>::run(MPI_Comm mpi_communicator)
{
    int num_cycles = FLAGS_num_refine_cycles;
    int mpi_size, mpi_rank;
    MPI_Comm_size(MPI_COMM_WORLD, &mpi_size);
    MPI_Comm_rank(MPI_COMM_WORLD, &mpi_rank);
    for (unsigned int cycle = 0; cycle < num_cycles; ++cycle) {
        if (mpi_rank == 0) {
            std::cout << "Cycle " << cycle << ':' << std::endl;

            if (cycle == 0) {
                GridGenerator::hyper_cube(triangulation, -1, 1);
                triangulation.refine_global(FLAGS_init_refine_level);
            } else
                refine_grid();
            std::cout << "   Number of active cells:       "
                      << triangulation.n_active_cells() << std::endl;
            setup_system();
            std::cout << "   Number of degrees of freedom: "
                      << dof_handler.n_dofs() << std::endl;
            assemble_system();
        }
        this->solve(MPI_COMM_WORLD);
        if (mpi_rank == 0) {
            if (FLAGS_vis_sol) {
                output_results(cycle);
            }
        }
    }
}

template <int dim>
void AdvectionProblem<dim>::run()
{
    int num_cycles = FLAGS_num_refine_cycles;
    for (unsigned int cycle = 0; cycle < num_cycles; ++cycle) {
        std::cout << "Cycle " << cycle << ':' << std::endl;

        if (cycle == 0) {
            GridGenerator::hyper_cube(triangulation, -1, 1);
            triangulation.refine_global(FLAGS_init_refine_level);
        } else
            refine_grid();
        std::cout << "   Number of active cells:       "
                  << triangulation.n_active_cells() << std::endl;
        setup_system();
        std::cout << "   Number of degrees of freedom: " << dof_handler.n_dofs()
                  << std::endl;
        assemble_system();
        this->solve();
        if (FLAGS_vis_sol) {
            output_results(cycle);
        }
    }
}

template <int dim>
GradientEstimation::EstimateScratchData<dim>::EstimateScratchData(
    const FiniteElement<dim> &fe, const Vector<double> &solution,
    Vector<float> &error_per_cell)
    : fe_midpoint_value(fe, QMidpoint<dim>(),
                        update_values | update_quadrature_points),
      solution(solution),
      error_per_cell(error_per_cell),
      cell_midpoint_value(1),
      neighbor_midpoint_value(1)
{
    active_neighbors.reserve(GeometryInfo<dim>::faces_per_cell *
                             GeometryInfo<dim>::max_children_per_face);
}

template <int dim>
GradientEstimation::EstimateScratchData<dim>::EstimateScratchData(
    const EstimateScratchData &scratch_data)
    : fe_midpoint_value(scratch_data.fe_midpoint_value.get_fe(),
                        scratch_data.fe_midpoint_value.get_quadrature(),
                        update_values | update_quadrature_points),
      solution(scratch_data.solution),
      error_per_cell(scratch_data.error_per_cell),
      cell_midpoint_value(1),
      neighbor_midpoint_value(1)
{}

template <int dim>
void GradientEstimation::estimate(const DoFHandler<dim> &dof_handler,
                                  const Vector<double> &solution,
                                  Vector<float> &error_per_cell)
{
    Assert(error_per_cell.size() ==
               dof_handler.get_triangulation().n_active_cells(),
           ExcInvalidVectorLength(
               error_per_cell.size(),
               dof_handler.get_triangulation().n_active_cells()));
    WorkStream::run(dof_handler.begin_active(), dof_handler.end(),
                    &GradientEstimation::template estimate_cell<dim>,
                    std::function<void(const EstimateCopyData &)>(),
                    EstimateScratchData<dim>(dof_handler.get_fe(), solution,
                                             error_per_cell),
                    EstimateCopyData());
}

template <int dim>
void GradientEstimation::estimate_cell(
    const typename DoFHandler<dim>::active_cell_iterator &cell,
    EstimateScratchData<dim> &scratch_data, const EstimateCopyData &)
{
    Tensor<2, dim> Y;
    scratch_data.fe_midpoint_value.reinit(cell);
    scratch_data.active_neighbors.clear();
    for (unsigned int face_n : GeometryInfo<dim>::face_indices())
        if (!cell->at_boundary(face_n)) {
            const auto face = cell->face(face_n);
            const auto neighbor = cell->neighbor(face_n);
            if (neighbor->is_active())
                scratch_data.active_neighbors.push_back(neighbor);
            else {
                if (dim == 1) {
                    auto neighbor_child = neighbor;
                    while (neighbor_child->has_children())
                        neighbor_child =
                            neighbor_child->child(face_n == 0 ? 1 : 0);
                    Assert(
                        neighbor_child->neighbor(face_n == 0 ? 1 : 0) == cell,
                        ExcInternalError());
                    scratch_data.active_neighbors.push_back(neighbor_child);
                } else
                    for (unsigned int subface_n = 0;
                         subface_n < face->n_children(); ++subface_n)
                        scratch_data.active_neighbors.push_back(
                            cell->neighbor_child_on_subface(face_n, subface_n));
            }
        }
    const Point<dim> this_center =
        scratch_data.fe_midpoint_value.quadrature_point(0);
    scratch_data.fe_midpoint_value.get_function_values(
        scratch_data.solution, scratch_data.cell_midpoint_value);
    Tensor<1, dim> projected_gradient;
    for (const auto &neighbor : scratch_data.active_neighbors) {
        scratch_data.fe_midpoint_value.reinit(neighbor);
        const Point<dim> neighbor_center =
            scratch_data.fe_midpoint_value.quadrature_point(0);
        scratch_data.fe_midpoint_value.get_function_values(
            scratch_data.solution, scratch_data.neighbor_midpoint_value);
        Tensor<1, dim> y = neighbor_center - this_center;
        const double distance = y.norm();
        y /= distance;
        for (unsigned int i = 0; i < dim; ++i)
            for (unsigned int j = 0; j < dim; ++j) Y[i][j] += y[i] * y[j];
        projected_gradient += (scratch_data.neighbor_midpoint_value[0] -
                               scratch_data.cell_midpoint_value[0]) /
                              distance * y;
    }
    AssertThrow(determinant(Y) != 0, ExcInsufficientDirections());
    const Tensor<2, dim> Y_inverse = invert(Y);
    const Tensor<1, dim> gradient = Y_inverse * projected_gradient;
    scratch_data.error_per_cell(cell->active_cell_index()) =
        (std::pow(cell->diameter(), 1 + 1.0 * dim / 2) * gradient.norm());
}

int main(int argc, char **argv)
{
    using namespace dealii;
    try {
        initialize_argument_parsing(&argc, &argv);
        MultithreadInfo::set_thread_limit();
        AdvectionProblem<2> advection_problem_2d;
        if (FLAGS_num_threads > 1) {
            int req_thread_support = MPI_THREAD_MULTIPLE;
            int prov_thread_support = MPI_THREAD_MULTIPLE;

            MPI_Init_thread(&argc, &argv, req_thread_support,
                            &prov_thread_support);
            if (prov_thread_support != req_thread_support) {
                std::cout << "Required thread support is " << req_thread_support
                          << " but provided thread support is only "
                          << prov_thread_support << std::endl;
            }
        } else {
            MPI_Init(&argc, &argv);
        }
        int rank = 0;
        MPI_Comm_rank(MPI_COMM_WORLD, &rank);

        if (FLAGS_dealii_orig) {
            if (rank == 0) {
                auto start_time = std::chrono::steady_clock::now();
                advection_problem_2d.run();
                auto elapsed_time = std::chrono::duration<double>(
                    std::chrono::steady_clock::now() - start_time);
                std::cout << "Total Time for setup+solve: "
                          << elapsed_time.count() << std::endl;
            }
        } else {
            auto start_time = std::chrono::steady_clock::now();
            advection_problem_2d.run(MPI_COMM_WORLD);
            auto elapsed_time = std::chrono::duration<double>(
                std::chrono::steady_clock::now() - start_time);
            if (rank == 0) {
                std::cout << "Total Time for setup+solve: "
                          << elapsed_time.count() << std::endl;
            }
        }
        MPI_Finalize();
    } catch (std::exception &exc) {
        std::cerr << std::endl
                  << std::endl
                  << "----------------------------------------------------"
                  << std::endl;
        std::cerr << "Exception on processing: " << std::endl
                  << exc.what() << std::endl
                  << "Aborting!" << std::endl
                  << "----------------------------------------------------"
                  << std::endl;
        return 1;
    } catch (...) {
        std::cerr << std::endl
                  << std::endl
                  << "----------------------------------------------------"
                  << std::endl;
        std::cerr << "Unknown exception!" << std::endl
                  << "Aborting!" << std::endl
                  << "----------------------------------------------------"
                  << std::endl;
        return 1;
    }
    return 0;
}


#include <iostream>
#include <cmath>
#include <algorithm>
#include <vector>
#include <boost/multiprecision/cpp_int.hpp>
using namespace boost::multiprecision;
using namespace std;
int main() {
    cpp_int a, b, c; cin >> a >> b >> c;

    cout << (a * b * c * (a + 1) * (b + 1) * (c + 1) / 8) % 998244353 << endl;
}


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once

#include "../util/ConvolutionTools.hpp"
#include "../util/FluidEigenMappings.hpp"
#include "../util/Novelty.hpp"
#include "../../data/FluidIndex.hpp"
#include "../../data/TensorTypes.hpp"
#include <Eigen/Core>

namespace fluid {
namespace algorithm {

class NoveltySegmentation
{

public:
  using ArrayXd = Eigen::ArrayXd;

  NoveltySegmentation(index maxKernelSize, index maxFilterSize)
      : mFilterBufferStorage(maxFilterSize), mNovelty(maxKernelSize)
  {}

  void init(index kernelSize, index filterSize, index nDims)
  {
    assert(kernelSize % 2);
    mNovelty.init(kernelSize, nDims);
    mFilterBuffer = mFilterBufferStorage.segment(0, filterSize);
    mFilterBuffer.setZero();
    mDebounceCount = 1;
  }

  double processFrame(const RealVectorView input, double threshold,
                      index minSliceLength)
  {
    double novelty = mNovelty.processFrame(_impl::asEigen<Eigen::Array>(input));
    double detected = 0.;
    index  filterSize = mFilterBuffer.size();
    if (filterSize > 1)
    {
      mFilterBuffer.segment(0, filterSize - 1) =
          mFilterBuffer.segment(1, filterSize - 1);
    }
    mPeakBuffer.segment(0, 2) = mPeakBuffer.segment(1, 2);
    mFilterBuffer(filterSize - 1) = novelty;
    mPeakBuffer(2) = mFilterBuffer.mean();
    if (mPeakBuffer(1) > mPeakBuffer(0) && mPeakBuffer(1) > mPeakBuffer(2) &&
        mPeakBuffer(1) > threshold && mDebounceCount == 0)
    {
      detected = 1.0;
      mDebounceCount = minSliceLength;
    }
    else
    {
      if (mDebounceCount > 0) mDebounceCount--;
    }
    return detected;
  }

private:
  ArrayXd mFilterBuffer;
  ArrayXd mFilterBufferStorage;
  ArrayXd mPeakBuffer{3};
  Novelty mNovelty;
  index   mDebounceCount{1};
};
} // namespace algorithm
} // namespace fluid