//---------------------------------------------------------------------------//
// Copyright (c) 2013-2014 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE TestAdjacentDifference
#include <boost/test/unit_test.hpp>

#include <boost/compute/system.hpp>
#include <boost/compute/command_queue.hpp>
#include <boost/compute/lambda.hpp>
#include <boost/compute/algorithm/iota.hpp>
#include <boost/compute/algorithm/copy.hpp>
#include <boost/compute/algorithm/fill.hpp>
#include <boost/compute/algorithm/all_of.hpp>
#include <boost/compute/algorithm/adjacent_difference.hpp>
#include <boost/compute/container/vector.hpp>

#include "check_macros.hpp"
#include "context_setup.hpp"

namespace compute = boost::compute;

BOOST_AUTO_TEST_CASE(adjacent_difference_int)
{
    using compute::int_;

    compute::vector<int_> a(5, context);
    compute::iota(a.begin(), a.end(), 0, queue);
    CHECK_RANGE_EQUAL(int_, 5, a, (0, 1, 2, 3, 4));

    compute::vector<int_> b(5, context);
    compute::vector<int_>::iterator iter =
        compute::adjacent_difference(a.begin(), a.end(), b.begin(), queue);
    BOOST_CHECK(iter == b.end());
    CHECK_RANGE_EQUAL(int_, 5, b, (0, 1, 1, 1, 1));

    int_ data[] = { 1, 9, 36, 48, 81 };
    compute::copy(data, data + 5, a.begin(), queue);
    CHECK_RANGE_EQUAL(int_, 5, a, (1, 9, 36, 48, 81));

    iter = compute::adjacent_difference(a.begin(), a.end(), b.begin(), queue);
    BOOST_CHECK(iter == b.end());
    CHECK_RANGE_EQUAL(int_, 5, b, (1, 8, 27, 12, 33));
}

BOOST_AUTO_TEST_CASE(adjacent_difference_first_eq_last)
{
    using compute::int_;

    compute::vector<int_> a(size_t(5), int_(1), queue);
    compute::vector<int_> b(size_t(5), int_(0), queue);
    compute::vector<int_>::iterator iter =
        compute::adjacent_difference(a.begin(), a.begin(), b.begin(), queue);
    BOOST_CHECK(iter == b.begin());
    CHECK_RANGE_EQUAL(int_, 5, b, (0, 0, 0, 0, 0));
}

BOOST_AUTO_TEST_CASE(adjacent_difference_first_eq_result)
{
    using compute::int_;

    compute::vector<int_> a(5, context);
    compute::iota(a.begin(), a.end(), 0, queue);
    CHECK_RANGE_EQUAL(int_, 5, a, (0, 1, 2, 3, 4));

    compute::vector<int_>::iterator iter =
        compute::adjacent_difference(a.begin(), a.end(), a.begin(), queue);
    BOOST_CHECK(iter == a.end());
    CHECK_RANGE_EQUAL(int_, 5, a, (0, 1, 1, 1, 1));
}

BOOST_AUTO_TEST_CASE(all_same)
{
    using compute::int_;

    compute::vector<int_> input(1000, context);
    compute::fill(input.begin(), input.end(), 42, queue);

    compute::vector<int_> output(input.size(), context);

    compute::adjacent_difference(
        input.begin(), input.end(), output.begin(), queue
    );

    int_ first;
    compute::copy_n(output.begin(), 1, &first, queue);
    BOOST_CHECK_EQUAL(first, 42);

    using compute::lambda::_1;

    BOOST_CHECK(
        compute::all_of(output.begin() + 1, output.end(), _1 == 0, queue)
    );
}

BOOST_AUTO_TEST_SUITE_END()


////////////////////////////////////////////////////////////////
// Orkid Media Engine
// Copyright 1996-2020, Michael T. Mayers.
// Distributed under the Boost Software License - Version 1.0 - August 17, 2003
// see http://www.boost.org/LICENSE_1_0.txt
////////////////////////////////////////////////////////////////

#include <ork/lev2/config.h>
#if defined(ENABLE_IGL)

#include <ork/kernel/orklut.hpp>
#include <ork/math/plane.h>
#include <ork/lev2/gfx/meshutil/submesh.h>
#include <ork/lev2/gfx/meshutil/igl.h>
#include <iostream>

#include <igl/collapse_edge.h>
#include <igl/doublearea.h>
#include <igl/edge_flaps.h>
#include <igl/is_edge_manifold.h>
#include <igl/is_irregular_vertex.h>
#include <igl/is_vertex_manifold.h>
#include <igl/PI.h>
#include <igl/per_vertex_normals.h>
#include <igl/per_face_normals.h>
#include <igl/per_corner_normals.h>
#include <igl/polygon_mesh_to_triangle_mesh.h>
#include <igl/shortest_edge_and_midpoint.h>
#include <igl/unique_edge_map.h>
#include <igl/avg_edge_length.h>

//#include <igl/arap.h>
//#include <igl/boundary_facets.h>
//#include <igl/circulation.h>
//#include <igl/decimate.h>
//#include <igl/exterior_edges.h>
//#include <igl/orientable_patches.h>
//#include <igl/randperm.h>
//#include <igl/slice.h>
//#include <igl/copyleft/cgal/mesh_boolean.h>
//#include <igl/MeshBooleanType.h>

#include <Eigen/Core>

namespace ork::meshutil {
//////////////////////////////////////////////////////////////////////////////
iglmesh_ptr_t submesh::toIglMesh(int numsides) const {
  return std::make_shared<IglMesh>(*this, numsides);
}
//////////////////////////////////////////////////////////////////////////////
IglMesh::IglMesh(const Eigen::MatrixXd& verts, const Eigen::MatrixXi& faces)
    : _verts(verts)
    , _faces(faces) {
  OrkAssert(_verts.cols() == 3); // make sure we have vec3's
}
//////////////////////////////////////////////////////////////////////////////
IglMesh::IglMesh(const submesh& inp_submesh, int numsides)
    : _verts(inp_submesh._vtxpool.GetNumVertices(), 3)
    , _faces(inp_submesh.GetNumPolys(numsides), numsides) {
  size_t numverts = inp_submesh._vtxpool.GetNumVertices();
  size_t numfaces = inp_submesh.GetNumPolys(numsides);
  _verts          = Eigen::MatrixXd(numverts, 3);
  _normals        = Eigen::MatrixXd(numverts, 3);
  _binormals      = Eigen::MatrixXd(numverts, 3);
  _tangents       = Eigen::MatrixXd(numverts, 3);
  _uvs            = Eigen::MatrixXd(numverts, 2);
  _colors         = Eigen::MatrixXd(numverts, 4);
  _faces          = Eigen::MatrixXi(numfaces, numsides);
  ///////////////////////////////////////////////
  // fill in vertices
  ///////////////////////////////////////////////
  for (int v = 0; v < numverts; v++) {
    const auto& inpvtx = inp_submesh._vtxpool.GetVertex(v);
    const auto& inpuv  = inpvtx.mUV[0];
    _verts.row(v) << inpvtx.mPos.x, inpvtx.mPos.y, inpvtx.mPos.z;
    _normals.row(v) << inpvtx.mNrm.x, inpvtx.mNrm.y, inpvtx.mNrm.z;
    _binormals.row(v) << inpuv.mMapBiNormal.x, inpuv.mMapBiNormal.y, inpuv.mMapBiNormal.z;
    _tangents.row(v) << inpuv.mMapTangent.x, inpuv.mMapTangent.y, inpuv.mMapTangent.z;
    _uvs.row(v) << inpuv.mMapTexCoord.x, inpuv.mMapTexCoord.y;
    _colors.row(v) << inpvtx.mCol[0].x, inpvtx.mCol[0].y, inpvtx.mCol[0].z, inpvtx.mCol[0].w;
  }
  ///////////////////////////////////////////////
  // fill in faces
  ///////////////////////////////////////////////
  orkvector<int> face_indices;
  inp_submesh.FindNSidedPolys(face_indices, numsides);
  for (int f = 0; f < face_indices.size(); f++) {
    const auto& face = inp_submesh.RefPoly(f);
    switch (numsides) {
      case 3:
        _faces.row(f) <<         //
            face.GetVertexID(0), // tri index 0
            face.GetVertexID(1), // tri index 1
            face.GetVertexID(2); // tri index 2
        break;
      case 4:
        _faces.row(f) <<         //
            face.GetVertexID(0), // tri index 0
            face.GetVertexID(1), // tri index 1
            face.GetVertexID(2), // tri index 2
            face.GetVertexID(3); // tri index 3
        break;
      default:
        OrkAssert(false);
        break;
    }
  }
}
//////////////////////////////////////////////////////////////////////////////
size_t IglMesh::numFaces() const {
  return _faces.rows();
}
//////////////////////////////////////////////////////////////////////////////
size_t IglMesh::numVertices() const {
  return _verts.rows();
}
//////////////////////////////////////////////////////////////////////////////
unique_edges_ptr_t IglMesh::uniqueEdges() const {
  auto rval = std::make_shared<UniqueEdges>();
  igl::unique_edge_map(_faces, rval->E, rval->uE, rval->EMAP, rval->_ue2e);
  rval->_count = rval->uE.rows();
  return rval;
}
//////////////////////////////////////////////////////////////////////////////
size_t IglMesh::numEdges() const {
  return uniqueEdges()->_count;
}
//////////////////////////////////////////////////////////////////////////////
size_t IglMesh::sidesPerFace() const {
  return _faces.cols();
}
//////////////////////////////////////////////////////////////////////////////
bool IglMesh::isVertexManifold() const {
  Eigen::MatrixXi B;
  return igl::is_vertex_manifold(_faces, B);
}
//////////////////////////////////////////////////////////////////////////////
bool IglMesh::isEdgeManifold() const {
  return igl::is_edge_manifold(_faces);
}
//////////////////////////////////////////////////////////////////////////////
double IglMesh::averageEdgeLength() const {
  return igl::avg_edge_length(_verts, _faces);
}
//////////////////////////////////////////////////////////////////////////////
fvec4 IglMesh::computeAreaStatistics() const {
  Eigen::VectorXd area;
  igl::doublearea(_verts, _faces, area);
  area              = area.array() / 2;
  double area_avg   = area.mean();
  double area_min   = area.minCoeff() / area_avg;
  double area_max   = area.maxCoeff() / area_avg;
  double area_sigma = sqrt(((area.array() - area_avg) / area_avg).square().mean());
  return fvec4(area_min, area_max, area_avg, area_sigma);
}
//////////////////////////////////////////////////////////////////////////////
fvec4 IglMesh::computeAngleStatistics() const {
  Eigen::MatrixXd angles;
  igl::internal_angles(_verts, _faces, angles);
  angles             = 360.0 * (angles / (2 * igl::PI)); // Convert to degrees
  double angle_avg   = angles.mean();
  double angle_min   = angles.minCoeff();
  double angle_max   = angles.maxCoeff();
  double angle_sigma = sqrt((angles.array() - angle_avg).square().mean());
  return fvec4(angle_min, angle_max, angle_avg, angle_sigma);
}
//////////////////////////////////////////////////////////////////////////////
size_t IglMesh::countIrregularVertices() const {
  // Count the number of irregular vertices, the border is ignored
  auto irregular                = igl::is_irregular_vertex(_verts, _faces);
  size_t vertex_count           = _verts.rows();
  size_t irregular_vertex_count = std::count(
      irregular.begin(), //
      irregular.end(),
      true);
  return irregular_vertex_count;
}
//////////////////////////////////////////////////////////////////////////////
size_t IglMesh::genus() const {

  // an orientable 2-manifold mesh M with g“handles” (i.e., genus)
  // has Euler-Poincaré characteristic χ(M) = V-E+F = 2(1-g)

  // 2(1-g)=v-e+f     : 2-2g == Euler-Poincaré characteristic
  //-2g=v-e+f-2
  // 2g=-v+e-f+2
  // 2g=2+e-f-v
  // g = (2+e-f-v)/2
  const size_t this_genus = (2 + numEdges() - numFaces() - numVertices()) / 2;
  return this_genus;
}
//////////////////////////////////////////////////////////////////////////////
submesh_ptr_t IglMesh::toSubMesh() const {
  auto subm           = std::make_shared<submesh>();
  size_t numFaces     = this->numFaces();
  size_t sidesPerFace = this->sidesPerFace();
  OrkAssert(sidesPerFace == 3 or sidesPerFace == 4);
  /////////////////////////////////////////////
  std::vector<vertex> submeshverts;
  submeshverts.resize(numFaces * sidesPerFace);
  /////////////////////////////////////////////
  OrkAssert(_verts.cols() == 3); // make sure we have vec3's
  size_t numVerts     = _verts.rows();
  size_t numNormals   = _normals.rows();
  size_t numBinormals = _binormals.rows();
  size_t numTangents  = _tangents.rows();
  size_t numUvs       = _uvs.rows();
  size_t numColors    = _colors.rows();
  auto generateVertex = [&](int faceindex, int facevtxindex) -> vertex { //
    vertex outv;
    const Eigen::MatrixXi& face = _faces.row(faceindex);
    int per_vert_index          = face(facevtxindex);
    /////////////////////////////////////////////
    // position
    /////////////////////////////////////////////
    auto inp_pos = _verts.row(per_vert_index);
    outv.mPos    = fvec3(inp_pos(0), inp_pos(1), inp_pos(2));
    /////////////////////////////////////////////
    // normal
    /////////////////////////////////////////////
    auto donormal = [&](int index) {
      OrkAssert(_normals.cols() == 3);
      auto inp  = _normals.row(index);
      outv.mNrm = fvec3(inp(0), inp(1), inp(2));
    };
    if (numNormals == numVerts) // per vertex
      donormal(per_vert_index);
    else if (numNormals == numFaces) // per face
      donormal(faceindex);
    else if (numNormals == 0) {
    } // no normals
    else
      OrkAssert(false);
    /////////////////////////////////////////////
    // binormal
    /////////////////////////////////////////////
    auto dobinormal = [&](int index) {
      OrkAssert(_binormals.cols() == 3);
      auto inp                 = _binormals.row(index);
      outv.mUV[0].mMapBiNormal = fvec3(inp(0), inp(1), inp(2));
    };
    if (numBinormals == numVerts) // per vertex
      dobinormal(per_vert_index);
    else if (numBinormals == numFaces) // per face
      dobinormal(faceindex);
    else if (numBinormals == 0) {
    } // no binormals
    else
      OrkAssert(false);
    /////////////////////////////////////////////
    // tangent
    /////////////////////////////////////////////
    auto dotangent = [&](int index) {
      OrkAssert(_tangents.cols() == 3);
      auto inp                = _tangents.row(index);
      outv.mUV[0].mMapTangent = fvec3(inp(0), inp(1), inp(2));
    };
    if (numTangents == numVerts) // per vertex
      dotangent(per_vert_index);
    else if (numTangents == numFaces) // per face
      dotangent(faceindex);
    else if (numTangents == 0) {
    } // no tangents
    else
      OrkAssert(false);
    /////////////////////////////////////////////
    // texturecoord
    /////////////////////////////////////////////
    auto dotexcoord = [&](int index) {
      OrkAssert(_uvs.cols() == 2);
      auto inp                 = _uvs.row(index);
      outv.mUV[0].mMapTexCoord = fvec2(inp(0), inp(1));
    };
    if (numUvs == numVerts) // per vertex
      dotexcoord(per_vert_index);
    else if (numUvs == numFaces) // per face
      dotexcoord(faceindex);
    else if (numUvs == 0) {
    } // no texcoords
    else
      OrkAssert(false);
    /////////////////////////////////////////////
    // color
    /////////////////////////////////////////////
    auto docolor = [&](int index) {
      auto inp = _colors.row(index);
      switch (_colors.cols()) {
        case 1: // luminance
          outv.mCol[0] = fvec4(inp(0), inp(0), inp(0), 1);
          break;
        case 3: // rgb
          outv.mCol[0] = fvec4(inp(0), inp(1), inp(2), 1);
          break;
        case 4: // rgba
          outv.mCol[0] = fvec4(inp(0), inp(1), inp(2), inp(3));
          break;
        default:
          OrkAssert(false);
          break;
      }
    };
    if (numColors == numVerts)
      docolor(per_vert_index);
    else if (numColors == numFaces)
      docolor(faceindex);
    else if (numColors == 1)
      docolor(0);
    else if (numColors == 0)
      outv.mCol[0] = fvec4(1, 1, 1, 1);
    else
      OrkAssert(false);
    /////////////////////////////////////////////
    return outv;
  }; // auto generateVertex = [&](int faceindex, int facevtxindex) -> vertex { //
  /////////////////////////////////////////////
  for (int f = 0; f < numFaces; f++) {
    switch (sidesPerFace) {
      case 3: {
        auto o0 = subm->newMergeVertex(generateVertex(f, 0));
        auto o1 = subm->newMergeVertex(generateVertex(f, 1));
        auto o2 = subm->newMergeVertex(generateVertex(f, 2));
        subm->MergePoly(poly(o0, o1, o2));
        break;
      }
      case 4: {
        auto o0 = subm->newMergeVertex(generateVertex(f, 0));
        auto o1 = subm->newMergeVertex(generateVertex(f, 1));
        auto o2 = subm->newMergeVertex(generateVertex(f, 2));
        auto o3 = subm->newMergeVertex(generateVertex(f, 3));
        subm->MergePoly(poly(o0, o1, o2, o3));
        break;
      }
      default:
        OrkAssert(false);
        break;
    }
  }
  return subm;
}
//////////////////////////////////////////////////////////////////////////////
iglmesh_ptr_t IglMesh::triangulated() const {
  auto rval = std::make_shared<IglMesh>();
  igl::polygon_mesh_to_triangle_mesh(_faces, rval->_faces);
  rval->_verts = _verts;
  return rval;
}
//////////////////////////////////////////////////////////////////////////////
iglmesh_ptr_t IglMesh::decimated(float amount) const {
  auto rval = std::make_shared<IglMesh>();
  Eigen::MatrixXd V;
  Eigen::MatrixXi F;
  Eigen::MatrixXd OV = _verts;
  Eigen::MatrixXi OF = _faces;
  // Prepare array-based edge data structures and priority queue
  Eigen::VectorXi EMAP;
  Eigen::MatrixXi E, EF, EI;
  typedef std::set<std::pair<double, int>> PriorityQueue;
  PriorityQueue Q;
  std::vector<PriorityQueue::iterator> Qit;
  // If an edge were collapsed, we'd collapse it to these points:
  Eigen::MatrixXd C;
  int num_collapsed;
  { // prep
    F = OF;
    V = OV;
    igl::edge_flaps(F, E, EMAP, EF, EI);
    Qit.resize(E.rows());

    C.resize(E.rows(), V.cols());
    Eigen::VectorXd costs(E.rows());
    Q.clear();
    for (int e = 0; e < E.rows(); e++) {
      double cost = e;
      Eigen::RowVectorXd p(1, 3);
      igl::shortest_edge_and_midpoint(e, V, F, E, EMAP, EF, EI, cost, p);
      C.row(e) = p;
      Qit[e]   = Q.insert(std::pair<double, int>(cost, e)).first;
    }
    num_collapsed = 0;
  }
  { // decimate
    while (not Q.empty()) {
      bool something_collapsed = false;
      // collapse edge
      const int max_iter = std::ceil(amount * Q.size());
      for (int j = 0; j < max_iter; j++) {
        if (not igl::collapse_edge(
                igl::shortest_edge_and_midpoint, //
                V,
                F,
                E,
                EMAP,
                EF,
                EI,
                Q,
                Qit,
                C)) {
          break;
        }
        something_collapsed = true;
        num_collapsed++;
      }
    }
  }
  rval->_verts = V;
  rval->_faces = F;
  return rval;
}
//////////////////////////////////////////////////////////////////////////////
Eigen::MatrixXd IglMesh::computeFaceNormals() const {
  Eigen::MatrixXd rval;
  igl::per_face_normals(_verts, _faces, rval);
  return rval;
}
Eigen::MatrixXd IglMesh::computeVertexNormals() const {
  Eigen::MatrixXd rval;
  igl::per_vertex_normals(_verts, _faces, rval);
  return rval;
}
Eigen::MatrixXd IglMesh::computeCornerNormals(float dihedral_angle) const {
  Eigen::MatrixXd rval;
  igl::per_corner_normals(_verts, _faces, dihedral_angle, rval);
  return rval;
}

//////////////////////////////////////////////////////////////////////////////
void submesh::igl_test() {
  auto trimesh = submesh();
  submeshTriangulate(*this, trimesh);
}
//////////////////////////////////////////////////////////////////////////////
} // namespace ork::meshutil
#endif


// This file is part of the dune-hdd project:
//   http://users.dune-project.org/projects/dune-hdd
// Copyright holders: Felix Schindler
// License: BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)

#ifndef DUNE_HDD_LINEARELLIPTIC_DISCRETIZATIONS_BLOCK_SWIPDG_HH
#define DUNE_HDD_LINEARELLIPTIC_DISCRETIZATIONS_BLOCK_SWIPDG_HH

#include <memory>
#include <algorithm>
#include <vector>
#include <map>
#include <set>
#include <cmath>
#include <limits>
#include <type_traits>

#include <boost/numeric/conversion/cast.hpp>

#include <dune/stuff/common/disable_warnings.hh>
# if HAVE_EIGEN
#   include <Eigen/Eigenvalues>
# endif

# include <dune/common/timer.hh>
# include <dune/common/dynmatrix.hh>

# if HAVE_ALUGRID
#   include <dune/grid/alugrid.hh>
# endif

# include <dune/geometry/quadraturerules.hh>
#include <dune/stuff/common/reenable_warnings.hh>

#include <dune/grid/multiscale/provider.hh>

#include <dune/stuff/common/logging.hh>
#include <dune/stuff/common/timedlogging.hh>
#include <dune/stuff/common/configuration.hh>
#include <dune/stuff/common/float_cmp.hh>
#include <dune/stuff/common/fixed_map.hh>
#include <dune/stuff/grid/layers.hh>
#include <dune/stuff/grid/boundaryinfo.hh>
#include <dune/stuff/functions/constant.hh>
#include <dune/stuff/functions/interfaces.hh>
#include <dune/stuff/la/container.hh>
#include <dune/stuff/la/solver.hh>
#include <dune/stuff/grid/walker.hh>

#include <dune/pymor/common/exceptions.hh>

#include <dune/gdt/spaces/dg.hh>
#include <dune/gdt/playground/spaces/block.hh>
#include <dune/gdt/playground/localevaluation/swipdg.hh>
#include <dune/gdt/discretefunction/default.hh>
#include <dune/gdt/operators/oswaldinterpolation.hh>
#include <dune/gdt/operators/projections.hh>
#include <dune/gdt/spaces/fv/default.hh>
#include <dune/gdt/spaces/rt/pdelab.hh>
#include <dune/gdt/playground/operators/fluxreconstruction.hh>
#include <dune/gdt/playground/products/swipdgpenalty.hh>
#include <dune/gdt/products/boundaryl2.hh>
#include <dune/gdt/products/l2.hh>
#include <dune/gdt/products/h1.hh>
#include <dune/gdt/products/elliptic.hh>
#include <dune/gdt/assembler/system.hh>

#include <dune/hdd/linearelliptic/problems/default.hh>
#include <dune/hdd/linearelliptic/problems/zero-boundary.hh>

#include "base.hh"
#include "swipdg.hh"

namespace Dune {
namespace HDD {
namespace LinearElliptic {
namespace Discretizations {

// forward, needed in the Traits
template< class GridImp, class RangeFieldImp, int rangeDim, int polynomialOrder = 1
        , Stuff::LA::ChooseBackend la_backend = Stuff::LA::default_sparse_backend >
class BlockSWIPDG;

namespace internal {

template< class GridType, class RangeFieldType, int dimRange, int polOrder, Stuff::LA::ChooseBackend la_backend >
class LocalDiscretizationsContainer
{
  typedef grid::Multiscale::ProviderInterface< GridType > GridProviderType;
public:
  typedef SWIPDG< GridType, Stuff::Grid::ChooseLayer::local, RangeFieldType, dimRange
                , polOrder, GDT::ChooseSpaceBackend::fem, la_backend > DiscretizationType;
  typedef SWIPDG< GridType, Stuff::Grid::ChooseLayer::local_oversampled, RangeFieldType, dimRange
                , polOrder, GDT::ChooseSpaceBackend::fem, la_backend > OversampledDiscretizationType;
  typedef typename DiscretizationType::ProblemType     ProblemType;
  typedef typename DiscretizationType::TestSpaceType   TestSpaceType;
  typedef typename DiscretizationType::AnsatzSpaceType AnsatzSpaceType;

private:
  typedef Problems::ZeroBoundary< ProblemType > FakeProblemType;
  typedef typename DiscretizationType::GridViewType::Intersection IntersectionType;

public:
  LocalDiscretizationsContainer(const GridProviderType& grid_provider,
                                const ProblemType& prob,
                                const std::vector< std::string >& only_these_products)
    : zero_boundary_problem_(prob)
    , all_dirichlet_boundary_config_(Stuff::Grid::BoundaryInfos::AllDirichlet< IntersectionType >::default_config())
    , all_neumann_boundary_config_(Stuff::Grid::BoundaryInfos::AllNeumann< IntersectionType >::default_config())
    , multiscale_boundary_config_(Stuff::Grid::BoundaryInfoConfigs::IdBased::default_config())
    , local_discretizations_(grid_provider.num_subdomains(), nullptr)
    , oversampled_discretizations_dirichlet_(grid_provider.num_subdomains(), nullptr)
    , oversampled_discretizations_neumann_(grid_provider.num_subdomains(), nullptr)
    , local_test_spaces_(grid_provider.num_subdomains(), nullptr)
    , local_ansatz_spaces_(grid_provider.num_subdomains(), nullptr)
  {
    multiscale_boundary_config_["neumann"] = "7";
    for (size_t ss = 0; ss < grid_provider.num_subdomains(); ++ss) {
      local_discretizations_[ss] = std::make_shared< DiscretizationType >(grid_provider,
                                                                          all_neumann_boundary_config_,
                                                                          zero_boundary_problem_,
                                                                          ss,
                                                                          only_these_products);
      local_test_spaces_[ss]   = std::make_shared< TestSpaceType >(  local_discretizations_[ss]->test_space());
      local_ansatz_spaces_[ss] = std::make_shared< AnsatzSpaceType >(local_discretizations_[ss]->ansatz_space());
    }
  }

protected:
  const FakeProblemType zero_boundary_problem_;
  const Stuff::Common::Configuration all_dirichlet_boundary_config_;
  const Stuff::Common::Configuration all_neumann_boundary_config_;
  Stuff::Common::Configuration multiscale_boundary_config_;
  std::vector< std::shared_ptr< DiscretizationType > > local_discretizations_;
  mutable std::vector< std::shared_ptr< OversampledDiscretizationType > > oversampled_discretizations_dirichlet_;
  mutable std::vector< std::shared_ptr< OversampledDiscretizationType > > oversampled_discretizations_neumann_;
  std::vector< std::shared_ptr< const TestSpaceType > > local_test_spaces_;
  std::vector< std::shared_ptr< const AnsatzSpaceType > > local_ansatz_spaces_;
}; // class LocalDiscretizationsContainer

template< class GridImp, class RangeFieldImp, int rangeDim, int polynomialOrder, Stuff::LA::ChooseBackend la_backend >
class BlockSWIPDGTraits
  : public ContainerBasedDefaultTraits< typename Stuff::LA::Container< RangeFieldImp, la_backend >::MatrixType,
                                        typename Stuff::LA::Container< RangeFieldImp, la_backend >::VectorType >
{
public:
  typedef BlockSWIPDG< GridImp, RangeFieldImp, rangeDim, polynomialOrder, la_backend > derived_type;
  typedef GridImp GridType;
  typedef RangeFieldImp     RangeFieldType;
  static const unsigned int dimRange = rangeDim;
  static const unsigned int polOrder = polynomialOrder;
private:
  friend class BlockSWIPDG< GridImp, RangeFieldImp, rangeDim, polynomialOrder, la_backend >;
  typedef grid::Multiscale::ProviderInterface< GridType > GridProviderType;
  typedef LocalDiscretizationsContainer< GridType, RangeFieldType, dimRange, polOrder, la_backend >
      LocalDiscretizationsContainerType;
  typedef typename LocalDiscretizationsContainerType::TestSpaceType   LocalTestSpaceType;
  typedef typename LocalDiscretizationsContainerType::AnsatzSpaceType LocalAnsatzSpaceType;
public:
  typedef GDT::Spaces::Block< LocalTestSpaceType >   TestSpaceType;
  typedef GDT::Spaces::Block< LocalAnsatzSpaceType > AnsatzSpaceType;
  typedef typename TestSpaceType::GridViewType GridViewType;
}; // class BlockSWIPDGTraits

} // namespace internal

/**
 * \attention The given problem is replaced by a Problems::ZeroBoundary.
 * \attention The given boundary info config is replaced by a Stuff::Grid::BoundaryInfos::AllDirichlet.
 * \attention The boundary info for the local oversampled discretizations is hardwired to dirichlet zero atm!
 */
template< class GridImp, class RangeFieldImp, int rangeDim, int polynomialOrder, Stuff::LA::ChooseBackend la_backend >
class BlockSWIPDG
  : internal::LocalDiscretizationsContainer< GridImp, RangeFieldImp, rangeDim, polynomialOrder, la_backend >
  , public ContainerBasedDefault< internal::BlockSWIPDGTraits< GridImp, RangeFieldImp, rangeDim
                                                             , polynomialOrder, la_backend > >

{
  typedef internal::LocalDiscretizationsContainer< GridImp, RangeFieldImp, rangeDim, polynomialOrder, la_backend >
    LocalDiscretizationsBaseType;
  typedef ContainerBasedDefault< internal::BlockSWIPDGTraits< GridImp, RangeFieldImp, rangeDim
                                                            , polynomialOrder, la_backend > > BaseType;
  typedef BlockSWIPDG< GridImp, RangeFieldImp, rangeDim, polynomialOrder, la_backend >        ThisType;
public:
  typedef internal::BlockSWIPDGTraits< GridImp, RangeFieldImp, rangeDim, polynomialOrder, la_backend > Traits;
  typedef typename BaseType::ProblemType     ProblemType;
  typedef typename BaseType::GridViewType    GridViewType;
  typedef typename BaseType::TestSpaceType   TestSpaceType;
  typedef typename BaseType::AnsatzSpaceType AnsatzSpaceType;
  typedef typename BaseType::EntityType      EntityType;
  typedef typename BaseType::DomainFieldType DomainFieldType;
  typedef typename BaseType::RangeFieldType  RangeFieldType;
  typedef typename BaseType::MatrixType      MatrixType;
  typedef typename BaseType::VectorType      VectorType;
  typedef typename BaseType::OperatorType    OperatorType;
  typedef typename BaseType::ProductType     ProductType;
  typedef typename BaseType::FunctionalType  FunctionalType;

  static const unsigned int dimDomain = BaseType::dimDomain;
  static const unsigned int dimRange  = BaseType::dimRange;

  typedef grid::Multiscale::ProviderInterface< GridImp > GridProviderType;
  typedef typename GridProviderType::GridType   GridType;
  typedef typename GridProviderType::MsGridType MsGridType;

  typedef typename Traits::LocalDiscretizationsContainerType::DiscretizationType            LocalDiscretizationType;
  typedef typename Traits::LocalDiscretizationsContainerType::OversampledDiscretizationType OversampledDiscretizationType;
  typedef typename TestSpaceType::PatternType PatternType;

private:
  using typename BaseType::AffinelyDecomposedMatrixType;
  using typename BaseType::AffinelyDecomposedVectorType;
  typedef Pymor::LA::AffinelyDecomposedConstContainer< MatrixType > AffinelyDecomposedConstMatrixType;
  typedef Pymor::LA::AffinelyDecomposedConstContainer< VectorType > AffinelyDecomposedConstVectorType;

public:
  typedef typename LocalDiscretizationType::ProblemType LocalProblemType;
  typedef typename LocalDiscretizationType::ProductType LocalProductType;

  static std::string static_id();

  BlockSWIPDG(const GridProviderType& grid_provider,
              const Stuff::Common::Configuration& bound_inf_cfg,
              const ProblemType& prob,
              const std::vector< std::string >& only_these_products = {});

  const std::vector< std::shared_ptr< LocalDiscretizationType > >& local_discretizations() const;

  void init(const bool prune = false);

  ssize_t num_subdomains() const;

  std::vector< ssize_t > neighbouring_subdomains(const ssize_t ss) const;

  VectorType localize_vector(const VectorType& global_vector, const size_t ss) const;

  VectorType globalize_vectors(const std::vector< VectorType >& local_vectors) const;

  VectorType* globalize_vectors_and_return_ptr(const std::vector< VectorType >& local_vectors) const;

  VectorType* localize_vector_and_return_ptr(const VectorType& global_vector, const ssize_t ss) const;

  ProductType get_local_product(const size_t ss, const std::string id) const;

  ProductType* get_local_product_and_return_ptr(const ssize_t ss, const std::string id) const;

  OperatorType get_local_operator(const size_t ss) const;

  OperatorType* get_local_operator_and_return_ptr(const ssize_t ss) const;

  OperatorType get_coupling_operator(const size_t ss, const size_t nn) const;

  OperatorType* get_coupling_operator_and_return_ptr(const ssize_t ss, const ssize_t nn) const;

  FunctionalType get_local_functional(const size_t ss) const;

  FunctionalType* get_local_functional_and_return_ptr(const ssize_t ss) const;

  VectorType solve_for_local_correction(const std::vector< VectorType >& local_vectors,
                                        const size_t subdomain,
                                        const Pymor::Parameter mu = Pymor::Parameter()) const;

  LocalDiscretizationType get_local_discretization(const size_t subdomain) const;

  LocalDiscretizationType* pb_get_local_discretization(const ssize_t subdomain) const;

  OversampledDiscretizationType get_oversampled_discretization(const size_t subdomain,
                                                               const std::string boundary_value_type) const;

  OversampledDiscretizationType* pb_get_oversampled_discretization(const ssize_t subdomain,
                                                                   const std::string boundary_value_type) const;

//  OversampledDiscretizationType* pb_get_oversampled_discretization(const ssize_t subdomain,
//                                                                   const std::string boundary_value_type,
//                                                                   const VectorType& boundary_values) const;

private:
  class CouplingAssembler
  {
    typedef Dune::DynamicMatrix< RangeFieldType > LocalMatrixType;
    typedef Dune::DynamicVector< RangeFieldType > LocalVectorType;
    typedef std::vector< std::vector< LocalMatrixType > > LocalMatricesContainerType;
    typedef std::vector< std::vector< LocalVectorType > > LocalVectorsContainerType;
    typedef std::vector< Dune::DynamicVector< size_t > > IndicesContainer;

    typedef typename LocalDiscretizationType::TestSpaceType LocalTestSpaceType;
    typedef typename LocalDiscretizationType::AnsatzSpaceType LocalAnsatzSpaceType;
    typedef typename MsGridType::CouplingGridPartType CouplingGridPartType;

    class LocalCodim1MatrixAssemblerApplication
    {
    public:
      virtual ~LocalCodim1MatrixAssemblerApplication(){}

      virtual void apply(const LocalTestSpaceType& /*inner_test_space*/,
                         const LocalAnsatzSpaceType& /*inner_ansatz_space*/,
                         const LocalTestSpaceType& /*outer_test_space*/,
                         const LocalAnsatzSpaceType& /*outer_ansatz_space*/,
                         const typename CouplingGridPartType::IntersectionType& /*_intersection*/,
                         LocalMatricesContainerType& /*_localMatricesContainer*/,
                         IndicesContainer& /*indicesContainer*/) const = 0;

      virtual std::vector< size_t > numTmpObjectsRequired() const = 0;
    };

    template< class LocalAssemblerType, class M >
    class LocalCodim1MatrixAssemblerWrapper
      : public LocalCodim1MatrixAssemblerApplication
    {
    public:
      LocalCodim1MatrixAssemblerWrapper(const LocalAssemblerType& localAssembler,
                                        Dune::Stuff::LA::MatrixInterface< M >& in_in_matrix,
                                        Dune::Stuff::LA::MatrixInterface< M >& in_out_matrix,
                                        Dune::Stuff::LA::MatrixInterface< M >& out_in_matrix,
                                        Dune::Stuff::LA::MatrixInterface< M >& out_out_matrix)
        : localMatrixAssembler_(localAssembler)
        , in_in_matrix_(in_in_matrix)
        , out_out_matrix_(out_out_matrix)
        , in_out_matrix_(in_out_matrix)
        , out_in_matrix_(out_in_matrix)
      {}

      virtual void apply(const LocalTestSpaceType& inner_test_space,
                         const LocalAnsatzSpaceType& inner_ansatz_space,
                         const LocalTestSpaceType& outer_test_space,
                         const LocalAnsatzSpaceType& outer_ansatz_space,
                         const typename CouplingGridPartType::IntersectionType& intersection,
                         LocalMatricesContainerType& localMatricesContainer,
                         IndicesContainer& indicesContainer) const
      {
        localMatrixAssembler_.assembleLocal(inner_test_space, inner_ansatz_space,
                                            outer_test_space, outer_ansatz_space,
                                            intersection,
                                            in_in_matrix_, out_out_matrix_, in_out_matrix_, out_in_matrix_,
                                            localMatricesContainer, indicesContainer);
      }

      virtual std::vector< size_t > numTmpObjectsRequired() const
      {
        return localMatrixAssembler_.numTmpObjectsRequired();
      }

    private:
      const LocalAssemblerType& localMatrixAssembler_;
      Dune::Stuff::LA::MatrixInterface< M >& in_in_matrix_;
      Dune::Stuff::LA::MatrixInterface< M >& out_out_matrix_;
      Dune::Stuff::LA::MatrixInterface< M >& in_out_matrix_;
      Dune::Stuff::LA::MatrixInterface< M >& out_in_matrix_;
    }; // class LocalCodim1MatrixAssemblerWrapper

  public:
    CouplingAssembler(const LocalTestSpaceType& inner_test_space,
                      const LocalAnsatzSpaceType& inner_ansatz_space,
                      const LocalTestSpaceType& outer_test_space,
                      const LocalAnsatzSpaceType& outer_ansatz_space,
                      const CouplingGridPartType& grid_part)
      : innerTestSpace_(inner_test_space)
      , innerAnsatzSpace_(inner_ansatz_space)
      , outerTestSpace_(outer_test_space)
      , outerAnsatzSpace_(outer_ansatz_space)
      , grid_part_(grid_part)
    {}

    ~CouplingAssembler()
    {
      clearLocalAssemblers();
    }

    void clearLocalAssemblers()
    {
      for (auto& element: localCodim1MatrixAssemblers_)
        delete element;
    }

    template< class L, class M >
    void addLocalAssembler(const GDT::LocalAssembler::Codim1CouplingMatrix< L >& localAssembler,
                           Dune::Stuff::LA::MatrixInterface< M >& in_in_matrix,
                           Dune::Stuff::LA::MatrixInterface< M >& in_out_matrix,
                           Dune::Stuff::LA::MatrixInterface< M >& out_in_matrix,
                           Dune::Stuff::LA::MatrixInterface< M >& out_out_matrix)
    {
      assert(in_in_matrix.rows() == innerTestSpace_.mapper().size());
      assert(in_in_matrix.cols() == innerAnsatzSpace_.mapper().size());
      assert(in_out_matrix.rows() == innerTestSpace_.mapper().size());
      assert(in_out_matrix.cols() == outerAnsatzSpace_.mapper().size());
      assert(out_in_matrix.rows() == outerTestSpace_.mapper().size());
      assert(out_in_matrix.cols() == innerAnsatzSpace_.mapper().size());
      assert(out_out_matrix.rows() == outerTestSpace_.mapper().size());
      assert(out_out_matrix.cols() == outerAnsatzSpace_.mapper().size());
      localCodim1MatrixAssemblers_.push_back(
            new LocalCodim1MatrixAssemblerWrapper< GDT::LocalAssembler::Codim1CouplingMatrix< L >, M >(
              localAssembler, in_in_matrix, in_out_matrix, out_in_matrix, out_out_matrix));
    }

    void assemble() const
    {
      // only do something, if there are local assemblers
      if (localCodim1MatrixAssemblers_.size() > 0) {
        // common tmp storage for all entities
        // * for the matrix assemblers
        std::vector< size_t > numberOfTmpMatricesNeeded(2, 0);
        for (auto& localCodim1MatrixAssembler : localCodim1MatrixAssemblers_) {
          const auto tmp = localCodim1MatrixAssembler->numTmpObjectsRequired();
          assert(tmp.size() == 2);
          numberOfTmpMatricesNeeded[0] = std::max(numberOfTmpMatricesNeeded[0], tmp[0]);
          numberOfTmpMatricesNeeded[1] = std::max(numberOfTmpMatricesNeeded[1], tmp[1]);
        }
        const size_t maxLocalSize = std::max(innerTestSpace_.mapper().maxNumDofs(),
                                             std::max(innerAnsatzSpace_.mapper().maxNumDofs(),
                                                      std::max(outerTestSpace_.mapper().maxNumDofs(),
                                                               outerAnsatzSpace_.mapper().maxNumDofs())));
        std::vector< LocalMatrixType > tmpLocalAssemblerMatrices( numberOfTmpMatricesNeeded[0],
                                                                  LocalMatrixType(maxLocalSize,
                                                                                maxLocalSize,
                                                                                RangeFieldType(0)));
        std::vector< LocalMatrixType > tmpLocalOperatorMatrices(numberOfTmpMatricesNeeded[1],
                                                                LocalMatrixType(maxLocalSize,
                                                                                maxLocalSize,
                                                                                RangeFieldType(0)));
        std::vector< std::vector< LocalMatrixType > > tmpLocalMatricesContainer;
        tmpLocalMatricesContainer.push_back(tmpLocalAssemblerMatrices);
        tmpLocalMatricesContainer.push_back(tmpLocalOperatorMatrices);
        // * for the global indices
        std::vector< Dune::DynamicVector< size_t > > tmpIndices = {
            Dune::DynamicVector< size_t >(maxLocalSize)
          , Dune::DynamicVector< size_t >(maxLocalSize)
          , Dune::DynamicVector< size_t >(maxLocalSize)
          , Dune::DynamicVector< size_t >(maxLocalSize)
        };

        // walk the grid
        const auto entityEndIt = grid_part_.template end< 0 >();
        for(auto entityIt = grid_part_.template begin< 0 >(); entityIt != entityEndIt; ++entityIt ) {
          const auto& entity = *entityIt;
          // walk the intersections
          const auto intersectionEndIt = grid_part_.iend(entity);
          for (auto intersectionIt = grid_part_.ibegin(entity);
               intersectionIt != intersectionEndIt;
               ++intersectionIt) {
            const auto& intersection = *intersectionIt;
            // for a coupling grid part, we can be sure to only get the inner coupling intersetcions
            // so no further check neccesarry then
            assert(intersection.neighbor() && !intersection.boundary());
            // call local matrix assemblers
            for (auto& localCodim1MatrixAssembler : localCodim1MatrixAssemblers_) {
              localCodim1MatrixAssembler->apply(innerTestSpace_, innerAnsatzSpace_,
                                                outerTestSpace_, outerAnsatzSpace_,
                                                intersection,
                                                tmpLocalMatricesContainer, tmpIndices);
            }
          } // walk the intersections
        } // walk the grid
      } // only do something, if there are local assemblers
    } // void assemble() const

  private:
    const LocalTestSpaceType& innerTestSpace_;
    const LocalAnsatzSpaceType& innerAnsatzSpace_;
    const LocalTestSpaceType& outerTestSpace_;
    const LocalAnsatzSpaceType& outerAnsatzSpace_;
    const CouplingGridPartType grid_part_;
    std::vector< LocalCodim1MatrixAssemblerApplication* > localCodim1MatrixAssemblers_;
  }; // class CouplingAssembler

  void add_local_to_global_pattern(const PatternType& local,
                                   const size_t test_subdomain,
                                   const size_t ansatz_subdomain,
                                   PatternType& global) const;

  void copy_local_to_global_matrix(const AffinelyDecomposedConstMatrixType& local_matrix,
                                   const PatternType& local_pattern,
                                   const size_t subdomain,
                                   const size_t neighbor,
                                   AffinelyDecomposedMatrixType& global_matrix) const;

  template< class ML, class MG >
  void copy_local_to_global_matrix(const Stuff::LA::MatrixInterface< ML >& local_matrix,
                                   const PatternType& local_pattern,
                                   const size_t test_subdomain,
                                   const size_t ansatz_subdomain,
                                   Stuff::LA::MatrixInterface< MG >& global_matrix) const
  {
    for (size_t local_ii = 0; local_ii < local_pattern.size(); ++local_ii) {
      const size_t global_ii = this->test_space().mapper().mapToGlobal(test_subdomain, local_ii);
      for (const size_t& local_jj : local_pattern.inner(local_ii)) {
        const size_t global_jj = this->ansatz_space().mapper().mapToGlobal(ansatz_subdomain, local_jj);
        global_matrix.add_to_entry(global_ii, global_jj, local_matrix.get_entry(local_ii, local_jj));
      }
    }
  } // ... copy_local_to_global_matrix(...)

  void copy_local_to_global_vector(const AffinelyDecomposedConstVectorType& local_vector,
                                   const size_t subdomain,
                                   AffinelyDecomposedVectorType& global_vector) const;

  template< class VL, class VG >
  void copy_local_to_global_vector(const Stuff::LA::VectorInterface< VL >& local_vector,
                                   const size_t subdomain,
                                   Stuff::LA::VectorInterface< VG >& global_vector) const
  {
    for (size_t local_ii = 0; local_ii < local_vector.size(); ++local_ii) {
      const size_t global_ii = this->test_space().mapper().mapToGlobal(subdomain, local_ii);
      global_vector.add_to_entry(global_ii, local_vector.get_entry(local_ii));
    }
  } // ... copy_local_to_global_vector(...)

  void assemble_boundary_contributions(const size_t subdomain) const;

  /**
   * \note  We take the matrices as input here becaus we would have to look them up in the maps otherwise. Since that
   *        has already been done above we save a little.
   */
  void assemble_coupling_contributions(const size_t subdomain,
                                       const size_t neighbour,
                                       AffinelyDecomposedMatrixType& inside_inside_matrix,
                                       AffinelyDecomposedMatrixType& inside_outside_matrix,
                                       AffinelyDecomposedMatrixType& outside_inside_matrix,
                                       AffinelyDecomposedMatrixType& outside_outside_matrix) const;

  void build_global_containers();

  template< class AffinelyDecomposedContainerType >
  ssize_t find_component(const AffinelyDecomposedContainerType& container,
                         const Pymor::ParameterFunctional& coefficient) const
  {
    for (size_t qq = 0; qq < boost::numeric_cast< size_t >(container.num_components()); ++qq)
      if (*(container.coefficient(qq)) == coefficient)
        return qq;
    return -1;
  } // ... find_component(...)

  const GridProviderType& grid_provider_;
  std::shared_ptr< const MsGridType > ms_grid_;
  const std::vector< std::string > only_these_products_;
  using BaseType::pattern_;
  std::vector< std::shared_ptr< AffinelyDecomposedMatrixType > > local_matrices_;
  std::vector< std::shared_ptr< AffinelyDecomposedVectorType > > local_vectors_;
  std::vector< std::map< size_t, std::shared_ptr< PatternType > > > inside_outside_patterns_;
  std::vector< std::map< size_t, std::shared_ptr< PatternType > > > outside_inside_patterns_;
  std::vector< std::map< size_t, std::shared_ptr< AffinelyDecomposedMatrixType > > > inside_outside_matrices_;
  std::vector< std::map< size_t, std::shared_ptr< AffinelyDecomposedMatrixType > > > outside_inside_matrices_;
}; // BlockSWIPDG

#if HAVE_ALUGRID && HAVE_DUNE_FEM
# if HAVE_DUNE_ISTL

extern template class BlockSWIPDG< ALUGrid< 2, 2, simplex, conforming, No_Comm >,
                                   double,
                                   1,
                                   1,
                                   Stuff::LA::ChooseBackend::istl_sparse >;

#   if HAVE_MPI

extern template class BlockSWIPDG< ALUGrid< 2, 2, simplex, conforming, MPI_Comm >,
                                   double,
                                   1,
                                   1,
                                   Stuff::LA::ChooseBackend::istl_sparse >;

#   endif // HAVE_MPI
# endif // HAVE_DUNE_ISTL
# if HAVE_EIGEN

extern template class BlockSWIPDG< ALUGrid< 2, 2, simplex, conforming, No_Comm >,
                                   double,
                                   1,
                                   1,
                                   Stuff::LA::ChooseBackend::eigen_sparse >;

#   if HAVE_MPI

extern template class BlockSWIPDG< ALUGrid< 2, 2, simplex, conforming, MPI_Comm >,
                                   double,
                                   1,
                                   1,
                                   Stuff::LA::ChooseBackend::eigen_sparse >;

#   endif // HAVE_MPI
# endif // HAVE_EIGEN
#endif // HAVE_ALUGRID && HAVE_DUNE_FEM && HAVE_DUNE_ISTL

template< Stuff::LA::ChooseBackend la, class G, class R, int r = 1, int p = 1 >
BlockSWIPDG< G, R, r, p, la > make_block_swipdg(const grid::Multiscale::ProviderInterface< G >& grid_provider,
                                                const DSC::Configuration& boundary_info,
                                                const ProblemInterface< typename G::template Codim< 0 >::Entity,
                                                                        typename G::ctype, G::dimension,
                                                                        R, r >& problem,
                                                const std::vector< std::string >& only_these_products = {})
{
  return BlockSWIPDG< G, R, r, p, la >(grid_provider, boundary_info, problem, only_these_products);
}

} // namespace Discretizations
} // namespace LinearElliptic
} // namespace HDD
} // namespace Dune

#endif // DUNE_HDD_LINEARELLIPTIC_DISCRETIZATIONS_BLOCK_SWIPDG_HH


#include <cctbx/boost_python/flex_fwd.h>

#include <smtbx/structure_factors/direct/standard_xray.h>
#include <smtbx/structure_factors/direct/table_based.h>

#include <boost/python/class.hpp>
#include <boost/python/with_custodian_and_ward.hpp>
#include <boost/python/return_internal_reference.hpp>
#include <boost/python/manage_new_object.hpp>

namespace smtbx { namespace structure_factors { namespace direct {
  namespace boost_python {

    template <class wt>
    struct fc_for_one_h_class : boost::python::class_<wt>
    {
      typedef typename wt::complex_type complex_type;
      typedef typename wt::float_type float_type;

      static void evaluate(wt &self, miller::index<> const &h) {
        self.evaluate(h);
      }

      static void linearise(wt &self, miller::index<> const &h) {
        self.linearise(h);
      }

      static complex_type f_calc(wt const &self) {
        return self.f_calc;
      }

      static float_type observable(wt const &self) {
        return self.observable;
      }

      static boost::python::object grad_f_calc(wt const &self) {
        using namespace boost::python;
        return self.is_linearisation() ? object(self.grad_f_calc.array())
                                       : object();
      }

      static boost::python::object grad_observable(wt const &self) {
        using namespace boost::python;
        return self.is_linearisation() ? object(self.grad_observable.array())
                                       : object();
      }

      fc_for_one_h_class(std::string const &name)
        : boost::python::class_<wt>(name.c_str(), boost::python::no_init)
      {
        using namespace boost::python;
        (*this)
          .def("evaluate" , evaluate , args("miller_index"))
          .def("linearise", linearise, args("miller_index"))
          .add_property("f_calc", f_calc)
          .add_property("observable", observable)
          .add_property("grad_f_calc", grad_f_calc)
          .add_property("grad_observable", grad_observable)
          ;
      }
    };

    template <template<typename> class ObservableType>
    struct observable_traits
    {};

    template <>
    struct observable_traits<one_h::modulus_squared>
    {
      static char const *name() { return "modulus_squared"; }
    };

    template <>
    struct observable_traits<one_h::modulus>
    {
      static char const *name() { return "modulus"; }
    };

    template <typename FloatType,
              template<typename> class ObservableType,
              template<typename> class ExpI2PiFunctor>
    struct fc_for_one_h_wrapper
    {
      typedef FloatType float_type;
      typedef ExpI2PiFunctor<float_type> exp_i_2pi_functor;
      typedef one_scatterer_one_h::scatterer_contribution<float_type>
        scatterer_contribution_type;
      static void wrap_custom_trigo(char const *core_name) {
        using namespace boost::python;
        typedef one_h::custom_trigonometry<FloatType,
                                           ObservableType,
                                           ExpI2PiFunctor>
                wt;
        std::string name(core_name);
        name += std::string("_with_custom_trigonometry");
        fc_for_one_h_class<wt>(name)
          .def(init<uctbx::unit_cell const &,
                    sgtbx::space_group const &,
                    af::shared< xray::scatterer<float_type> > const &,
                    exp_i_2pi_functor const &,
                    scatterer_contribution_type *,
                    bool>
               ((arg("unit_cell"),
                 arg("space_group"),
                 arg("scatterers"),
                 arg("exp_i_2pi_functor"),
                 arg("scatter_contribution"),
                 arg("own_scatterer_contribution") = false))
                [with_custodian_and_ward<1, 2,
                 with_custodian_and_ward<1, 3,
                 with_custodian_and_ward<1, 4,
                 with_custodian_and_ward<1, 5,
                 with_custodian_and_ward<1, 6> > > > >()])
          ;
        }

      static void wrap_std_trigo(char const *core_name) {
        using namespace boost::python;
        typedef one_h::std_trigonometry<FloatType,
                                        ObservableType>
                wt;
        std::string name(core_name);
        name += std::string("_with_std_trigonometry");
        fc_for_one_h_class<wt>(name)
          .def(init<uctbx::unit_cell const &,
                    sgtbx::space_group const &,
                    af::shared< xray::scatterer<float_type> > const &,
                    scatterer_contribution_type *,
                    bool>
               ((arg("unit_cell"),
                 arg("space_group"),
                 arg("scatterers"),
                 arg("scatter_contribution"),
                 arg("own_scatterer_contribution") = false))
                [with_custodian_and_ward<1, 2,
                 with_custodian_and_ward<1, 3,
                 with_custodian_and_ward<1, 4,
                 with_custodian_and_ward<1, 5> > > >()])
          ;
      }

      static void wrap() {
        std::string core_name = "f_calc_";
        core_name += observable_traits<ObservableType>::name();
        wrap_custom_trigo(core_name.c_str());
        wrap_std_trigo(core_name.c_str());
      }
    };

    template <typename FloatType>
    struct scatterer_contribution_wrapper {
      typedef one_scatterer_one_h::scatterer_contribution<FloatType> wt;

      static void wrap() {
        using namespace boost::python;
        class_<wt, boost::noncopyable>("scatterer_contribution", no_init)
          .def("get", &wt::get,
            (arg("scatterer_index"),
             arg("h")))
          .def("at_d_star_sq", &wt::at_d_star_sq,
            (arg("d_start_sq")), return_internal_reference<>())
          ;
      }
    };

    template <typename FloatType>
    struct isotropic_scatterer_contribution_wrapper {
      typedef one_scatterer_one_h::isotropic_scatterer_contribution<FloatType>
        wt;
      typedef one_scatterer_one_h::scatterer_contribution<FloatType>
        scatterer_contribution_type;

      static void wrap() {
        using namespace boost::python;
        class_<wt,
          bases<scatterer_contribution_type> >
          ("isotropic_scatterer_contribution", no_init)
          .def(init<af::shared< xray::scatterer<FloatType> > const &,
            xray::scattering_type_registry const &>(
            (arg("scatterers"),
             arg("scattering_type_registry"))))
          .def(init<af::shared< xray::scatterer<FloatType> > const &,
            xray::scattering_type_registry const &,
            uctbx::unit_cell const &,
            cctbx::xray::observations<FloatType> const &>(
            (arg("scatterers"),
              arg("scattering_type_registry"),
              arg("unit_cell"),
              arg("reflections"))))
          ;
      }
    };

    template <typename FloatType>
    struct table_based_wrapper {
      typedef table_based::builder<FloatType> wt;
      typedef one_scatterer_one_h::scatterer_contribution<FloatType>
        scatterer_contribution_type;

      static void wrap() {
        using namespace boost::python;
        class_<wt, boost::noncopyable>("table_based_scatterer_contribution", no_init)
          .def("build", &wt::build,
            (arg("scatterers"),
              arg("file_name"),
              arg("space_group"),
              arg("anomalous_flag")),
            return_value_policy<manage_new_object>())
          .staticmethod("build")
          .def("build_lookup_based_for_tests", &wt::build_lookup_based_for_tests,
          (arg("unit_cell"),
            arg("space_group"),
            arg("scatterers"),
            arg("scattering_type_registry"),
            arg("indices")),
            return_value_policy<manage_new_object>())
          .staticmethod("build_lookup_based_for_tests")
          ;
      }
    };

    void wrap_standard_xray() {
      fc_for_one_h_wrapper<double, one_h::modulus_squared,
                           cctbx::math::cos_sin_table>::wrap();

      fc_for_one_h_wrapper<double, one_h::modulus,
                           cctbx::math::cos_sin_table>::wrap();

      scatterer_contribution_wrapper<double>::wrap();
      isotropic_scatterer_contribution_wrapper<double>::wrap();
      table_based_wrapper<double>::wrap();
    }
  }
}}}


//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Author: Edward Yunkai Cui
// Description: Thie is a customized model plugin class, in which we
//              implemented the low level controllers of each individual
//              model. Those controllers including joint position
//              control, joint speed control, model plane motion control,
//              model plane position and orientation control
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
/*! \file Defines the modelplugin for low level control of modules*/
#ifndef _GAZEBO_MODEL_CONTROLLER_HH_
#define _GAZEBO_MODEL_CONTROLLER_HH_

#include <iostream>
#include <cmath>
#include <boost/bind.hpp>

#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
// Classes for communication between different plugins
#include "collision_message.pb.h"
#include "command_message.pb.h"
// Library for colored log text
#include "ColorLog.hh"
// The head file which contains the joint_plus structure
#include "SmoresJoint.hh"

#define PI 3.1415926
#define EXECUTIONERROR 0.08

using std::vector;
using std::string;
/// shared_ptr of GzString object
typedef const boost::shared_ptr<const gazebo::msgs::GzString> GzStringPtr;
/// shared_ptr of Pose
typedef const boost::shared_ptr<const gazebo::msgs::Pose> PosePtr;
/// shared_ptr of CommandMessage
typedef const boost::shared_ptr<const command_message::msgs::CommandMessage>
    CommandMessagePtr;

namespace gazebo
{
/// The modelplugin that actually controls the model and provide low level apis
/*! This class is a gazebo modelplugin.
    All the low level controllers of SMORES robot are defined here.
*/
class ModuleController : public ModelPlugin
{
 public:
  /// Constructor
  ModuleController();
  /// Destructor
  ~ModuleController();

  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  //+  Useful functions to get model status and other tool functions  +
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  /// Calculates the angular velocity of a revolute joint
  /*!
    \param joint Joint object pointer whose revolution rate needs to be
    calculate
    \param axis_index Index of the axis, for a single joint type,
    it is always 0
    \return Joint speed in rad/s
  */
  double RevolutionSpeedCal(physics::JointPtr joint, const int axis_index);
  /// Gets the coordinates and direction of the current model
  /*!
    \return The global coordinates of the current model,
            which is a math::Pose object
  */
  math::Pose GetModelCentralCoor(void);
  /// Calculates the anlge of the line connected two points in the world frame
  /*!
    \param start_point Coordinates of the Start point
    \param end_point Coordinates of the End point
    \return Angle between 2 points in rad in the global frame
            math::Angle object
  */
  math::Angle AngleCalculation2Points(math::Vector2d start_point,
      math::Vector2d end_point);
  /// Apply a complementary filter a raw sensor value
  /*!
    \param filtering_value The value need to be filtered
    \param complement_filter_par Complementry filter factor, defined in the
            following equation:
            complement_filter_par*value_history + (1 - complement_filter_par)
            *filtering_value
    \param *value_history The pointer points to the prrior filtering value.
    /return The filered value
  */
  double ComplementaryFilter(double filtering_value,
      double complement_filter_par, double *value_history);
  /// Applies a complementary filter with a default factor of 0.9
  /*!
    \param filtering_value The value need to be filtered
    \param *value_history The pointer points to the prrior filtering value.
    /return The result the filered value
  */
  double ComplementaryFilter(double filtering_value, double *value_history);
  /// Returns the SmoresJoint structure according to the node ID
  /*!
    \param node_ID ID of node
    \return Corresponding SmoresJoint object
  */
  SmoresJoint & GetJointPlus(int node_ID);
  /// Returns the index of joint axis according to the node ID
  /*!
    \param node_ID ID of node
    \return Index of the corresponding axis
  */
  int GetJointAxis(int node_ID);
  /// Returns the angle of the specified joint
  /*!
    \param current_joint Current joint intersted
    \param rot_axis Axis rotated along
    \return Joint angle position in a math::Angle object
  */
  math::Angle GetJointAngle(physics::JointPtr current_joint, int rot_axis);

 private:
  /// Callback function when model plugin has been loaded
  /*! Including register GaitMessage receiving callback
    \param _parent Current WorldPtr object
    \param _sdf ElementPtr object of the current world
  */
  void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
  /// Callback when receive welcome informaation from world plugin
  /*! TODO: currently there is no reaction when receiving welcome information
    \param msg GzStringPtr object of the incoming message
  */
  void WelcomInfoProcessor(GzStringPtr &msg);
  /// Plugin initialization function
  /*!
    \param parent_model Pointer of the model object
  */
  void SystemInitialization(physics::ModelPtr parent_model);
  /// Main control loop function, running in 1000Hz
  void OnSystemRunning(const common::UpdateInfo & /*_info*/);
  /// Collision Topics Publisher and Subscriber initialization
  /*! Used by magnetic connection */
  void CollisionPubAndSubInitialization(void);
  /// Callback that decode collision information and populate it to worldplugin
  /*! Used by magnetic connection */
  void CollisionReceivingCallback(GzStringPtr &msg);
  /// Callback that decodes commands from worldplugin
  /*!
    \param msg CommandMessagePtr object of the incoming message
  */
  void CommandDecoding(CommandMessagePtr &msg);

  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  //+            Low level model control functions                    +
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  /// This function force to set joint to a specific angle
  /*!
    Only used in simulation
    \param current_joint The joint interested
    \param rot_axis  Desired rotation axis
    \param position Desired position
  */
  void SetJointAngleForce(physics::JointPtr current_joint, int rot_axis,
      double position);
  /// Applies a PID controller to control the joint position
  /*!
    \param angle_desired_radian Desired angle to achieve in rad
    \param desire_speed Desired Speed in percentage of the maximum speed
    \param *current_joint SmoresJoint object of the current joint
  */
  void JointPIDController(double angle_desired_radian,
      double desire_speed, SmoresJoint *current_joint);
  /// Applies a PID controller to control the joint position with a fixed speed
  /*!
    Default speed 80% of the maximum speed.
    \param angle_desired_radian Desired angle to achieve in rad
    \param *current_joint Current joint
  */
  void JointPIDController(double angle_desired_radian, SmoresJoint *current_joint);
  /// Update all SmoresJoint object in the main loop
  void JointAngleUpdateInJointPlus(void);
  /// Aet the rotation rate of the joint
   /*!
    The unit of this speed is rad/s
    \param CurrentJoint Current Joint
    \param RotAxis The axis desired to rotate along
    \param SpeedDesired Desired Speed in rad/s
  */
  void SetJointSpeed(physics::JointPtr CurrentJoint, int RotAxis,
      double SpeedDesired);

  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  //+            Command Execution Tracking Functions                 +
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  /// Track the status of all joint angles
  /*! Send feedback message to worldplugin when joint position acheived*/
  void JointAngleTracking(void);
  /// Track the position of the module
  /*! Send feedback message to worldplugin when model position acheived*/
  void PositionTracking(void);

  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  //+          Functions that control the model's planar motion       +
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  /// Control the orientation of the planar motion of SMORES on the ground
  /*!
    /param desired_angle Desired angle
    /param current_angle Current angle
    /param current_speed Current speed
  */
  void AnglePIDController(math::Angle desired_angle, math::Angle current_angle,
      math::Vector2d current_speed);
  /// Control the model drive to a specific point on the ground plane
  /*!
    \param DesiredPoint Desired point to drive to
    \param DesiredOrientation Desired Orientation to drive to
  */
  void Move2Point(math::Vector2d DesiredPoint, math::Angle DesiredOrientation);
 public:
  /// Current Model Pointer
  physics::ModelPtr model;
  /// Publisher of the collision information
  transport::PublisherPtr collisionInfoToServer;
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  //+                       Model Pramaters                           +
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  /// Default Joint Angle PID controller parameter
  /*!
    First digit is Kp,
    Second digit is Ki and
    Third digit is Kd
  */
  math::Vector3 jointAngleKPID;
  /// Default Joint Angle PID controller parameter
  /*!
    First digit is Kp,
    Second digit is Ki and
    Third digit is Kd
  */
  math::Vector3 modelAngleKPID;
  /// Maximium rotation rate of each joint, unit: rad/s
  double maxiRotationRate;  // Maximium rotation rate of each joint
  /// Acceleration rate of the planar motion
  double accelerationRate;
  /// Planar motion control threshold
  double planarMotionStopThreshold;
  /// Radius of the wheels
  double wheelRadius;
 private:
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  //+       Pointers of Model and joints and a emhanced struct        +
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  /// Pointer to right wheel joint
  physics::JointPtr jointWR;
  /// Pointer to left wheel joint
  physics::JointPtr jointWL;
  /// Pointer to front wheel joint
  physics::JointPtr jointWF;
  /// Pointer to the central bending joint
  physics::JointPtr jointCB;
  /// Enhanced joint object of right wheel joint
  SmoresJoint jointWRP;
  /// Enhanced joint object of left wheel joint
  SmoresJoint jointWLP;
  /// Enhanced joint object of front wheel joint
  SmoresJoint jointWFP;
  /// Enhanced joint object of central bending joint
  SmoresJoint jointCBP;

  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  //+                     Model plugin events                         +
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  /// The event that will be refreshed in every iteration of the simulation
  event::ConnectionPtr updateConnection;

  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  //+         Communication Publishers and Subscribers                +
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  /// Subscriber of the welcome information
  transport::SubscriberPtr welcomeInfoSub;
  /// Subscriber of the command
  transport::SubscriberPtr commandSub;
  /// Publisher of the command
  transport::PublisherPtr  commandPub;
  /// Subscribers of 4 links
  transport::SubscriberPtr linkCollisonSub[4];

  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  //+                       Useful Buffers                            +
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  /// The vector that is a list of names of all connected models
  vector<string> nameOfConnectedModels;

  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  //+                 Model Execution Variables                       +
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  /// Execution state
  /*!
    0 for waiting mode,
    1 for planar motion mode,
    2 for joint control mode,
    3 for direct control mode
  */
  int executionState;
  /// Desired joint angles
  double jointAngleShouldBe[4];
  /// Desired joint speeds
  double jointSpeeds[4];
  /// Desired robot position when controled to move to a position
  math::Pose targetPosition;
  /// Left wheel speed
  double lftWheelSpeed;
  /// Right wheel speed
  double rgtWheelSpeed;
  /// Execution status
  bool startExecution;
  // TODO: command id should be implemented in the future
}; // class ModuleController
} // namespace gazebo
#endif


/*    Copyright (c) 2010-2018, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#include <boost/bind.hpp>

#include "Tudat/Astrodynamics/OrbitDetermination/LightTimeCorrectionPartials/lightTimeCorrectionPartial.h"
#include "Tudat/Astrodynamics/OrbitDetermination/LightTimeCorrectionPartials/firstOrderRelativisticLightTimeCorrectionPartial.h"
#include "Tudat/Astrodynamics/OrbitDetermination/LightTimeCorrectionPartials/jw_lighttime_correction_partial.h"

namespace tudat
{

namespace observation_partials
{

//! Function to get the function returning the light-time correction partial for given correction partial and parameter.

std::pair< std::function< LightTimeCorrectionPartial::SingleOneWayRangePartialReturnType(
        const std::vector< Eigen::Vector6d >&, const std::vector< double >& ) >, bool >
getLightTimeParameterPartialFunction(
        const estimatable_parameters::EstimatebleParameterIdentifier parameterId,
        const std::shared_ptr< LightTimeCorrectionPartial > lightTimeCorrectionPartial )
{
    // Declare return type, set second part to 0 (no dependency found).
    std::pair< std::function< LightTimeCorrectionPartial::SingleOneWayRangePartialReturnType(
                const std::vector< Eigen::Vector6d >&, const std::vector< double >& ) >, bool > partialFunction;
    partialFunction.second = 0;

    // Check type of light-time correction
    switch( lightTimeCorrectionPartial->getCorrectionType( ) )
    {
    // Correction type of 1st-order relativistic
    case observation_models::first_order_relativistic:
    {
        // Check consistency of input.
        std::shared_ptr< FirstOrderRelativisticLightTimeCorrectionPartial > currentLightTimeCorrectorPartial =
                std::dynamic_pointer_cast< FirstOrderRelativisticLightTimeCorrectionPartial >( lightTimeCorrectionPartial );
        if( currentLightTimeCorrectorPartial == nullptr )
        {
            std::string errorMessage = "Error when getting light time correction partial function, type " +
                    std::to_string( lightTimeCorrectionPartial->getCorrectionType( ) ) +
                       "is inconsistent.";
            throw std::runtime_error( errorMessage );
        }

        // Set partial of gravitational parameter
        if( parameterId.first == estimatable_parameters::gravitational_parameter )
        {
            // Retrieve function from FirstOrderRelativisticLightTimeCorrectionPartial if correction depends on
            // body associated with parameter.
            std::vector< std::string > perturbingBodies = currentLightTimeCorrectorPartial->getPerturbingBodies( );
            std::vector< std::string >::iterator findIterator = std::find(
                        perturbingBodies.begin( ), perturbingBodies.end( ), parameterId.second.first );
            if( findIterator != perturbingBodies.end( ) )
            {
                int bodyIndex = std::distance( perturbingBodies.begin( ),  findIterator );
                partialFunction = std::make_pair(
                            std::bind( &FirstOrderRelativisticLightTimeCorrectionPartial::wrtBodyGravitationalParameter,
                                         currentLightTimeCorrectorPartial, std::placeholders::_1, std::placeholders::_2, bodyIndex ), 1 );
            }
        }
        else if( parameterId.first == estimatable_parameters::ppn_parameter_gamma )
        {
            partialFunction = std::make_pair(
                        std::bind( &FirstOrderRelativisticLightTimeCorrectionPartial::wrtPpnParameterGamma,
                                     currentLightTimeCorrectorPartial, std::placeholders::_1, std::placeholders::_2 ), 1 );
        }
        break;
    }

    case observation_models::jw_lighttime:
    {
        // Check consistency of input.
        std::shared_ptr< jw_lighttime_correction_partial > currentLightTimeCorrectorPartial =
                std::dynamic_pointer_cast< jw_lighttime_correction_partial >( lightTimeCorrectionPartial );
        if( currentLightTimeCorrectorPartial == nullptr )
        {
            std::string errorMessage = "Error when getting light time correction partial function, type " +
                    std::to_string( lightTimeCorrectionPartial->getCorrectionType( ) ) +
                       "is inconsistent with expected type, jw_lighttime.";
            throw std::runtime_error( errorMessage );
        }

        // Set partial of gravitational parameter
        if( parameterId.first == estimatable_parameters::gravitational_parameter )
        {
            // Retrieve function from jw_lighttime_correction_partial if correction depends on
            // body associated with parameter.
            std::vector< std::string > perturbingBodies = currentLightTimeCorrectorPartial->getPerturbingBodies( );
            std::vector< std::string >::iterator findIterator = std::find(
                        perturbingBodies.begin( ), perturbingBodies.end( ), parameterId.second.first );
            if( findIterator != perturbingBodies.end( ) )
            {
                int bodyIndex = std::distance( perturbingBodies.begin( ),  findIterator );
                partialFunction = std::make_pair(
                            std::bind( &jw_lighttime_correction_partial::wrtBodyGravitationalParameter,
                                         currentLightTimeCorrectorPartial, std::placeholders::_1, std::placeholders::_2, bodyIndex ), 1 );
            }
        }
        else if( parameterId.first == estimatable_parameters::ppn_parameter_gamma )
        {
            partialFunction = std::make_pair(
                        std::bind( &jw_lighttime_correction_partial::wrtPpnParameterGamma,
                                     currentLightTimeCorrectorPartial, std::placeholders::_1, std::placeholders::_2 ), 1 );
        }
        break;
    }

    default:
        std::string errorMessage = "Error, light time correction type " + std::to_string(
                    lightTimeCorrectionPartial->getCorrectionType( ) ) + "not found when creating partial ";
        throw std::runtime_error( errorMessage );
    }

    return partialFunction;
}

}

}


#include <math.h>
#include <Eigen/KroneckerProduct>
#include "Core/Utilities/Tools/MatrixDecomposition.h"
USING_QPANDA

static void upper_partition(int order, MatrixOperator &entries)
{
    auto index = (int)std::log2(entries.size() + 1) - (int)std::log2(order) - 1;

    for (auto cdx = 0; cdx < order - 1; ++cdx)
    {
        for (auto rdx = 0; rdx < order - cdx - 1; ++rdx)
        {
            auto Entry = entries[cdx][rdx];

            Entry.first += order;
            Entry.second[index] = MatrixUnit::SINGLE_P1;

            entries[cdx + order].emplace_back(Entry);
        }
    }
}

static bool entry_requirement(const MatrixSequence& units, int udx, int cdx)
{
    int M = 0;
    while (cdx)
    {
        cdx >>= 1;
        M += cdx ? 1 : 0;
    }

    //if 1 ≤ j ≤ m and cj = lj' = 1 , return true
    auto unit = units[units.size() - udx - 1];
    return udx >= 1
        && udx <= M
        && cdx == 1
        && unit == MatrixUnit::SINGLE_P1;
}

static bool steps_requirement(const MatrixSequence& units, int udx, int cdx)
{
    int M = 0;
    while (cdx)
    {
        cdx >>= 1;
        M += cdx ? 1 : 0;
    }

    //if j = n and none of cn...cm+1 is 1 , return true
    if (udx != units.size() - 1)
    {
        return false;
    }
    else
    {
        auto iter = std::find(units.begin(), units.end() - M, MatrixUnit::SINGLE_P1);
        return (units.end() - M) == iter;
    }
}

static void under_partition(int order, MatrixOperator& entries)
{
    auto qubits =(int)std::log2(entries.size() + 1);

    for (auto cdx = 1; cdx < order; ++cdx)
    {
        if (cdx & 1)
        {
            for (auto rdx = 0; rdx < order; ++rdx)
            {
                auto entry = entries[0][rdx + order - 1].first ^ cdx;
                entries[cdx].emplace_back(make_pair(entry, entries[cdx - 1][rdx + order - 1].second));
            }

            auto &units = entries[cdx].back().second;
            for (auto idx = 0; idx < (int)std::log2(order); ++idx)
            {
                if ((cdx >> idx) & 1)
                {
                    units[qubits - idx - 1] = MatrixUnit::SINGLE_P1;
                }
                else
                {
                    units[qubits - idx - 1] = MatrixUnit::SINGLE_I2;
                }
            }
        }
        else
        {
            for (auto rdx = 0; rdx < order; ++rdx)
            {
                auto units = entries[0][rdx + order].second;
                auto entry = entries[0][rdx + order].first ^ cdx;

                for (auto udx = 0; udx < qubits; ++udx)
                {
                    bool steps_accord = entry_requirement(units, udx, cdx);
                    bool entry_accord = entry_requirement(units, udx, cdx);

                    if (steps_accord)
                    {
                        units[udx] = MatrixUnit::SINGLE_P1;
                    }
                    else if (entry_accord)
                    {
                        units[udx] = MatrixUnit::SINGLE_P0;
                    }
                    else
                    {
                    }
                }

                entries[cdx].emplace_back(make_pair(entry, units));
            }

            auto &units = entries[0].back().second;
            for (auto idx = 0; idx < (int)std::log2(order); ++idx)
            {
                if ((cdx >> idx) & 1)
                {
                    units[qubits - idx - 1] = MatrixUnit::SINGLE_P1;
                }
                else
                {
                    units[qubits - idx - 1] = MatrixUnit::SINGLE_I2;
                }
            }
        }
    }
}

static void voluation(Eigen::MatrixXcf& matrix, MatrixOperator& entries)
{
    auto qubits = (int)std::log2(matrix.rows());

    MatrixSequence Cns(qubits, MatrixUnit::SINGLE_I2);
    Cns.back() = MatrixUnit::SINGLE_V2;
    entries.front().emplace_back(make_pair(1, Cns));

    ColumnOperator& column = entries.front();
    for (auto idx = 1; idx < qubits; ++idx)
    {
        size_t path = 1ull << idx;
        for (auto opt = 0; opt < (1 << idx) - 1; ++opt)
        {
            auto units = column[opt].second;

            // 1 : none of cn−1, . . . , c1 equals 1
            // * : otherwise
            auto iter = std::find(units.begin() + 1, units.end(), MatrixUnit::SINGLE_P1);
            if (units.end() == iter)
            {
                units.front() = MatrixUnit::SINGLE_P1;;
            }
            else
            {
                units.front() = MatrixUnit::SINGLE_I2;;
            }

            column.emplace_back(make_pair(opt + path + 1, units));
        }

        MatrixSequence Lns(qubits, MatrixUnit::SINGLE_I2);
        Lns[qubits - idx - 1] = MatrixUnit::SINGLE_V2;

        column.emplace_back(make_pair((1ull << idx), Lns));
    }
}

static void controller(MatrixSequence &sequence, const Eigen::Matrix2cf U2, Eigen::MatrixXcf &matrix)
{
    Eigen::Matrix2cf P0;
    Eigen::Matrix2cf P1;
    Eigen::Matrix2cf I2;

    P0 << Eigen::scomplex(1, 0), Eigen::scomplex(0, 0),
          Eigen::scomplex(0, 0), Eigen::scomplex(0, 0);
    P1 << Eigen::scomplex(0, 0), Eigen::scomplex(0, 0),
          Eigen::scomplex(0, 0), Eigen::scomplex(1, 0);
    I2 << Eigen::scomplex(1, 0), Eigen::scomplex(0, 0),
          Eigen::scomplex(0, 0), Eigen::scomplex(1, 0);

    std::map<MatrixUnit, std::function<Eigen::Matrix2cf()>> mapping =
    {
        { MatrixUnit::SINGLE_P0, [&]() {return P0; } },
        { MatrixUnit::SINGLE_P1, [&]() {return P1; } },
        { MatrixUnit::SINGLE_I2, [&]() {return I2; } },
        { MatrixUnit::SINGLE_V2, [&]() {return U2 - I2; } }
    };

    auto order = sequence.size();
    Eigen::MatrixXcf Un = Eigen::MatrixXcf::Identity(1, 1);
    Eigen::MatrixXcf In = Eigen::MatrixXcf::Identity(1ull << order, 1ull << order);

    for (const auto &val : sequence)
    {
        Eigen::Matrix2cf M2 = mapping.find(val)->second();
        Un = Eigen::kroneckerProduct(Un, M2).eval();
    }

    matrix = In + Un;
}

static void operation(Eigen::MatrixXcf& matrix, MatrixOperator& entries)
{
    for (auto cdx = 0; cdx < entries.size(); ++cdx)
    {
        for (auto idx = 0; idx < entries[cdx].size(); ++idx)
        {
            auto rdx = entries[cdx][idx].first;
            auto opt = entries[cdx][idx].second;

            if (Eigen::scomplex(0, 0) == matrix(rdx, cdx))
            {
                continue;;
            }
            else
            {
                auto order = opt.size();

                Eigen::Matrix2cf C2; /*placeholder*/
                C2 << Eigen::scomplex(0, 1), Eigen::scomplex(0, 1),
                      Eigen::scomplex(0, 1), Eigen::scomplex(0, 1);

                Eigen::MatrixXcf Cn;
                controller(opt, C2, Cn);

                Qnum indices(2);
                for (Eigen::Index index = 0; index < (1ull << order); ++index)
                {
                    if (Cn(rdx, index) != Eigen::scomplex(0, 0))
                    {
                        indices[index == rdx] = index;
                    }
                }

                Eigen::scomplex C0 = matrix(indices[0], cdx);
                Eigen::scomplex C1 = matrix(indices[1], cdx);

                Eigen::scomplex V11 = std::conj(C0) / std::sqrt(std::norm(C0) + std::norm(C1));
                Eigen::scomplex V12 = std::conj(C1) / std::sqrt(std::norm(C0) + std::norm(C1));
                Eigen::scomplex V21 =  C1 / std::sqrt(std::norm(C0) + std::norm(C1));
                Eigen::scomplex V22 = -C0 / std::sqrt(std::norm(C0) + std::norm(C1));

                Eigen::Matrix2cf V2;
                V2 << V11 , V12 , V21 , V22;

                Eigen::MatrixXcf Un;
                controller(opt, V2, Un);

                matrix = Un * matrix;
            }
        }
    }
}

static void partition(Eigen::MatrixXcf& sub_matrix, MatrixOperator &entries)
{
    Eigen::Index order = sub_matrix.rows();
    if (1 == order)
    {
        return;
    }
    else
    {
        Eigen::MatrixXcf corner = sub_matrix.topLeftCorner(order / 2, order / 2);

        partition(corner, entries);

        upper_partition(order / 2, entries);
        under_partition(order / 2, entries);
    }
}

static void general_scheme(Eigen::MatrixXcf& matrix)
{
    MatrixOperator entries;
    for (auto idx = 1; idx < matrix.cols(); ++idx)
    {
        ColumnOperator Co;
        entries.emplace_back(Co);
    }

    voluation(matrix, entries);
    partition(matrix, entries);
    operation(matrix, entries);
}

void QMatrix::decompose()
{
    auto order = (int)std::log2(this->size());

    Eigen::MatrixXcf matrix = Eigen::MatrixXcf::Zero(order, order);
    for (auto rdx = 0; rdx < order; ++rdx)
    {
        for (auto cdx = 0; cdx < order; ++cdx)
        {
            matrix(rdx, cdx) = this->at(rdx*order + cdx);
        }
    }

    if (!matrix.isUnitary(1e-3))
    {
        QCERR("Non-unitary matrix");
        return;
    }

    general_scheme(matrix);
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>
#include <boost/numeric/itl/krylov/async_executor.hpp>

#include <chrono>
#include <thread>

int main()
{
    mtl::vampir_trace<9999> tracer;
    // For a more realistic example set size to 1000 or larger
    const int size = 300, N = size * size;

    typedef mtl::compressed2D<double>  matrix_type;
    mtl::compressed2D<double>          A(N, N);
    laplacian_setup(A, size, size);

    itl::pc::identity<matrix_type>     P(A);

    mtl::dense_vector<double> x(N, 1.0), b(N);

    b = A * x;
    x= 0;
    const int iter_limit= 100000;
    itl::cyclic_iteration<double> iter(b, iter_limit, 0.0, 0.0, iter_limit);

    auto my_cg_solver= itl::make_cg_solver(A, P);
    itl::async_executor<decltype(my_cg_solver)> async_exec(my_cg_solver);

    // auto async_ex= itl::make_async_executor(itl::make_cg_solver(A, P));
    async_exec.start_solve(x, b, iter);

    std::this_thread::sleep_for( std::chrono::milliseconds( 50 ) );
    async_exec.interrupt();

    if (iter.iterations() == iter_limit)
        throw "Executor was not interrupted!\n";
    else
        std::cout << "Yeah, it was interrupted!!!\n";

    return 0;
}


/**
 * \file
 * \copyright
 * Copyright (c) 2012-2019, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 *
 */

#include "RichardsFlowMaterialProperties.h"

#include <logog/include/logog.hpp>
#include <boost/math/special_functions/pow.hpp>

#include "MaterialLib/Fluid/FluidProperty.h"
#include "MaterialLib/PorousMedium/Porosity/Porosity.h"
#include "MaterialLib/PorousMedium/Storage/Storage.h"
#include "MaterialLib/PorousMedium/UnsaturatedProperty/CapillaryPressure/CapillaryPressureSaturation.h"
#include "MaterialLib/PorousMedium/UnsaturatedProperty/CapillaryPressure/CreateCapillaryPressureModel.h"
#include "MaterialLib/PorousMedium/UnsaturatedProperty/RelativePermeability/CreateRelativePermeabilityModel.h"
#include "MaterialLib/PorousMedium/UnsaturatedProperty/RelativePermeability/RelativePermeability.h"
#include "MeshLib/Mesh.h"
#include "MeshLib/PropertyVector.h"
#include "ParameterLib/Parameter.h"
#include "ParameterLib/SpatialPosition.h"

namespace ProcessLib
{
namespace RichardsFlow
{
RichardsFlowMaterialProperties::RichardsFlowMaterialProperties(
    MeshLib::PropertyVector<int> const* const material_ids,
    std::unique_ptr<MaterialLib::Fluid::FluidProperties>&& fluid_properties,
    std::vector<std::unique_ptr<MaterialLib::PorousMedium::Permeability>>&&
        intrinsic_permeability_models,
    std::vector<std::unique_ptr<MaterialLib::PorousMedium::Porosity>>&&
        porosity_models,
    std::vector<std::unique_ptr<MaterialLib::PorousMedium::Storage>>&&
        storage_models,
    std::vector<std::unique_ptr<
        MaterialLib::PorousMedium::CapillaryPressureSaturation>>&&
        capillary_pressure_models,
    std::vector<
        std::unique_ptr<MaterialLib::PorousMedium::RelativePermeability>>&&
        relative_permeability_models)
    : _material_ids(material_ids),
      _fluid_properties(std::move(fluid_properties)),
      _intrinsic_permeability_models(std::move(intrinsic_permeability_models)),
      _porosity_models(std::move(porosity_models)),
      _storage_models(std::move(storage_models)),
      _capillary_pressure_models(std::move(capillary_pressure_models)),
      _relative_permeability_models(std::move(relative_permeability_models))
{
    DBUG("Create material properties for Richards flow.");
}

int RichardsFlowMaterialProperties::getMaterialID(const std::size_t element_id)
{
    if (!_material_ids)
    {
        return 0;
    }

    assert(element_id < _material_ids->size());
    return (*_material_ids)[element_id];
}

double RichardsFlowMaterialProperties::getFluidDensity(const double p,
                                                       const double T) const
{
    ArrayType vars;
    vars[static_cast<int>(MaterialLib::Fluid::PropertyVariableType::T)] = T;
    vars[static_cast<int>(MaterialLib::Fluid::PropertyVariableType::p)] = p;
    return _fluid_properties->getValue(
        MaterialLib::Fluid::FluidPropertyType::Density, vars);
}

double RichardsFlowMaterialProperties::getFluidViscosity(const double p,
                                                         const double T) const
{
    ArrayType vars;
    vars[static_cast<int>(MaterialLib::Fluid::PropertyVariableType::T)] = T;
    vars[static_cast<int>(MaterialLib::Fluid::PropertyVariableType::p)] = p;
    return _fluid_properties->getValue(
        MaterialLib::Fluid::FluidPropertyType::Viscosity, vars);
}

Eigen::MatrixXd RichardsFlowMaterialProperties::getPermeability(
    const int material_id, const double t,
    const ParameterLib::SpatialPosition& pos, const int /*dim*/) const
{
    return _intrinsic_permeability_models[material_id]->getValue(t, pos, 0.0,
                                                                 0.0);
}

double RichardsFlowMaterialProperties::getPorosity(
    const int material_id, const double t,
    const ParameterLib::SpatialPosition& pos, const double /*p*/,
    const double T, const double porosity_variable) const
{
    return _porosity_models[material_id]->getValue(t, pos, porosity_variable,
                                                   T);
}

double RichardsFlowMaterialProperties::getStorage(
    const int material_id, const double /*t*/,
    const ParameterLib::SpatialPosition& /*pos*/, const double /*p*/,
    const double /*T*/, const double storage_variable) const
{
    // \todo getValue() can be extended for non
    // constant storage model
    return _storage_models[material_id]->getValue(storage_variable);
}

double RichardsFlowMaterialProperties::getRelativePermeability(
    const double /*t*/, const ParameterLib::SpatialPosition& /*pos*/,
    const double /*p*/, const double /*T*/, const double saturation) const
{
    return _relative_permeability_models[0]->getValue(saturation);
}

double RichardsFlowMaterialProperties::getRelativePermeabilityDerivative(
    const double /*t*/, const ParameterLib::SpatialPosition& /*pos*/,
    const double /*p*/, const double /*T*/, const double saturation) const
{
    return _relative_permeability_models[0]->getdValue(saturation);
}

double RichardsFlowMaterialProperties::getSaturation(
    const int material_id, const double /*t*/,
    const ParameterLib::SpatialPosition& /*pos*/, const double /*p*/,
    const double /*T*/, const double pc) const
{
    return _capillary_pressure_models[material_id]->getSaturation(pc);
}

double RichardsFlowMaterialProperties::getSaturationDerivative(
    const int material_id, const double /*t*/,
    const ParameterLib::SpatialPosition& /*pos*/, const double /*p*/,
    const double /*T*/, const double saturation) const
{
    const double dpcdsw =
        _capillary_pressure_models[material_id]->getdPcdS(saturation);
    return 1 / dpcdsw;
}

double RichardsFlowMaterialProperties::getSaturationDerivative2(
    const int material_id, const double /*t*/,
    const ParameterLib::SpatialPosition& /*pos*/, const double /*p*/,
    const double /*T*/, const double saturation) const
{
    const double dpcdsw =
        _capillary_pressure_models[material_id]->getdPcdS(saturation);
    const double d2pcdsw2 =
        _capillary_pressure_models[material_id]->getd2PcdS2(saturation);
    return -d2pcdsw2 / boost::math::pow<3>(dpcdsw);
}
}  // namespace RichardsFlow
}  // namespace ProcessLib


/*
 * ============================================================================
 *       Filename:  RecoTauMVATransform.cc
 *
 *    Description:  Transform TaNC output according to decay mode.
 *        Created:  10/22/2010 15:36:12
 *         Author:  Evan K. Friis (UC Davis), evan.klose.friis@cern.ch
 * ============================================================================
 */

#include <boost/ptr_container/ptr_map.hpp>
#include <memory>

#include "RecoTauTag/RecoTau/interface/TauDiscriminationProducerBase.h"
#include "RecoTauTag/RecoTau/interface/PFTauDecayModeTools.h"

#include "DataFormats/TauReco/interface/PFTau.h"
#include "DataFormats/TauReco/interface/PFTauFwd.h"

#include "FWCore/MessageLogger/interface/MessageLogger.h"
#include "FWCore/ParameterSet/interface/ParameterSet.h"

#include <FWCore/ParameterSet/interface/ConfigurationDescriptions.h>
#include <FWCore/ParameterSet/interface/ParameterSetDescription.h>

#include "TGraph.h"

namespace {

// Build the transformation function from the PSet format
std::unique_ptr<TGraph> buildTransform(const edm::ParameterSet &pset) {
  double min = pset.getParameter<double>("min");
  double max = pset.getParameter<double>("max");
  const std::vector<double> &values =
      pset.getParameter<std::vector<double> >("transform");
  double stepSize = (max - min)/(values.size()-1);
  std::unique_ptr<TGraph> output(new TGraph(values.size()));
  for (size_t step = 0; step < values.size(); ++step) {
    double x = min + step*stepSize;
    output->SetPoint(step, x, values[step]);
  }
  return output;
}

}

class RecoTauMVATransform : public PFTauDiscriminationProducerBase {
  public:
    explicit RecoTauMVATransform(const edm::ParameterSet& pset);
    ~RecoTauMVATransform() override {}

    void beginEvent(const edm::Event&, const edm::EventSetup&) override;
    double discriminate(const reco::PFTauRef&) const override;

    static void fillDescriptions(edm::ConfigurationDescriptions & descriptions);
  private:
    // Map a decay mode to a transformation
    typedef boost::ptr_map<reco::PFTau::hadronicDecayMode, TGraph> TransformMap;
    TransformMap transforms_;
    // Points to raw TaNC output
    edm::InputTag input_;
    edm::Handle<reco::PFTauDiscriminator> disc_;
};

RecoTauMVATransform::RecoTauMVATransform(const edm::ParameterSet& pset)
  :PFTauDiscriminationProducerBase(pset) {
  input_ = pset.getParameter<edm::InputTag>("toTransform");
  typedef std::vector<edm::ParameterSet> VPSet;
  const VPSet& transforms = pset.getParameter<VPSet>("transforms");
  prediscriminantFailValue_ = -2.0;
  for(auto const& transform : transforms) {
    unsigned int nCharged = transform.getParameter<unsigned int>("nCharged");
    unsigned int nPiZeros = transform.getParameter<unsigned int>("nPiZeros");
    // Get the transform
    const edm::ParameterSet &transformImpl =
        transform.getParameter<edm::ParameterSet>("transform");
    // Get the acutal decay mode
    reco::PFTau::hadronicDecayMode decayMode =
        reco::tau::translateDecayMode(nCharged, nPiZeros);

    if (!transforms_.count(decayMode)) {
      // Add it
      transforms_.insert(decayMode, buildTransform(transformImpl).get());
    } else {
      edm::LogError("DecayModeNotUnique") << "The tau decay mode with "
        "nCharged/nPiZero = " << nCharged << "/" << nPiZeros <<
        " dm: " << decayMode <<
        " is associated to multiple MVA transforms, "
        "the second instantiation is being ignored!!!";
    }
  }
}

// Update our discriminator handle at the begninng of the event
void
RecoTauMVATransform::beginEvent(const edm::Event& evt, const edm::EventSetup&) {
  evt.getByLabel(input_, disc_);
}

double RecoTauMVATransform::discriminate(const reco::PFTauRef& tau) const {
  // Check if we support this decay mode:
  TransformMap::const_iterator transformIter =
      transforms_.find(tau->decayMode());
  // Unsupported DM
  if (transformIter == transforms_.end())
    return prediscriminantFailValue_;
  const TGraph *transform = transformIter->second;
  // Get the discriminator output to transform
  double value = (*disc_)[tau];
  double result = transform->Eval(value);
  return result;
}

void
RecoTauMVATransform::fillDescriptions(edm::ConfigurationDescriptions& descriptions) {
  // recoTauMVATransform
  edm::ParameterSetDescription desc;

  desc.add<edm::InputTag>("toTransform");

  edm::ParameterSetDescription vpsd_transforms;
  vpsd_transforms.add<uint32_t>("nCharged");
  vpsd_transforms.add<uint32_t>("nPiZeros");

  edm::ParameterSetDescription pset_transform;
  pset_transform.add<double>("min");
  pset_transform.add<double>("max");
  pset_transform.add<std::vector<double>>("transform");
  vpsd_transforms.add<edm::ParameterSetDescription>("transform", pset_transform);

  //            name          description       no defaults items
  desc.addVPSet("transforms", vpsd_transforms);

  fillProducerDescriptions(desc); // inherited from the base

  descriptions.add("recoTauMVATransform", desc);
}

#include "FWCore/Framework/interface/MakerMacros.h"
DEFINE_FWK_MODULE(RecoTauMVATransform);