text:
//  Copyright (c) 2011-2013 Thomas Heller
//
//  SPDX-License-Identifier: BSL-1.0
//  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)

#if !defined(JACOBI_SMP_NO_pika)
#include <pika/init.hpp>
#endif

#include <pika/modules/program_options.hpp>

#include <boost/spirit/include/phoenix.hpp>
#include <boost/spirit/include/qi_action.hpp>
#include <boost/spirit/include/qi_auxiliary.hpp>
#include <boost/spirit/include/qi_char.hpp>
#include <boost/spirit/include/qi_numeric.hpp>
#include <boost/spirit/include/qi_operator.hpp>
#include <boost/spirit/include/qi_parse.hpp>
#include <boost/spirit/include/qi_string.hpp>
#include <boost/spirit/include/support_istream_iterator.hpp>

#include <cstddef>
#include <fstream>
#include <iostream>
#include <string>
#include <vector>

using pika::program_options::options_description;
using pika::program_options::parse_command_line;
using pika::program_options::store;
using pika::program_options::value;
using pika::program_options::variables_map;

#include "jacobi_nonuniform.hpp"

namespace jacobi_smp {

    void jacobi_kernel_nonuniform(crs_matrix<double> const& A,
        std::vector<double>& dst, std::vector<double> const& src,
        std::vector<double> const& b, std::size_t row)
    {
        double result = b[row];
        double div = 1.0;
        const std::size_t begin = A.row_begin(row);
        const std::size_t end = A.row_end(row);

        for (std::size_t j = begin; j < end; ++j)
        {
            if (row == j)
                div = div / A.values[j];
            else
                result -= A.values[j] * src[A.indices[j]];
        }
        dst[row] = result * div;
    }
}    // namespace jacobi_smp

namespace qi = boost::spirit::qi;
namespace phx = boost::phoenix;

void init(
    jacobi_smp::crs_matrix<double>& M, std::size_t dim, std::size_t non_zeros)
{
    M.values.reserve(non_zeros);
    M.indices.reserve(non_zeros);
    M.rows.reserve(dim + 1);
    M.rows.push_back(0);
}

void add_entry(jacobi_smp::crs_matrix<double>& M, std::size_t& row,
    std::size_t& n, std::size_t j, std::size_t i, double v)
{
    M.values.push_back(v);
    M.indices.push_back(j);
    ++n;
    if (i != row)
    {
        row = i;
        M.rows.push_back(M.rows.back() + n);
        n = 0;
    }
}

int pika_main(variables_map& vm)
{
#if !defined(JACOBI_SMP_NO_pika)
    pika::scoped_finalize f;
#endif

    {
        std::size_t iterations = vm["iterations"].as<std::size_t>();
        std::size_t block_size = vm["block-size"].as<std::size_t>();
        std::string matrix = vm["matrix"].as<std::string>();
        std::string mode = vm["mode"].as<std::string>();

        jacobi_smp::crs_matrix<double> A;
        std::vector<double> x;
        std::vector<double> b;

        {
            std::ifstream file(matrix.c_str());
            file.unsetf(std::ios_base::skipws);
            using iterator = boost::spirit::istream_iterator;
            iterator begin(file);
            iterator end;

            std::size_t current_row = 0;
            std::size_t non_zero_row = 0;
            std::size_t dim = 0;
            std::size_t non_zero_entries = 0;

            qi::phrase_parse(begin, end,
                (qi::int_[phx::ref(dim) = qi::_1] >> qi::int_ >>
                    qi::int_[phx::ref(non_zero_entries) = qi::_1])[phx::bind(
                    init, phx::ref(A), dim, non_zero_entries)] >>
                    *(    // entry
                        (qi::int_ >> qi::int_ >> qi::double_)[phx::bind(
                            add_entry, phx::ref(A), phx::ref(current_row),
                            phx::ref(non_zero_row), qi::_1, qi::_2, qi::_3)]),
                qi::space |
                    (qi::lit("%") >> *(!qi::eol >> qi::char_) >> qi::eol));

            if (dim == 0)
            {
                std::cerr << "Parsed zero non zero values in matrix file "
                          << matrix << "\n";
                return 1;
            }

            std::cout << "A: " << dim << "x" << dim
                      << " number of non zeros: " << non_zero_entries << "\n";
        }
        if (vm.count("vector"))
        {
            std::string vector = vm["vector"].as<std::string>();
            std::ifstream file(vector.c_str());
            file.unsetf(std::ios_base::skipws);
            using iterator = boost::spirit::istream_iterator;
            iterator begin(file);
            iterator end;
            qi::phrase_parse(begin, end, *qi::double_, qi::ascii::space, b);
        }
        else
        {
            b = std::vector<double>(A.rows.size() - 1, 1.0);
        }
        std::cout << "b: " << b.size() << "\n";

        if (mode == "solve")
        {
            jacobi_smp::jacobi(A, b, iterations, block_size);
        }
        else if (mode == "statistics")
        {
            std::size_t min_per_row = (std::numeric_limits<std::size_t>::max)();
            std::size_t max_per_row = 0;
            double mean_per_row = 0.0;
            for (std::size_t r = 0; r < b.size(); ++r)
            {
                const std::size_t begin = A.row_begin(r);
                const std::size_t end = A.row_end(r);
                std::size_t n_row = end - begin;
                mean_per_row += double(n_row);
                if (n_row > max_per_row)
                {
                    max_per_row = n_row;
                }
                if (n_row < min_per_row)
                {
                    min_per_row = n_row;
                }
            }
            std::cout << "Matrix has " << A.values.size()
                      << " non zero entries\n";
            std::cout << "order: " << b.size() << "x" << b.size() << "\n";
            std::cout << "Entries per row:\n";
            std::cout << "\tmax " << max_per_row << "\n";
            std::cout << "\tmin " << min_per_row << "\n";
            std::cout << "\tmean " << mean_per_row / double(b.size()) << "\n";
            std::cout << "Density is: "
                      << double(A.values.size()) / double(b.size() * b.size())
                      << "\n";
        }
        else
        {
            std::cout << "Unknown mode " << mode << "\n";
            return 1;
        }
    }

    return 0;
}

int main(int argc, char** argv)
{
    options_description desc_cmd(
        "usage: " PIKA_APPLICATION_STRING " [options]");

    // clang-format off
    desc_cmd.add_options()
        ("iterations", value<std::size_t>()->default_value(1000),
         "Number of iterations")
        ("block-size", value<std::size_t>()->default_value(256),
         "Block size of the different chunks to calculate in parallel")
        ("matrix", value<std::string>(),
         "Filename of the input matrix (Matrix Market format)")
        ("vector", value<std::string>(), "Filename of the right hand side vector")
        ("mode", value<std::string>()->default_value("solve"),
        "Mode of the program, can be solve or statistics (default: solve)");
    // clang-format on

#if defined(JACOBI_SMP_NO_pika)
    variables_map vm;
    desc_cmd.add_options()("help", "This help message");
    store(parse_command_line(argc, argv, desc_cmd), vm);
    if (vm.count("help"))
    {
        std::cout << desc_cmd;
        return 1;
    }
    return pika_main(vm);
#else
    pika::init_params init_args;
    init_args.desc_cmdline = desc_cmd;

    return pika::init(pika_main, argc, argv, init_args);
#endif
}

max_stars_repo_name: pika-org/pika

max_issues_repo_name: pika-org/pika

max_forks_repo_name: pika-org/pika


text:
#include "hsm/details/flatten_transition_table.h"
#include "hsm/details/state.h"
#include "hsm/details/transition_table.h"
#include "hsm/front/transition_tuple.h"

#include <gtest/gtest.h>

#include <boost/hana/at.hpp>
#include <boost/hana/experimental/printable.hpp>
#include <boost/hana/front.hpp>
#include <boost/hana/size.hpp>
#include <boost/hana/tuple.hpp>
#include <boost/hana/type.hpp>

#include <array>

using namespace ::testing;
using namespace hsm;
using namespace boost::hana;

namespace {

struct event {
};
struct guard {
};
struct action {
};

struct T {
    // Non constexpr data member
    std::string data = "42";
};

    struct P{
        static constexpr auto make_transition_table()
        {
            return hsm::transition_table(hsm::transition(
                state<T> {}, hsm::event<event> {}, guard {}, action {}, state<T> {}));
        }

        // Non constexpr data member
        std::string data = "42";
    };

    struct S {
        static constexpr auto make_transition_table()
        {
            return hsm::transition_table(hsm::transition(
                state<T> {}, hsm::event<event> {}, guard {}, action {}, state<P> {}));
        }

        // Non constexpr data member
        std::string data = "42";
    };

class FlattenTransitionTableTests : public Test {
};
}

TEST_F(FlattenTransitionTableTests, should_flatten_with_parent_state)
{
    constexpr auto transitions = flatten_transition_table(state<S> {});

    ASSERT_EQ(size_c<2>, size(transitions));
}

ext: cpp

max_stars_repo_name: gjasny/hsm

max_issues_repo_name: gjasny/hsm

max_forks_repo_name: gjasny/hsm


text:
#pragma once

#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/filesystem.hpp>
#include <boost/log/attributes.hpp>
#include <boost/log/attributes/named_scope.hpp>
#include <boost/log/attributes/timer.hpp>
#include <boost/log/common.hpp>
#include <boost/log/expressions.hpp>
#include <boost/log/expressions/keyword.hpp>
#include <boost/log/sinks/sync_frontend.hpp>
#include <boost/log/sinks/text_file_backend.hpp>
#include <boost/log/sources/logger.hpp>
#include <boost/log/sources/severity_channel_logger.hpp>
#include <boost/log/sources/severity_logger.hpp>
#include <boost/log/support/date_time.hpp>
#include <boost/log/trivial.hpp>
#include <boost/log/utility/setup/common_attributes.hpp>
#include <boost/log/utility/setup/console.hpp>
#include <boost/log/utility/setup/file.hpp>
#include <boost/log/utility/setup/filter_parser.hpp>
#include <boost/log/utility/setup/formatter_parser.hpp>
#include <boost/log/utility/setup/from_stream.hpp>
#include <fstream>
#include "core.hpp"

namespace logging = boost::log;
using namespace logging::trivial;
#define DEFAULT_LOG_CONFIG "log.cfg"
#define DB_LOG_SEV(scl, level, message)                               \
  BOOST_LOG_SEV((scl), (level)) << __FILE__ << ":" << __LINE__ << " " \
                                << __FUNCTION__ << "() " << (message)

#define DB_LOG(level, message)                                   \
  BOOST_LOG_TRIVIAL(level) << __FILE__ << ":" << __LINE__ << " " \
                           << __FUNCTION__ << "() " << (message)

#define DB_CHECK(cond, level, message) \
  do {                                 \
    if (cond) {                        \
      DB_LOG(level, message);          \
    }                                  \
  } while (0)

// severity_level
#define ErrCheck(cond, level, err_code, message) \
  do {                                           \
    if (cond) {                                  \
      DB_LOG(level, message);                    \
      return (err_code);                         \
    }                                            \
  } while (0)

bool init_log_environment(std::string cfg = DEFAULT_LOG_CONFIG) {
  if (!boost::filesystem::exists("./log/")) {
    boost::filesystem::create_directory("./log/");
  }
  logging::add_common_attributes();

  logging::register_simple_formatter_factory<severity_level, char>("Severity");
  logging::register_simple_filter_factory<severity_level, char>("Severity");

  std::ifstream file(cfg);
  try {
    logging::init_from_stream(file);
  } catch (const std::exception& e) {
    std::cerr << "init_logger is fail, read log config file fail. curse: "
              << e.what() << std::endl;
    exit(-2);
  }
  return true;
}

ext: hpp

max_stars_repo_name: wangAlpha/SimpleDB

max_issues_repo_name: wangAlpha/SimpleDB

max_forks_repo_name: wangAlpha/SimpleDB


text:
#include "multibrot_opencl_calculator.h"

#include <boost/format.hpp>
#include <unordered_map>

namespace {
static const char* kMainProgram = R"(
/*
Requires a definition:
- REAL_T floating point type (acceptable types are float and, if device supports, half and double),
- RESULT_T - type of result pixel component (either uchar or ushort)
- RESULT_MAX - max value accepted by RESULT_T (either CHAR_MAX or USHRT_MAX)
- POWER_FUNC - name of a function that executes power of Z and adds C
  Function must take the following parameters:
  - Z = (zreal, zimg) - input and output number
  - zlen_sqr - square of absolute value of Z (square)
  - power
  - C = (real, img) - the second addend
  In general this function should do the following mathematical operation:
  (zreal, zimg) = (zreal, zimg) ^ power + (real, img)
  Different functions are used for optimisation purposes
*/

// Preprocessor magic based on https://stackoverflow.com/a/1489985
#define PASTER_2(x,y) x ## y
#define PASTER_3(x,y,z) x ## y ## z
#define EVALUATOR_2(x,y)  PASTER_2(x,y)
#define EVALUATOR_3(x,y,z)  PASTER_3(x,y,z)
#define CONVERT EVALUATOR_3(convert_, RESULT_T, _sat)
#define REAL_T4 EVALUATOR_2(REAL_T, 4)

// Universal complex number power function, supports all real powers but the slowest
void UniversalPowerOfComplex(
    __private REAL_T* restrict zreal,
    __private REAL_T* restrict zimg,
    const REAL_T zlen_sqr,
    const REAL_T power,
    const REAL_T real,
    const REAL_T img
)
{
    // Formula for z^p is a multi-value function, by we use a basic plane only
    REAL_T multiplier = powr( (REAL_T)(zlen_sqr), (REAL_T)(0.5*power) );
    // TODO danger zone here - at first iteration atan2 gets zero as both arguments
    // Fix UB somehow
    REAL_T phi = atan2( *zimg, *zreal );
    *zreal = multiplier * cos( power * phi ) + real;
    *zimg = multiplier * sin( power * phi ) + img;
}

// Should be used when power is 1
void Power1OfComplex(
    __private REAL_T* restrict zreal,
    __private REAL_T* restrict zimg,
    const REAL_T zlen_sqr,
    const REAL_T power,
    const REAL_T real,
    const REAL_T img
)
{
    *zreal += real;
    *zimg += img;
}

// Should be used when power is 2
void SquareOfComplex(
    __private REAL_T* restrict zreal,
    __private REAL_T* restrict zimg,
    const REAL_T zlen_sqr,
    const REAL_T power,
    const REAL_T real,
    const REAL_T img
)
{
    REAL_T zreal_new = (*zreal) * (*zreal) - (*zimg) * (*zimg) + real;
    *zimg = 2 * (*zreal) * (*zimg) + img;
    *zreal = zreal_new;
}

// Should be used when power is 3
void CubeOfComplex(
    __private REAL_T* restrict zreal,
    __private REAL_T* restrict zimg,
    const REAL_T zlen_sqr,
    const REAL_T power,
    const REAL_T real,
    const REAL_T img
)
{
    REAL_T zreal_new = pown(*zreal, 3) - 3 * (*zreal) * (*zimg) * (*zimg) + real;
    *zimg = 3 * (*zreal) * (*zreal) * (*zimg) - pown(*zimg, 3) + img;
    *zreal = zreal_new;
}

/*
    Calculate number of iterations that point after which point
    escaped the set.
    Returns raw number of iterations.
*/
REAL_T CalcPointOnMultibrotSet( REAL_T real, REAL_T img, REAL_T power, ushort max_iter_number )
{
    REAL_T iter_number = 0;
    REAL_T zreal = 0;
    REAL_T zimg = 0;
    REAL_T zlen_sqr = 0;
    while ( zlen_sqr < 2*2 && iter_number < max_iter_number )
    {
        POWER_FUNC( &zreal, &zimg, zlen_sqr, power, real, img );

        zlen_sqr = zreal*zreal + zimg*zimg;
        iter_number += 1;
    }
    return iter_number;
}

#ifdef COLOR_ENABLED
/*
    Scales iteration number and picks a color.
    May be used only when RESULT_T is uchar4 or ushort4.
    Useful for building color images.
*/
RESULT_T ProcessIterationNumber( REAL_T iter_number, ushort max_iter_number )
{
    // Calculate a color in HSV
    // This algorithm is based on https://www.codingame.com/playgrounds/2358/how-to-plot-the-mandelbrot-set/adding-some-colors
    REAL_T value = iter_number < max_iter_number ? RESULT_MAX : 0;
    REAL_T hue = ( RESULT_MAX / max_iter_number ) * iter_number;
    // Saturation is always max.
    // Convert to RGB
    REAL_T hue_section = hue / ( RESULT_MAX / 6 ); // hue_section is in range [0; 6]
    uchar i1 = convert_uchar_sat_rtz( hue_section / 2 );
    uchar i2 = convert_uchar_sat_rtz( remainder( hue_section, 2 ) );
    uchar ic = ( i1 + i2 ) % 3;
    uchar ix = ( i1 - i2 + 1 ) % 3;

    REAL_T rgb[3] = { 0 };
    const REAL_T chroma = value; // * saturation
    REAL_T second_color = chroma * ( 1 - fabs( remainder( hue_section, 2 ) - 1 ) );
    rgb[ic] = chroma;
    rgb[ix] = second_color;

    REAL_T4 result = (REAL_T4)( vload3( 0, rgb ), RESULT_MAX );
    return CONVERT( result );
}
#else
/*
    Scales iteration number so it uses all available values from 0 to max supported by a given
    number type.
    May be used only when RESULT_T is uchar or ushort.
    Useful for building grayscale images.
*/
RESULT_T ProcessIterationNumber( REAL_T iter_number, ushort max_iter_number )
{
    iter_number = ( RESULT_MAX / max_iter_number ) * iter_number;
    iter_number = RESULT_MAX - iter_number;
    return CONVERT( iter_number );
}
#endif

/*
    Build an image of Mandelbrot or Multibrot set.

    (rmin, imin) - complex value that corresponds to the left top corner of the image,
    (rmax, imax) - right bottom corner.

    Output is a pointer to buffer that will contain pixel data. Data format is defined by RESULT_T.

    Must be called with two-dimensional work assignment, where dimensions are used as image
    size (first dimension is width, second dimension is height).
    Pixels are stored in row-major order.
*/
__kernel void MultibrotSetKernel(
    REAL_T rmin, REAL_T imin,
    REAL_T rmax, REAL_T imax,
    REAL_T power,
    ushort max_iter_number,
    __global RESULT_T* restrict output
)
{
    REAL_T rstep = ( rmax - rmin ) / get_global_size(0);
    REAL_T istep = ( imax - imin ) / get_global_size(1);

    size_t row_width = get_global_size(0);
    size_t result_index = get_global_id(1) * row_width + get_global_id(0);

    REAL_T real = rmin + get_global_id(0) * rstep;
    REAL_T img = imin + get_global_id(1) * istep;
    REAL_T multibrot_val = CalcPointOnMultibrotSet( real, img, power, max_iter_number );
    RESULT_T result = ProcessIterationNumber( multibrot_val, max_iter_number );
    output[result_index] = result;
}
)";

template <typename T>
struct TempValueConstants {
    static const char* opencl_type_name;
    static const char* required_extension;
};

const char* TempValueConstants<half_float::half>::opencl_type_name = "half";
const char* TempValueConstants<half_float::half>::required_extension = "cl_khr_fp16";

const char* TempValueConstants<float>::opencl_type_name = "float";
const char* TempValueConstants<float>::required_extension = "";

const char* TempValueConstants<double>::opencl_type_name = "double";
const char* TempValueConstants<double>::required_extension = "cl_khr_fp64";

template <typename P>
struct ResultTypeConstants {
    static const char* result_type_name;
    static const char* result_max_val_macro;
    static const int result_max_val;
    static const bool color_enabled;
};

// Grayscale 8 bit
const char* ResultTypeConstants<cl_uchar>::result_type_name = "uchar";
const char* ResultTypeConstants<cl_uchar>::result_max_val_macro = "UCHAR_MAX";
const int ResultTypeConstants<cl_uchar>::result_max_val = CL_UCHAR_MAX;
const bool ResultTypeConstants<cl_uchar>::color_enabled = false;

// Grayscale 16 bit
const char* ResultTypeConstants<cl_ushort>::result_type_name = "ushort";
const char* ResultTypeConstants<cl_ushort>::result_max_val_macro = "USHRT_MAX";
const int ResultTypeConstants<cl_ushort>::result_max_val = CL_USHRT_MAX;
const bool ResultTypeConstants<cl_ushort>::color_enabled = false;

// RGB 8 bit
const char* ResultTypeConstants<cl_uchar4>::result_type_name = "uchar4";
const char* ResultTypeConstants<cl_uchar4>::result_max_val_macro = "UCHAR_MAX";
const int ResultTypeConstants<cl_uchar4>::result_max_val = CL_UCHAR_MAX;
const bool ResultTypeConstants<cl_uchar4>::color_enabled = true;

// RGB 16 bit
const char* ResultTypeConstants<cl_ushort4>::result_type_name = "ushort4";
const char* ResultTypeConstants<cl_ushort4>::result_max_val_macro = "USHRT_MAX";
const int ResultTypeConstants<cl_ushort4>::result_max_val = CL_USHRT_MAX;
const bool ResultTypeConstants<cl_ushort4>::color_enabled = true;
}  // namespace

template <typename T, typename P>
MultibrotOpenClCalculator<T, P>::MultibrotOpenClCalculator(
    const boost::compute::device& device, const boost::compute::context& context,
    size_t max_width_pix, size_t max_height_pix)
    : device_(device),
      context_(context),
      queue_(context, device, boost::compute::command_queue::enable_profiling),
      max_width_pix_(max_width_pix),
      max_height_pix_(max_height_pix),
      output_device_vector_(max_width_pix * max_height_pix, context) {
    BuildKernels();
}

template <typename T, typename P>
std::string MultibrotOpenClCalculator<T, P>::PrepareCompilerOptions(const std::string& power_func) {
    return (boost::format("-Werror -DREAL_T=%1% -DRESULT_T=%2% -DRESULT_MAX=%3% "
                          "-DPOWER_FUNC=%4% %5% ") %
            TempValueConstants<T>::opencl_type_name % ResultTypeConstants<P>::result_type_name %
            ResultTypeConstants<P>::result_max_val_macro % power_func %
            (ResultTypeConstants<P>::color_enabled ? "-DCOLOR_ENABLED" : ""))
        .str();
}

template <typename T, typename P>
void MultibrotOpenClCalculator<T, P>::BuildKernels() {
    // Collecting required extensions
    std::string required_extension = TempValueConstants<T>::required_extension;
    std::vector<std::string> extensions;
    if (!required_extension.empty()) {
        extensions.push_back(required_extension);
    }

    static const std::unordered_map<double /* power */, std::string> kFixedPowerFunctions = {
        {1.0, "Power1OfComplex"},
        {2.0, "SquareOfComplex"},
        {3.0, "CubeOfComplex"},
    };

    for (const auto& d : kFixedPowerFunctions) {
        specialized_kernels_.emplace(
            d.first, Utils::BuildKernel(
                         "MultibrotSetKernel", context_, kMainProgram,
                         PrepareCompilerOptions(d.second), extensions));
    }

    universal_kernel_ = Utils::BuildKernel(
        "MultibrotSetKernel", context_, kMainProgram,
        PrepareCompilerOptions("UniversalPowerOfComplex"), extensions);
}

template <typename T, typename P>
void MultibrotOpenClCalculator<T, P>::ExecutePrecalculateChecks(
    size_t width_pix, size_t height_pix, int max_iterations) {
    EXCEPTION_ASSERT(width_pix <= max_width_pix_);
    EXCEPTION_ASSERT(height_pix <= max_height_pix_);
    // Verify that given max iterations is valid for given pixel bit depth
    EXCEPTION_ASSERT(max_iterations <= ResultTypeConstants<P>::result_max_val);
}


text:
// Copyright 2004, 2005 The Trustees of Indiana University.

// 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)

//  Authors: Jeremiah Willcock
//           Douglas Gregor
//           Andrew Lumsdaine
#ifndef BOOST_GRAPH_ERDOS_RENYI_GENERATOR_HPP
#define BOOST_GRAPH_ERDOS_RENYI_GENERATOR_HPP

#include <cassert>
#include <iterator>
#include <utility>
#include <boost/shared_ptr.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/random/geometric_distribution.hpp>
#include <boost/type_traits/is_base_and_derived.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/config/no_tr1/cmath.hpp>

namespace boost {

  template<typename RandomGenerator, typename Graph>
  class erdos_renyi_iterator
  {
    typedef typename graph_traits<Graph>::directed_category directed_category;
    typedef typename graph_traits<Graph>::vertices_size_type vertices_size_type;
    typedef typename graph_traits<Graph>::edges_size_type edges_size_type;

    BOOST_STATIC_CONSTANT
      (bool,
       is_undirected = (is_base_and_derived<undirected_tag,
                                            directed_category>::value
                        || is_same<undirected_tag, directed_category>::value));

  public:
    typedef std::input_iterator_tag iterator_category;
    typedef std::pair<vertices_size_type, vertices_size_type> value_type;
    typedef const value_type& reference;
    typedef const value_type* pointer;
    typedef void difference_type;

    erdos_renyi_iterator() : gen(), n(0), edges(0), allow_self_loops(false) {}
    erdos_renyi_iterator(RandomGenerator& gen, vertices_size_type n,
                         double fraction = 0.0, bool allow_self_loops = false)
      : gen(&gen), n(n), edges(edges_size_type(fraction * n * n)),
        allow_self_loops(allow_self_loops)
    {
      if (is_undirected) edges = edges / 2;
      next();
    }

    erdos_renyi_iterator(RandomGenerator& gen, vertices_size_type n,
                         edges_size_type m, bool allow_self_loops = false)
      : gen(&gen), n(n), edges(m),
        allow_self_loops(allow_self_loops)
    {
      next();
    }

    reference operator*() const { return current; }
    pointer operator->() const { return &current; }

    erdos_renyi_iterator& operator++()
    {
      --edges;
      next();
      return *this;
    }

    erdos_renyi_iterator operator++(int)
    {
      erdos_renyi_iterator temp(*this);
      ++(*this);
      return temp;
    }

    bool operator==(const erdos_renyi_iterator& other) const
    { return edges == other.edges; }

    bool operator!=(const erdos_renyi_iterator& other) const
    { return !(*this == other); }

  private:
    void next()
    {
      uniform_int<vertices_size_type> rand_vertex(0, n-1);
      current.first = rand_vertex(*gen);
      do {
        current.second = rand_vertex(*gen);
      } while (current.first == current.second && !allow_self_loops);
    }

    RandomGenerator* gen;
    vertices_size_type n;
    edges_size_type edges;
    bool allow_self_loops;
    value_type current;
  };

  template<typename RandomGenerator, typename Graph>
  class sorted_erdos_renyi_iterator
  {
    typedef typename graph_traits<Graph>::directed_category directed_category;
    typedef typename graph_traits<Graph>::vertices_size_type vertices_size_type;
    typedef typename graph_traits<Graph>::edges_size_type edges_size_type;

    BOOST_STATIC_CONSTANT
      (bool,
       is_undirected = (is_base_and_derived<undirected_tag,
                                            directed_category>::value
                        || is_same<undirected_tag, directed_category>::value));

  public:
    typedef std::input_iterator_tag iterator_category;
    typedef std::pair<vertices_size_type, vertices_size_type> value_type;
    typedef const value_type& reference;
    typedef const value_type* pointer;
    typedef void difference_type;

    sorted_erdos_renyi_iterator()
      : gen(), rand_vertex(0.5), n(0), allow_self_loops(false),
    src((std::numeric_limits<vertices_size_type>::max)()), tgt(0), prob(0) {}
    sorted_erdos_renyi_iterator(RandomGenerator& gen, vertices_size_type n,
                    double prob = 0.0,
                                bool allow_self_loops = false)
      : gen(),
        // The "1.0 - prob" in the next line is to work around a Boost.Random
        // (and TR1) bug in the specification of geometric_distribution.  It
        // should be replaced by "prob" when the issue is fixed.
        rand_vertex(1.0 - prob),
    n(n), allow_self_loops(allow_self_loops), src(0), tgt(0), prob(prob)
    {
      this->gen.reset(new uniform_01<RandomGenerator>(gen));

      if (prob == 0.0) {src = (std::numeric_limits<vertices_size_type>::max)(); return;}
      next();
    }

    reference operator*() const { return current; }
    pointer operator->() const { return &current; }

    sorted_erdos_renyi_iterator& operator++()
    {
      next();
      return *this;
    }

    sorted_erdos_renyi_iterator operator++(int)
    {
      sorted_erdos_renyi_iterator temp(*this);
      ++(*this);
      return temp;
    }

    bool operator==(const sorted_erdos_renyi_iterator& other) const
    { return src == other.src && tgt == other.tgt; }

    bool operator!=(const sorted_erdos_renyi_iterator& other) const
    { return !(*this == other); }

  private:
    void next()
    {
      using std::sqrt;
      using std::floor;

      // In order to get the edges from the generator in sorted order, one
      // effective (but slow) procedure would be to use a
      // bernoulli_distribution for each legal (src, tgt) pair.  Because of the
      // O(n^2) cost of that, a geometric distribution is used.  The geometric
      // distribution tells how many times the bernoulli_distribution would
      // need to be run until it returns true.  Thus, this distribution can be
      // used to step through the edges which are actually present.  Everything
      // beyond "tgt += increment" is done to effectively convert linear
      // indexing (the partial sums of the geometric distribution output) into
      // graph edges.
      assert (src != (std::numeric_limits<vertices_size_type>::max)());
      vertices_size_type increment = rand_vertex(*gen);
      tgt += increment;
      if (is_undirected) {
    // Update src and tgt based on position of tgt
    // Basically, we want the greatest src_increment such that (in \bbQ):
    // src_increment * (src + allow_self_loops + src_increment - 1/2) <= tgt
    // The result of the LHS of this, evaluated with the computed
    // src_increment, is then subtracted from tgt
    double src_minus_half = (src + allow_self_loops) - 0.5;
    double disc = src_minus_half * src_minus_half + 2 * tgt;
    double src_increment_fp = floor(sqrt(disc) - src_minus_half);
    vertices_size_type src_increment = vertices_size_type(src_increment_fp);
    if (src + src_increment >= n) {
      src = n;
    } else {
      tgt -= (src + allow_self_loops) * src_increment +
         src_increment * (src_increment - 1) / 2;
      src += src_increment;
    }
      } else {
    // Number of out edge positions possible from each vertex in this graph
    vertices_size_type possible_out_edges = n - (allow_self_loops ? 0 : 1);
    src += (std::min)(n - src, tgt / possible_out_edges);
    tgt %= possible_out_edges;
      }
      // Set end of graph code so (src, tgt) will be the same as for the end
      // sorted_erdos_renyi_iterator
      if (src >= n) {src = (std::numeric_limits<vertices_size_type>::max)(); tgt = 0;}
      // Copy (src, tgt) into current
      current.first = src;
      current.second = tgt;
      // Adjust for (src, src) edge being forbidden
      if (!allow_self_loops && tgt >= src) ++current.second;
    }

    shared_ptr<uniform_01<RandomGenerator> > gen;
    geometric_distribution<vertices_size_type> rand_vertex;
    vertices_size_type n;
    bool allow_self_loops;
    vertices_size_type src, tgt;
    value_type current;
    double prob;
  };

} // end namespace boost

#endif // BOOST_GRAPH_ERDOS_RENYI_GENERATOR_HPP


text:
#include <iomanip>
#include <cstdlib>
#include <math.h>
#include <cmath>
#include <random>
#include <Eigen/Eigen>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

#include "gtest/gtest.h"
#include "../include/unscented_kalman_filter.h"

namespace rviz {
  cv::Point2i cv_offset(
      Eigen::Vector2d e_p, int image_width=2000, int image_height=2000){
    cv::Point2i output;
    output.x = int(e_p(0) * 100) + image_width/2;
    output.y = image_height - int(e_p(1) * 100) - image_height/3;
    return output;
  };

  void ellipse_drawing(
    cv::Mat bg_img, Eigen::Matrix2d pest, Eigen::Vector2d center,
    cv::Scalar ellipse_color=cv::Scalar(0, 0, 255)
  ){
    Eigen::EigenSolver<Eigen::Matrix2d> ces(pest);
    Eigen::Matrix2d e_value = ces.pseudoEigenvalueMatrix();
    Eigen::Matrix2d e_vector = ces.pseudoEigenvectors();

    double angle = std::atan2(e_vector(0, 1), e_vector(0, 0));
    cv::ellipse(
      bg_img,
      cv_offset(center, bg_img.cols, bg_img.rows),
      cv::Size(e_value(0,0)*100, e_value(1,1)*100),
      angle / M_PI * 180,
      0,
      360,
      ellipse_color,
      2,
      4);
  };
}

namespace ukf {
// Constan velocity model
auto motion_model_cv = [](Eigen::Vector4d x, Eigen::Vector2d u, double dt)-> Eigen::Vector4d {
  Eigen::Matrix4d F;
  F <<1.0,  dt,   0.,   0.,
          0., 1.0,   0.,   0.,
          0.,   0., 1.0,   dt,
          0.,   0.,   0.,  1.0;

  return F * x;
};

auto observation_model_cv = [](Eigen::Vector4d x) -> Eigen::Vector2d {
  Eigen::Matrix<double, 2, 4> H;
  H << 1., 0., 0., 0.,
       0., 0., 1., 0.;
  return H * x;
};

auto wrap_angles_default = [](Eigen::VectorXd x) -> Eigen::VectorXd {
  return x;
};

auto linear_motion_model = [](Eigen::Vector2d x, Eigen::Vector2d u, double dt)-> Eigen::Vector2d {
  Eigen::Matrix2d F;
  F <<  1.0, dt,
        0, 1.0;

  return F * x;
};

auto observation_model = [](Eigen::Vector2d x) -> Eigen::Vector2d {
  Eigen::Matrix<double, 2, 2> H;
  H << 1., 0.,
       0., 1.;
  return H * x;
};

auto observation_model_2x4 = [](Eigen::Vector4d x) -> Eigen::Vector2d {
  // State to measurement function H, is with dimensions dim_z x dim_x with ones  in the locations to be updated.
  Eigen::Matrix<double, 2, 4> H;
  H << 1., 0., 0., 0.,
       0., 1., 0., 0.;
  return H * x;
};

auto motion_model_4x4 = [](Eigen::Vector4d x, Eigen::Vector2d u, double dt) -> Eigen::Vector4d {
  Eigen::Matrix4d F;
  F <<1.0,   0.,   0.,   0.,
        0., 1.0,   0.,   0.,
        0.,   0., 1.0,   0.,
        0.,   0.,   0.,  0.; // TODO: Should this be a 1.0? PythonRobotics has 0.0

  // control input v (m/s), yaw_rate (rad/s)
  Eigen::Matrix<double, 4, 2> B;
  B << dt * std::cos(x(2)),  0.,
       dt * std::sin(x(2)),  0.,
                        0.0,  dt,
                        1.0,  0.0;
  return F * x + B * u;
};

auto wrap_angles = [](Eigen::Vector4d x) -> Eigen::Vector4d{
  // When state is represented by [x y yaw v].
  Eigen::Vector4d angles_wrapped = x;

  while (angles_wrapped(2) < -M_PI) {
    angles_wrapped(2) += (2*M_PI);
  }

  while (angles_wrapped(2) > M_PI) {
    angles_wrapped(2) -= (2*M_PI);
  }

  return angles_wrapped;
};

Eigen::VectorXd residuals(Eigen::VectorXd const& sigma, Eigen::VectorXd const& x)
{
  return sigma - x;
}

Eigen::VectorXd residuals_x(Eigen::VectorXd const& sigma, Eigen::VectorXd const& x)
{
  // Residuals when state is represented by [x y yaw v].
  Eigen::VectorXd residuals(x.rows());
  residuals(0) = sigma(0) - x(0);
  residuals(1) = sigma(1) - x(1);
  residuals(3) = sigma(3) - x(3);

  auto yaw_residual = sigma(2) - x(2);

  while (yaw_residual < -M_PI) {
    yaw_residual += (2*M_PI);
  }

  while (yaw_residual > M_PI) {
    yaw_residual -= (2*M_PI);
  }
  //assert((yaw_residual < M_PI) && (yaw_residual > -M_PI));
  residuals(2) = yaw_residual;
  return residuals;
}

Eigen::VectorXd mean_fn_x(Eigen::MatrixXd const& sigmas, Eigen::VectorXd const& Wm)
{
  // State mean when state is represented by [x y yaw v].
  Eigen::VectorXd mean(sigmas.cols());
  mean.setZero();
  double ss = 0.0;
  double sc = 0.0;

  for (auto i=0; i< sigmas.rows(); ++i) {
    Eigen::VectorXd s = sigmas.row(i);

    mean(0) += s(0) * Wm(i);
    mean(1) += s(1) * Wm(i);
    mean(3) += s(3) * Wm(i);
    ss += std::sin(s(2)) * Wm(i);
    sc += std::cos(s(2)) * Wm(i);
  }
  mean(2) = std::atan2(ss,sc);
  return mean;
}

Eigen::VectorXd mean_fn(Eigen::MatrixXd const& sigmas, Eigen::VectorXd const& Wm)
{
  return sigmas.transpose() * Wm;
}

auto subtract_fn = residuals;
auto subtract_fn_x = residuals_x;
}

TEST(UKFTest, NumSigmaTest) {
  auto points = ukf::MerweScaledSigmaPoints(2, 0.1,0.1,0.1, ukf::subtract_fn);
  auto n_sigmas = points.num_sigmas();
  EXPECT_EQ(n_sigmas, 5);
}

TEST(UKFTest, SigmaPoints2DTest) {
  auto n = 2;
  auto alpha = 1e-3;
  auto beta = 2.0;
  auto kappa = 0.0;
  auto points = ukf::MerweScaledSigmaPoints(n, alpha, beta, kappa, ukf::subtract_fn);

  Eigen::Vector2d x;
  x << 1.,2.;

  Eigen::Matrix2d P;
  P << 2.,1.2,
       1.2,2.;

  auto sigmas = points.sigma_points(x, P);
  Eigen::Matrix<double, 5, 2> sigmas_expected;

  sigmas_expected << 1., 2.,
                    1.002, 2.0012,
                    1., 2.0016,
                    0.9980, 1.9988,
                    1., 1.9984;

  ASSERT_TRUE((sigmas - sigmas_expected).norm() < 1e-5);
}

TEST(UKFTest, SigmaPointsTest) {
  auto points = ukf::MerweScaledSigmaPoints(4, 1e-3,2.0,0.0, ukf::subtract_fn);
  Eigen::Vector4d x = Eigen::Vector4d::Zero();
  Eigen::Matrix4d P = Eigen::Matrix4d::Identity();
  auto sigmas = points.sigma_points(x, P);

  Eigen::Matrix<double, 9, 4> sigmas_expected;
  sigmas_expected << 0., 0., 0., 0.,
                    0.0020, 0., 0., 0.,
                    0., 0.0020, 0., 0.,
                    0., 0., 0.0020, 0.,
                    0., 0., 0., 0.0020,
                    -0.0020, 0., 0., 0.,
                    0., -0.0020, 0., 0.,
                    0., 0., -0.0020, 0.,
                    0., 0., 0., -0.0020;

  ASSERT_TRUE((sigmas - sigmas_expected).norm() < 1e-5);
  ASSERT_TRUE(std::abs(points.get_wm().sum()-1.) < 1e-5);
}

TEST(UKFTest, ComputeWeightsTest) {
  auto n = 2;
  auto alpha = 1e-3;
  auto beta = 2.0;
  auto kappa = 0.0;
  auto points = ukf::MerweScaledSigmaPoints(n, alpha, beta, kappa, ukf::subtract_fn);
  auto wm = points.get_wm();

  Eigen::Matrix<double,5,1> wm_expected;
  wm_expected << -999998.9999712,
                  249999.9999928,
                  249999.9999928,
                  249999.9999928,
                  249999.9999928;

  ASSERT_TRUE((wm - wm_expected).norm() < 1e-5);
}

TEST(UKFTest, UnscentedTransformTest) {
  auto n = 2;
  auto alpha = 1e-3;
  auto beta = 2.0;
  auto kappa = 0.0;
  auto points = ukf::MerweScaledSigmaPoints(n, alpha, beta, kappa, ukf::subtract_fn);

  Eigen::Vector2d x;
  x << 1.,2.;

  Eigen::Matrix2d P;
  P << 2.,1.2,
       1.2,2.;

  Eigen::Matrix2d noise_cov;
  noise_cov << 0.,0.,
               0.,0.;

  auto sigmas = points.sigma_points(x, P);

  auto pair = ukf::unscented_transform(sigmas, points.get_wm(), points.get_wc(), noise_cov, ukf::residuals, ukf::mean_fn);

  ASSERT_TRUE((std::get<0>(pair) - x).norm() < 1e-5);
  ASSERT_TRUE((std::get<1>(pair) - P).norm() < 1e-5);
}

TEST(UKFTest, ResidualsTest) {
  auto n = 2;
  auto alpha = 0.001;
  auto beta = 2.0;
  auto kappa = 0.0;
  auto points = ukf::MerweScaledSigmaPoints(n, alpha, beta, kappa, ukf::subtract_fn);

  Eigen::Vector2d x;
  x << 1.,2.;

  Eigen::Matrix2d P;
  P << 2.,1.2,
       1.2,2.;

  auto sigmas = points.sigma_points(x, P);

  Eigen::MatrixXd residuals = ukf::residuals(sigmas.row(1), x);
  Eigen::Matrix<double,2,1> residuals_expected;
  residuals_expected << 0.002,0.0012;
  ASSERT_TRUE((residuals - residuals_expected).norm() < 1e-5);
}

TEST(UKFTest, UKFPredictTest) {
  auto n = 2;
  auto alpha = 0.001;
  auto beta = 2.0;
  auto kappa = 0.0;
  auto points = ukf::MerweScaledSigmaPoints(n, alpha, beta, kappa, ukf::subtract_fn);

  Eigen::Vector2d x;
  x << 1.,2.;

  Eigen::Matrix2d P;
  P << 2.,1.2,
       1.2,2.;

  Eigen::Vector2d u;
  u << 1.0, 0.1;

  Eigen::VectorXd  z = Eigen::VectorXd::Zero(n);

  auto dim_z = n;
  auto dt = 0.1;
  auto ukf = ukf::UnscentedKalmanFilter(
                n,
                dim_z,
                dt,
                points,
                ukf::wrap_angles_default,
                ukf::linear_motion_model,
                ukf::observation_model,
                ukf::residuals,
                ukf::residuals,
                ukf::mean_fn,
                ukf::mean_fn);

  ukf.predict(u);
  ukf.update(z);
  //TODO: FINISH TESTING
}

TEST(UKFTest, CrossVarianceTest) {
  auto n = 2;
  auto dim_z = 2;
  auto dt = 0.1;
  auto alpha = 0.001;
  auto beta = 2.0;
  auto kappa = 0.0;
  auto points = ukf::MerweScaledSigmaPoints(n, alpha, beta, kappa, ukf::subtract_fn);

  Eigen::Vector2d x;
  x << 1.,2.;

  Eigen::Matrix2d P;
  P << 2.,1.2,
       1.2,2.;

  // control input
  Eigen::Vector2d u;
  u << 1.0, 0.1;

  Eigen::VectorXd  z = Eigen::VectorXd::Zero(n);

  // Get sigmas_f (process)
  Eigen::MatrixXd sigmas = points.sigma_points(x, P);
  auto n_sigmas = points.num_sigmas();
  Eigen::MatrixXd sigmas_f = Eigen::MatrixXd::Zero(n_sigmas, n);

  // Pass the sigmas through the process model.
  for (auto i=0; i<n_sigmas; i++) {
    sigmas_f.row(i) = ukf::linear_motion_model(sigmas.row(i), u, dt);
  }

  // Get sigmas_h (measurement)
  Eigen::MatrixXd sigmas_h = Eigen::MatrixXd::Zero(n_sigmas, dim_z);

  // Pass the sigmas processed by the process(motion) model through the observation model.
  for (auto i=0; i<n_sigmas; i++) {
    sigmas_h.row(i) = ukf::observation_model(sigmas_f.row(i));
  }

  auto ukf = ukf::UnscentedKalmanFilter(
                n,
                dim_z,
                dt,
                points,
                ukf::wrap_angles_default,
                ukf::linear_motion_model,
                ukf::observation_model,
                ukf::residuals,
                ukf::residuals,
                ukf::mean_fn,
                ukf::mean_fn);

  auto CV = ukf.cross_variance(x, z,sigmas_f, sigmas_h);
  // TODO: Finish test
}

TEST(UKFTest, SimulationSimpleCVTest) {
  // Constant Velocity model
  auto n = 4; // [x, x_dot, y, y_dot]
  auto dt = 0.1;
  auto alpha = 0.1;
  auto beta = 2.0;
  auto kappa = 1.0;
  auto points = ukf::MerweScaledSigmaPoints(n, alpha, beta, kappa, ukf::subtract_fn);

  // Dont really need, but current implementation requires a u to pass to motion_model.
  Eigen::Vector2d u;
  u << 0.0, 0.0;

  // noisy control input
  Eigen::Vector2d ud;

  // observation z
  Eigen::Vector2d z;

  // dead reckoning
  Eigen::Vector4d xDR;
  xDR << 0.1,1.0,0.1,1.0;

  // ground truth reading
  Eigen::Vector4d xTrue;
  xTrue << 0.1,1.0,0.1,1.0;

  Eigen::Vector4d xEst;
  xEst << 0.1,1.0,0.1,1.0;

  std::vector<Eigen::Vector4d> hxDR;
  std::vector<Eigen::Vector4d> hxTrue;
  std::vector<Eigen::Vector4d> hxEst;
  std::vector<Eigen::Vector2d> hz;

  Eigen::Matrix4d PEst = Eigen::Matrix4d::Identity();

  // Motion model covariance
  Eigen::Matrix4d Q = Eigen::Matrix4d::Identity();
  Q(0,0)=0.25 * (dt*dt*dt*dt) * 0.02;
  Q(0,1)=0.5 * (dt*dt*dt) * 0.02;
  Q(1,0)=0.5 * (dt*dt*dt) * 0.02;
  Q(1,1)=(dt*dt) * 0.02;
  Q(2,2)=0.25 * (dt*dt*dt*dt) * 0.02;
  Q(2,3)=0.5 * (dt*dt*dt) * 0.02;
  Q(3,2)=0.5 * (dt*dt*dt) * 0.02;
  Q(3,3)=(dt*dt) * 0.02;

  Eigen::Matrix2d R = Eigen::Matrix2d::Identity();
  R(0,0)=0.09;
  R(1,1)=0.09;

  // Motion model simulation error
  Eigen::Matrix2d Qsim = Eigen::Matrix2d::Identity();
  Qsim(0,0)=0.25 * (dt*dt*dt*dt) * 0.02;
  Qsim(1,1)=(dt*dt) * 0.02;

  // Observation model simulation error
  Eigen::Matrix2d Rsim = Eigen::Matrix2d::Identity();
  Rsim(0,0)=0.09;
  Rsim(1,1)=0.09;

  auto ukf = ukf::UnscentedKalmanFilter(
                n,
                z.rows(),
                dt,
                points,
                ukf::wrap_angles_default,
                ukf::motion_model_cv,
                ukf::observation_model_cv,
                ukf::residuals,
                ukf::residuals,
                ukf::mean_fn,
                ukf::mean_fn);
    ukf.Q = Q;
    ukf.R = R;
    auto time = 0.0;

    std::random_device rd{};
    std::mt19937 gen{rd()};
    std::normal_distribution<> gaussian_d{0,1};

    cv::namedWindow("ukf");
    while (time < 50.0) {
      time += dt;

      ud(0) = u(0) + gaussian_d(gen) * Qsim(0,0);
      ud(1) = u(1) + gaussian_d(gen) * Qsim(1,1);

      xTrue = ukf::motion_model_cv(xTrue, u, dt);
      //std::cout << "xTrue" << xTrue << std::endl;
      xDR = ukf::motion_model_cv(xDR, ud, dt);

      z(0) = time + gaussian_d(gen) * Rsim(0,0);
      z(1) = time + gaussian_d(gen) * Rsim(1,1);

      ukf.predict(ud);
      ukf.update(z);

      auto xEst = ukf.x_post;

      hxDR.push_back(xDR);
      hxTrue.push_back(xTrue);
      hxEst.push_back(xEst);
      hz.push_back(z);

      //visualization

      cv::Mat bg(700,1500, CV_8UC3, cv::Scalar(255,255,255));

      for(unsigned int j=0; j<hxDR.size(); j++){

        // Green groundtruth
        // State is found at index 0 and 2.
        Eigen::Vector2d x_true;
        x_true << hxTrue[j](0), hxTrue[j](2);
        cv::circle(bg, rviz::cv_offset(x_true, bg.cols, bg.rows),
                   7, cv::Scalar(0,255,0), 2);

        // blue estimation
        Eigen::Vector2d x_est;
        x_est << hxEst[j](0),hxEst[j](2);
        cv::circle(bg, rviz::cv_offset(x_est, bg.cols, bg.rows),
                   10, cv::Scalar(255,0,0), 5);

        // black dead reckoning
        Eigen::Vector2d x_dr;
        x_dr << hxDR[j](0),hxDR[j](2);
        cv::circle(bg, rviz::cv_offset(x_dr, bg.cols, bg.rows),
                   7, cv::Scalar(0, 0, 0), -1);
      }

      // red observation
      for(unsigned int i=0; i<hz.size(); i++){
        cv::circle(bg, rviz::cv_offset(hz[i], bg.cols, bg.rows),
                 7, cv::Scalar(0, 0, 255), -1);
      }

      Eigen::Vector2d ellipse_x_est;
      ellipse_x_est << xEst(0), xEst(2);
      rviz::ellipse_drawing(bg, PEst.block(0,0,2,2), ellipse_x_est);

      cv::imshow("ukf", bg);
      cv::waitKey(5);

    }
}

TEST(UKFTest, SimulationTest) {
  auto n = 4; // [x y yaw v]'
  auto dt = 0.1;
  auto alpha = 0.001;
  auto beta = 2.0;
  auto kappa = 0.0;
  auto points = ukf::MerweScaledSigmaPoints(n, alpha, beta, kappa, ukf::subtract_fn_x);

  // control input v (m/s), yaw_rate (rad/s)
  Eigen::Vector2d u;
  u << 1.0, 0.1;

  // noisy control input
  Eigen::Vector2d ud;

  // observation z
  Eigen::Vector2d z;

  // dead reckoning
  Eigen::Vector4d xDR;
  xDR << 0.1,0.1,0.0,1.0;

  // ground truth reading
  Eigen::Vector4d xTrue;
  xTrue << 0.1,0.1,0.0,1.0;;

  Eigen::Vector4d xEst;
  xEst << 0.1,0.1,0.0,1.0;;

  std::vector<Eigen::Vector4d> hxDR;
  std::vector<Eigen::Vector4d> hxTrue;
  std::vector<Eigen::Vector4d> hxEst;
  std::vector<Eigen::Vector2d> hz;

  Eigen::Matrix4d PEst = Eigen::Matrix4d::Identity();

  // Motional model covariance
  Eigen::Matrix4d Q = Eigen::Matrix4d::Identity();
  Q(0,0)=0.25 * (dt*dt*dt*dt) * 0.02;
  Q(0,1)=0.5 * (dt*dt*dt) * 0.02;
  Q(1,0)=0.5 * (dt*dt*dt) * 0.02;
  Q(1,1)=(dt*dt) * 0.02;
  Q(2,2)=0.25 * (dt*dt*dt*dt) * 0.02;
  Q(2,3)=0.5 * (dt*dt*dt) * 0.02;
  Q(3,2)=0.5 * (dt*dt*dt) * 0.02;
  Q(3,3)=(dt*dt) * 0.02;

  Eigen::Matrix2d R = Eigen::Matrix2d::Identity();
  R(0,0)=0.09;
  R(1,1)=0.09;

  // Motion model simulation error
  Eigen::Matrix2d Qsim = Eigen::Matrix2d::Identity();
  Qsim(0,0)=0.09;
  Qsim(1,1)=0.09;

  // Observation model simulation error
  Eigen::Matrix2d Rsim = Eigen::Matrix2d::Identity();
  Rsim(0,0)=0.09;
  Rsim(1,1)=0.09;

  auto ukf = ukf::UnscentedKalmanFilter(
                n,
                z.rows(),
                dt,
                points,
                ukf::wrap_angles,
                ukf::motion_model_4x4,
                ukf::observation_model_2x4,
                ukf::residuals_x,
                ukf::residuals,
                ukf::mean_fn_x,
                ukf::mean_fn);

  // Initial settings.
  ukf.x = xTrue;
  ukf.Q = Q;
  ukf.R = R;

  auto time = 0.0;

  std::random_device rd{};
  std::mt19937 gen{rd()};
  std::normal_distribution<> gaussian_d{0,1};

  cv::namedWindow("ukf");
  auto count = 0;
  while (time < 100.0) {
    time += dt;

    ud(0) = u(0) + gaussian_d(gen) * Qsim(0,0);
    ud(1) = u(1) + gaussian_d(gen) * Qsim(1,1);

    xTrue = ukf::motion_model_4x4(xTrue, u, dt);

    xDR = ukf::motion_model_4x4(xDR, ud, dt);

    z(0) = xTrue(0) + gaussian_d(gen) * Rsim(0,0);
    z(1) = xTrue(1) + gaussian_d(gen) * Rsim(1,1);

    ukf.predict(ud);
    ukf.update(z);

    auto xEst = ukf.x_post;

    hxDR.push_back(xDR);
    hxTrue.push_back(xTrue);
    hxEst.push_back(xEst);
    hz.push_back(z);

    // Visualization adapted from https://github.com/onlytailei/CppRobotics
    cv::Mat bg(700,1500, CV_8UC3, cv::Scalar(255,255,255));
    for(unsigned int j=0; j<hxDR.size(); j++){

      // green groundtruth
      cv::circle(bg, rviz::cv_offset(hxTrue[j].head(2), bg.cols, bg.rows),
                 7, cv::Scalar(0,255,0), -1);

      // blue estimation
      cv::circle(bg, rviz::cv_offset(hxEst[j].head(2), bg.cols, bg.rows),
                 10, cv::Scalar(255,0,0), 5);

      // black dead reckoning
      cv::circle(bg, rviz::cv_offset(hxDR[j].head(2), bg.cols, bg.rows),
                 7, cv::Scalar(0, 0, 0), -1);
    }

    // red observation
    for(unsigned int i=0; i<hz.size(); i++){
      cv::circle(bg, rviz::cv_offset(hz[i], bg.cols, bg.rows),
               7, cv::Scalar(0, 0, 255), -1);
    }

    rviz::ellipse_drawing(bg, PEst.block(0,0,2,2), xEst.head(2));
    cv::imshow("ukf", bg);
    cv::waitKey(5);

    std::string int_count = std::to_string(count++);
    cv::imwrite("/home/tasuku/ws/CppLocalization/src/video/ukf_"+int_count+".png", bg);
  }
}

TEST(UKFTest, InitTest) {
  auto dim_x = 4;
  auto dim_z = 2;
  auto dt = 1.0;
  auto alpha = 1e-3;
  auto beta = 2.0;
  auto kappa = 0.0;
  auto points = ukf::MerweScaledSigmaPoints(dim_x, alpha, beta, kappa, ukf::subtract_fn);

  ukf::UnscentedKalmanFilter ukf = ukf::UnscentedKalmanFilter(
                                                dim_x,
                                                dim_z,
                                                dt,
                                                points,
                                                ukf::wrap_angles_default,
                                                ukf::linear_motion_model,
                                                ukf::observation_model,
                                                ukf::residuals,
                                                ukf::residuals,
                                                ukf::mean_fn,
                                                ukf::mean_fn);
  uint dim = ukf.x.size();
  EXPECT_EQ(dim, 4);
}

max_stars_repo_name: surfertas/CppLocalization

max_issues_repo_name: surfertas/CppLocalization

max_forks_repo_name: surfertas/CppLocalization


text:
/** MIT License
Copyright (c) 2017 Sudarshan Raghunathan
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*
*
*@copyright Copyright 2017 Sudarshan Raghunathan
*@file test_navig.cpp
*@author Sudarshan Raghunathan
*@brief  Unit Test for all the functions in the turtlebot navigation class
*/
#include <cv_bridge/cv_bridge.h>
#include <gtest/gtest.h>
#include <ros/package.h>
#include <cstdlib>
#include <string>
#include <sstream>
#include <opencv2/highgui/highgui.hpp>
#include <boost/thread.hpp>
#include "opencv2/opencv.hpp"
#include "ros/ros.h"
#include "ros/console.h"
#include "xrrobot.hpp"
#include "xr_line_follower/pos.h"

/**
*@brief Testing if detection works accurately and publishes straight accurately
*@return int dir which is the direction to move in
*/
double ang_vel(int direction) {
    ros::NodeHandle n;
    turtlebot bot;
    geometry_msgs::Twist velocity;
    ros::Publisher pub = n.advertise<geometry_msgs::Twist>
    ("/cmd_vel_mux/input/teleop", 10);
    bot.dir = direction;
    ros::Rate rate(10);
    ros::spinOnce();
    bot.vel_cmd(velocity, pub, rate);
    rate.sleep();
    return velocity.angular.z;
}
/**
*@brief Testing if detection works accurately and publishes straight accurately
*@return int dir which is the direction to move in
*/
double linear_vel(int direction) {
    ros::NodeHandle n;
    xrrobot bot;
    geometry_msgs::Twist velocity;
    ros::Publisher pub = n.advertise<geometry_msgs::Twist>
    ("/cmd_vel_mux/input/teleop", 10);

    bot.dir = direction;
    ros::Rate rate(100);
    ros::spinOnce();
    bot.vel_cmd(velocity, pub, rate);
    rate.sleep();
    return velocity.linear.x;
}
/**
*@brief Function to spin the callbacks at a specific rate
*/
void processThread(void) {
    ros::Rate rate(10);
    while (ros::ok()) {
        ros::spinOnce();
        rate.sleep();
    }
}
/**
*@brief Testing if message subscriber is working properly
*/
TEST(TestROS, TestPubSub) {
  ros::NodeHandle nh;
  geometry_msgs::Twist velocity;
  ros::Rate rate(10);
  turtlebot bot;
  ros::Publisher pub = nh.advertise<xr_line_follower::pos>
    ("direction", 1000);
  ros::Subscriber sub = nh.subscribe("/direction",
        1, &turtlebot::dir_sub, &bot);

    xr_line_follower::pos msg;
    msg.direction = 0;
    pub.publish(msg);

    ros::spinOnce();
    rate.sleep();
    EXPECT_EQ(1, sub.getNumPublishers());
    EXPECT_EQ(1, pub.getNumSubscribers());
    EXPECT_EQ(msg.direction, bot.dir);
}
/**
*@brief Testing if velocity published is for moving straight
*/
TEST(TestVelocity, Teststraight_vel) {
    double rot = ang_vel(1);
    double trans = linear_vel(1);
    EXPECT_EQ(0, rot);
    EXPECT_EQ(0.15, trans);
}
/**
*@brief Testing if velocity published is for turning left
*/
TEST(TestDirections, Testleft_vel) {
    double rot = ang_vel(0);
    double trans = linear_vel(0);
    EXPECT_EQ(0.15, rot);
    EXPECT_EQ(0.1, trans);
}
/**
*@brief Testing if velocity published is for turning right
*/
TEST(TestDirections, Testright_vel) {
    double rot = ang_vel(2);
    double trans = linear_vel(2);
    EXPECT_EQ(-0.15, rot);
    EXPECT_EQ(0.1, trans);
}
/**
*@brief Testing if velocity published is for searching
*/
TEST(TestDirections, Testsearch_vel) {
    double rot = ang_vel(3);
    double trans = linear_vel(3);
    EXPECT_EQ(0.25, rot);
    EXPECT_EQ(0, trans);
}
/**
 *@brief Function to run all the tests for the navigation node
 *@param argc is the number of arguments of the main function
 *@param argv is the array of arugments
 *@return result of the tests
 */
int main(int argc, char **argv) {
    ros::init(argc, argv, "test_velocity");
    testing::InitGoogleTest(&argc, argv);
    ros::NodeHandle nh;
    boost::thread th(processThread);
    int test_flag = RUN_ALL_TESTS();
    ros::shutdown();
    th.join();
    return test_flag;
}


text:
// Copyright (c) 2012 Hasso-Plattner-Institut fuer Softwaresystemtechnik GmbH. All rights reserved.
#include "base.h"
#include "incremental.h"

#include <boost/foreach.hpp>
#include <map>

namespace hyrise {
namespace layouter {

void IncrementalCandidateLayouter::clearState() {
  _splittedSets.clear();
  _unusedPartitions.clear();

  _query_conversion = std::map<unsigned, unsigned>();
  _reverse_query_conversion = std::map<unsigned, unsigned>();
}

void IncrementalCandidateLayouter::detectAffectedPartitions(Query* q) {
  // Now get all containers and compare to the best original
  // layout
  Result currentBest = getBestResult();
  Layout::internal_layout_t currentLayout = currentBest.layout.raw();

  // Create a set for easier deletion
  set_subset_t current = set_subset_t(q->queryAttributes.begin(), q->queryAttributes.end());

  // Compare the current query set against all others
  for (size_t j = 0; j < currentLayout.size(); ++j) {
    subset_t base = currentLayout[j];

    bool found = false;
    BOOST_FOREACH(unsigned baseAttr, base) {
      BOOST_FOREACH(unsigned queryAttr, current) {
        if (baseAttr == queryAttr) {
          current.erase(queryAttr);
          found = true;
          break;  // exit to container
        }
      }

      if (found) {
        _splittedSets.push_back(base);
        break;  // exit to next container
      }
    }

    // Capture all unsused partitions
    if (!found) {
      _unusedPartitions.push_back(base);
    }
  }
}

Schema IncrementalCandidateLayouter::buildSchema(subset_t attributes, Query* q) {
  // Modify Schema
  Schema s = schema;
  s.attributes = attributes;
  s.nbAttributes = attributes.size();

  // Add new queries
  s.queries.push_back(q);

  _unusedCostForPartitions = std::vector<double>(s.queries.size(), 0.0);

  // remove all queries that are not used and rewrite the uesed
  // ones to the new layout
  std::vector<Query*> newQueries;
  for (size_t j = 0; j < s.queries.size(); ++j) {
    Query* newQ = new Query(*s.queries[j]);

    // we have to check if this accesses the original
    // attributes
    bool found = false;
    for (size_t i = 0; i < newQ->queryAttributes.size(); ++i) {
      if (_query_conversion.count(newQ->queryAttributes[i]) > 0)
        found = true;

      newQ->queryAttributes[i] = _query_conversion[newQ->queryAttributes[i]];
    }

    if (found)
      newQueries.push_back(newQ);
    else {
      // Cache the cost for the query
      // TODO check if the last query may not be part of this check
      _unusedCostForPartitions[j] = getBestResult().cost[j];
      delete newQ;
    }
  }

  s.queries = newQueries;

  return s;
}

Result IncrementalCandidateLayouter::rewriteResult(Result affectedBest) {
  // Results contain only those parts that were modified, we
  // need to attach the rest. Since we rewrote the original
  // queries and containers, we have to reverse this
  Layout rewritten;

  BOOST_FOREACH(subset_t t, affectedBest.layout.raw()) {
    subset_t newContainer;
    BOOST_FOREACH(unsigned i, t) { newContainer.push_back(_reverse_query_conversion[i]); }

    rewritten.add(newContainer);
  }

  // Add the unused partitions to the new result layout
  BOOST_FOREACH(subset_t t, _unusedPartitions)
  rewritten.add(t);

  // SInce we cached the cost for all queries (including the new
  // one) we can now insert the modified cost at the slots where
  // the cost are 0 because the query is not part of the cached
  // ones
  for (size_t i = 0, j = 0; i < _unusedCostForPartitions.size(); ++i) {
    if (_unusedCostForPartitions[i] == 0.0) {
      _unusedCostForPartitions[i] = affectedBest.cost[j++];
    }
  }

  // Recalculate cost
  // Result newresult(rewritten, getCost(rewritten));
  Result newresult(rewritten, _unusedCostForPartitions);
  return newresult;
}

void IncrementalCandidateLayouter::incrementalLayout(Query* q) {
  clearState();
  detectAffectedPartitions(q);

  // Perform layout for each of the queries for the layout
  // subset
  subset_t qa;
  unsigned counter = 0;
  BOOST_FOREACH(subset_t i, _splittedSets) {
    BOOST_FOREACH(unsigned j, i) {
      // Width of an attribute
      qa.push_back(4);
      _query_conversion[j] = counter;
      _reverse_query_conversion[counter++] = j;
    }
  }

  // Now we instantiate a new base layouter and find all
  // matching layouts
  CandidateLayouter bl;
  Schema mappedSchema = buildSchema(qa, q);

  bl.layout(mappedSchema, HYRISE_COST);

  // // update my schema etc
  schema.queries.push_back(q);

  // // clear all results
  results.clear();

  // // TODO iterate over all results and add them
  Result mappedBest = bl.getBestResult();
  Result best = rewriteResult(mappedBest);
  results.push_back(best);
}
}
}  // namespace hyrise::layouter


text:
#ifndef _BOOST_RPC_RAW_HPP_
#define _BOOST_RPC_RAW_HPP_
#include <boost/reflect/reflect.hpp>

#include <boost/rpc/varint.hpp>
#include <boost/rpc/required.hpp>
#include <boost/rpc/errors.hpp>
#include <boost/cmt/log/log.hpp>
#include <boost/rpc/base64.hpp>
#include <sstream>
#include <iostream>
#include <map>
#include <iomanip>

#include <boost/fusion/container/vector.hpp>
#include <boost/fusion/algorithm.hpp>
#include <boost/fusion/support/is_sequence.hpp>
#include <boost/fusion/sequence/intrinsic/size.hpp>
#include <boost/rpc/datastream.hpp>

namespace boost { namespace rpc { namespace raw {

    template<typename Stream, typename T>
    void unpack( Stream& s, boost::optional<T>& v );
    template<typename Stream, typename T>
    void pack( Stream& s, const boost::optional<T>& v );
    template<typename Stream> inline void unpack( Stream& s, std::string& v );

    template<typename Stream, typename T>
    inline void pack( Stream& s, const T& v );
    template<typename Stream, typename T>
    inline void unpack( Stream& s, T& v );

    template<typename Stream, typename T, template<typename> class Alloc, template<typename,typename> class Container>
    inline void pack( Stream& s, const Container<T,Alloc<T> >& value );
    template<typename Stream, typename T, template<typename> class Alloc, template<typename,typename> class Container>
    inline void unpack( Stream& s, Container<T,Alloc<T> >& value );

    template<typename Stream, typename T, class Compare, template<typename> class Alloc, template<typename,class,typename> class Container>
    inline void pack( Stream& s, const Container<T,Compare,Alloc<T> >& value );
    template<typename Stream, typename T, class Compare, template<typename> class Alloc, template<typename,class,typename> class Container>
    inline void unpack( Stream& s, Container<T,Compare,Alloc<T> >& value );

    template<typename Stream, typename Key, typename Value>
    void pack( Stream& s, const std::map<Key,Value>& value );
    template<typename Stream, typename Key, typename Value>
    void unpack( Stream& s, std::map<Key,Value>& value );

    template<typename Stream> inline void pack( Stream& s, const signed_int& v ) {
      uint32_t val = (v.value<<1) ^ (v.value>>31);
      do {
        uint8_t b = uint8_t(val) & 0x7f;
        val >>= 7;
        b |= ((val > 0) << 7);
        s.put(b);
      } while( val );
    }
    template<typename Stream> inline void pack( Stream& s, const unsigned_int& v ) {
      uint64_t val = v.value;
      do {
        uint8_t b = uint8_t(val) & 0x7f;
        val >>= 7;
        b |= ((val > 0) << 7);
        s.put(b);
      }while( val );
    }

    template<typename Stream> inline void pack( Stream& s, const bool& v ) { pack( s, uint8_t(v) );     }
    template<typename Stream> inline void pack( Stream& s, const char* v ) { pack( s, std::string(v) ); }

    template<typename Stream, typename T>
    inline void pack( Stream& s, const required<T>& v ) { pack( s, *v); }

    template<typename Stream, typename T>
    inline void pack( Stream& s, const boost::optional<T>& v ) {
      pack( s, bool(v) );
      if( v )
        pack( s, *v );
    }
    template<typename Stream> inline void pack( Stream& s, const std::vector<char>& value ) {
      pack( s, unsigned_int(value.size()) );
      if( value.size() )
        s.write( &value.front(), value.size() );
    }
    template<typename Stream> inline void pack( Stream& s, const std::string& v )  {
      pack( s, unsigned_int(v.size()) );
      if( v.size() )
        s.write( v.c_str(), v.size() );
    }

    template<typename Stream> inline void unpack( Stream& s, signed_int& vi ) {
      uint32_t v = 0; char b = 0; int by = 0;
      do {
        s.get(b);
        v |= uint32_t(uint8_t(b) & 0x7f) << by;
        by += 7;
      } while( uint8_t(b) & 0x80 );
      vi.value = ((v>>1) ^ (v>>31)) + (v&0x01);
      vi.value = v&0x01 ? vi.value : -vi.value;
      vi.value = -vi.value;
    }
    template<typename Stream> inline void unpack( Stream& s, unsigned_int& vi ) {
      uint64_t v = 0; char b = 0; uint8_t by = 0;
      do {
          s.get(b);
          v |= uint32_t(uint8_t(b) & 0x7f) << by;
          by += 7;
      } while( uint8_t(b) & 0x80 );
      vi.value = v;
    }
    template<typename Stream> inline void unpack( Stream& s, bool& v ) {
      uint8_t b;
      unpack( s, b );
      v=b;
    }
    template<typename Stream> inline void unpack( Stream& s, std::vector<char>& value ) {
      unsigned_int size; unpack( s, size );
      value.resize(size.value);
      if( value.size() )
        s.read( &value.front(), value.size() );
    }
    template<typename Stream> inline void unpack( Stream& s, std::string& v )  {
      std::vector<char> tmp; unpack(s,tmp);
      v = std::string(tmp.begin(),tmp.end());
    }

    template<typename Stream, typename T>
    void unpack( Stream& s, boost::optional<T>& v ) {
      bool b; unpack( s, b );
      if( b ) { v = T(); unpack( s, *v ); }
    }
    template<typename Stream, typename T>
    void unpack( const Stream& s, required<T>& v ) {
      v = T(); unpack( s, *v );
    }
    //template<typename Stream, typename T, template<typename> class Alloc, template<typename,typename> class Container>
    //void unpack( const Stream& s, Container<T,Alloc<T> >& value );

    template<typename Stream, typename Key, typename Value>
    void unpack( const Stream& s, std::map<Key,Value>& value );
    template<typename Stream, typename Key, typename Value>
    void unpack( const Stream& s, std::pair<Key,Value>& value );
    template<typename Stream, typename Value>
    void unpack( const Stream& s, std::map<std::string,Value>& val );

    namespace detail {

      template<typename Stream, typename Class>
      struct pack_object_visitor {
        pack_object_visitor(const Class& _c, Stream& _s)
        :c(_c),s(_s){}

        /**
            VC++ does not understand the difference between:
            optional<T> Class::* and T Class::*
        */
        template<typename T>
        inline void pack_helper( const T& v, const char* name )const {
          boost::rpc::raw::pack( s, v );
        }
        template<typename T>
        inline void pack_helper( const boost::optional<T>& v, const char* name )const {
          if( !!v ) boost::rpc::raw::pack( s, *v );
        }
        template<typename T, typename C, T(C::*p)>
        inline void operator()( const char* name )const {
          pack_helper( c.*p, name );
        }
        private:
          const Class& c;
          Stream&      s;
      };

      template<typename Stream, typename Class>
      struct unpack_object_visitor {
        unpack_object_visitor(Class& _c, Stream& _s)
        :c(_c),s(_s){}

        template<typename T, typename C, T(C::*p)>
        inline void operator()( const char* name )const {
          boost::rpc::raw::unpack( s, c.*p );
        }
        private:
          Class&  c;
          Stream& s;
      };

      template<typename Stream>
      struct pack_sequence {
         pack_sequence( Stream& _s ):s(_s){}
         template<typename T>
         void operator() ( const T& v )const { boost::rpc::raw::pack(s,v); }
         Stream&    s;
      };

      template<typename Stream>
      struct unpack_sequence {
         unpack_sequence( Stream& _s ):s(_s){}
         template<typename T>
         void operator() ( T& v )const { boost::rpc::raw::unpack(s,v); }
         Stream&  s;
      };

      template<typename IsPod=boost::false_type>
      struct if_pod{
        template<typename Stream, typename T>
        static inline void pack( Stream& s, const T& v ) {
          s << v;
        }
        template<typename Stream, typename T>
        static inline void unpack( Stream& s, T& v ) {
          s >> v;
        }
      };

      template<>
      struct if_pod<boost::true_type> {
        template<typename Stream, typename T>
        static inline void pack( Stream& s, const T& v ) {
          s.write( (char*)&v, sizeof(v) );
        }
        template<typename Stream, typename T>
        static inline void unpack( Stream& s, T& v ) {
          s.read( (char*)&v, sizeof(v) );
        }
      };

      template<typename IsReflected=boost::false_type>
      struct if_reflected {
        template<typename Stream, typename T>
        static inline void pack( Stream& s, const T& v ) {
          if_pod<typename boost::is_pod<T>::type>::pack(s,v);
        }
        template<typename Stream, typename T>
        static inline void unpack( Stream& s, T& v ) {
          if_pod<typename boost::is_pod<T>::type>::unpack(s,v);
        }
      };
      template<>
      struct if_reflected<boost::true_type> {
        template<typename Stream, typename T>
        static inline void pack( Stream& s, const T& v ) {
          boost::reflect::reflector<T>::visit( pack_object_visitor<Stream,T>( v, s ) );
        }
        template<typename Stream, typename T>
        static inline void unpack( Stream& s, T& v ) {
          boost::reflect::reflector<T>::visit( unpack_object_visitor<Stream,T>( v, s ) );
        }
      };

      template<typename IsFusionSeq>
      struct if_fusion_seq {
        template<typename Stream, typename T>
        inline static void pack( Stream& s, const T& v ) {
          pack_sequence<Stream> pack_vector( s );
          boost::fusion::for_each( v, pack_vector );
        }
        template<typename Stream, typename T>
        inline static void unpack( Stream& s, T& v ) {
          unpack_sequence<Stream> unpack_vector(s);
          boost::fusion::for_each( v, unpack_vector );
        }
      };

      template<>
      struct if_fusion_seq<boost::mpl::false_> {
        template<typename Stream, typename T>
        inline static void pack( Stream& s, const T& v ) {
          if_reflected<typename boost::reflect::reflector<T>::is_defined>::pack(s,v);
        }
        template<typename Stream, typename T>
        inline static void unpack( Stream& s, T& v ) {
          if_reflected<typename boost::reflect::reflector<T>::is_defined>::unpack(s,v);
        }
      };
    } // namesapce detail

    template<typename Stream, typename T, class Compare, template<typename> class Alloc, template<typename,class,typename> class Container>
    inline void pack( Stream& s, const Container<T,Compare,Alloc<T> >& value ) {
      pack( s, unsigned_int(value.size()) );
      typename Container<T,Compare,Alloc<T> >::const_iterator itr = value.begin();
      typename Container<T,Compare,Alloc<T> >::const_iterator end = value.end();
      while( itr != end ) {
        boost::rpc::raw::pack( s, *itr );
        ++itr;
      }
    }

    template<typename Stream, typename T, class Compare, template<typename> class Alloc, template<typename,class,typename> class Container>
    inline void unpack( Stream& s, Container<T,Compare,Alloc<T> >& value ) {
      unsigned_int size; unpack( s, size );
      for( uint32_t i = 0; i < size.value; ++i ) {
        T v;
        boost::rpc::raw::unpack(s, v);
        value.insert(v);
      }
    }

    template<typename Stream, typename T, template<typename> class Alloc, template<typename,typename> class Container>
    inline void pack( Stream& s, const Container<T,Alloc<T> >& value ) {
      pack( s, unsigned_int(value.size()) );
      typename Container<T,Alloc<T> >::const_iterator itr = value.begin();
      typename Container<T,Alloc<T> >::const_iterator end = value.end();
      while( itr != end ) {
        boost::rpc::raw::pack( s, *itr );
        ++itr;
      }
    }

    template<typename Stream, typename T, template<typename> class Alloc, template<typename,typename> class Container>
    inline void unpack( Stream& s, Container<T,Alloc<T> >& value ) {
      unsigned_int size; unpack( s, size );
      value.resize(size.value);
      typename Container<T,Alloc<T> >::iterator itr = value.begin();
      typename Container<T,Alloc<T> >::iterator end = value.end();
      while( itr != end ) {
        boost::rpc::raw::unpack( s, *itr );
        ++itr;
      }
    }

    // support for pair!
    template<typename Stream, typename Key, typename Value>
    inline void pack( Stream& s, const std::pair<Key,Value>& val ) {
      boost::rpc::raw::pack( s, val.first );
      boost::rpc::raw::pack( s, val.second );
    }
    // support for pair!
    template<typename Stream, typename Key, typename Value>
    void unpack( Stream& s, std::pair<Key,Value>& val ) {
      boost::rpc::raw::unpack( s, val.first );
      boost::rpc::raw::unpack( s, val.second );
    }

    // support arbitrary key/value containers as an array of pairs
    template<typename Stream, typename Key, typename Value>
    void pack( Stream& s, const std::map<Key,Value>& value ) {
      pack( s, unsigned_int(value.size()) );
      typename std::map<Key,Value>::const_iterator itr = value.begin();
      typename std::map<Key,Value>::const_iterator end = value.end();
      while( itr != end ) {
        boost::rpc::raw::pack( s, *itr );
        ++itr;
      }
    }

    template<typename Stream, typename Key, typename Value>
    inline void unpack( Stream& s, std::map<Key,Value>& value ) {
      unsigned_int size; unpack( s, size );
      value.clear();
      for( uint32_t i = 0; i < size.value; ++i ) {
        Key k;
        boost::rpc::raw::unpack(s,k);
        boost::rpc::raw::unpack(s,value[k]);
      }
    }

    template<typename Stream, typename T>
    inline void pack( Stream& s, const T& v ) {
      detail::if_fusion_seq< typename boost::fusion::traits::is_sequence<T>::type >::pack(s,v);
    }
    template<typename Stream, typename T>
    inline void unpack( Stream& s, T& v ) {
      detail::if_fusion_seq< typename boost::fusion::traits::is_sequence<T>::type >::unpack(s,v);
    }

    template<typename T>
    inline void pack_vec( std::vector<char>& s, const T& v ) {
      datastream<size_t> ps;
      raw::pack(ps,v );
      s.resize(ps.tellp());
      if( s.size() ) {
        datastream<char*>  ds( &s.front(), s.size() );
        raw::pack(ds,v);
      }
    }

    template<typename T>
    inline void unpack_vec( const std::vector<char>& s, T& v ) {
      if( s.size() ) {
        datastream<const char*>  ds( &s.front(), s.size() );
        raw::unpack(ds,v);
      }
    }

    template<typename T>
    inline void pack( char* d, uint32_t s, const T& v ) {
      datastream<char*> ds(d,s);
      raw::pack(ds,v );
    }

    template<typename T>
    inline void unpack( const char* d, uint32_t s, T& v ) {
      datastream<const char*>  ds( d, s );
      raw::unpack(ds,v);
    }

} } } // namespace boost::rpc::raw

#endif // BOOST_RPC_RAW_HPP


text:
//
//  multiply.cpp
//  BGV-Adder
//
//  Created by Hindrik Stegenga on 27/10/2020.
//  Copyright © 2020 RUG. All rights reserved.
//

#include "multiply.hpp"
#include <helib/FHE.h>
#include <NTL/ZZX.h>
#include <NTL/tools.h>

using helib::Ctxt;

long compute_multiplication(i16 lhs, i16 rhs) {
    long k = 128; // Security parameter
    long L = 16; // Number of levels in the modulus default is 16
    long c = 3; // Nr of columns in key switch matrix.
    long w = 64; // secret key hamming weight

    long p = 1021; // plaintext base default = 1021
    long d = 0; // Degree of field extension
    long r = 1; // hensel lifting

    // Determine a value for m
    auto m = helib::FindM(k, L, c, p, d, 0, 0);
    // Setup context
    auto context = helib::Context(m, p, r);
    // Build mod chain
    helib::buildModChain(context, L, c);

    // Generating secret key and public key
    NTL::ZZX encryption_polynomial = context.alMod.getFactorsOverZZ()[0];
    auto secretKey = helib::SecKey(context);
    secretKey.GenSecKey();
    const helib::PubKey& publicKey = secretKey;

    // Initialize ciphertexts
    Ctxt lhs_ciphertext(publicKey), rhs_ciphertext(publicKey);

    // Encrypt them using the public key
    publicKey.Encrypt(lhs_ciphertext, NTL::ZZX(lhs));
    publicKey.Encrypt(rhs_ciphertext, NTL::ZZX(rhs));

    // Compute multiplication of a and b
    lhs_ciphertext *= rhs_ciphertext;

    Ctxt cipher_result = lhs_ciphertext;

    // Decrypt the results using secret key and convert back from
    // polynomial representation to numeric.
    long return_value = 0;
    NTL::ZZX plaintext_result;
    // Decrypt using secret key
    secretKey.Decrypt(plaintext_result, cipher_result);

    // Compensate for negative numbers by checking if it's larger than p/2,
    // In such case it wrapped around due to negative numbers
    conv(return_value, plaintext_result[0]);
    if (return_value > p / 2) {
        return_value += (-1 * p);
    }

    return return_value;
}

max_stars_repo_name: HindrikStegenga/fhe-toolkit-macos

max_issues_repo_name: HindrikStegenga/fhe-toolkit-macos

max_forks_repo_name: HindrikStegenga/fhe-toolkit-macos


text:
/****************************************************************************
 * Copyright (c) 2017-2021 by the ArborX authors                            *
 * All rights reserved.                                                     *
 *                                                                          *
 * This file is part of the ArborX library. ArborX is                       *
 * distributed under a BSD 3-clause license. For the licensing terms see    *
 * the LICENSE file in the top-level directory.                             *
 *                                                                          *
 * SPDX-License-Identifier: BSD-3-Clause                                    *
 ****************************************************************************/

#include <Kokkos_Core.hpp>

#define BOOST_TEST_NO_MAIN
#include <boost/test/unit_test.hpp>

#if defined(ARBORX_MPI_UNIT_TEST)
#include <mpi.h>
#endif

struct ExecutionEnvironmentScopeGuard
{
  ExecutionEnvironmentScopeGuard(int argc, char *argv[])
  {
#if defined(ARBORX_MPI_UNIT_TEST)
    MPI_Init(&argc, &argv);
#endif
    Kokkos::initialize(argc, argv);
  }
  ~ExecutionEnvironmentScopeGuard()
  {
    Kokkos::finalize();
#if defined(ARBORX_MPI_UNIT_TEST)
    MPI_Finalize();
#endif
  }
};

bool init_function() { return true; }

int main(int argc, char *argv[])
{
  ExecutionEnvironmentScopeGuard scope_guard(argc, argv);
  return boost::unit_test::unit_test_main(&init_function, argc, argv);
}

max_stars_repo_name: wjge/ArborX

max_issues_repo_name: wjge/ArborX

max_forks_repo_name: aprokop/ArborX


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

#include <atomic>

#include <boost/noncopyable.hpp>

namespace vtslibs { namespace storage {

class OpenFiles : boost::noncopyable
{
public:
    static void inc() { ++count_; }
    static void dec() { --count_; }
    static int count() { return count_; }

    static int treshold() { return threshold_; }
    static void treshold(int value) { threshold_ = value; }
    static bool critical() { return count_ >= threshold_; }

private:
    static std::atomic<int> count_;
    static int threshold_;
};

} } // namespace vtslibs::storage

#endif // vtslibs_storage_openfiles_hpp_included_

max_stars_repo_name: melowntech/vts-libs

max_issues_repo_name: HanochZhu/vts-browser-unity-plugin

max_forks_repo_name: HanochZhu/vts-browser-unity-plugin


text:
// Copyright 2017 Yahoo Holdings. Licensed under the terms of the Apache 2.0 license. See LICENSE in the project root.
#include "distribution_hash_normalizer.h"
#include <vespa/vespalib/stllike/asciistream.h>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix_core.hpp>
#include <boost/spirit/include/phoenix_object.hpp>
#include <boost/fusion/include/adapt_struct.hpp>
#include <boost/optional.hpp>
#include <boost/variant/recursive_wrapper.hpp>
#include <vector>
#include <algorithm>
#include <iterator>
#include <functional>

#include <vespa/log/bufferedlogger.h>
LOG_SETUP(".storage.bucketdb.distribution_hash_normalizer");

// TODO
// This code can be removed once we have a model out which ensures consistent
// ordering of nodes in the stor-distribution config.

namespace qi = boost::spirit::qi;
namespace ascii = boost::spirit::ascii;
namespace phoenix = boost::phoenix;

namespace {

struct GroupSet;

using Children = boost::variant<
    std::vector<unsigned int>,
    boost::recursive_wrapper<GroupSet>
>;

struct Group {
    uint16_t index;
    boost::optional<double> capacity;
    Children children;
    ~Group() {}
};

struct GroupSet {
    std::string distribution_spec;
    std::vector<Group> subgroups;
};

} // anon ns

// Fusion adaptations must be in global scope.
BOOST_FUSION_ADAPT_STRUCT(
    ::Group,
    (uint16_t, index)
    (boost::optional<double>, capacity)
    (Children, children)
)

BOOST_FUSION_ADAPT_STRUCT(
    ::GroupSet,
    (std::string, distribution_spec)
    (std::vector<Group>, subgroups)
)

namespace storage {
namespace {

// Boost.Spirit v2 grammar for parsing the output of lib::Group::getConfigHash.
template <typename Iterator>
struct HashGrammar
    : qi::grammar<Iterator, Group()>
{
    HashGrammar()
        : HashGrammar::base_type(group)
    {
        using qi::uint_;
        using qi::double_;
        using ascii::char_;
        /*
         * This grammar makes the (reasonable) assumption that you can't have
         * empty groups.
         *
         * Quick Spirit PEG DSL syntax primer for any two arbitrary parsers
         * a and b (all subcomponents of parsers are themselves parsers):
         *
         *   'X'     : character literal match parser
         *   a >> b  : a must be followed by b ("a b" in EBNF)
         *   -a      : optional ("a?" in EBNF)
         *   a | b   : a or b must match (same as in EBNF)
         *   +a      : match 1 or more times ("a+" in EBNF)
         *   *a      : kleene star; 0 or more times ("a*" in EBNF)
         *   a - b   : difference; a but not b
         *
         * Please see Boost.Spirit docs on how these map to parser attributes
         * (optional maps to boost::optional of nested attribute, + or kleene
         * star maps to an iterable range (std::vector) of nested attributes,
         * a | b maps to a boost::variant of the attributes of a and b,
         * a >> b maps to a boost::tuple of the attributes and so on; usually
         * fairly intuitive).
         */
        group =
                 '('
              >> uint_
              >> -('c' >> double_)
              >> ( +(';' >> uint_)
                 | subgroups
                 )
              >> ')';

        subgroups = ('d' >> distr_spec >> +group);

        distr_spec = +(char_ - '('); // Match everything until open paren.
    }

    qi::rule<Iterator, Group()> group;
    qi::rule<Iterator, GroupSet()> subgroups;
    qi::rule<Iterator, std::string()> distr_spec;
};

template <typename Range, typename Predicate>
auto ordered_by(const Range& range, Predicate pred) {
    std::vector<typename Range::value_type> copy(
            std::begin(range), std::end(range));
    std::sort(copy.begin(), copy.end(), pred);
    return copy;
}

void emit_normalized_groups(vespalib::asciistream& out, const Group& g);

struct InOrderGroupVisitor : boost::static_visitor<void> {
    vespalib::asciistream& _out;
    InOrderGroupVisitor(vespalib::asciistream& out)
        : _out(out)
    {
    }

    void operator()(const std::vector<unsigned int>& nodes) const {
        for (uint16_t node : ordered_by(nodes, std::less<void>())) {
            _out << ';' << node;
        }
    }

    void operator()(const GroupSet& gs) const {
        _out << 'd' << gs.distribution_spec;
        auto index_less_than = [](auto& lhs, auto& rhs) {
            return lhs.index < rhs.index;
        };
        // Ordering will also copy nested subgroups, but the number of known
        // Vespa installations with nested subgroups is currently somewhere
        // around the high end of zero.
        for (auto& g : ordered_by(gs.subgroups, index_less_than)) {
            emit_normalized_groups(_out, g);
        }
    }
};

void emit_normalized_groups(vespalib::asciistream& out, const Group& g) {
    out << '(' << g.index;
    if (g.capacity) {
        out << 'c' << *g.capacity;
    }
    boost::apply_visitor(InOrderGroupVisitor(out), g.children);
    out << ')';
}

} // anon ns

// We keep the grammar around across multiple normalized() calls because
// constructing the grammar object(s) isn't free.
struct DistributionHashNormalizer::ParserImpl {
    using Iterator = vespalib::string::const_iterator;
    HashGrammar<Iterator> grammar;
};

DistributionHashNormalizer::DistributionHashNormalizer()
    : _impl(std::make_unique<ParserImpl>())
{
}

// Required here because of incomplete ParserImpl in header.
DistributionHashNormalizer::~DistributionHashNormalizer()
{
}

vespalib::string
DistributionHashNormalizer::normalize(vespalib::stringref hash) const
{
    Group root;

    auto iter = hash.begin();
    const bool ok = qi::parse(iter, hash.end(), _impl->grammar, root);
    if (!ok || iter != hash.end()) {
        vespalib::string hash_str = hash; // stringref might not be zero-term'd.
        LOGBT(warning, hash_str.c_str(),
              "Unable to parse compact distribution config "
              "representation: '%s'",
              hash_str.c_str());
        return hash; // Fallback to input on parse failure.
    }

    vespalib::asciistream out;
    emit_normalized_groups(out, root);

    return out.str();
}

} // storage

max_stars_repo_name: atveit/vespa

max_issues_repo_name: atveit/vespa

max_forks_repo_name: atveit/vespa