#pragma once
#ifndef WTL_GRN_HPP_
#define WTL_GRN_HPP_

#include <string>
#include <sstream>

#include <boost/graph/adjacency_list.hpp>

#include <boost/graph/graphviz.hpp>
#include <boost/graph/graphml.hpp>

/////////1/////////2/////////3/////////4/////////5/////////6/////////7/////////

enum graph_frequency_t {graph_frequency};

namespace boost {
    BOOST_INSTALL_PROPERTY(graph, frequency);
}

/////////1/////////2/////////3/////////4/////////5/////////6/////////7/////////
namespace wtl {
/////////1/////////2/////////3/////////4/////////5/////////6/////////7/////////

typedef boost::property<boost::vertex_index1_t, std::string,
        boost::property<boost::vertex_index2_t, std::string> > VertexProperty;
typedef boost::property<boost::edge_weight_t, double> EdgeProperty;

typedef std::map<std::string, std::string> AttrMap;

typedef boost::property<boost::graph_graph_attribute_t, AttrMap,
        boost::property<boost::graph_vertex_attribute_t, AttrMap,
        boost::property<boost::graph_edge_attribute_t, AttrMap> > > GraphProperty;

typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::bidirectionalS,
//                       boost::no_property, EdgeProperty, GraphProperty> Graph;
                       VertexProperty, EdgeProperty, GraphProperty> Graph;

/////////1/////////2/////////3/////////4/////////5/////////6/////////7/////////

inline Graph make_graph(const std::vector<std::vector<double> >& adj_matrix) {
    const size_t order = adj_matrix.size();
    std::vector<std::pair<size_t, size_t> > adj_list;
    std::vector<double> weights;
    for (size_t i=0; i<order; ++i) {
        const auto& row = adj_matrix[i];
        for (size_t j=0; j<order; ++j) {
            if (row[j] != 0.0) {
                adj_list.push_back({j, i});
                weights.push_back(row[j]);
            }
        }
    }
    Graph graph(adj_list.begin(), adj_list.end(), weights.begin(), order);
    return graph;
}

template <class GraphType, class Value> inline
void insert_graphattr(GraphType* graph, const std::string& key, const Value& value) {
    auto attrmap = boost::get_property(*graph, boost::graph_graph_attribute);
    attrmap[key] = std::to_string(value);
    boost::set_property(*graph, boost::graph_graph_attribute, attrmap);
}

/////////1/////////2/////////3/////////4/////////5/////////6/////////7/////////

template <class Values>
class property_writer {
  public:
    property_writer(const Values& v, const std::string& key)
        : values_(v), key_(key) {}

    template <class VertexOrEdge>
    void operator()(std::ostream& ost, const VertexOrEdge& voe) const {
        ost << "[" << key_ << "=" << boost::get(values_, voe) << "]";
    }
  private:
    const Values values_;
    const std::string key_;
};
template <class Values> inline
property_writer<Values>
make_property_writer(const Values& values, const std::string& key="label") {
    return property_writer<Values>(values, key);
}

/////////1/////////2/////////3/////////4/////////5/////////6/////////7/////////

template <class M1, class M2>
class double_writer {
  public:
    double_writer(const M1& m1, const std::string& key1, const M2& m2, const std::string& key2)
        : m1_(m1), key1_(key1), m2_(m2), key2_(key2) {}

    template <class VertexOrEdge>
    void operator()(std::ostream& ost, const VertexOrEdge& voe) const {
        ost << "["
            << key1_ << "=" << boost::get(m1_, voe) << ","
            << key2_ << "=" << boost::get(m2_, voe) << "]";
    }
  private:
    const M1 m1_;
    const std::string key1_;
    const M2 m2_;
    const std::string key2_;
};
template <class M1, class M2> inline
double_writer<M1, M2>
make_double_writer(const M1& m1, const std::string& key1,
                   const M2& m2, const std::string& key2) {
    return double_writer<M1, M2>(m1, key1, m2, key2);
}

/////////1/////////2/////////3/////////4/////////5/////////6/////////7/////////

inline std::ostream& write_graphviz(std::ostream& ost,
                 const std::vector<std::vector<double> >& adj_matrix,
                 const size_t frequency=0,
                 const std::vector<std::string>& normal_input={},
                 const std::vector<std::string>& novel_input={})
{
    Graph graph = make_graph(adj_matrix);
    for (size_t i=0; i<normal_input.size(); ++i) {
        boost::put(boost::vertex_index1, graph,
                   boost::vertex(i, graph), normal_input[i]);
    }
    for (size_t i=0; i<novel_input.size(); ++i) {
        boost::put(boost::vertex_index2, graph,
                   boost::vertex(i, graph), novel_input[i]);
    }
    insert_graphattr(&graph, "frequency", frequency);
    boost::write_graphviz(ost, graph,
        make_double_writer(boost::get(boost::vertex_index1, graph), "normal",
                           boost::get(boost::vertex_index2, graph), "novel"),
        make_property_writer(boost::get(boost::edge_weight, graph), "weight"),
        boost::make_graph_attributes_writer(graph));
    return ost;
}

template <class GraphType> inline
std::string str_graphml(GraphType& graph) {
    std::ostringstream oss;
    oss << std::fixed;
    oss.precision(5);
    boost::dynamic_properties dp;
 //   dp.property("node_id", boost::get(boost::vertex_index, graph));
    dp.property("input", boost::get(typename GraphType::vertex_property_type::tag_type(), graph));
    dp.property("weight", boost::get(typename GraphType::edge_property_type::tag_type(), graph));
    boost::write_graphml(oss, graph, dp, /*ordered_vertices=*/true);
    return oss.str();
}

/////////1/////////2/////////3/////////4/////////5/////////6/////////7/////////
} // namespace wtl
/////////1/////////2/////////3/////////4/////////5/////////6/////////7/////////

#endif // WTL_GRN_HPP_


// Copyright (C) 2011 by the BEM++ Authors
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.

#include "bempp/common/config_ahmed.hpp"

#ifdef WITH_AHMED

#include "../type_template.hpp"
#include "../check_arrays_are_close.hpp"

#include "assembly/context.hpp"
#include "assembly/discrete_boundary_operator.hpp"
#include "assembly/laplace_3d_adjoint_double_layer_boundary_operator.hpp"
#include "assembly/laplace_3d_double_layer_boundary_operator.hpp"
#include "assembly/laplace_3d_hypersingular_boundary_operator.hpp"
#include "assembly/laplace_3d_single_layer_boundary_operator.hpp"
#include "assembly/helmholtz_3d_hypersingular_boundary_operator.hpp"
#include "assembly/maxwell_3d_single_layer_boundary_operator.hpp"
#include "assembly/modified_helmholtz_3d_adjoint_double_layer_boundary_operator.hpp"
#include "assembly/modified_helmholtz_3d_double_layer_boundary_operator.hpp"
#include "assembly/modified_helmholtz_3d_single_layer_boundary_operator.hpp"
#include "assembly/modified_helmholtz_3d_hypersingular_boundary_operator.hpp"
#include "assembly/numerical_quadrature_strategy.hpp"

#include "grid/grid_factory.hpp"

#include "space/piecewise_constant_scalar_space.hpp"
#include "space/piecewise_linear_continuous_scalar_space.hpp"
#include "space/piecewise_linear_discontinuous_scalar_space.hpp"
#include "space/raviart_thomas_0_vector_space.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/type_traits/is_complex.hpp>

using namespace Bempp;

namespace Bempp
{

namespace
{

template <typename T> T initWaveNumber();
template <> float initWaveNumber() { return 1.2f; }
template <> double initWaveNumber(){ return 1.2; }
template <> std::complex<float> initWaveNumber()
{ return std::complex<float>(1.2f, 0.7f); }
template <> std::complex<double> initWaveNumber()
{ return std::complex<double>(1.2, 0.7); }

}

}

// Tests

BOOST_AUTO_TEST_SUITE(SyntheticBoundaryOperator)

BOOST_AUTO_TEST_CASE_TEMPLATE(aca_of_synthetic_single_layer_operator_agrees_with_dense_assembly_for_pwise_constants,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
        params, "../../meshes/sphere-h-0.4.msh", false /* verbose */);

    shared_ptr<Space<BFT> > pwiseConstants(
        new PiecewiseConstantScalarSpace<BFT>(grid));
    shared_ptr<Space<BFT> > pwiseLinears(
        new PiecewiseLinearContinuousScalarSpace<BFT>(grid));

    AccuracyOptions accuracyOptions;
    accuracyOptions.doubleRegular.setRelativeQuadratureOrder(2);
    accuracyOptions.singleRegular.setRelativeQuadratureOrder(2);
    shared_ptr<NumericalQuadratureStrategy<BFT, RT> > quadStrategy(
                new NumericalQuadratureStrategy<BFT, RT>(accuracyOptions));

    AssemblyOptions assemblyOptionsDense;
    assemblyOptionsDense.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<Context<BFT, RT> > contextDense(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsDense));

    BoundaryOperator<BFT, RT> opDense =
            laplace3dSingleLayerBoundaryOperator<BFT, RT>(
                contextDense, pwiseConstants, pwiseLinears, pwiseConstants);
    arma::Mat<RT> weakFormDense = opDense.weakForm()->asMatrix();

    AssemblyOptions assemblyOptionsAca;
    assemblyOptionsAca.setVerbosityLevel(VerbosityLevel::LOW);
    AcaOptions acaOptions;
    acaOptions.mode = AcaOptions::LOCAL_ASSEMBLY;
    assemblyOptionsAca.switchToAcaMode(acaOptions);
    shared_ptr<Context<BFT, RT> > contextAca(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsAca));

    BoundaryOperator<BFT, RT> opAca =
            laplace3dSingleLayerBoundaryOperator<BFT, RT>(
                contextAca, pwiseConstants, pwiseLinears, pwiseConstants);
    arma::Mat<RT> weakFormAca = opAca.weakForm()->asMatrix();

    BOOST_CHECK(check_arrays_are_close<ValueType>(
                    weakFormDense, weakFormAca, 2. * acaOptions.eps));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(aca_of_synthetic_single_layer_operator_agrees_with_dense_assembly_for_pwise_linears,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
        params, "../../meshes/sphere-h-0.4.msh", false /* verbose */);

    shared_ptr<Space<BFT> > pwiseConstants(
        new PiecewiseConstantScalarSpace<BFT>(grid));
    shared_ptr<Space<BFT> > pwiseLinears(
        new PiecewiseLinearContinuousScalarSpace<BFT>(grid));

    AccuracyOptions accuracyOptions;
    accuracyOptions.doubleRegular.setRelativeQuadratureOrder(2);
    accuracyOptions.singleRegular.setRelativeQuadratureOrder(2);
    shared_ptr<NumericalQuadratureStrategy<BFT, RT> > quadStrategy(
                new NumericalQuadratureStrategy<BFT, RT>(accuracyOptions));

    AssemblyOptions assemblyOptionsDense;
    assemblyOptionsDense.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<Context<BFT, RT> > contextDense(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsDense));

    BoundaryOperator<BFT, RT> opDense =
            laplace3dSingleLayerBoundaryOperator<BFT, RT>(
                contextDense, pwiseLinears, pwiseLinears, pwiseLinears);
    arma::Mat<RT> weakFormDense = opDense.weakForm()->asMatrix();

    AssemblyOptions assemblyOptionsAca;
    assemblyOptionsAca.setVerbosityLevel(VerbosityLevel::LOW);
    AcaOptions acaOptions;
    acaOptions.mode = AcaOptions::LOCAL_ASSEMBLY;
    assemblyOptionsAca.switchToAcaMode(acaOptions);
    shared_ptr<Context<BFT, RT> > contextAca(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsAca));

    BoundaryOperator<BFT, RT> opAca =
            laplace3dSingleLayerBoundaryOperator<BFT, RT>(
                contextAca, pwiseLinears, pwiseLinears, pwiseLinears);
    arma::Mat<RT> weakFormAca = opAca.weakForm()->asMatrix();

    BOOST_CHECK(check_arrays_are_close<ValueType>(
                    weakFormDense, weakFormAca, 2. * acaOptions.eps));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(aca_of_synthetic_double_layer_operator_agrees_with_dense_assembly,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
        params, "../../meshes/sphere-h-0.4.msh", false /* verbose */);

    shared_ptr<Space<BFT> > pwiseConstants(
        new PiecewiseConstantScalarSpace<BFT>(grid));
    shared_ptr<Space<BFT> > pwiseLinears(
        new PiecewiseLinearContinuousScalarSpace<BFT>(grid));

    AccuracyOptions accuracyOptions;
    accuracyOptions.doubleRegular.setRelativeQuadratureOrder(2);
    accuracyOptions.singleRegular.setRelativeQuadratureOrder(2);
    shared_ptr<NumericalQuadratureStrategy<BFT, RT> > quadStrategy(
                new NumericalQuadratureStrategy<BFT, RT>(accuracyOptions));

    AssemblyOptions assemblyOptionsDense;
    assemblyOptionsDense.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<Context<BFT, RT> > contextDense(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsDense));

    BoundaryOperator<BFT, RT> opDense =
            laplace3dDoubleLayerBoundaryOperator<BFT, RT>(
                contextDense, pwiseLinears, pwiseLinears, pwiseConstants);
    arma::Mat<RT> weakFormDense = opDense.weakForm()->asMatrix();

    AssemblyOptions assemblyOptionsAca;
    assemblyOptionsAca.setVerbosityLevel(VerbosityLevel::LOW);
    AcaOptions acaOptions;
    acaOptions.mode = AcaOptions::LOCAL_ASSEMBLY;
    assemblyOptionsAca.switchToAcaMode(acaOptions);
    shared_ptr<Context<BFT, RT> > contextAca(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsAca));

    BoundaryOperator<BFT, RT> opAca =
            laplace3dDoubleLayerBoundaryOperator<BFT, RT>(
                contextAca, pwiseLinears, pwiseLinears, pwiseConstants);
    arma::Mat<RT> weakFormAca = opAca.weakForm()->asMatrix();

    BOOST_CHECK(check_arrays_are_close<ValueType>(
                    weakFormDense, weakFormAca, 2. * acaOptions.eps));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(aca_of_synthetic_adjoint_double_layer_operator_agrees_with_dense_assembly,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
        params, "../../meshes/sphere-h-0.4.msh", false /* verbose */);

    shared_ptr<Space<BFT> > pwiseConstants(
        new PiecewiseConstantScalarSpace<BFT>(grid));
    shared_ptr<Space<BFT> > pwiseLinears(
        new PiecewiseLinearContinuousScalarSpace<BFT>(grid));

    AccuracyOptions accuracyOptions;
    accuracyOptions.doubleRegular.setRelativeQuadratureOrder(2);
    accuracyOptions.singleRegular.setRelativeQuadratureOrder(2);
    shared_ptr<NumericalQuadratureStrategy<BFT, RT> > quadStrategy(
                new NumericalQuadratureStrategy<BFT, RT>(accuracyOptions));

    AssemblyOptions assemblyOptionsDense;
    assemblyOptionsDense.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<Context<BFT, RT> > contextDense(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsDense));

    BoundaryOperator<BFT, RT> opDense =
            laplace3dAdjointDoubleLayerBoundaryOperator<BFT, RT>(
                contextDense, pwiseConstants, pwiseConstants, pwiseLinears);
    arma::Mat<RT> weakFormDense = opDense.weakForm()->asMatrix();

    AssemblyOptions assemblyOptionsAca;
    assemblyOptionsAca.setVerbosityLevel(VerbosityLevel::LOW);
    AcaOptions acaOptions;
    acaOptions.mode = AcaOptions::LOCAL_ASSEMBLY;
    assemblyOptionsAca.switchToAcaMode(acaOptions);
    shared_ptr<Context<BFT, RT> > contextAca(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsAca));

    BoundaryOperator<BFT, RT> opAca =
            laplace3dAdjointDoubleLayerBoundaryOperator<BFT, RT>(
                contextAca, pwiseConstants, pwiseConstants, pwiseLinears);
    arma::Mat<RT> weakFormAca = opAca.weakForm()->asMatrix();

    BOOST_CHECK(check_arrays_are_close<ValueType>(
                    weakFormDense, weakFormAca, 2. * acaOptions.eps));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(aca_of_synthetic_hypersingular_operator_agrees_with_dense_assembly_in_symmetric_case,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
        params, "../../meshes/sphere-h-0.4.msh", false /* verbose */);

    shared_ptr<Space<BFT> > pwiseConstants(
        new PiecewiseConstantScalarSpace<BFT>(grid));
    shared_ptr<Space<BFT> > pwiseLinears(
        new PiecewiseLinearContinuousScalarSpace<BFT>(grid));

    AccuracyOptions accuracyOptions;
    accuracyOptions.doubleRegular.setRelativeQuadratureOrder(2);
    accuracyOptions.singleRegular.setRelativeQuadratureOrder(2);
    shared_ptr<NumericalQuadratureStrategy<BFT, RT> > quadStrategy(
                new NumericalQuadratureStrategy<BFT, RT>(accuracyOptions));

    AssemblyOptions assemblyOptionsDense;
    assemblyOptionsDense.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<Context<BFT, RT> > contextDense(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsDense));

    BoundaryOperator<BFT, RT> opDense =
            laplace3dHypersingularBoundaryOperator<BFT, RT>(
                contextDense, pwiseLinears, pwiseConstants, pwiseLinears);
    arma::Mat<RT> weakFormDense = opDense.weakForm()->asMatrix();

    AssemblyOptions assemblyOptionsAca;
    assemblyOptionsAca.setVerbosityLevel(VerbosityLevel::LOW);
    AcaOptions acaOptions;
    acaOptions.mode = AcaOptions::LOCAL_ASSEMBLY;
    assemblyOptionsAca.switchToAcaMode(acaOptions);
    shared_ptr<Context<BFT, RT> > contextAca(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsAca));

    BoundaryOperator<BFT, RT> opAca =
            laplace3dHypersingularBoundaryOperator<BFT, RT>(
                contextAca, pwiseLinears, pwiseConstants, pwiseLinears);
    arma::Mat<RT> weakFormAca = opAca.weakForm()->asMatrix();

    BOOST_CHECK(check_arrays_are_close<ValueType>(
                    weakFormDense, weakFormAca, 2. * acaOptions.eps));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(aca_of_synthetic_hypersingular_operator_agrees_with_dense_assembly_in_asymmetric_case,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
        params, "../../meshes/sphere-h-0.4.msh", false /* verbose */);

    shared_ptr<Space<BFT> > pwiseConstants(
        new PiecewiseConstantScalarSpace<BFT>(grid));
    shared_ptr<Space<BFT> > pwiseLinears(
        new PiecewiseLinearContinuousScalarSpace<BFT>(grid));
    shared_ptr<Space<BFT> > pwiseLinears2(
        new PiecewiseLinearContinuousScalarSpace<BFT>(grid));

    AccuracyOptions accuracyOptions;
    accuracyOptions.doubleRegular.setRelativeQuadratureOrder(2);
    accuracyOptions.singleRegular.setRelativeQuadratureOrder(2);
    shared_ptr<NumericalQuadratureStrategy<BFT, RT> > quadStrategy(
                new NumericalQuadratureStrategy<BFT, RT>(accuracyOptions));

    AssemblyOptions assemblyOptionsDense;
    assemblyOptionsDense.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<Context<BFT, RT> > contextDense(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsDense));

    BoundaryOperator<BFT, RT> opDense =
            laplace3dHypersingularBoundaryOperator<BFT, RT>(
                contextDense, pwiseLinears, pwiseConstants, pwiseLinears);
    arma::Mat<RT> weakFormDense = opDense.weakForm()->asMatrix();

    AssemblyOptions assemblyOptionsAca;
    assemblyOptionsAca.setVerbosityLevel(VerbosityLevel::LOW);
    AcaOptions acaOptions;
    acaOptions.mode = AcaOptions::LOCAL_ASSEMBLY;
    assemblyOptionsAca.switchToAcaMode(acaOptions);
    shared_ptr<Context<BFT, RT> > contextAca(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsAca));

    // domain != dualToRange
    BoundaryOperator<BFT, RT> opAca =
            laplace3dHypersingularBoundaryOperator<BFT, RT>(
                contextAca, pwiseLinears, pwiseConstants, pwiseLinears2);
    arma::Mat<RT> weakFormAca = opAca.weakForm()->asMatrix();

    BOOST_CHECK(check_arrays_are_close<ValueType>(
                    weakFormDense, weakFormAca, 2. * acaOptions.eps));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(aca_of_synthetic_modified_helmholtz_single_layer_operator_agrees_with_dense_assembly_for_pwise_linears,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
        params, "../../meshes/sphere-h-0.4.msh", false /* verbose */);

    RT waveNumber = initWaveNumber<RT>();

    shared_ptr<Space<BFT> > pwiseConstants(
        new PiecewiseConstantScalarSpace<BFT>(grid));
    shared_ptr<Space<BFT> > pwiseLinears(
        new PiecewiseLinearContinuousScalarSpace<BFT>(grid));

    AccuracyOptions accuracyOptions;
    accuracyOptions.doubleRegular.setRelativeQuadratureOrder(2);
    accuracyOptions.singleRegular.setRelativeQuadratureOrder(2);
    shared_ptr<NumericalQuadratureStrategy<BFT, RT> > quadStrategy(
                new NumericalQuadratureStrategy<BFT, RT>(accuracyOptions));

    AssemblyOptions assemblyOptionsDense;
    assemblyOptionsDense.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<Context<BFT, RT> > contextDense(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsDense));

    BoundaryOperator<BFT, RT> opDense =
            modifiedHelmholtz3dSingleLayerBoundaryOperator<BFT, RT, RT>(
                contextDense, pwiseLinears, pwiseLinears, pwiseLinears,
                waveNumber);
    arma::Mat<RT> weakFormDense = opDense.weakForm()->asMatrix();

    AssemblyOptions assemblyOptionsAca;
    assemblyOptionsAca.setVerbosityLevel(VerbosityLevel::LOW);
    AcaOptions acaOptions;
    acaOptions.mode = AcaOptions::LOCAL_ASSEMBLY;
    assemblyOptionsAca.switchToAcaMode(acaOptions);
    shared_ptr<Context<BFT, RT> > contextAca(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsAca));

    BoundaryOperator<BFT, RT> opAca =
            modifiedHelmholtz3dSingleLayerBoundaryOperator<BFT, RT, RT>(
                contextAca, pwiseLinears, pwiseLinears, pwiseLinears, waveNumber);
    arma::Mat<RT> weakFormAca = opAca.weakForm()->asMatrix();

    BOOST_CHECK(check_arrays_are_close<ValueType>(
                    weakFormDense, weakFormAca, 2. * acaOptions.eps));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(aca_of_synthetic_modified_helmholtz_double_layer_operator_agrees_with_dense_assembly,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
        params, "../../meshes/sphere-h-0.4.msh", false /* verbose */);

    RT waveNumber = initWaveNumber<RT>();

    shared_ptr<Space<BFT> > pwiseConstants(
        new PiecewiseConstantScalarSpace<BFT>(grid));
    shared_ptr<Space<BFT> > pwiseLinears(
        new PiecewiseLinearContinuousScalarSpace<BFT>(grid));

    AccuracyOptions accuracyOptions;
    accuracyOptions.doubleRegular.setRelativeQuadratureOrder(2);
    accuracyOptions.singleRegular.setRelativeQuadratureOrder(2);
    shared_ptr<NumericalQuadratureStrategy<BFT, RT> > quadStrategy(
                new NumericalQuadratureStrategy<BFT, RT>(accuracyOptions));

    AssemblyOptions assemblyOptionsDense;
    assemblyOptionsDense.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<Context<BFT, RT> > contextDense(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsDense));

    BoundaryOperator<BFT, RT> opDense =
            modifiedHelmholtz3dDoubleLayerBoundaryOperator<BFT, RT, RT>(
                contextDense, pwiseLinears, pwiseLinears, pwiseConstants,
                waveNumber);
    arma::Mat<RT> weakFormDense = opDense.weakForm()->asMatrix();

    AssemblyOptions assemblyOptionsAca;
    assemblyOptionsAca.setVerbosityLevel(VerbosityLevel::LOW);
    AcaOptions acaOptions;
    acaOptions.mode = AcaOptions::LOCAL_ASSEMBLY;
    assemblyOptionsAca.switchToAcaMode(acaOptions);
    shared_ptr<Context<BFT, RT> > contextAca(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsAca));

    BoundaryOperator<BFT, RT> opAca =
            modifiedHelmholtz3dDoubleLayerBoundaryOperator<BFT, RT, RT>(
                contextAca, pwiseLinears, pwiseLinears, pwiseConstants,
                waveNumber);
    arma::Mat<RT> weakFormAca = opAca.weakForm()->asMatrix();

    BOOST_CHECK(check_arrays_are_close<ValueType>(
                    weakFormDense, weakFormAca, 2. * acaOptions.eps));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(aca_of_synthetic_modified_helmholtz_adjoint_double_layer_operator_agrees_with_dense_assembly,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
        params, "../../meshes/sphere-h-0.4.msh", false /* verbose */);

    RT waveNumber = initWaveNumber<RT>();

    shared_ptr<Space<BFT> > pwiseConstants(
        new PiecewiseConstantScalarSpace<BFT>(grid));
    shared_ptr<Space<BFT> > pwiseLinears(
        new PiecewiseLinearContinuousScalarSpace<BFT>(grid));

    AccuracyOptions accuracyOptions;
    accuracyOptions.doubleRegular.setRelativeQuadratureOrder(2);
    accuracyOptions.singleRegular.setRelativeQuadratureOrder(2);
    shared_ptr<NumericalQuadratureStrategy<BFT, RT> > quadStrategy(
                new NumericalQuadratureStrategy<BFT, RT>(accuracyOptions));

    AssemblyOptions assemblyOptionsDense;
    assemblyOptionsDense.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<Context<BFT, RT> > contextDense(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsDense));

    BoundaryOperator<BFT, RT> opDense =
            modifiedHelmholtz3dAdjointDoubleLayerBoundaryOperator<BFT, RT, RT>(
                contextDense, pwiseConstants, pwiseConstants, pwiseLinears,
                waveNumber);
    arma::Mat<RT> weakFormDense = opDense.weakForm()->asMatrix();

    AssemblyOptions assemblyOptionsAca;
    assemblyOptionsAca.setVerbosityLevel(VerbosityLevel::LOW);
    AcaOptions acaOptions;
    acaOptions.mode = AcaOptions::LOCAL_ASSEMBLY;
    assemblyOptionsAca.switchToAcaMode(acaOptions);
    shared_ptr<Context<BFT, RT> > contextAca(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsAca));

    BoundaryOperator<BFT, RT> opAca =
            modifiedHelmholtz3dAdjointDoubleLayerBoundaryOperator<BFT, RT, RT>(
                contextAca, pwiseConstants, pwiseConstants, pwiseLinears,
                waveNumber);
    arma::Mat<RT> weakFormAca = opAca.weakForm()->asMatrix();

    BOOST_CHECK(check_arrays_are_close<ValueType>(
                    weakFormDense, weakFormAca, 2. * acaOptions.eps));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(aca_of_synthetic_helmholtz_hypersingular_operator_agrees_with_dense_assembly_in_symmetric_case,
                              ValueType, complex_result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
        params, "../../meshes/sphere-h-0.4.msh", false /* verbose */);

    RT waveNumber = initWaveNumber<RT>();

    shared_ptr<Space<BFT> > pwiseConstants(
        new PiecewiseConstantScalarSpace<BFT>(grid));
    shared_ptr<Space<BFT> > pwiseLinears(
        new PiecewiseLinearContinuousScalarSpace<BFT>(grid));

    AccuracyOptions accuracyOptions;
    accuracyOptions.doubleRegular.setRelativeQuadratureOrder(2);
    accuracyOptions.singleRegular.setRelativeQuadratureOrder(2);
    shared_ptr<NumericalQuadratureStrategy<BFT, RT> > quadStrategy(
                new NumericalQuadratureStrategy<BFT, RT>(accuracyOptions));

    AssemblyOptions assemblyOptionsDense;
    assemblyOptionsDense.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<Context<BFT, RT> > contextDense(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsDense));

    BoundaryOperator<BFT, RT> opDense =
            helmholtz3dHypersingularBoundaryOperator<BFT>(
                contextDense, pwiseLinears, pwiseConstants, pwiseLinears,
                waveNumber);
    arma::Mat<RT> weakFormDense = opDense.weakForm()->asMatrix();

    AssemblyOptions assemblyOptionsAca;
    assemblyOptionsAca.setVerbosityLevel(VerbosityLevel::LOW);
    AcaOptions acaOptions;
    acaOptions.mode = AcaOptions::LOCAL_ASSEMBLY;
    assemblyOptionsAca.switchToAcaMode(acaOptions);
    shared_ptr<Context<BFT, RT> > contextAca(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsAca));

    BoundaryOperator<BFT, RT> opAca =
            helmholtz3dHypersingularBoundaryOperator<BFT>(
                contextAca, pwiseLinears, pwiseConstants, pwiseLinears, waveNumber);
    arma::Mat<RT> weakFormAca = opAca.weakForm()->asMatrix();

    BOOST_CHECK(check_arrays_are_close<ValueType>(
                    weakFormDense, weakFormAca, 2. * acaOptions.eps));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(aca_of_synthetic_helmholtz_hypersingular_operator_agrees_with_dense_assembly_in_asymmetric_case,
                              ValueType, complex_result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
        params, "../../meshes/sphere-h-0.4.msh", false /* verbose */);

    RT waveNumber = initWaveNumber<RT>();

    shared_ptr<Space<BFT> > pwiseConstants(
        new PiecewiseConstantScalarSpace<BFT>(grid));
    shared_ptr<Space<BFT> > pwiseLinears(
        new PiecewiseLinearContinuousScalarSpace<BFT>(grid));
    shared_ptr<Space<BFT> > pwiseLinears2(
        new PiecewiseLinearContinuousScalarSpace<BFT>(grid));

    AccuracyOptions accuracyOptions;
    accuracyOptions.doubleRegular.setRelativeQuadratureOrder(2);
    accuracyOptions.singleRegular.setRelativeQuadratureOrder(2);
    shared_ptr<NumericalQuadratureStrategy<BFT, RT> > quadStrategy(
                new NumericalQuadratureStrategy<BFT, RT>(accuracyOptions));

    AssemblyOptions assemblyOptionsDense;
    assemblyOptionsDense.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<Context<BFT, RT> > contextDense(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsDense));

    BoundaryOperator<BFT, RT> opDense =
            helmholtz3dHypersingularBoundaryOperator<BFT>(
                contextDense, pwiseLinears, pwiseConstants, pwiseLinears,
                waveNumber);
    arma::Mat<RT> weakFormDense = opDense.weakForm()->asMatrix();

    AssemblyOptions assemblyOptionsAca;
    assemblyOptionsAca.setVerbosityLevel(VerbosityLevel::LOW);
    AcaOptions acaOptions;
    acaOptions.mode = AcaOptions::LOCAL_ASSEMBLY;
    assemblyOptionsAca.switchToAcaMode(acaOptions);
    shared_ptr<Context<BFT, RT> > contextAca(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsAca));

    // Internal domain different from dualToRange
    BoundaryOperator<BFT, RT> opAca =
            helmholtz3dHypersingularBoundaryOperator<BFT>(
                contextAca, pwiseLinears, pwiseConstants, pwiseLinears2,
                waveNumber);
    arma::Mat<RT> weakFormAca = opAca.weakForm()->asMatrix();

    BOOST_CHECK(check_arrays_are_close<ValueType>(
                    weakFormDense, weakFormAca, 2. * acaOptions.eps));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(aca_of_synthetic_modified_helmholtz_hypersingular_operator_agrees_with_dense_assembly_in_symmetric_case,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
        params, "../../meshes/sphere-h-0.4.msh", false /* verbose */);

    RT waveNumber = initWaveNumber<RT>();

    shared_ptr<Space<BFT> > pwiseConstants(
        new PiecewiseConstantScalarSpace<BFT>(grid));
    shared_ptr<Space<BFT> > pwiseLinears(
        new PiecewiseLinearContinuousScalarSpace<BFT>(grid));

    AccuracyOptions accuracyOptions;
    accuracyOptions.doubleRegular.setRelativeQuadratureOrder(2);
    accuracyOptions.singleRegular.setRelativeQuadratureOrder(2);
    shared_ptr<NumericalQuadratureStrategy<BFT, RT> > quadStrategy(
                new NumericalQuadratureStrategy<BFT, RT>(accuracyOptions));

    AssemblyOptions assemblyOptionsDense;
    assemblyOptionsDense.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<Context<BFT, RT> > contextDense(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsDense));

    BoundaryOperator<BFT, RT> opDense =
            modifiedHelmholtz3dHypersingularBoundaryOperator<BFT, RT, RT>(
                contextDense, pwiseLinears, pwiseConstants, pwiseLinears,
                waveNumber);
    arma::Mat<RT> weakFormDense = opDense.weakForm()->asMatrix();

    AssemblyOptions assemblyOptionsAca;
    assemblyOptionsAca.setVerbosityLevel(VerbosityLevel::LOW);
    AcaOptions acaOptions;
    acaOptions.mode = AcaOptions::LOCAL_ASSEMBLY;
    assemblyOptionsAca.switchToAcaMode(acaOptions);
    shared_ptr<Context<BFT, RT> > contextAca(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsAca));

    BoundaryOperator<BFT, RT> opAca =
            modifiedHelmholtz3dHypersingularBoundaryOperator<BFT, RT, RT>(
                contextAca, pwiseLinears, pwiseConstants, pwiseLinears,
                waveNumber);
    arma::Mat<RT> weakFormAca = opAca.weakForm()->asMatrix();

    BOOST_CHECK(check_arrays_are_close<ValueType>(
                    weakFormDense, weakFormAca, 2. * acaOptions.eps));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(aca_of_synthetic_modified_helmholtz_hypersingular_operator_agrees_with_dense_assembly_in_asymmetric_case,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
        params, "../../meshes/sphere-h-0.4.msh", false /* verbose */);

    RT waveNumber = initWaveNumber<RT>();

    shared_ptr<Space<BFT> > pwiseConstants(
        new PiecewiseConstantScalarSpace<BFT>(grid));
    shared_ptr<Space<BFT> > pwiseLinears(
        new PiecewiseLinearContinuousScalarSpace<BFT>(grid));
    shared_ptr<Space<BFT> > pwiseLinears2(
        new PiecewiseLinearContinuousScalarSpace<BFT>(grid));

    AccuracyOptions accuracyOptions;
    accuracyOptions.doubleRegular.setRelativeQuadratureOrder(2);
    accuracyOptions.singleRegular.setRelativeQuadratureOrder(2);
    shared_ptr<NumericalQuadratureStrategy<BFT, RT> > quadStrategy(
                new NumericalQuadratureStrategy<BFT, RT>(accuracyOptions));

    AssemblyOptions assemblyOptionsDense;
    assemblyOptionsDense.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<Context<BFT, RT> > contextDense(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsDense));

    BoundaryOperator<BFT, RT> opDense =
            modifiedHelmholtz3dHypersingularBoundaryOperator<BFT, RT, RT>(
                contextDense, pwiseLinears, pwiseConstants, pwiseLinears,
                waveNumber);
    arma::Mat<RT> weakFormDense = opDense.weakForm()->asMatrix();

    AssemblyOptions assemblyOptionsAca;
    assemblyOptionsAca.setVerbosityLevel(VerbosityLevel::LOW);
    AcaOptions acaOptions;
    acaOptions.mode = AcaOptions::LOCAL_ASSEMBLY;
    assemblyOptionsAca.switchToAcaMode(acaOptions);
    shared_ptr<Context<BFT, RT> > contextAca(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsAca));

    // Internal domain different from dualToRange
    BoundaryOperator<BFT, RT> opAca =
            modifiedHelmholtz3dHypersingularBoundaryOperator<BFT, RT, RT>(
                contextAca, pwiseLinears, pwiseConstants, pwiseLinears2,
                waveNumber);
    arma::Mat<RT> weakFormAca = opAca.weakForm()->asMatrix();

    BOOST_CHECK(check_arrays_are_close<ValueType>(
                    weakFormDense, weakFormAca, 2. * acaOptions.eps));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(aca_of_synthetic_maxwell_single_layer_operator_agrees_with_dense_assembly_in_symmetric_case,
                              ValueType, complex_result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
        params, "../../meshes/sphere-h-0.4.msh", false /* verbose */);

    RT waveNumber = initWaveNumber<RT>();

    shared_ptr<Space<BFT> > vectorPwiseLinears(
        new RaviartThomas0VectorSpace<BFT>(grid));

    AccuracyOptions accuracyOptions;
    accuracyOptions.doubleRegular.setRelativeQuadratureOrder(2);
    accuracyOptions.singleRegular.setRelativeQuadratureOrder(2);
    shared_ptr<NumericalQuadratureStrategy<BFT, RT> > quadStrategy(
                new NumericalQuadratureStrategy<BFT, RT>(accuracyOptions));

    AssemblyOptions assemblyOptionsDense;
    assemblyOptionsDense.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<Context<BFT, RT> > contextDense(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsDense));

    BoundaryOperator<BFT, RT> opDense =
            maxwell3dSingleLayerBoundaryOperator<BFT>(
                contextDense,
                vectorPwiseLinears, vectorPwiseLinears, vectorPwiseLinears,
                waveNumber);
    arma::Mat<RT> weakFormDense = opDense.weakForm()->asMatrix();

    AssemblyOptions assemblyOptionsAca;
    assemblyOptionsAca.setVerbosityLevel(VerbosityLevel::LOW);
    AcaOptions acaOptions;
    acaOptions.mode = AcaOptions::LOCAL_ASSEMBLY;
    assemblyOptionsAca.switchToAcaMode(acaOptions);
    shared_ptr<Context<BFT, RT> > contextAca(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsAca));

    // Internal domain different from dualToRange
    BoundaryOperator<BFT, RT> opAca =
            maxwell3dSingleLayerBoundaryOperator<BFT>(
                contextAca,
                vectorPwiseLinears, vectorPwiseLinears, vectorPwiseLinears,
                waveNumber);
    arma::Mat<RT> weakFormAca = opAca.weakForm()->asMatrix();

    BOOST_CHECK(check_arrays_are_close<ValueType>(
                    weakFormDense, weakFormAca, 2. * acaOptions.eps));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(aca_of_synthetic_maxwell_single_layer_operator_agrees_with_dense_assembly_in_asymmetric_case,
                              ValueType, complex_result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
        params, "../../meshes/sphere-h-0.4.msh", false /* verbose */);

    RT waveNumber = initWaveNumber<RT>();

    shared_ptr<Space<BFT> > vectorPwiseLinears(
        new RaviartThomas0VectorSpace<BFT>(grid));
    shared_ptr<Space<BFT> > vectorPwiseLinears2(
        new RaviartThomas0VectorSpace<BFT>(grid));

    AccuracyOptions accuracyOptions;
    accuracyOptions.doubleRegular.setRelativeQuadratureOrder(2);
    accuracyOptions.singleRegular.setRelativeQuadratureOrder(2);
    shared_ptr<NumericalQuadratureStrategy<BFT, RT> > quadStrategy(
                new NumericalQuadratureStrategy<BFT, RT>(accuracyOptions));

    AssemblyOptions assemblyOptionsDense;
    assemblyOptionsDense.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<Context<BFT, RT> > contextDense(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsDense));

    BoundaryOperator<BFT, RT> opDense =
            maxwell3dSingleLayerBoundaryOperator<BFT>(
                contextDense,
                vectorPwiseLinears, vectorPwiseLinears, vectorPwiseLinears,
                waveNumber);
    arma::Mat<RT> weakFormDense = opDense.weakForm()->asMatrix();

    AssemblyOptions assemblyOptionsAca;
    assemblyOptionsAca.setVerbosityLevel(VerbosityLevel::LOW);
    AcaOptions acaOptions;
    acaOptions.mode = AcaOptions::LOCAL_ASSEMBLY;
    assemblyOptionsAca.switchToAcaMode(acaOptions);
    shared_ptr<Context<BFT, RT> > contextAca(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsAca));

    // Internal domain different from dualToRange
    BoundaryOperator<BFT, RT> opAca =
            maxwell3dSingleLayerBoundaryOperator<BFT>(
                contextAca,
                vectorPwiseLinears, vectorPwiseLinears, vectorPwiseLinears2,
                waveNumber);
    arma::Mat<RT> weakFormAca = opAca.weakForm()->asMatrix();

    BOOST_CHECK(check_arrays_are_close<ValueType>(
                    weakFormDense, weakFormAca, 2. * acaOptions.eps));
}

BOOST_AUTO_TEST_SUITE_END()

#endif // WITH_AHMED


// Copyright 2014-2015 SDL plc
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//     http://www.apache.org/licenses/LICENSE-2.0
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/** \file

    compute expectation of feature values over probability distribution (weight
    values are logprobs). used by Optimization.
*/

#ifndef HYP__HYPERGRAPH_FEATUREEXPECTATIONS_HPP
#define HYP__HYPERGRAPH_FEATUREEXPECTATIONS_HPP
#pragma once

#include <sdl/Hypergraph/CastHypergraph.hpp>
#include <sdl/Hypergraph/FeatureWeightTpl.hpp>
#include <sdl/Hypergraph/IHypergraph.hpp>
#include <sdl/Hypergraph/InsideAlgorithm.hpp>
#include <sdl/Hypergraph/OutsideAlgorithm.hpp>
#include <sdl/Hypergraph/Weight.hpp>
#include <sdl/Hypergraph/WeightsFwdDecls.hpp>
#include <sdl/Util/IsDebugBuild.hpp>
#include <sdl/Util/LogHelper.hpp>
#include <sdl/Util/LogMath.hpp>
#include <sdl/Util/Map.hpp>
#include <boost/ptr_container/ptr_vector.hpp>
#include <cassert>
#include <cmath>

namespace sdl {
namespace Hypergraph {

namespace {
template <class Map>
inline void printMapInfo(Map const& map, int n = 10) {
  SDL_DEBUG(Hypergraph.FeatureExpectations, "Map with " << map.size() << " feature expectations. First " << n
                                                        << " values:");
  typedef typename Map::const_iterator Iter;
  for (Iter it = map.begin(); it != map.end() && n; ++it, --n) {
    SDL_DEBUG(Hypergraph.FeatureExpectations, "  expectation[" << it->first << "]=" << it->second);
  }
}
}

/**
   Gets called for each arc in the hypergraph and adds up the
   expected feature values (as neglog numbers) that it finds on
   them. Works correctly for both types of feature arcs (viterbi and
   expectation).
*/
template <class FloatT, class MapT>
struct AccumulateExpectedValuesFct {

  typedef LogWeightTpl<FloatT> LogW;
  typedef MapT Map;

  HypergraphBase const& hg_;
  Map* pResultMap_;
  mutable boost::ptr_vector<LogW> insideWeights_;
  mutable boost::ptr_vector<LogW> outsideWeights_;

  AccumulateExpectedValuesFct(HypergraphBase const& hg, boost::ptr_vector<LogW> const& insideWeights,
                              boost::ptr_vector<LogW> const& outsideWeights, Map* pResultMap)
      : hg_(hg), insideWeights_(insideWeights), outsideWeights_(outsideWeights), pResultMap_(pResultMap) {}

  /**
     Accumulates feature expectations found on a feature
     weight arc. (Gets called with *expectation* feature weights.)
  */
  void operator()(ArcTpl<FeatureWeightTpl<FloatT, Map, Expectation>> const* arc) const {
    FloatT posteriorArcWeight = computeInsideTimesOutside(arc).value_;
    typedef typename Map::value_type MapValueType;
    Util::NeglogPlusFct<FloatT> logPlusBy;
    for (MapValueType const& idValue : arc->weight_)
      updateBy(logPlusBy, *pResultMap_, idValue.first, posteriorArcWeight + idValue.second);  // times prob
  }

  /**
     Accumulates feature expectations found on a feature
     weight arc. (Gets called with *viterbi* feature weights.)
  */
  void operator()(ArcTpl<FeatureWeightTpl<FloatT, Map, TakeMin>> const* arc) const {
    FloatT posteriorArcWeight = computeInsideTimesOutside(arc).value_ + arc->weight_.value_;
    // unlike Expectation, which already includes this factor in the feature values
    SDL_DEBUG(Hypergraph.FeatureExpectations, "Accumulate feature expectations for "
                                                  << *arc << " with posterior weight " << posteriorArcWeight);
    typedef typename Map::value_type MapValueType;
    Util::NeglogPlusFct<FloatT> logPlusBy;
    for (MapValueType const& idValue : arc->weight_)
      updateBy(logPlusBy, *pResultMap_, idValue.first, posteriorArcWeight - log(idValue.second));
    // times prob (which was in linear space, not -log,  because of TakeMin aka FeatureWeight)
    if (Util::isDebugBuild()) printMapInfo(*pResultMap_);
  }

 private:
  template <class Arc>
  LogWeightTpl<FloatT> computeInsideTimesOutside(Arc const& arc) const {
    assert(outsideWeights_.size() > arc->head());
    LogW result(outsideWeights_[arc->head()]);
    StateIdContainer const& tails = arc->tails();
    for (StateIdContainer::const_iterator i = tails.begin(), e = tails.end(); i != e; ++i) {
      StateId const tailId = *i;
      if (!hg_.isAxiom(tailId)) Hypergraph::timesBy(insideWeights_[tailId], result);
    }
    return result;
  }
};

/**
   Computes the expected value of each feature in the
   hypergraph.

   \param pResultMap A pointer to map that will hold the result.

   \return Sum of all path weights (used for normalization)
*/
template <class Arc>
typename Arc::Weight::FloatT computeFeatureExpectations(IHypergraph<Arc> const& hg,
                                                        typename Arc::Weight::Map* pResultMap) {
  SDL_TRACE(Hypergraph, "computeFeatureExpectations");
  typedef typename Arc::Weight Weight;
  typedef typename Weight::Map Map;
  typedef typename Weight::FloatT FloatT;

  // Cast orig arcs to log arcs:
  typedef LogWeightTpl<FloatT> LogW;
  typedef ArcTpl<LogW> LogArc;
  CastHypergraph<Arc, LogArc> logWeightHg(hg);

  // Run inside/outside algorithm in log semiring:
  boost::ptr_vector<LogW> insideWeights;
  boost::ptr_vector<LogW> outsideWeights;
  insideAlgorithm(logWeightHg, &insideWeights, false);
  outsideAlgorithm(logWeightHg, insideWeights, &outsideWeights, false);

  hg.forArcs(AccumulateExpectedValuesFct<FloatT, Map>(hg, insideWeights, outsideWeights, pResultMap));

  // Negative log of sum over all paths (used for global
  // normalization):
  assert(hg.final() != kNoState);
  assert(insideWeights.size() > hg.final());
  LogW const& pathsSum = insideWeights[hg.final()];

  // Normalize and remove neglog of feature expectations:
  typedef typename Arc::Weight::Map::value_type ValT;
  for (ValT& val : *pResultMap) {
    val.second = exp(-val.second + pathsSum.value_);
  }
  if (Util::isDebugBuild()) printMapInfo(*pResultMap);

  return pathsSum.value_;
}

}}

#endif


//=======================================================================
// Copyright 2013 University of Warsaw.
// Authors: Piotr Wygocki
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

#ifndef BOOST_GRAPH_AUGMENT_HPP
#define BOOST_GRAPH_AUGMENT_HPP

#include <boost/graph/filtered_graph.hpp>

namespace boost {
namespace detail {

template <class Graph, class ResCapMap>
filtered_graph<const Graph, is_residual_edge<ResCapMap> >
residual_graph(const Graph& g, ResCapMap residual_capacity) {
    return filtered_graph<const Graph, is_residual_edge<ResCapMap> >
        (g, is_residual_edge<ResCapMap>(residual_capacity));
}

template <class Graph, class PredEdgeMap, class ResCapMap,
         class RevEdgeMap>
inline void
augment(const Graph& g,
        typename graph_traits<Graph>::vertex_descriptor src,
        typename graph_traits<Graph>::vertex_descriptor sink,
        PredEdgeMap p,
        ResCapMap residual_capacity,
        RevEdgeMap reverse_edge)
{
    typename graph_traits<Graph>::edge_descriptor e;
    typename graph_traits<Graph>::vertex_descriptor u;
    typedef typename property_traits<ResCapMap>::value_type FlowValue;

    // find minimum residual capacity along the augmenting path
    FlowValue delta = (std::numeric_limits<FlowValue>::max)();
    e = get(p, sink);
    do {
        BOOST_USING_STD_MIN();
        delta = min BOOST_PREVENT_MACRO_SUBSTITUTION(delta, get(residual_capacity, e));
        u = source(e, g);
        e = get(p, u);
    } while (u != src);

    // push delta units of flow along the augmenting path
    e = get(p, sink);
    do {
        put(residual_capacity, e, get(residual_capacity, e) - delta);
        put(residual_capacity, get(reverse_edge, e), get(residual_capacity, get(reverse_edge, e)) + delta);
        u = source(e, g);
        e = get(p, u);
    } while (u != src);
}

} // namespace detail
} //namespace boost

#endif /* BOOST_GRAPH_AUGMENT_HPP */


// #include <boost/graph/adjacency_list.hpp>
// #include <boost/graph/connected_components.hpp>
// #include <boost/lexical_cast.hpp>

// [[Rcpp::plugins(cpp14)]]
#include <Rcpp.h>
// [[Rcpp::depends(RcppEigen)]]
#include <RcppEigen.h>
// [[Rcpp::depends(RcppProgress)]]

// [[Rcpp::plugins(openmp)]]
// no need to have this: #include <omp.h>

// #include <progress.hpp>

#include <iomanip>
#include <iostream>
#include <numeric>
#include <vector>

#include "eigen_util.hh"
#include "std_util.hh"
#include "math.hh"
#include "util.hh"
#include "check.hh"

#ifdef __cplusplus
extern "C" {
#endif

#include "bgzf.h"
#include "kstring.h"

#ifdef __cplusplus
}
#endif

#ifndef MMUTIL_HH_
#define MMUTIL_HH_

using Scalar = float;
using SpMat = Eigen::SparseMatrix<Scalar, Eigen::RowMajor, std::ptrdiff_t>;
using Index = SpMat::Index;
using MSpMat = Eigen::MappedSparseMatrix<Scalar>;

using Mat = typename Eigen::
    Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>;
using Vec = typename Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
using IntMat = typename Eigen::
    Matrix<std::ptrdiff_t, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>;
using IntVec = typename Eigen::Matrix<std::ptrdiff_t, Eigen::Dynamic, 1>;

/////////////////////////////
// simple helper functions //
/////////////////////////////

std::tuple<Index, Index, Scalar>
parse_triplet(const std::tuple<Index, Index, Scalar> &tt);

std::tuple<Index, Index, Scalar>
parse_triplet(const Eigen::Triplet<Scalar> &tt);

std::vector<std::string> copy(const Rcpp::StringVector &r_vec);

void copy(const Rcpp::StringVector &r_vec, std::vector<std::string> &vec);

#endif


/*=============================================================================
  Copyright (c) 2010-2016 Bolero MURAKAMI
  https://github.com/bolero-MURAKAMI/Sprig

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

#include <sprig/config/config.hpp>

#ifdef SPRIG_USING_PRAGMA_ONCE
#	pragma once
#endif	// #ifdef SPRIG_USING_PRAGMA_ONCE

#include <functional>
#include <boost/numeric/interval.hpp>

namespace sprig {
	//
	// interval_correction
	//
	template<typename Value>
	class interval_correction {
	public:
		typedef Value value_type;
		typedef boost::numeric::interval<value_type> interval_type;
	private:
		interval_type interval_;
	public:
		interval_correction() {}
		explicit interval_correction(interval_type const& v)
			: interval_(v)
		{}
		template<typename T>
		interval_correction(interval_correction<T> const& other)
			: interval_(other.interval_)
		{}
		template<typename Lower, typename Upper>
		interval_correction(Lower const& l, Upper const& u)
			: interval_(l, u)
		{}
		virtual ~interval_correction() {}
		interval_type const& get() const {
			return interval_;
		}
		template<typename T>
		void set(T const& v) {
			interval_ = v;
		}
		template<typename Lower, typename Upper>
		void set(Lower const& l, Upper const& u) {
			interval_.set(l, u);
		}
		void internal(value_type& value) const {
			if (interval_.lower() > value) {
				value = interval_.lower();
			} else if (interval_.upper() < value) {
				value = interval_.upper();
			}
		}
		value_type get_internal(value_type const& value) const {
			if (interval_.lower() > value) {
				return interval_.lower();
			} else if (interval_.upper() < value) {
				return interval_.upper();
			}
			return value;
		}
		bool is_internal(value_type const& value) const {
			return (interval_.lower() > value || interval_.upper() < value)
				? false
				: true;
		}
		void external(value_type& value) const {
			if (interval_.lower() < value) {
				value = interval_.lower();
			} else if (interval_.upper() > value) {
				value = interval_.upper();
			}
		}
		value_type get_external(value_type const& value) const {
			if (interval_.lower() < value) {
				return interval_.lower();
			} else if (interval_.upper() > value) {
				return interval_.upper();
			}
			return value;
		}
		bool is_external(value_type const& value) const {
			return (interval_.lower() < value || interval_.upper() > value)
				? false
				: true;
		}
		template<typename T>
		void internal(T& value) const {
			if (interval_.lower() > value) {
				value = interval_.lower();
			} else if (interval_.upper() < value) {
				value = interval_.upper();
			}
		}
		template<typename T>
		T get_internal(T const& value) const {
			if (interval_.lower() > value) {
				return interval_.lower();
			} else if (interval_.upper() < value) {
				return interval_.upper();
			}
			return value;
		}
		template<typename T>
		bool is_internal(T const& value) const {
			return (interval_.lower() > value || interval_.upper() < value)
				? false
				: true;
		}
		template<typename T>
		void external(T& value) const {
			if (interval_.lower() < value) {
				value = interval_.lower();
			} else if (interval_.upper() > value) {
				value = interval_.upper();
			}
		}
		template<typename T>
		T get_external(T const& value) const {
			if (interval_.lower() < value) {
				return interval_.lower();
			} else if (interval_.upper() > value) {
				return interval_.upper();
			}
			return value;
		}
		template<typename T>
		bool is_external(T const& value) const {
			return (interval_.lower() < value || interval_.upper() > value)
				? false
				: true;
		}
	};
	//
	// internal_interval_correction
	//
	template<typename Value>
	class internal_interval_correction
		: public interval_correction<Value>
		, public std::unary_function<Value, void>
	{
	public:
		typedef interval_correction<Value> base_type;
		typedef typename base_type::value_type value_type;
	public:
		internal_interval_correction() {}
		template<typename T>
		internal_interval_correction(T const& v)
			: base_type(v)
		{}
		template<typename Lower, typename Upper>
		internal_interval_correction(Lower const& l, Upper const& u)
			: base_type(l, u)
		{}
		virtual ~internal_interval_correction() {}
		void operator()(value_type const& value) const {
			this->internal(value);
		}
		template<typename T>
		void operator()(T const& value) const {
			this->internal(value);
		}
	};
	//
	// external_interval_correction
	//
	template<typename Value>
	class external_interval_correction
		: public interval_correction<Value>
		, public std::unary_function<Value, void>
	{
	public:
		typedef interval_correction<Value> base_type;
		typedef typename base_type::value_type value_type;
	public:
		external_interval_correction() {}
		template<typename T>
		external_interval_correction(T const& v)
			: base_type(v)
		{}
		template<typename Lower, typename Upper>
		external_interval_correction(Lower const& l, Upper const& u)
			: base_type(l, u)
		{}
		virtual ~external_interval_correction() {}
		void operator()(value_type const& value) const {
			this->external(value);
		}
		template<typename T>
		void operator()(T const& value) const {
			this->external(value);
		}
	};
	//
	// get_internal_interval_correction
	//
	template<typename Value>
	class get_internal_interval_correction
		: public interval_correction<Value>
		, public std::unary_function<Value, Value>
	{
	public:
		typedef interval_correction<Value> base_type;
		typedef typename base_type::value_type value_type;
	public:
		get_internal_interval_correction() {}
		template<typename T>
		get_internal_interval_correction(T const& v)
			: base_type(v)
		{}
		template<typename Lower, typename Upper>
		get_internal_interval_correction(Lower const& l, Upper const& u)
			: base_type(l, u)
		{}
		virtual ~get_internal_interval_correction() {}
		value_type operator()(value_type const& value) const {
			return this->get_internal(value);
		}
		template<typename T>
		T operator()(T const& value) const {
			return this->get_internal(value);
		}
	};
	//
	// get_external_interval_correction
	//
	template<typename Value>
	class get_external_interval_correction
		: public interval_correction<Value>
		, public std::unary_function<Value, Value>
	{
	public:
		typedef interval_correction<Value> base_type;
		typedef typename base_type::value_type value_type;
	public:
		get_external_interval_correction() {}
		template<typename T>
		get_external_interval_correction(T const& v)
			: base_type(v)
		{}
		template<typename Lower, typename Upper>
		get_external_interval_correction(Lower const& l, Upper const& u)
			: base_type(l, u)
		{}
		virtual ~get_external_interval_correction() {}
		value_type operator()(value_type const& value) const {
			return this->get_external(value);
		}
		template<typename T>
		T operator()(T const& value) const {
			return this->get_external(value);
		}
	};
	//
	// is_internal_interval_correction
	//
	template<typename Value>
	class is_internal_interval_correction
		: public interval_correction<Value>
		, public std::unary_function<Value, bool>
	{
	public:
		typedef interval_correction<Value> base_type;
		typedef typename base_type::value_type value_type;
	public:
		is_internal_interval_correction() {}
		template<typename T>
		is_internal_interval_correction(T const& v)
			: base_type(v)
		{}
		template<typename Lower, typename Upper>
		is_internal_interval_correction(Lower const& l, Upper const& u)
			: base_type(l, u)
		{}
		virtual ~is_internal_interval_correction() {}
		bool operator()(value_type const& value) const {
			return this->is_internal(value);
		}
		template<typename T>
		bool operator()(T const& value) const {
			return this->is_internal(value);
		}
	};
	//
	// is_external_interval_correction
	//
	template<typename Value>
	class is_external_interval_correction
		: public interval_correction<Value>
		, public std::unary_function<Value, bool>
	{
	public:
		typedef interval_correction<Value> base_type;
		typedef typename base_type::value_type value_type;
	public:
		is_external_interval_correction() {}
		template<typename T>
		is_external_interval_correction(T const& v)
			: base_type(v)
		{}
		template<typename Lower, typename Upper>
		is_external_interval_correction(Lower const& l, Upper const& u)
			: base_type(l, u)
		{}
		virtual ~is_external_interval_correction() {}
		bool operator()(value_type const& value) const {
			return this->is_external(value);
		}
		template<typename T>
		bool operator()(T const& value) const {
			return this->is_external(value);
		}
	};
}	// namespace sprig

#endif	// #ifndef SPRIG_CORRECTION_HPP


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

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

#include <boost/simd/function/definition/extract.hpp>
#include <boost/simd/function/definition/bitwise_cast.hpp>
#include <boost/simd/constant/allbits.hpp>
#include <boost/simd/meta/cardinal_of.hpp>
#include <boost/simd/detail/dispatch/meta/scalar_of.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>

namespace boost { namespace simd
{
  namespace bs = boost::simd;
  template<typename T> BOOST_FORCEINLINE bool assert_all( T const& t )
  {
    for(std::size_t i = 0; i != cardinal_of<T>::value; ++i)
      if(!extract(t, i)) return false; //extract(t, i);
    return true;
  }

  template<typename T> BOOST_FORCEINLINE bool assert_is_mask( T const& t )
  {
    for(std::size_t i = 0; i != cardinal_of<T>::value; ++i)
    {
      typedef dispatch::scalar_of_t<T> sT;
      typedef dispatch::as_integer_t<sT, unsigned> uT;

      sT v = extract(t, i);
      uT uv = bitwise_cast<uT>(v);

      if(uv != uT(0) && uv != Allbits<uT>()) return false;
    }
    return true;
  }

  template<typename A0, typename A1>
  BOOST_FORCEINLINE bool assert_good_shift( A1 const& t, std::true_type const&)
  {
    for(std::size_t i = 0; i != cardinal_of<A0>::value; ++i)
    {
      typedef dispatch::scalar_of_t<A1> sA1;
      typedef dispatch::scalar_of_t<A0> sA0;
      const sA1 N = sizeof(sA0)*8;
      sA1 v = extract(t, i);

      if(v >= N)  return false;
    }

    return true;
  }

  template<typename A0, typename A1>
  BOOST_FORCEINLINE bool assert_good_shift( A1 const& t, std::false_type const&)
  {
    for(std::size_t i = 0; i != cardinal_of<A0>::value; ++i)
    {
      typedef dispatch::scalar_of_t<A1> sA1;
      typedef dispatch::scalar_of_t<A0> sA0;
      const sA1 N = sizeof(sA0)*8;
      sA1 v = extract(t, i);

      if(v < sA1(0) || v >= N)  return false;
    }

    return true;
  }

  template<typename A0, typename A1>
  BOOST_FORCEINLINE bool assert_good_shift( A1 const& t )
  {
    return assert_good_shift<A0>(t, typename std::is_unsigned<dispatch::scalar_of_t<A1>>::type{});
  }

} }

#endif


#include <metaSMT/frontend/QF_BV.hpp>
#include <metaSMT/support/lazy.hpp>

#include <metaSMT/support/disable_warnings.hpp>
#include <boost/test/unit_test.hpp>
#include <metaSMT/support/enable_warnings.hpp>

#include <numeric>

// lazy headers

using namespace metaSMT;
using namespace logic;
using namespace logic::QF_BV;
namespace proto = boost::proto;

BOOST_FIXTURE_TEST_SUITE(lazy_t, Solver_Fixture )

BOOST_AUTO_TEST_CASE( andiN )
{
  typedef ContextType::result_type result_type;

  boost::function< result_type ( predicate const &, predicate const & ) >
    lazy_andiN = metaSMT::lazy( ctx, And( arg1, Not(arg2)) ) ;

  predicate x = new_variable();
  predicate y = new_variable();

  assumption( ctx, metaSMT::logic::equal(And(x, Not(y)), lazy_andiN(x,y)) );
  BOOST_REQUIRE( solve(ctx) );

  assumption( ctx, nequal(And(x, Not(y)), lazy_andiN(x,y)) );
  BOOST_REQUIRE( !solve(ctx) );
}

BOOST_AUTO_TEST_CASE( lazy_result_type )
{
  typedef ContextType::result_type result_type;

  boost::function< result_type ( predicate const &, result_type const & ) >
    lazy_Xor = metaSMT::lazy( ctx, Xor( arg1, arg2) ) ;

  predicate x = new_variable();
  result_type y = evaluate(ctx, new_variable());

  assumption(ctx, metaSMT::logic::equal( Xor(x,y), lazy_Xor(x,y)) );
  BOOST_REQUIRE( solve(ctx) );

  assumption(ctx, nequal( Xor(x,y), lazy_Xor(x,y)) );
  BOOST_REQUIRE( !solve(ctx) );
}

BOOST_AUTO_TEST_CASE( lazy_accumulate )
{
  unsigned count = 100;
  std::vector< predicate > preds;
  for (unsigned i = 0; i < count; ++i) {
    preds.push_back( new_variable() );
  }

  ContextType::result_type initial = evaluate(ctx, False);

  metaSMT::assertion( ctx, std::accumulate(preds.begin(), preds.end(), initial,
        metaSMT::lazy(ctx, Xor(arg1, arg2) )
  ));

  BOOST_REQUIRE( solve(ctx) );
}

BOOST_AUTO_TEST_SUITE_END() //lazy_t

//  vim: ft=cpp:ts=2:sw=2:expandtab


#pragma once
#include <iostream>
#include <algorithm>
#include <vector>
#include <cmath>

#include "boost/graph/graph_traits.hpp"
#include "boost/graph/adjacency_list.hpp"

#include "rrbt_params.hpp"
#include "rrbt_utils.hpp"
#include "kdtree.h"

#include <Eigen/Dense>
#include <Eigen/Core>

using namespace boost;

typedef int nodeType;

#if DIM == 2
	typedef Eigen::Matrix<double, 2, 2, Eigen::DontAlign> Mat;
#elif DIM == 3
	typedef Eigen::Matrix<double, 3, 3, Eigen::DontAlign> Mat;
#endif

class RRBT {
public:
	struct Node {
		Point coords;
		double yaw;
		int index, parent;
		double cost, sigma, J;
		Mat cov;
	};

	Utils utils;

	typedef adjacency_list<vecS, vecS, undirectedS, RRBT::Node> RRG;
	typedef RRG::vertex_descriptor vertex_t;
	typedef RRG::edge_descriptor edge_t;

	RRBT::RRG Graph;

	void initTree(int &nIter)
	{
		kd_rrbt = kd_create(3);
		nodeNum.reserve(nIter);
		nodeParents.reserve(nIter);
	}

	vertex_t addNode(Point point, double yaw, int idx, int parent, double cost, double sigma, double J, Mat cov) {
		vertex_t v = add_vertex(Graph);

		Graph[v].coords = point;
		Graph[v].yaw = yaw;
		Graph[v].index = idx;
		Graph[v].cost = cost;
		Graph[v].sigma = sigma;
		Graph[v].parent = parent;
		Graph[v].J = J;
		Graph[v].cov = cov;

		nodeNum.push_back(idx);
		if (kd_insert3(static_cast<kdtree*>(kd_rrbt), point.x, point.y, point.z, &nodeNum[idx]) != 0)
			std::cerr << "Unable to insert node!" << std::endl;

		return v;
	}

	void modifyNode(int nodeIdx, int parent, float cost, float sigma) {
		Graph[nodeIdx].parent = parent;
		Graph[nodeIdx].cost = cost;
		Graph[nodeIdx].sigma = sigma;
		Graph[nodeIdx].index = nodeIdx;
	}

	float computeDistNodes(Point p1, Point p2) {
		float dist = 0;
		dist = utils.computeDistPts(p1, p2);

		return dist;
	}

	float computeCov(Point point, Point beacon) {

	}

	int nearestNeighbor(Point &q)
	{
		queryPt[0] = q.x;
		queryPt[1] = q.y;
		queryPt[2] = q.z;

		knnResults = kd_nearest(static_cast<kdtree*>(kd_rrbt), queryPt);
		resIdx = (int*)kd_res_item(knnResults, resultPt);

		int idx = *resIdx;

		kd_res_free(knnResults);
		return idx;
	}

	void knnSearch(Point &q, int &radius, std::vector <int> &results)
	{
		double queryPt[3] = { q.x, q.y, q.z };

		knnResults = kd_nearest_range(static_cast<kdtree*>(kd_rrbt), queryPt, radius);
		results.clear();

		while (!kd_res_end(knnResults)) {
			//std::cout << "Found " << kd_res_size(knnResults) << " results" << std::endl;
			resIdx = (int*)kd_res_item(knnResults, resultPt);
			results.push_back(*resIdx);
			//std::cout << "Nearest neighbor data: " << *pch << std::endl;

			kd_res_next(knnResults);
		}
		kd_res_free(knnResults);
	}

	bool onPath(RRBT &rrbt, int node, int goal)
	{
		int evalNode;
		evalNode = rrbt.Graph[goal].parent;

		if (evalNode == node)
			return true;

		while (evalNode != -1) {
			if (evalNode == node)
				return true;
			else
				evalNode = rrbt.Graph[evalNode].parent;
		}
		return false;
	}

	void identifyParents(int idx)
	{
		int node;
		node = Graph[idx].parent;

		std::fill(nodeParents.begin(), nodeParents.end(), 0);

		while (node != -1) {
			nodeParents[node] = 1;
			node = Graph[node].parent;
		}
	}

	int BALL_SIZE = 20;

	std::vector <bool> nodeParents;

private:
	void *kd_rrbt;
	double *queryPt = new double[3];
	double *resultPt = new double[3];

	int *resIdx;
	struct kdres *knnResults;

	std::vector <int> nodeNum;

	/*
	int nearestNeighbor(RRBT &rrbt, Point q)
	{
		double minDist = 10000, dist = 0;
		int nn = 0;
		for (unsigned int i = 0; i < num_vertices(rrbt.Graph); ++i) {
			dist = rrbt.computeDistNodes(q, rrbt.Graph[i].coords);
			if (dist < minDist && dist != 0) {
				minDist = dist;
				nn = i;
			}
		}
		return nn;
	}

	std::vector <int> knnSearch(RRBT &rrbt, Point q, int radius)
	{
		std::vector <int> neighbors;
		double dist = 0;
		for (unsigned int i = 0; i < num_vertices(rrbt.Graph); ++i) {
			dist = rrbt.computeDistNodes(q, rrbt.Graph[i].coords);
			if (dist <= radius && dist != 0) {
				neighbors.push_back(i);
			}
		}
		return neighbors;
	}
	*/
};


// Copyright 2020 Erik Kouters (Falcons)
// SPDX-License-Identifier: Apache-2.0
/*
 * main.cpp
 *
 *  Created on: Jan 14, 2018
 *      Author: Erik Kouters
 */

#include <boost/thread.hpp>

// Falcons facilities
#include "cDiagnostics.hpp"
#include "falconsCommon.hpp"
#include "tracing.hpp"

// internal
#include "int/cVelocityTransformData.hpp"
#include "int/cVelocityTransformMain.hpp"

#include "int/adapters/cRTDBInputAdapter.hpp"
#include "int/adapters/cRTDBOutputAdapter.hpp"

cVelocityTransformData _vtData;
cVelocityTransformMain _vtMain;

cRTDBInputAdapter _rtdbInputAdapter;
cRTDBOutputAdapter _rtdbOutputAdapter;

boost::thread _workerThreadFeedback;
boost::thread _workerThreadSetpoint;

boost::numeric::ublas::matrix<double> _matrix;
boost::numeric::ublas::matrix<double> _inverseMatrix;

iterateFunctionType _iterateFeedbackFunc;
publishFeedbackFunctionType _publishFeedbackFunc;

iterateFunctionType _iterateSetpointFunc;
publishTargetFunctionType _publishTargetFunc;

boost::thread _workerThreadWaitForMotorFeedback;

void iterateFeedback()
{
    _workerThreadFeedback.interrupt();
}

void iterateSetpoint()
{
    _workerThreadSetpoint.interrupt();
}

/*!
 * Used to publish RobotData feedback to WorldModel
 */
void publishFeedback()
{
    vt_robot_data robotData;
    _vtData.getFeedbackRobotData(robotData);

    _rtdbOutputAdapter.setRobotDisplacementFeedback(robotData);
    _rtdbOutputAdapter.setRobotVelocityFeedback(robotData);
}

/*!
 * Used to publish MotorsData target to PeripheralsInterface
 */
void publishTarget()
{
    vt_motors_data target;
    _vtData.getTargetMotorsData(target);

    _rtdbOutputAdapter.setMotorVelocitySetpoint(target);
}

template<class T> static boost::numeric::ublas::matrix<T> InvertMatrix (const boost::numeric::ublas::matrix<T>& input) {
    boost::numeric::ublas::matrix<T> inverse(input.size1(), input.size2());

    // create a working copy of the input
    boost::numeric::ublas::matrix<T> A(input);
    // create a permutation matrix for the LU-factorization
    boost::numeric::ublas::permutation_matrix<std::size_t> pm(A.size1());

    // perform LU-factorization
    int res = lu_factorize(A,pm);
    if( res != 0 )
    {
        throw std::runtime_error("Could not inverse motion matrix.");
    }

    // create identity matrix of "inverse"
    inverse.assign(boost::numeric::ublas::identity_matrix<T>(A.size1()));

    // backsubstitute to get the inverse
    lu_substitute(A, pm, inverse);

    return inverse;
}

int main(int argc, char **argv)
{
    // Determine YAML to load.
    std::string configFileAbs = determineConfig("VelocityTransform");

    INIT_TRACE_HOT_FLUSH;

    try
    {

        // iterate functions
        _iterateFeedbackFunc = boost::bind(&iterateFeedback);
        _iterateSetpointFunc = boost::bind(&iterateSetpoint);

        // Bind publishFeedback function to vcData
        _publishFeedbackFunc = boost::bind(&publishFeedback);
        _vtData.publishFeedback = _publishFeedbackFunc;

        // Bind publishTarget function to vcData
        _publishTargetFunc = boost::bind(&publishTarget);
        _vtData.publishTarget = _publishTargetFunc;

        // RTDB adapters
        _rtdbInputAdapter = cRTDBInputAdapter(&_vtData, _iterateFeedbackFunc, _iterateSetpointFunc);
        _rtdbOutputAdapter = cRTDBOutputAdapter();

        // setup services
        _vtMain._vtDataClass = &_vtData;

        // Load motor matrix
        // TODO - move motor matrix to config file.
        _matrix = boost::numeric::ublas::matrix<double>(3,3);
        _matrix(0,0) = -0.5;
        _matrix(1,0) = -0.5;
        _matrix(2,0) = 1.0;
        _matrix(0,1) = -0.866;
        _matrix(1,1) = 0.866;
        _matrix(2,1) = 0.0;
        _matrix(0,2) = 0.22;
        _matrix(1,2) = 0.22;
        _matrix(2,2) = 0.2266;
        _inverseMatrix = InvertMatrix(_matrix);
        _vtData.setMatrix(_matrix);
        _vtData.setInvMatrix(_inverseMatrix);

        // velocityTransform
        _workerThreadFeedback = boost::thread(boost::bind(&cVelocityTransformMain::iterateFeedback, &_vtMain));
        _workerThreadSetpoint = boost::thread(boost::bind(&cVelocityTransformMain::iterateSetpoint, &_vtMain));

        // wait for motor feedback in a separate thread
        _workerThreadWaitForMotorFeedback = boost::thread(boost::bind(&cRTDBInputAdapter::waitForMotorFeedback, &_rtdbInputAdapter));

        // wait for robot velocity setpoint in the main thread
        _rtdbInputAdapter.waitForRobotVelocitySetpoint();
    }
    catch (std::exception &e)
    {
        std::cerr << "Error occurred:" << e.what() << std::endl;
        TRACE_ERROR("Error occurred: %s", e.what());
        return 1;
    }
    std::cerr << "Exiting..." << std::endl;
    return 0;
}


#pragma once

//#include <ADEKF/ceres/jet.h>
#include "ADEKFUtils.h"
#include "ADEKF.h"

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

namespace adekf
{

    //using namespace Eigen;
    //using namespace std::placeholders;

    /**
     * An RTS Smoother Implementation based on the ADEKF
     * @tparam Filter The Filter to be used for estimation
     */
    template <typename State>
    class Naive_RTS_Smoother : public ADEKF<State>
    {

    public:
        using Covariance = typename ADEKF<State>::Covariance;
        using ScalarType = typename ADEKF<State>::ScalarType;
        static constexpr size_t DOF = DOFOf<State>;
        static constexpr double weighted_mean_epsilon = 1e-8;
        aligned_vector<State> old_mus,smoothed_mus, predicted_mus;
        aligned_vector<Covariance> old_sigmas,smoothed_sigmas,predicted_sigmas;

        Naive_RTS_Smoother(const State &_mu, const Covariance &_sigma) : ADEKF<State>(_mu, _sigma) { storeEstimation(); storePredictedEstimation(); }
        Naive_RTS_Smoother(const ADEKF<State> &filter) : ADEKF<State>(filter) { storeEstimation();storePredictedEstimation(); }
        /**
         * Stores the current mean and covariance of the filter
         */
        void storeEstimation()
        {
            old_mus.push_back(this->mu);
            smoothed_mus.push_back(this->mu);
            old_sigmas.push_back(this->sigma);
            smoothed_sigmas.push_back(this->sigma);
        }

         void storePredictedEstimation()
        {
            predicted_mus.push_back(this->mu);
            predicted_sigmas.push_back(this->sigma);
        }
        template <typename Head, typename... Tail>
        std::tuple<Tail...> tuple_tail(const std::tuple<Head, Tail...> &tuple)
        {
            return apply([](auto head, auto... tail) {
                return std::make_tuple(tail...);
            },
                         tuple);
        }

        template <typename DynamicModel, typename tstate, typename NoiseVector, typename... Controls, typename... ExpandedControls>
        void applyDynamicModel(DynamicModel dynamicModel, tstate &input, const NoiseVector &noise, const std::tuple<Controls...> &controls, const ExpandedControls& ...  expanded_controls)
        {
             if constexpr(std::tuple_size<std::tuple<Controls ...>>::value > 0)
            {
                applyDynamicModel(dynamicModel, input, noise, tuple_tail(controls), expanded_controls..., std::get<0>(controls));
            }
            else
            {
                dynamicModel(input, noise, expanded_controls...);
            }
        }

        template <int NoiseDim, typename DynamicModel, typename... Controls>
        Covariance getNonAdditiveDynamicNoise(const State &smoothed, DynamicModel dynamicModel, const SquareMatrixType<double,NoiseDim> &Q, const std::tuple<Controls...> &controls)
        {
            auto derivator = adekf::getDerivator<DOF + NoiseDim>();
            auto input = eval(smoothed + derivator.template head<DOF>());
            //Todo u vernünftig machen
            applyDynamicModel(dynamicModel, input, derivator.template tail<NoiseDim>(), controls);
            auto result = (input - smoothed);
            //std::cout << result << std::endl;
            MatrixType<double, DOF, DOF + NoiseDim> F(DOF, DOF + NoiseDim);
            //Eigen::Matrix<double,DOF,1> diff;
            for (size_t j = 0; j < DOF; ++j)
            {
                F.row(j) = result(j).v;
                //diff(j)=result(j).a;
            }
            return eval(F.template rightCols<NoiseDim>() * Q * F.template rightCols<NoiseDim>().transpose());
        }

static inline size_t indefinite_counter=0;
        template <int NoiseDim, typename DynamicModel, typename... Controls>
        void smoothSingleStep(size_t k, DynamicModel & dynamicModel, const Eigen::Matrix<double,NoiseDim,NoiseDim> &Q,const std::tuple<Controls ...> & controls,const State & smoothed_mu_kplus,const Covariance & smoothed_sigma_kplus){
                MatrixType<ScalarType,DOF, DOF> Fk(DOF, DOF);
                Covariance SmootherGain;

                auto derivator = adekf::getDerivator<DOF>();
                auto input = eval(old_mus[k] + derivator);
                applyDynamicModel(dynamicModel,input,MatrixType<ScalarType,NoiseDim, 1>::Zero(), controls);
                auto result1 = eval(input - predicted_mus[k+1]);
                MatrixType<ScalarOf(this->mu),DOF, 1> diff1;

                for (size_t j = 0; j < DOF; ++j)
                {
                    Fk.row(j) = result1(j).v;
                    diff1(j) = result1(j).a;
                }

                SmootherGain = old_sigmas[k] * Fk.transpose() * (predicted_sigmas[k+1].inverse());
                smoothed_mus[k] = old_mus[k] + (SmootherGain * (smoothed_mu_kplus- predicted_mus[k+1]));
                smoothed_sigmas[k] = (old_sigmas[k] + SmootherGain *(smoothed_sigma_kplus- predicted_sigmas[k+1]) * SmootherGain.transpose());
                if(!isPositiveDefinite(smoothed_sigmas[k]))
                indefinite_counter++;
                assurePositiveDefinite(smoothed_sigmas[k]);
        }

        template <int NoiseDim, typename DynamicModel, typename... Controls>
        void smoothIntervalWithNonAdditiveNoise(size_t steps, int start, DynamicModel & dynamicModel, const Eigen::Matrix<double,NoiseDim,NoiseDim> &Q, const std::vector<std::tuple<Controls...>> &all_controls)
        {
            if (start < 0)
            {
                start = old_mus.size() - 1 + start;
            }
            assert(start - steps >= 0 && "Can not smooth more estimates than available");
            assert(start < old_mus.size() - 1 && "Can not smooth a state which belongs to future");
            assert(all_controls.size() == old_mus.size() - 1 && "Requires all control inputs for the dynamic model.");
            for (size_t k = start; k >= old_mus.size() - steps - 1; k--)
            {
                //std::cout << "At iteration: " << k << std::endl;
                smoothSingleStep(k,dynamicModel,Q,all_controls[k],smoothed_mus[k+1], smoothed_sigmas[k+1]);

            }
        }

        template <int NoiseDim, typename DynamicModel, typename... Controls>
        void smoothAllWithNonAdditiveNoise(DynamicModel & dynamicModel, const Eigen::Matrix<double,NoiseDim,NoiseDim> &Q, const std::vector<std::tuple<Controls...>> &all_controls)
        {
            smoothIntervalWithNonAdditiveNoise(old_mus.size() - 2, -1, dynamicModel, Q, all_controls);
        }
    };

    /**
     * General Deduction Template for the Naive_RTS_Smoother based on StateRetriever.
     * This is needed so you can type Naive_RTS_Smoother smoother(Filter{...}) without template arguments
     */
    /*template <typename State>
    Naive_RTS_Smoother(ADEKF<State> &&) -> Naive_RTS_Smoother<State>;
    template <typename State>
    Naive_RTS_Smoother(ADEKF<State> ) -> Naive_RTS_Smoother<State>;
*/

} // namespace adekf


#ifndef MULTIPRECISION_TYPES_
#define MULTIPRECISION_TYPES_

#include <Eigen/Core>
#include <boost/multiprecision/cpp_dec_float.hpp>
#include "cpp_dec_float_50_support_eigen.hpp"

// Typedefs for multiprecision eigen matrices -- column-major by default
typedef Eigen::Matrix<boost::multiprecision::cpp_dec_float_50, Eigen::Dynamic, Eigen::Dynamic> MatrixMp;
typedef Eigen::Matrix<boost::multiprecision::cpp_dec_float_50, Eigen::Dynamic, 1> ColumnVectorMp;
typedef Eigen::Matrix<boost::multiprecision::cpp_dec_float_50, 1, Eigen::Dynamic> RowVectorMp;

// Typedefs for double precision eigen matrices -- column-major by default
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> MatrixDp;
typedef Eigen::Matrix<double, Eigen::Dynamic, 1> ColumnVectorDp;
typedef Eigen::Matrix<double, 1, Eigen::Dynamic> RowVectorDp;

namespace mp = boost::multiprecision;

#endif //MULTIPRECISION_TYPES_