#include "modules/bio_base/dna_base_set.h"
#include "modules/io/utils.h"

#include <map>
#include <algorithm>
#include <boost/bind.hpp>

// Private loookup table class, uses static construction to fill table
// Implements IUPAC Nucleic Acid Code
// As describe here: http://www.dna.affrc.go.jp/misc/MPsrch/InfoIUPAC.html
// Note: we ignore the 'U' case, since this is for DNA

class code_table
{
public:
	code_table()
	{
		dna_base_set a = dna_base('A');
		dna_base_set c = dna_base('C');
		dna_base_set g = dna_base('G');
		dna_base_set t = dna_base('T');
		add('-', dna_base_set());
		add('A', a);
		add('C', c);
		add('G', g);
		add('T', t);
		add('M', a | c);
		add('R', a | g);
		add('W', a | t);
		add('S', c | g);
		add('Y', c | t);
		add('K', g | t);
		add('V', a | c | g);
		add('H', a | c | t);
		add('D', a | g | t);
		add('B', c | g | t);
		add('N', a | c | g | t);
	}
	char lookup(const dna_base_set& b) const { return m_reverse.find(b)->second; }
	const dna_base_set& lookup(char c) const { return m_forward.find(c)->second; }
private:
	void add(char c, dna_base_set b)
	{
		m_forward.insert(std::make_pair(c,b));
		m_reverse.insert(std::make_pair(b,c));
	}
	std::map<char, dna_base_set> m_forward;
	std::map<dna_base_set, char> m_reverse;
};

static code_table the_table;  // Global for static construction

dna_base_set::dna_base_set(char c)
	: m_set(the_table.lookup(c).m_set)
{}

char dna_base_set::as_code()
{
	return the_table.lookup(*this);
}

void print_a_base(const dna_base& base, std::string& result, const std::string& separator)
{
	result = printstring("%s%c%s", result.c_str(), (char)base, separator.c_str());
}

std::string dna_base_set::as_list(char sep)
{
	std::string str_sep;
	if (sep) str_sep.push_back(sep);
	std::string result;
	dna_base_set::reduce(boost::bind(print_a_base, _1, _2, str_sep), *this, result);
	if (sep && result.size()) return result.substr(0, result.size() - 1);
	return result;
}

//                                             ABCD..GH..K.MN...RST.VW.Y.
static constexpr char k_iupac_complements[] = "TVGH..CD..M.KN...YSA.BW.R.";
static constexpr int k_num_iupac_complements =
    sizeof(k_iupac_complements) - 1 /* don't count the \0 at end */;

void reverse_complement_iupac_string(std::string& iupac_string) {
  std::reverse(iupac_string.begin(), iupac_string.end());
  std::transform(iupac_string.begin(), iupac_string.end(), iupac_string.begin(),
                 [](const char& c) -> char {
                   int idx = int(c) - 'A';
                   if (idx < 0 || idx >= k_num_iupac_complements) {
                     return c;
                   } else {
                     return k_iupac_complements[idx];
                   }
                 });
}


#define BOOST_TEST_DYN_LINK
#ifdef STAND_ALONE
#   define BOOST_TEST_MODULE Main
#endif
#include <boost/test/unit_test.hpp>
#include <boost/uuid/random_generator.hpp>
#include <boost/uuid/uuid_io.hpp>
#include "ranking_response.h"
#include "explore.h"

const int NUM_ACTIONS = 10;
namespace e = exploration;
namespace r = reinforcement_learning;

BOOST_AUTO_TEST_CASE(basic_explore_test) {
  const float epsilon = 0.2f;
  const auto top_action_id = 0;
  float pdf[NUM_ACTIONS];
  auto scode = e::generate_epsilon_greedy(epsilon, top_action_id, pdf, pdf + NUM_ACTIONS);
  BOOST_CHECK_EQUAL(scode, S_EXPLORATION_OK);
  uint32_t chosen_index;
  scode = e::sample_after_normalizing(7791, pdf, pdf + NUM_ACTIONS, chosen_index);
  BOOST_CHECK_EQUAL(scode, S_EXPLORATION_OK);
}

BOOST_AUTO_TEST_CASE(swap_action_test) {
  r::ranking_response resp;
  std::vector<float> scores = { 1.0f,2.0f,3.0f,4.0f,5.0f };
  for ( auto f : scores )
    resp.push_back((int)f, f);

  e::swap_chosen(std::begin(resp), std::end(resp), 2);

  std::vector<float> expected_scores = { 3.0f,2.0f,1.0f,4.0f,5.0f };
  auto idx = 0;
  for ( auto ap : resp ) {
    BOOST_CHECK_EQUAL(ap.probability, expected_scores[idx]);
    BOOST_CHECK_EQUAL(ap.action_id, (int)expected_scores[idx++]);
  }
}

BOOST_AUTO_TEST_CASE(swap_fail_test) {
  r::ranking_response resp;
  std::vector<float> scores = { 1.0f,2.0f,3.0f,4.0f,5.0f };
  for ( auto f : scores )
    resp.push_back((int)f, f);

  const auto err_code = e::swap_chosen(std::begin(resp), std::end(resp), 5);

  // Check that swap failed
  BOOST_CHECK_EQUAL(err_code, E_EXPLORATION_BAD_RANGE);

  // Check that nothing changed
  auto expected_scores = scores;
  auto idx = 0;
  for ( auto ap : resp ) {
    BOOST_CHECK_EQUAL(ap.probability, expected_scores[idx]);
    BOOST_CHECK_EQUAL(ap.action_id, (int)expected_scores[idx++]);
  }
}


#include <iostream>
#include <fstream>
#include <sstream>
#include <unordered_map>
#include <boost/format.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/progress.hpp>
#include <boost/program_options.hpp>
#include <boost/log/trivial.hpp>
#include <boost/log/utility/setup/console.hpp>
#define LOG(x) BOOST_LOG_TRIVIAL(x)
#include "csvlint.h"
#define USE_OPENMP 1
#include "bigtext.h"

using namespace std;
using namespace boost;
namespace po = boost::program_options;
namespace ba = boost::accumulators;
namespace qi = boost::spirit::qi;
typedef ba::accumulator_set<double, ba::stats<ba::tag::mean, ba::tag::variance>> Acc;

size_t topk = 20;

struct Column {
    vector<csvlint::crange> strings;
    size_t missing;
};

struct Chunk {
    vector<string> lines;
    vector<csvlint::crange> cols;
    vector<Column> data;
    size_t total;
};

size_t total (vector<Chunk> const &chunks) {
    size_t v = 0;
    for (auto const &ch: chunks) {
        v += ch.total;
    }
    return v;
}

size_t good (vector<Chunk> const &chunks) {
    size_t v = 0;
    for (auto const &ch: chunks) {
        v += ch.data.size();
    }
    return v;
}

unsigned c999 (string const &s) {
    unsigned i = 0;
    while (i < s.size() && s[i] == '9') {
        ++i;
    }
    return i;
}

void stat_number (csvlint::Field const &f,  vector<Chunk> const &chunks, string *out) {
    unordered_map<string, size_t> cnts;
    unsigned col = f.column;
    size_t missing = 0;
    unsigned max_len = 0;
    for (auto const &ch: chunks) {
        for (auto e: ch.data[col].strings) {
            ++cnts[string(e.begin(), e.end())];
            unsigned sz = e.end() - e.begin();
            if (sz > max_len) {
                max_len = sz;
            }
        }
        missing += ch.data[col].missing;
    }
    if ((missing > 0) && (cnts.size() <= 1)) return;
    if ((missing == 0) && (cnts.size() <= 2)) return;
    vector<string> special;

    {
        string minus;
        for (auto const &e: cnts) {
            if (e.first.empty()) continue;
            if (e.first[0] == '-') {
                if (minus.size()) goto pass;
                minus = e.first;
            }
        }
        if (minus.size()) {
            special.push_back(minus);
        }
pass:;
    }

    {
        for (auto const &e: cnts) {
            if (e.first.empty()) continue;
            unsigned c = c999(e.first);
            if (c < 2) continue;
            if (c + 1 < max_len) continue;
            if (e.first.size() < max_len) continue;
            special.push_back(e.first);
        }
    }
    if (special.empty() && missing == 0) return;
    ostringstream ss;
    if (missing) {
        ss << f.name << "\tNA" << endl;
    }
    for (string const &s: special) {
        ss << f.name << '\t' << s << endl;
    }
    *out = ss.str();
}

void stat_string (csvlint::Field const &f, vector<Chunk> const &chunks, string *out, char C='S') {
    unordered_map<string, size_t> cnts;
    unsigned col = f.column;
    size_t missing = 0;
    for (auto const &ch: chunks) {
        for (auto e: ch.data[col].strings) {
            ++cnts[string(e.begin(), e.end())];
        }
        missing += ch.data[col].missing;
    }
    if ((missing > 0) && (cnts.size() <= 1)) return;
    if ((missing == 0) && (cnts.size() <= 2)) return;
    int sp = 0;
    ostringstream ss;
    size_t n = cnts[""];
    if (n> 0) {
        ss << f.name << "\t\"\"" << endl;
        ++sp;
    }
    n = cnts["-1"];
    if (n> 0) {
        ss << f.name << "\t\"-1\"" << endl;
        ++sp;
    }
    n = cnts["NA"];
    if (n> 0) {
        ss << f.name << "\t\"NA\"" << endl;
        ++sp;
    }
    n = cnts["N/A"];
    if (n> 0) {
        ss << f.name << "\t\"N/A\"" << endl;
        ++sp;
    }
    if (sp) {
        *out = ss.str();
    }
}

void stat_column (csvlint::Field const &f, vector<Chunk> const &chunks, string *out) {
    if (f.type == csvlint::TYPE_NUMERIC) {
        stat_number(f, chunks, out);
    }
    else if (f.type == csvlint::TYPE_STRING) {
        stat_string(f, chunks, out);
    }
    else BOOST_VERIFY(0);
}

int main (int argc, char *argv[]) {
    unsigned guess_size;

    string input_path;
    string output_path;
    po::options_description desc_visible("General options");
    desc_visible.add_options()
    ("help,h", "produce help message.")
    ("input,I", po::value(&input_path), "input path")
    (",M", po::value(&guess_size)->default_value(10), "")
    ("topk", po::value(&topk)->default_value(topk), "")
    ;
    po::options_description desc("Allowed options");
    desc.add(desc_visible);

    po::positional_options_description p;
    p.add("input", 1);

    po::variables_map vm;
    po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), vm);
    po::notify(vm);

    if (vm.count("help") || vm.count("input") == 0) {
        cout << "Usage: csvlint-stat [OTHER OPTIONS]... <data>" << endl;
        cout << desc_visible << endl;
        return 0;
    }

    boost::log::add_console_log(cerr);

    csvlint::Format fmt;
    fmt.train(input_path, guess_size * 1024 * 1024);

    cerr << "Parsing text..." << endl;
    BigText<Chunk> text(input_path, fmt.data_offset, '\n', fmt.max_line, 10 * 1024*1024);

    for (auto &ch: text) {
        ch.total = 0;
        ch.data.resize(fmt.fields.size());
        for (auto &v: ch.data) {
            v.missing = 0;
        }
    }
    text.lines ([&fmt](char const *begin, char const *end, Chunk *ch, size_t i_in_block) {
        auto &cols = ch->cols;
        ch->lines.emplace_back(begin, end);
        bool r = fmt.parse(csvlint::crange(std::ref(ch->lines.back())), &cols);
        if (r) {
            for (unsigned i = 0; i < fmt.fields.size(); ++i) {
                Column &data = ch->data[i];
                csvlint::crange e = cols[i];
                if (e.missing()) {
                    ++data.missing;
                }
                else {
                    data.strings.push_back(e);
                }
            }
        }
        ++ch->total;
    });

    cerr << total(text) << " total lines." << endl;
    cerr << good(text) << " good lines." << endl;

    vector<string> stats(fmt.fields.size());

    cerr << "Counting numbers..." << endl;
    progress_display progress(fmt.fields.size(), cerr);
#pragma omp parallel for
    for (unsigned i = 0; i < fmt.fields.size(); ++i) {
        stat_column(fmt.fields[i], text, &stats[i]);
#pragma omp critical
        ++progress;
    }

    for (auto &st: stats) {
        if (st.empty()) continue;
        cout << st;
    }

    return 0;
}


#include "dumpGraph.h"

#include <ros/time.h>
#include <pcl/io/pcd_io.h>
#include <pcl/common/transforms.h>

#include <boost/filesystem.hpp>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/slam/BetweenFactor.h>

void dump(const std::string& dump_directory,
  const gtsam::ISAM2& isam,
  const gtsam::Values& isam_current_estimate,
  const std::vector<double>& keyframe_stamps,
  const std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr>& corner_cloud_keyframes,
  const std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr>& surf_cloud_keyframes,
  const std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr>& outlier_cloud_keyframes
) {
  boost::filesystem::create_directories(dump_directory);

  std::vector<gtsam::Pose3> keyframe_poses(isam_current_estimate.size());

  std::ofstream graph_ofs(dump_directory + "/graph.g2o");
  for(const auto& vertex : isam_current_estimate) {
    Eigen::Matrix4d pose = vertex.value.cast<gtsam::Pose3>().matrix();
    keyframe_poses[vertex.key] = vertex.value.cast<gtsam::Pose3>();
    Eigen::Vector3d t = pose.block<3, 1>(0, 3) + Eigen::Vector3d::Random();
    Eigen::Quaterniond q(pose.block<3, 3>(0, 0));
    graph_ofs << "VERTEX_SE3:QUAT " << vertex.key << " " << t.x() << " " << t.y() << " " << t.z() << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << "\n";
  }
  graph_ofs << "FIX 0" << "\n";

  std::cout << "factors:" << isam.getFactorsUnsafe().size() << std::endl;
  for(const auto& factor_: isam.getFactorsUnsafe()) {
    auto between_factor = boost::dynamic_pointer_cast<gtsam::BetweenFactor<gtsam::Pose3>>(factor_);
    if(between_factor) {
      Eigen::Matrix4d relative = between_factor->measured().matrix();
      Eigen::Vector3d t = relative.block<3, 1>(0, 3);
      Eigen::Quaterniond q(relative.block<3, 3>(0, 0));

      graph_ofs << "EDGE_SE3:QUAT " << between_factor->key1() << " " << between_factor->key2();
      graph_ofs << " " << t.x() << " " << t.y() << " " << t.z() << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w();

      Eigen::VectorXd vars = 1.0 / between_factor->noiseModel()->sigmas().array();
      Eigen::MatrixXd inf = vars.asDiagonal();

      for(int i = 0; i < inf.rows(); i++) {
        for(int j=i; j<inf.cols(); j++) {
          graph_ofs << " " << inf(i, j);
        }
      }
      graph_ofs << "\n";
    }
  }

  for(int i = 0; i < corner_cloud_keyframes.size(); i++) {
    std::string keyframe_directory = (boost::format("%s/%06d") % dump_directory % i).str();
    boost::filesystem::create_directories(keyframe_directory);

    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
    *cloud += *corner_cloud_keyframes[i];
    *cloud += *surf_cloud_keyframes[i];
    *cloud += *outlier_cloud_keyframes[i];

    Eigen::Isometry3f camera2lidar = Eigen::AngleAxisf(M_PI / 2.0f, Eigen::Vector3f::UnitX()) * Eigen::AngleAxisf(M_PI / 2.0, Eigen::Vector3f::UnitY()) * Eigen::Isometry3f::Identity();

    pcl::PointCloud<pcl::PointXYZI>::Ptr transformed(new pcl::PointCloud<pcl::PointXYZI>());
    pcl::transformPointCloud(*cloud, *transformed, camera2lidar);

    // cloud = transformPointCloud(cloud, keyframe_poses[i]);
    pcl::io::savePCDFileBinary(keyframe_directory + "/cloud.pcd", *transformed);

    ros::Time stamp(keyframe_stamps[i]);

    std::ofstream data_ofs(keyframe_directory + "/data");
    data_ofs << "stamp " << stamp.sec << " " << stamp.nsec << "\n";
    data_ofs << "estimate\n" << keyframe_poses[i].matrix() << "\n";
    data_ofs << "odom\n" << keyframe_poses[i].matrix() << "\n";
    data_ofs << "accum_distance -1" << "\n";
    data_ofs << "id " << i << "\n";
  }
}


/*
 * gpcxx/canonic/algebras.hpp
 * Date: 2015-09-02
 * Author: Karsten Ahnert (karsten.ahnert@gmx.de)
 * Copyright: Karsten Ahnert
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or
 * copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef GPCXX_CANONIC_ALGEBRAS_HPP_INCLUDED
#define GPCXX_CANONIC_ALGEBRAS_HPP_INCLUDED

#include <gpcxx/canonic/group.hpp>

#include <boost/optional.hpp>

#include <algorithm>
#include <vector>

namespace gpcxx {

template< typename Node >
class algebras
{
public:

    using node_type = Node;
    using group_type = group< node_type >;
    using optional_group = boost::optional< group_type >;

    void add_group( group_type g )
    {
        add_associative_operation( g.operation() );
        if( ! get_group( g.operation() ) )
            m_groups.emplace_back( g );
    }

    void add_abelian_group( group_type g )
    {
        add_group( g );
        add_abelian_magma( g.operation() );
    }

    void add_associative_operation( node_type n )
    {
        if( ! is_associative( n ) )
            m_associative_operations.emplace_back( n );
    }

    void add_abelian_magma( node_type n )
    {
        if( ! is_commutative( n ) )
            m_abelian_magmas.emplace_back( n );
    }

    bool is_commutative( node_type const& node ) const
    {
        return get_commutative_iter( node ) != m_abelian_magmas.end();
    }

    bool is_associative( node_type const& node ) const
    {
        return get_associative_iter( node )  != m_associative_operations.end();
    }

    optional_group get_group( node_type const& node ) const
    {
        auto iter = get_group_iter( node );
        if( iter == m_groups.end() ) return optional_group {};
        else return optional_group { *iter };
    }

    optional_group  get_group_from_inverse_operation( node_type const& node ) const
    {
        auto iter = get_group_from_inverse_operation_iter( node );
        if( iter == m_groups.end() ) return optional_group {};
        else return optional_group { *iter };
    }

    optional_group  get_group_from_inverse_function( node_type const& node ) const
    {
        auto iter = get_group_from_inverse_function_iter( node );
        if( iter == m_groups.end() ) return optional_group {};
        else return optional_group { *iter };
    }

private:

    auto get_commutative_iter( node_type const& node ) const
    {
        auto pred = [&node]( auto const& n ) { return node == n; };
        return std::find_if( m_abelian_magmas.begin() , m_abelian_magmas.end() , pred );
    }

    auto get_associative_iter( node_type const& node ) const
    {
        auto pred = [&node]( auto const& n ) { return node == n; };
        return std::find_if( m_associative_operations.begin() , m_associative_operations.end() , pred );
    }

    auto get_group_iter( node_type const& node ) const
    {
        auto pred = [&node]( auto const& n ) { return node == n.operation(); };
        return std::find_if( m_groups.begin() , m_groups.end() , pred );
    }

    auto get_group_from_inverse_operation_iter( node_type const& node ) const
    {
        auto pred = [&node]( auto const& n ) { return node == n.inverse_operation(); };
        return std::find_if( m_groups.begin() , m_groups.end() , pred );
    }

    auto get_group_from_inverse_function_iter( node_type const& node ) const
    {
        auto pred = [&node]( auto const& n ) { return node == n.inverse_function(); };
        return std::find_if( m_groups.begin() , m_groups.end() , pred );
    }

    std::vector< node_type > m_abelian_magmas;

    // TODO: find a better name, the real algebra name
    std::vector< node_type > m_associative_operations;

    std::vector< group_type > m_groups;
};

} // namespace gpcxx

#endif // GPCXX_CANONIC_ALGEBRAS_HPP_INCLUDED


//  Copyright John Maddock 2009.
//  Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt)

#include <boost/integer/static_log2.hpp> // must be the only #include!

int main()
{
   boost::static_log2_argument_type arg = 0;
   (void)arg;
   boost::static_log2_result_type result = boost::static_log2<30>::value;
   (void)result;
}


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

#ifndef DUNE_GDT_LOCALEVALUATION_SIPDG_HH
#define DUNE_GDT_LOCALEVALUATION_SIPDG_HH

#include <tuple>

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

#include <dune/common/dynmatrix.hh>

#include <dune/stuff/common/color.hh>
#ifndef NDEBUG
# ifndef DUNE_GDT_LOCALEVALUATION_SIPDG_DISABLE_WARNINGS
#   include <dune/stuff/common/timedlogging.hh>
# endif
#endif
#include <dune/stuff/common/type_utils.hh>
#include <dune/stuff/functions/interfaces.hh>

#include "interface.hh"

namespace Dune {
namespace GDT {
namespace LocalEvaluation {

/**
 *  \brief      Contains local evaluations for the symmetric interior penalty discontinuous Galerkin (SIPDG)
 *              discretization.
 *
 *              For the choice of penalization and the role of the user input see Epshteyn, Riviere (2007):
 *              "Estimation of penalty parameters for symmetric interior penalty Galerkin methods"
 */
namespace SIPDG {

// forwards
template< class LocalizableFunctionImp >
class Inner;

template< class LocalizableFunctionImp >
class BoundaryLHS;

template< class LocalizableDiffusionFunctionImp, class LocalizableDirichletFunctionImp >
class BoundaryRHS;

namespace internal {

template< class LocalizableFunctionImp >
class InnerTraits
{
  static_assert(Stuff::is_localizable_function< LocalizableFunctionImp >::value,
                "LocalizableFunctionImp has to be a localizable function!");
public:
  typedef Inner< LocalizableFunctionImp >                     derived_type;
  typedef LocalizableFunctionImp                              LocalizableFunctionType;
  typedef typename LocalizableFunctionType::EntityType        EntityType;
  typedef typename LocalizableFunctionType::DomainFieldType   DomainFieldType;
  typedef typename LocalizableFunctionType::LocalfunctionType LocalfunctionType;
  typedef std::tuple< std::shared_ptr< LocalfunctionType > >  LocalfunctionTupleType;
  static const size_t                                         dimDomain = LocalizableFunctionType::dimDomain;
}; // class InnerTraits

template< class LocalizableFunctionImp >
class BoundaryLHSTraits
{
  static_assert(Stuff::is_localizable_function< LocalizableFunctionImp >::value,
                "LocalizableFunctionImp has to be a localizable function!");
public:
  typedef LocalizableFunctionImp                              LocalizableFunctionType;
  typedef BoundaryLHS< LocalizableFunctionImp >               derived_type;
  typedef typename LocalizableFunctionType::EntityType        EntityType;
  typedef typename LocalizableFunctionType::DomainFieldType   DomainFieldType;
  typedef typename LocalizableFunctionType::LocalfunctionType LocalfunctionType;
  typedef std::tuple< std::shared_ptr< LocalfunctionType > >  LocalfunctionTupleType;
  static const size_t                                         dimDomain = LocalizableFunctionType::dimDomain;
}; // class BoundaryLHSTraits

template< class LocalizableDiffusionFunctionImp, class LocalizableDirichletFunctionImp >
class BoundaryRHSTraits
{
  static_assert(Stuff::is_localizable_function< LocalizableDiffusionFunctionImp >::value,
                "LocalizableDiffusionFunctionImp has to be a localizable function!");
  static_assert(Stuff::is_localizable_function< LocalizableDirichletFunctionImp >::value,
                "LocalizableDirichletFunctionImp has to be a localizable function!");
  static_assert(std::is_same< typename LocalizableDiffusionFunctionImp::EntityType,
                              typename LocalizableDirichletFunctionImp::EntityType >::value,
                "EntityTypes have to agree!");
  static_assert(std::is_same< typename LocalizableDiffusionFunctionImp::DomainFieldType,
                              typename LocalizableDirichletFunctionImp::DomainFieldType >::value,
                "DomainFieldTypes have to agree!");
  static_assert(LocalizableDiffusionFunctionImp::dimDomain == LocalizableDirichletFunctionImp::dimDomain,
                "Dimensions have to agree");
public:
  typedef BoundaryRHS< LocalizableDiffusionFunctionImp, LocalizableDirichletFunctionImp >   derived_type;
  typedef LocalizableDiffusionFunctionImp                              LocalizableDiffusionFunctionType;
  typedef LocalizableDirichletFunctionImp                              LocalizableDirichletFunctionType;
  typedef typename LocalizableDiffusionFunctionType::LocalfunctionType LocalDiffusionFunctionType;
  typedef typename LocalizableDirichletFunctionType::LocalfunctionType LocalDirichletFunctionType;
  typedef std::tuple< std::shared_ptr< LocalDiffusionFunctionType >,
                      std::shared_ptr< LocalDirichletFunctionType > >  LocalfunctionTupleType;
  typedef typename LocalizableDiffusionFunctionType::EntityType        EntityType;
  typedef typename LocalizableDiffusionFunctionType::DomainFieldType   DomainFieldType;
  static const size_t dimDomain = LocalizableDiffusionFunctionType::dimDomain;
}; // class BoundaryRHSTraits

/**
 * \note see Epshteyn, Riviere, 2007
 */
static inline double default_beta(const size_t dimDomain)
{
  return 1.0/(dimDomain - 1.0);
}

/**
 * \note see Epshteyn, Riviere, 2007
 */
static inline double inner_sigma(const size_t pol_order)
{
  double sigma = 1.0;
  if (pol_order <= 1)
    sigma *= 8.0;
  else if (pol_order <= 2)
    sigma *= 20.0;
  else if (pol_order <= 3)
    sigma *= 38.0;
  else {
#ifndef NDEBUG
# ifndef DUNE_GDT_LOCALEVALUATION_SIPDG_DISABLE_WARNINGS
    DSC::TimedLogger().get("gdt.localevaluation.sipdg.inner").warn()
        << "a polynomial order of " << pol_order << " is untested!\n"
        << "  #define DUNE_GDT_LOCALEVALUATION_SIPDG_DISABLE_WARNINGS to statically disable this warning\n"
        << "  or dynamically disable warnings of the TimedLogger() instance!" << std::endl;
# endif
#endif
    sigma *= 50.0;
  }
  return sigma;
} // ... inner_sigma(...)

/**
 * \note see Epshteyn, Riviere, 2007
 */
static inline double boundary_sigma(const size_t pol_order)
{
  double sigma = 1.0;
  if (pol_order <= 1)
    sigma *= 14.0;
  else if (pol_order <= 2)
    sigma *= 38.0;
  else if (pol_order <= 3)
    sigma *= 74.0;
  else {
#ifndef NDEBUG
# ifndef DUNE_GDT_LOCALEVALUATION_SIPDG_DISABLE_WARNINGS
    DSC::TimedLogger().get("gdt.localevaluation.sipdg.inner").warn()
        << "a polynomial order of " << pol_order << " is untested!\n"
        << "  #define DUNE_GDT_LOCALEVALUATION_SIPDG_DISABLE_WARNINGS to statically disable this warning\n"
        << "  or dynamically disable warnings of the TimedLogger() instance!" << std::endl;
# endif
#endif
    sigma *= 100.0;
  }
  return sigma;
} // ... boundary_sigma(...)

} // namespace internal

/**
 * see Epshteyn, Riviere, 2007 for the meaning of beta
 */
template< class LocalizableFunctionImp >
class Inner
  : public LocalEvaluation::Codim1Interface< internal::InnerTraits< LocalizableFunctionImp >, 4 >
{
public:
  typedef internal::InnerTraits< LocalizableFunctionImp > Traits;
  typedef typename Traits::LocalizableFunctionType        LocalizableFunctionType;
  typedef typename Traits::LocalfunctionTupleType         LocalfunctionTupleType;
  typedef typename Traits::EntityType                     EntityType;
  typedef typename Traits::DomainFieldType                DomainFieldType;
  static const size_t                                     dimDomain = Traits::dimDomain;

  Inner(const LocalizableFunctionType& inducingFunction, const double beta = internal::default_beta(dimDomain))
    : inducingFunction_(inducingFunction)
    , beta_(beta)
  {}

  /// \name Required by LocalEvaluation::Codim1Interface< ..., 4 >
  /// \{

  LocalfunctionTupleType localFunctions(const EntityType& entity) const
  {
    return std::make_tuple(inducingFunction_.local_function(entity));
  }

  /**
   * \brief extracts the local functions and calls the correct order() method
   */
  template< class R, size_t rT, size_t rCT, size_t rA, size_t rCA >
  size_t order(const LocalfunctionTupleType& localFunctionsEntity,
               const LocalfunctionTupleType& localFunctionsNeighbor,
               const Stuff::LocalfunctionSetInterface
                   < EntityType, DomainFieldType, dimDomain, R, rT, rCT >& testBaseEntity,
               const Stuff::LocalfunctionSetInterface
                   < EntityType, DomainFieldType, dimDomain, R, rA, rCA >& ansatzBaseEntity,
               const Stuff::LocalfunctionSetInterface
                   < EntityType, DomainFieldType, dimDomain, R, rT, rCT >& testBaseNeighbor,
               const Stuff::LocalfunctionSetInterface
                   < EntityType, DomainFieldType, dimDomain, R, rA, rCA >& ansatzBaseNeighbor) const
  {
    const auto localFunctionEntity = std::get< 0 >(localFunctionsEntity);
    const auto localFunctionNeighbor = std::get< 0 >(localFunctionsNeighbor);
    return order(*localFunctionEntity, *localFunctionNeighbor,
                 testBaseEntity, ansatzBaseEntity,
                 testBaseNeighbor, ansatzBaseNeighbor);
  }

  /**
   * \brief extracts the local functions and calls the correct evaluate() method
   */
  template< class IntersectionType, class R, size_t rT, size_t rCT, size_t rA, size_t rCA >
  void evaluate(const LocalfunctionTupleType& localFunctionsEntity,
                const LocalfunctionTupleType& localFunctionsNeighbor,
                const Stuff::LocalfunctionSetInterface
                    < EntityType, DomainFieldType, dimDomain, R, rT, rCT >& testBaseEntity,
                const Stuff::LocalfunctionSetInterface
                    < EntityType, DomainFieldType, dimDomain, R, rA, rCA >& ansatzBaseEntity,
                const Stuff::LocalfunctionSetInterface
                    < EntityType, DomainFieldType, dimDomain, R, rT, rCT >& testBaseNeighbor,
                const Stuff::LocalfunctionSetInterface
                    < EntityType, DomainFieldType, dimDomain, R, rA, rCA >& ansatzBaseNeighbor,
                const IntersectionType& intersection,
                const Dune::FieldVector< DomainFieldType, dimDomain - 1 >& localPoint,
                Dune::DynamicMatrix< R >& entityEntityRet,
                Dune::DynamicMatrix< R >& neighborNeighborRet,
                Dune::DynamicMatrix< R >& entityNeighborRet,
                Dune::DynamicMatrix< R >& neighborEntityRet) const
  {
    const auto localFunctionEntity = std::get< 0 >(localFunctionsEntity);
    const auto localFunctionNeighbor = std::get< 0 >(localFunctionsNeighbor);
    evaluate(*localFunctionEntity, *localFunctionNeighbor,
             testBaseEntity, ansatzBaseEntity,
             testBaseNeighbor, ansatzBaseNeighbor,
             intersection, localPoint,
             entityEntityRet,
             neighborNeighborRet,
             entityNeighborRet,
             neighborEntityRet);
  }

  /// \}
  /// \name Actual implementation of order
  /// \{

  template< class R, size_t rL, size_t rCL, size_t rT, size_t rCT, size_t rA, size_t rCA >
  size_t order(const Stuff::LocalfunctionInterface
                   < EntityType, DomainFieldType, dimDomain, R, rL, rCL >& localFunctionEntity,
               const Stuff::LocalfunctionInterface
                   < EntityType, DomainFieldType, dimDomain, R, rL, rCL >& localFunctionNeighbor,
               const Stuff::LocalfunctionSetInterface
                   < EntityType, DomainFieldType, dimDomain, R, rT, rCT >& testBaseEntity,
               const Stuff::LocalfunctionSetInterface
                   < EntityType, DomainFieldType, dimDomain, R, rA, rCA >& ansatzBaseEntity,
               const Stuff::LocalfunctionSetInterface
                   < EntityType, DomainFieldType, dimDomain, R, rT, rCT >& testBaseNeighbor,
               const Stuff::LocalfunctionSetInterface
                   < EntityType, DomainFieldType, dimDomain, R, rA, rCA >& ansatzBaseNeighbor) const
  {
    return std::max(localFunctionEntity.order(), localFunctionNeighbor.order())
         + std::max(testBaseEntity.order(), testBaseNeighbor.order())
         + std::max(ansatzBaseEntity.order(), ansatzBaseNeighbor.order());
  }

  /// \}
  /// \name Actual implementation of evaluate
  /// \{

  /**
   *  \brief  Computes the ipdg fluxes in a primal setting.
   *  \tparam IntersectionType Type of the codim 1 Intersection
   *  \tparam R         RangeFieldType
   */
  template< class IntersectionType, class R >
  void evaluate(const Stuff::LocalfunctionInterface< EntityType, DomainFieldType, 2, R, 1, 1 >& localFunctionEntity,
                const Stuff::LocalfunctionInterface< EntityType, DomainFieldType, 2, R, 1, 1 >& localFunctionNeighbor,
                const Stuff::LocalfunctionSetInterface< EntityType, DomainFieldType, 2, R, 1, 1 >& testBaseEntity,
                const Stuff::LocalfunctionSetInterface< EntityType, DomainFieldType, 2, R, 1, 1 >& ansatzBaseEntity,
                const Stuff::LocalfunctionSetInterface< EntityType, DomainFieldType, 2, R, 1, 1 >& testBaseNeighbor,
                const Stuff::LocalfunctionSetInterface< EntityType, DomainFieldType, 2, R, 1, 1 >& ansatzBaseNeighbor,
                const IntersectionType& intersection,
                const Dune::FieldVector< DomainFieldType, 1 >& localPoint,
                Dune::DynamicMatrix< R >& entityEntityRet,
                Dune::DynamicMatrix< R >& neighborNeighborRet,
                Dune::DynamicMatrix< R >& entityNeighborRet,
                Dune::DynamicMatrix< R >& neighborEntityRet) const
  {
    // clear ret
    entityEntityRet *= 0.0;
    neighborNeighborRet *= 0.0;
    entityNeighborRet *= 0.0;
    neighborEntityRet *= 0.0;
    // convert local point (which is in intersection coordinates) to entity/neighbor coordinates
    const auto localPointEn = intersection.geometryInInside().global(localPoint);
    const auto localPointNe = intersection.geometryInOutside().global(localPoint);
    const auto unitOuterNormal = intersection.unitOuterNormal(localPoint);
    // evaluate local function
    const auto functionValueEn = localFunctionEntity.evaluate(localPointEn);
    const auto functionValueNe = localFunctionNeighbor.evaluate(localPointNe);
    // compute penalty (see Epshteyn, Riviere, 2007)
    const auto max_polorder = std::max(testBaseEntity.order(),
                                       std::max(ansatzBaseEntity.order(),
                                                std::max(testBaseNeighbor.order(),
                                                         ansatzBaseNeighbor.order())));
    const R sigma = internal::inner_sigma(max_polorder);
    const R penalty = sigma / std::pow(intersection.geometry().volume(), beta_);
    // evaluate bases
    // * entity
    //   * test
    const auto rowsEn = testBaseEntity.size();
    const auto testValuesEn = testBaseEntity.evaluate(localPointEn);
    const auto testGradientsEn = testBaseEntity.jacobian(localPointEn);
    //   * ansatz
    const auto colsEn = ansatzBaseEntity.size();
    const auto ansatzValuesEn = ansatzBaseEntity.evaluate(localPointEn);
    const auto ansatzGradientsEn = ansatzBaseEntity.jacobian(localPointEn);
    // * neighbor
    //   * test
    const auto rowsNe = testBaseNeighbor.size();
    const auto testValuesNe = testBaseNeighbor.evaluate(localPointNe);
    const auto testGradientsNe = testBaseNeighbor.jacobian(localPointNe);
    //   * ansatz
    const auto colsNe = ansatzBaseNeighbor.size();
    const auto ansatzValuesNe = ansatzBaseNeighbor.evaluate(localPointNe);
    const auto ansatzGradientsNe = ansatzBaseNeighbor.jacobian(localPointNe);
    // compute the evaluations
    assert(entityEntityRet.rows() >= rowsEn);
    assert(entityEntityRet.cols() >= colsEn);
    assert(entityNeighborRet.rows() >= rowsEn);
    assert(entityNeighborRet.cols() >= colsNe);
    assert(neighborEntityRet.rows() >= rowsNe);
    assert(neighborEntityRet.cols() >= colsEn);
    assert(neighborNeighborRet.rows() >= rowsNe);
    assert(neighborNeighborRet.cols() >= colsNe);
    // loop over all entity test basis functions
    for (size_t ii = 0; ii < rowsEn; ++ii) {
      auto& entityEntityRetRow = entityEntityRet[ii];
      auto& entityNeighborRetRow = entityNeighborRet[ii];
      // loop over all entity ansatz basis functions
      for (size_t jj = 0; jj < colsEn; ++jj) {
        // consistency term
        entityEntityRetRow[jj]
            += -0.5 * functionValueEn * (ansatzGradientsEn[jj][0] * unitOuterNormal) * testValuesEn[ii];
        // symmetry term
        entityEntityRetRow[jj]
            += -0.5 * ansatzValuesEn[jj] * functionValueEn * (testGradientsEn[ii][0] * unitOuterNormal);
        // penalty term
        entityEntityRetRow[jj] += penalty * ansatzValuesEn[jj] * testValuesEn[ii];
      } // loop over all entity ansatz basis functions
      // loop over all neighbor ansatz basis functions
      for (size_t jj = 0; jj < colsNe; ++jj) {
        // consistency term
        entityNeighborRetRow[jj]
            += -0.5 * functionValueNe * (ansatzGradientsNe[jj][0] * unitOuterNormal) * testValuesEn[ii];
        // symmetry term
        entityNeighborRetRow[jj]
            += 0.5 * ansatzValuesNe[jj] * functionValueEn * (testGradientsEn[ii][0] * unitOuterNormal);
        // penalty term
        entityNeighborRetRow[jj] += -1.0 * penalty * ansatzValuesNe[jj] * testValuesEn[ii];
      } // loop over all neighbor ansatz basis functions
    } // loop over all entity test basis functions
    // loop over all neighbor test basis functions
    for (size_t ii = 0; ii < rowsNe; ++ii) {
      auto& neighborEntityRetRow = neighborEntityRet[ii];
      auto& neighborNeighborRetRow = neighborNeighborRet[ii];
      // loop over all entity ansatz basis functions
      for (size_t jj = 0; jj < colsEn; ++jj) {
        // consistency term
        neighborEntityRetRow[jj]
            += 0.5 * functionValueEn * (ansatzGradientsEn[jj][0] * unitOuterNormal) * testValuesNe[ii];
        // symmetry term
        neighborEntityRetRow[jj]
            += -0.5 * ansatzValuesEn[jj] * functionValueNe * (testGradientsNe[ii][0] * unitOuterNormal);
        // penalty term
        neighborEntityRetRow[jj] += -1.0 * penalty * ansatzValuesEn[jj] * testValuesNe[ii];
      } // loop over all entity ansatz basis functions
      // loop over all neighbor ansatz basis functions
      for (size_t jj = 0; jj < colsNe; ++jj) {
        // consistency term
        neighborNeighborRetRow[jj]
            += 0.5 * functionValueNe * (ansatzGradientsNe[jj][0] * unitOuterNormal) * testValuesNe[ii];
        // symmetry term
        neighborNeighborRetRow[jj]
            += 0.5 * ansatzValuesNe[jj] * functionValueNe * (testGradientsNe[ii][0] * unitOuterNormal);
        // penalty term
        neighborNeighborRetRow[jj] += penalty * ansatzValuesNe[jj] * testValuesNe[ii];
      } // loop over all neighbor ansatz basis functions
    } // loop over all neighbor test basis functions
  } // ... evaluate(...)

  /// \}

private:
  const LocalizableFunctionType& inducingFunction_;
  const double beta_;
}; // class Inner

/**
 * see Epshteyn, Riviere, 2007 for the meaning of beta
 */
template< class LocalizableFunctionImp >
class BoundaryLHS
    : public LocalEvaluation::Codim1Interface< internal::BoundaryLHSTraits< LocalizableFunctionImp >, 2 >
{
public:
  typedef internal::BoundaryLHSTraits< LocalizableFunctionImp > Traits;
  typedef typename Traits::LocalizableFunctionType              LocalizableFunctionType;
  typedef typename Traits::LocalfunctionTupleType               LocalfunctionTupleType;
  typedef typename Traits::EntityType                           EntityType;
  typedef typename Traits::DomainFieldType                      DomainFieldType;
  static const size_t                                           dimDomain = Traits::dimDomain;

  BoundaryLHS(const LocalizableFunctionType& inducingFunction, const double beta = internal::default_beta(dimDomain))
    : inducingFunction_(inducingFunction)
    , beta_(beta)
  {}

  /// \name Required by LocalEvaluation::Codim1Interface< ..., 2>
  /// \{

  LocalfunctionTupleType localFunctions(const EntityType& entity) const
  {
    return std::make_tuple(inducingFunction_.local_function(entity));
  }

  /**
   * \brief extracts the local functions and calls the correct order() method
   */
  template< class R, size_t rT, size_t rCT, size_t rA, size_t rCA >
  size_t order(const LocalfunctionTupleType localFuncs,
               const Stuff::LocalfunctionSetInterface< EntityType, DomainFieldType, dimDomain, R, rT, rCT >& testBase,
               const Stuff::LocalfunctionSetInterface< EntityType, DomainFieldType, dimDomain, R, rA, rCA >& ansatzBase)
  const
  {
    return order(*std::get< 0 >(localFuncs), testBase, ansatzBase);
  }

  /**
   * \brief extracts the local functions and calls the correct evaluate() method
   */
  template< class IntersectionType, class R, size_t rT, size_t rCT, size_t rA, size_t rCA >
  void evaluate(const LocalfunctionTupleType localFuncs,
                const Stuff::LocalfunctionSetInterface
                    < EntityType, DomainFieldType, dimDomain, R, rT, rCT >& testBase,
                const Stuff::LocalfunctionSetInterface
                    < EntityType, DomainFieldType, dimDomain, R, rA, rCA >& ansatzBase,
                const IntersectionType& intersection,
                const Dune::FieldVector< DomainFieldType, dimDomain - 1 >& localPoint,
                Dune::DynamicMatrix< R >& ret) const
  {
    evaluate(*std::get< 0 >(localFuncs), testBase, ansatzBase, intersection, localPoint, ret);
  }

  /// \}
  /// \name Actual implementation of order
  /// \{

  /**
   * \return localFunction.order() + testBase.order() + ansatzBase.order();
   */
  template< class R, size_t rL, size_t rCL, size_t rT, size_t rCT, size_t rA, size_t rCA >
  size_t order(const Stuff::LocalfunctionInterface< EntityType, DomainFieldType, dimDomain, R, rL, rCL >& localFunction,
               const Stuff::LocalfunctionSetInterface< EntityType, DomainFieldType, dimDomain, R, rT, rCT >& testBase,
               const Stuff::LocalfunctionSetInterface< EntityType, DomainFieldType, dimDomain, R, rA, rCA >& ansatzBase)
  const
  {
    return localFunction.order() + testBase.order() + ansatzBase.order();
  }

  /// \}
  /// \name Actual implementation of evaluate
  /// \{

  template< class IntersectionType, class R >
  void evaluate(const Stuff::LocalfunctionInterface< EntityType, DomainFieldType, 2, R, 1, 1 >& localFunction,
                const Stuff::LocalfunctionSetInterface< EntityType, DomainFieldType, 2, R, 1, 1 >& testBase,
                const Stuff::LocalfunctionSetInterface< EntityType, DomainFieldType, 2, R, 1, 1 >& ansatzBase,
                const IntersectionType& intersection,
                const Dune::FieldVector< DomainFieldType, 1 >& localPoint,
                Dune::DynamicMatrix< R >& ret) const
  {
    // clear ret
    ret *= 0.0;
    // get local point (which is in intersection coordinates) in entity coordinates
    const auto localPointEntity = intersection.geometryInInside().global(localPoint);
    const auto unitOuterNormal = intersection.unitOuterNormal(localPoint);
    // evaluate local function
    const auto functionValue = localFunction.evaluate(localPointEntity);
    // compute penalty (see Epshteyn, Riviere, 2007)
    const auto max_polorder = std::max(testBase.order(), ansatzBase.order());
    const R sigma = internal::boundary_sigma(max_polorder);
    const R penalty = sigma / std::pow(intersection.geometry().volume(), beta_);
    // evaluate bases
    // * test
    const auto rows = testBase.size();
    const auto testValues = testBase.evaluate(localPointEntity);
    const auto testGradients = testBase.jacobian(localPointEntity);
    // * ansatz
    const auto cols = ansatzBase.size();
    const auto ansatzValues = ansatzBase.evaluate(localPointEntity);
    const auto ansatzGradients = ansatzBase.jacobian(localPointEntity);
    // compute products
    assert(ret.rows() >= rows);
    assert(ret.cols() >= cols);
    // loop over all test basis functions
    for (size_t ii = 0; ii < rows; ++ii) {
      auto& retRow = ret[ii];
      // loop over all ansatz basis functions
      for (size_t jj = 0; jj < cols; ++jj) {
        // consistency term
        retRow[jj] += -1.0 * functionValue * (ansatzGradients[jj][0] * unitOuterNormal) * testValues[ii];
        // symmetry term
        retRow[jj] += -1.0 * ansatzValues[jj] * functionValue * (testGradients[ii][0] * unitOuterNormal);
        // penalty term
        retRow[jj] += penalty * ansatzValues[jj] * testValues[ii];
      } // loop over all ansatz basis functions
    } // loop over all test basis functions
  } // ... evaluate(...)

  /// \}
private:
  const LocalizableFunctionType& inducingFunction_;
  const double beta_;
}; // class BoundaryLHS

/**
 * see Epshteyn, Riviere, 2007 for the meaning of beta
 */
template< class LocalizableDiffusionFunctionImp, class LocalizableDirichletFunctionImp >
class BoundaryRHS
  : public LocalEvaluation::Codim1Interface< internal::BoundaryRHSTraits< LocalizableDiffusionFunctionImp,
                                                                LocalizableDirichletFunctionImp >, 1 >
{
public:
  typedef internal::BoundaryRHSTraits< LocalizableDiffusionFunctionImp, LocalizableDirichletFunctionImp > Traits;
  typedef typename Traits::LocalizableDiffusionFunctionType LocalizableDiffusionFunctionType;
  typedef typename Traits::LocalizableDirichletFunctionType LocalizableDirichletFunctionType;
  typedef typename Traits::LocalfunctionTupleType           LocalfunctionTupleType;
  typedef typename Traits::EntityType                       EntityType;
  typedef typename Traits::DomainFieldType                  DomainFieldType;
  static const size_t                                       dimDomain = Traits::dimDomain;

  BoundaryRHS(const LocalizableDiffusionFunctionType& diffusion,
              const LocalizableDirichletFunctionType& dirichlet,
              const double beta = internal::default_beta(dimDomain))
    : diffusion_(diffusion)
    , dirichlet_(dirichlet)
    , beta_(beta)
  {}

  /// \name Required by LocalEvaluation::Codim1Interface< ...., 1 >
  /// \{

  LocalfunctionTupleType localFunctions(const EntityType& entity) const
  {
    return std::make_tuple(diffusion_.local_function(entity), dirichlet_.local_function(entity));
  }

  /**
   * \brief extracts the local functions and calls the correct order() method
   */
  template< class R, size_t r, size_t rC >
  size_t order(const LocalfunctionTupleType localFuncs,
               const Stuff::LocalfunctionSetInterface< EntityType, DomainFieldType, dimDomain, R, r, rC >& testBase)
  const
  {
    const auto localDiffusion = std::get< 0 >(localFuncs);
    const auto localDirichlet = std::get< 1 >(localFuncs);
    return order(*localDiffusion, *localDirichlet, testBase);
  }

  /**
   * \brief extracts the local functions and calls the correct evaluate() method
   */
  template< class IntersectionType, class R, size_t r, size_t rC >
  void evaluate(const LocalfunctionTupleType localFuncs,
                const Stuff::LocalfunctionSetInterface< EntityType, DomainFieldType, dimDomain, R, r, rC >& testBase,
                const IntersectionType& intersection,
                const Dune::FieldVector< DomainFieldType, dimDomain - 1 >& localPoint,
                Dune::DynamicVector< R >& ret) const
  {
    const auto localDiffusion = std::get< 0 >(localFuncs);
    const auto localDirichlet = std::get< 1 >(localFuncs);
    evaluate(*localDiffusion, *localDirichlet, testBase, intersection, localPoint, ret);
  }

  /// \}
  /// \name Actual implementation of order
  /// \{

  /**
   *  \return std::max(testOrder + dirichletOrder, diffusionOrder + testGradientOrder + dirichletOrder);
   */
  template< class R, size_t rLF, size_t rCLF, size_t rLR, size_t rCLR, size_t rT, size_t rCT >
  size_t order(const Stuff::LocalfunctionInterface
                            < EntityType, DomainFieldType, dimDomain, R, rLF, rCLF >& localDiffusion,
                        const Stuff::LocalfunctionInterface
                            < EntityType, DomainFieldType, dimDomain, R, rLR, rCLR >& localDirichlet,
                        const Stuff::LocalfunctionSetInterface
                            < EntityType, DomainFieldType, dimDomain, R, rT, rCT >& testBase) const
  {
    const size_t testOrder = testBase.order();
    const size_t testGradientOrder = boost::numeric_cast< size_t >(std::max(ssize_t(testOrder) - 1, ssize_t(0)));
    const size_t diffusionOrder = localDiffusion.order();
    const size_t dirichletOrder = localDirichlet.order();
    return std::max(testOrder + dirichletOrder, diffusionOrder + testGradientOrder + dirichletOrder);
  } // ... order(...)

  /// \}
  /// \name Actual implementation of evaluate
  /// \{

  template< class IntersectionType, class R >
  void evaluate(const Stuff::LocalfunctionInterface< EntityType, DomainFieldType, dimDomain, R, 1, 1 >& localDiffusion,
                const Stuff::LocalfunctionInterface< EntityType, DomainFieldType, dimDomain, R, 1, 1 >& localDirichlet,
                const Stuff::LocalfunctionSetInterface< EntityType, DomainFieldType, dimDomain, R, 1, 1 >& testBase,
                const IntersectionType& intersection,
                const Dune::FieldVector< DomainFieldType, dimDomain - 1 >& localPoint,
                Dune::DynamicVector< R >& ret) const
  {
    // clear ret
    ret *= 0.0;
    // get local point (which is in intersection coordinates) in entity coordinates
    const auto localPointEntity = intersection.geometryInInside().global(localPoint);
    const auto unitOuterNormal = intersection.unitOuterNormal(localPoint);
    // evaluate local functions
    const auto diffusionValue = localDiffusion.evaluate(localPointEntity);
    const auto dirichletValue = localDirichlet.evaluate(localPointEntity);
    // compute penalty (see Epshteyn, Riviere, 2007)
    const auto polorder = testBase.order();
    const R sigma = internal::boundary_sigma(polorder);
    const R penalty = sigma / std::pow(intersection.geometry().volume(), beta_);
    // evaluate basis
    const auto size = testBase.size();
    const auto testValues = testBase.evaluate(localPointEntity);
    const auto testGradients = testBase.jacobian(localPointEntity);
    // compute
    assert(ret.size() >= size);
    // loop over all test basis functions
    for (size_t ii = 0; ii < size; ++ii) {
      // symmetry term
      ret[ii] += -1.0 * dirichletValue * diffusionValue * (testGradients[ii][0] * unitOuterNormal);
      // penalty term
      ret[ii] += penalty * dirichletValue * testValues[ii];
    } // loop over all test basis functions
  } // ... evaluate(...)

  /// \{

private:
  const LocalizableDiffusionFunctionType& diffusion_;
  const LocalizableDirichletFunctionType& dirichlet_;
  const double beta_;
}; // class BoundaryRHS

} // namespace SIPDG
} // namespace LocalEvaluation
} // namespace GDT
} // namespace Dune

#endif // DUNE_GDT_LOCALEVALUATION_SIPDG_HH


#include <iostream>
#include <vector>
#include <random>
#include <fstream>
#include <numeric>
#include <chrono>
#include <math.h>
#include <assert.h>
#include <time.h>
#include <random>
#include "generator.hh"
#include <boost/beast/core.hpp>
#include <boost/beast/http.hpp>
#include <boost/beast/version.hpp>
#include <boost/asio.hpp>

namespace beast = boost::beast;     // from <boost/beast.hpp>
namespace http = beast::http;       // from <boost/beast/http.hpp>
namespace net = boost::asio;        // from <boost/asio.hpp>
using tcp = net::ip::tcp;           // from <boost/asio/ip/tcp.hpp>

std::string Key_Generator::generate_key_(unsigned length)
{
    std::string new_key;

    for(unsigned i = 0; i < length; i++){
        new_key += ALPHANUMERIC[a_dist(gen)];
    }

    return new_key;
}

unsigned Key_Generator::generate_key_length_()
{
    return unsigned(dist(gen));
}

std::vector<std::string> Key_Generator::generate_key_pool_(){
    std::vector<std::string> res;
    for(unsigned i = 0; i < KEY_POOL_SIZE; i++){
        res.push_back(generate_key_(generate_key_length_()));
    }
    return res;
}

std::string Key_Generator::get_key_from_pool_(){
    std::uniform_int_distribution<> distribution1(0, KEY_POOL_SIZE - 1);
    return key_pool[distribution1(gen)];
}
std::string Key_Generator::generate_key()
{
    return get_key_from_pool_();
}

void Key_Generator::add_key_to_cache(std::string key){
    key_cache.push_back(key);
    //std::cout << "ADDED KEY " << key << "\n";

}

bool Key_Generator::is_key_in_cache(std::string key){

    if (std::count(key_cache.begin(),key_cache.end(),key))
    {
        return true;
    }
    else
    {
        return false;
    }

}

std::string Key_Generator::get_key_from_cache(){
    auto cache_size = static_cast<int> (key_cache.size());
    auto rand_idx = rand() % cache_size;
    //std::cout << "\n Index of key retrieved from key_cache: " << rand_idx << " key: " << key_cache[rand_idx] << "\n";
    return key_cache[rand_idx];
}

unsigned Key_Generator::get_key_cache_length(){
    return key_cache.size();
}

void Key_Generator::delete_key_from_cache(std::string key){
    key_cache.erase(std::remove(key_cache.begin(), key_cache.end(), key), key_cache.end());
}
unsigned Value_Generator::generate_value_length_()
{
    return unsigned(dist(gen));
}

    std::string Value_Generator::generate_value_(unsigned length)
{
    std::string new_val;
    unsigned length_to_use = (length % 500) + 1;

    for(unsigned i = 0; i < length_to_use; i++){
        new_val += ALPHANUMERIC[a_dist(gen)];
    }
    return new_val;
}

std::string Value_Generator::generate_value()
{
    return generate_value_(generate_value_length_());
}

double Method_Generator::generate_probability()
{
return dist(gen);
}

std::string Method_Generator::generate_method()
{
    auto probability = Method_Generator::generate_probability();
    if (probability <= get_cutoff)
    {
        return std::string("GET");
    }
    else if (probability <= set_cutoff)
    {
        return std::string("PUT");
    }
    else
    {
        return std::string("DELETE");
    }
}

double Request_Generator::generate_probability()
{
return dist(gen);
}

http::request<http::string_body> Request_Generator::generate_request()
{

    std::string target = "/";
    auto version = 11;
    std::string method = method_generator_.generate_method();
    std::string random_key = key_generator_.generate_key();

    if (method == "GET")
    {
        if(key_generator_.get_key_cache_length() > 0){
            //Pick a known key 80% of the time, 20% of the time totally random
            auto probability = generate_probability();
            //std::cout << "PROBABILITY: " << probability << "\n";
            if (probability <= known_cutoff){
                target = key_generator_.get_key_from_cache();
                //std::cout << "TARGET: " << target << "\n";
                http::request<http::string_body> req{http::verb::get, target, version};
                return req;
            }

        }
        target = "/" + random_key;
            http::request<http::string_body> req{http::verb::get, target, version};
            return req;
        return req;

    }
    else if (method == "PUT")
    {
        std::string value = value_generator_.generate_value();
        target = "/"+ random_key + "/" + value;
        std::string key_for_cache = "/" + random_key;
        http::request<http::string_body> req{http::verb::put, target, version};
         if (key_generator_.is_key_in_cache(key_for_cache) == false){
            key_generator_.add_key_to_cache(key_for_cache);
        }
        return req;

        //if not in key cache, add to key cache

    }
    else if (method == "DELETE")
    {
        target = "/" + random_key;
        if(key_generator_.get_key_cache_length() > 0){
            //Pick a known key 80% of the time, 20% of the time totally random
            auto probability = generate_probability();
            if (probability <= known_cutoff){
                auto key = key_generator_.get_key_from_cache();
                key_generator_.delete_key_from_cache(key);
            }
        }
        http::request<http::string_body> req{http::verb::delete_, target, version};
        return req;
    }
    else
    {
        std::cout << "error invalid method\n";
        http::request<http::string_body> default_req;
        return default_req;
    }

}


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

#include "Architecture/Architecture.hpp"

#include <boost/graph/biconnected_components.hpp>
#include <unordered_set>
#include <vector>

#include "Graphs/ArticulationPoints.hpp"
#include "Utils/Json.hpp"
#include "Utils/UnitID.hpp"

namespace tket {

Architecture Architecture::create_subarch(
    const std::vector<Node>& subarc_nodes) {
  Architecture subarc(subarc_nodes);
  for (auto [u1, u2] : get_all_edges_vec()) {
    if (subarc.node_exists(u1) && subarc.node_exists(u2)) {
      subarc.add_connection(u1, u2);
    }
  }
  return subarc;
}

// Given a vector of lengths of lines, returns a vector of lines of these sizes
// comprised of architecture nodes
std::vector<node_vector_t> Architecture::get_lines(
    std::vector<unsigned> required_lengths) const {
  // check total length doesn't exceed number of nodes
  if (std::accumulate(required_lengths.begin(), required_lengths.end(), 0u) >
      n_nodes()) {
    throw ArchitectureInvalidity(
        "Not enough nodes to satisfy required lengths.");
  }
  std::sort(
      required_lengths.begin(), required_lengths.end(),
      std::greater<unsigned>());

  UndirectedConnGraph curr_graph(get_undirected_connectivity());
  std::vector<node_vector_t> found_lines;
  for (unsigned length : required_lengths) {
    std::vector<Vertex> longest(
        graphs::longest_simple_path(curr_graph, length));
    if (longest.size() >= length) {
      longest.resize(length);
      // convert Vertex to Node
      node_vector_t to_add;
      for (Vertex v : longest) {
        to_add.push_back(curr_graph[v]);
      }
      found_lines.push_back(to_add);
      for (Vertex v : longest) {
        boost::clear_vertex(v, curr_graph);
      }
    }
  }
  return found_lines;
}

std::set<Node> Architecture::get_articulation_points(
    const Architecture& subarc) const {
  return graphs::get_subgraph_aps<Node>(
      get_undirected_connectivity(), subarc.get_undirected_connectivity());
}

std::set<Node> Architecture::get_articulation_points() const {
  std::set<Vertex> aps;
  UndirectedConnGraph undir_g = get_undirected_connectivity();
  boost::articulation_points(undir_g, std::inserter(aps, aps.begin()));
  std::set<Node> ret;
  for (Vertex ap : aps) {
    ret.insert(undir_g[ap]);
  }
  return ret;
}

static bool lexicographical_comparison(
    const std::vector<std::size_t>& dist1,
    const std::vector<std::size_t>& dist2) {
  return std::lexicographical_compare(
      dist1.begin(), dist1.end(), dist2.begin(), dist2.end());
}

std::optional<Node> Architecture::find_worst_node(
    const Architecture& original_arch) {
  node_set_t ap = get_articulation_points();
  node_set_t min_nodes = min_degree_nodes();

  std::set<Node> bad_nodes;
  std::set_difference(
      min_nodes.cbegin(), min_nodes.cend(), ap.cbegin(), ap.cend(),
      std::inserter(bad_nodes, bad_nodes.begin()));

  if (bad_nodes.empty()) {
    return std::nullopt;
  }

  std::vector<std::size_t> worst_distances, temp_distances;
  Node worst_node = *bad_nodes.begin();
  worst_distances = get_distances(worst_node);
  for (Node temp_node : bad_nodes) {
    temp_distances = get_distances(temp_node);

    int distance_comp =
        tri_lexicographical_comparison(temp_distances, worst_distances);
    if (distance_comp == 1) {
      worst_node = temp_node;
      worst_distances = temp_distances;
    } else if (distance_comp == -1) {
      std::vector<std::size_t> temp_distances_full =
          original_arch.get_distances(temp_node);
      std::vector<std::size_t> worst_distances_full =
          original_arch.get_distances(worst_node);
      if (lexicographical_comparison(
              temp_distances_full, worst_distances_full)) {
        worst_node = temp_node;
        worst_distances = temp_distances;
      }
    }
  }
  return worst_node;
}

node_set_t Architecture::remove_worst_nodes(unsigned num) {
  node_set_t out;
  Architecture original_arch(*this);
  for (unsigned k = 0; k < num; k++) {
    std::optional<Node> v = find_worst_node(original_arch);
    if (v.has_value()) {
      remove_node(v.value());
      out.insert(v.value());
    }
  }
  return out;
}

int tri_lexicographical_comparison(
    const std::vector<std::size_t>& dist1,
    const std::vector<std::size_t>& dist2) {
  // add assertion that these are the same size distance vectors
  auto start_dist1 = dist1.cbegin();
  auto start_dist2 = dist2.cbegin();

  while (start_dist1 != dist1.cend()) {
    if (start_dist2 == dist2.cend() || *start_dist2 < *start_dist1) {
      return 0;
    } else if (*start_dist1 < *start_dist2) {
      return 1;
    }
    ++start_dist1;
    ++start_dist2;
  }
  return -1;
}

MatrixXb Architecture::get_connectivity() const {
  unsigned n = n_nodes();
  MatrixXb connectivity = MatrixXb(n, n);
  for (unsigned i = 0; i != n; ++i) {
    for (unsigned j = 0; j != n; ++j) {
      connectivity(i, j) =
          edge_exists(Node(i), Node(j)) | edge_exists(Node(j), Node(i));
    }
  }
  return connectivity;
}

void to_json(nlohmann::json& j, const Architecture::Connection& link) {
  j.push_back(link.first);
  j.push_back(link.second);
}

void from_json(const nlohmann::json& j, Architecture::Connection& link) {
  link.first = j.at(0).get<Node>();
  link.second = j.at(1).get<Node>();
}

void to_json(nlohmann::json& j, const Architecture& ar) {
  // Preserve the internal order of ids since Placement depends on this
  auto uid_its = ar.nodes();
  std::vector<Node> nodes{uid_its.begin(), uid_its.end()};
  j["nodes"] = nodes;

  nlohmann::json links;
  for (const Architecture::Connection& con : ar.get_all_edges_vec()) {
    nlohmann::json entry;
    entry["link"] = con;
    entry["weight"] = ar.get_connection_weight(con.first, con.second);
    links.push_back(entry);
  }
  j["links"] = links;
}

void from_json(const nlohmann::json& j, Architecture& ar) {
  for (const Node& n : j.at("nodes").get<node_vector_t>()) {
    ar.add_node(n);
  }
  for (const auto& j_entry : j.at("links")) {
    Architecture::Connection l =
        j_entry.at("link").get<Architecture::Connection>();
    unsigned w = j_entry.at("weight").get<unsigned>();
    ar.add_connection(l.first, l.second, w);
  }
}

void to_json(nlohmann::json& j, const FullyConnected& ar) {
  auto uid_its = ar.nodes();
  std::vector<Node> nodes{uid_its.begin(), uid_its.end()};
  j["nodes"] = nodes;
}

void from_json(const nlohmann::json& j, FullyConnected& ar) {
  for (const Node& n : j.at("nodes").get<node_vector_t>()) {
    ar.add_node(n);
  }
}

//////////////////////////////////////
//      Architecture subclasses     //
//////////////////////////////////////

RingArch::RingArch(unsigned numberOfNodes)
    : Architecture(get_edges(numberOfNodes)) {}

std::vector<Architecture::Connection> RingArch::get_edges(
    unsigned numberOfNodes) {
  std::vector<Connection> edges;
  for (unsigned i = 0; i < numberOfNodes; i++) {
    Node n1("ringNode", i);
    Node n2("ringNode", (i + 1) % numberOfNodes);
    edges.push_back({n1, n2});
  }
  return edges;
}

SquareGrid::SquareGrid(
    const unsigned dim_r, const unsigned dim_c, const unsigned _layers)
    : Architecture(get_edges(dim_r, dim_c, _layers)),
      dimension_r{dim_r},
      dimension_c{dim_c},
      layers{_layers} {}

std::vector<Architecture::Connection> SquareGrid::get_edges(
    const unsigned dim_r, const unsigned dim_c, const unsigned layers) {
  std::vector<Connection> edges;
  for (unsigned l = 0; l < layers; l++) {
    for (unsigned ver = 0; ver < dim_r; ver++) {
      for (unsigned hor = 0; hor < dim_c; hor++) {
        Node n("gridNode", ver, hor, l);
        if (hor != dim_c - 1) {
          Node h_neighbour("gridNode", ver, hor + 1, l);
          edges.push_back({n, h_neighbour});
        }
        if (ver != dim_r - 1) {
          Node v_neighbour("gridNode", ver + 1, hor, l);
          edges.push_back({n, v_neighbour});
        }
        if (l != layers - 1) {
          Node l_neighbour("gridNode", ver, hor, l + 1);
          edges.push_back({n, l_neighbour});
        }
      }
    }
  }
  return edges;
}

}  // namespace tket


#include <signal.h>
#include <stdio.h>
#include <stdlib.h>

#include <iostream>
#include <limits>
#include <boost/ptr_container/ptr_vector.hpp>
#include <vector>
#include <cmath>

//#include <lcm/lcm.h>
//#include <lcmtypes/bot_core.h>
//#include "visualization/collections.hpp"
//#include <lcmtypes/kmcl_pf_cloud_t.h>
#include <bot_core/bot_core.h>

//#include <bot_param/param_client.h>
//#include <bot_param/param_util.h>

#include "particle_filter.hpp"

using namespace std;

double vis_scale =20; // multiply the values so that mr-viewer2 can view them

long load_data(char const * szName, vector <vector <double> > &meas){
  FILE * fObs = fopen(szName,"rt");
  if (!fObs){
    cout << szName << " was not found in the specified path\n";
    exit(-1);
  }
  char* szBuffer = new char[1024];
  fgets(szBuffer, 1024, fObs);
  long lIterates = strtol(szBuffer, NULL, 10);

  for(long i = 0; i < lIterates; ++i){
      fgets(szBuffer, 1024, fObs);

      double x_meas = strtod(strtok(szBuffer, ",\r\n "), NULL);
      double y_meas = strtod(strtok(NULL, ",\r\n "), NULL);
      vector<double> meas_one;
      meas_one.push_back(x_meas);
      meas_one.push_back(y_meas);
      meas.push_back(meas_one);
    }
  fclose(fObs);

  delete [] szBuffer;

  return lIterates;
}

int main(int argc, char* argv[]){

  //lcm_t* publish_lcm = lcm_create(NULL);
  long N_p = 10000;

//  pf = new ParticleFilter(publish_lcm, N_p,init_pose,
//      config->get_initial_var(),  config->get_rng_seed(),resample_threshold_);
  Eigen::Isometry3d init_pose;
  init_pose.setIdentity();

  ParticleFilter pf(publish_lcm, N_p,init_pose,
      config->get_initial_var(),  config->get_rng_seed(),resample_threshold_);

  ParticleFilter pf(N_p);
  vector < vector < double > > meas;

  //Load observations
  int lIterates = load_data("data.csv",meas);
  cout << "Input Data:\n";
  for(int n=1 ; n < lIterates ; ++n) {
    cout << n << ": " << meas[n][0]  << ", " << meas[n][1]<< "   is data\n";
  }

  pf_state mean_state = pf.Integrate();
  vector <double> null_meas;
  null_meas.push_back( 0.0);
  null_meas.push_back( 0.0);
  //pf.SendParticlesLCM(mean_state, null_meas);
  int pause;
  cout << "Enter a number to start the particle filter: ";
  cin >> pause;

  for (int i=0;i<lIterates;i++){
    vector <double> current_meas;
    current_meas.push_back( meas[i][0]);
    current_meas.push_back( meas[i][1]);

    pf.MoveParticles(i,current_meas[0],current_meas[1]);
    double ESS = pf.ConsiderResample();
    cout << ESS/N_p << " is ESS| " << i << " iteration\n";
    pf_state mean_state = pf.Integrate();
    //pf.SendParticlesLCM(mean_state,current_meas);

    //    int pause;
    //    cout << "Enter a number to continue the particle filter: ";
    //    cin >> pause;
    //      sleep(1);
  }

  cout << "finished\n";

  return 0;
}