/*
 * This file is part of SpatialCL, a library for the spatial processing of
 * particles.
 *
 * Copyright (c) 2017 Aksel Alpay
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this
 *    list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER 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.
 */

#include <iostream>
#include <vector>

#include <boost/preprocessor/stringize.hpp>

#include <SpatialCL/tree.hpp>
#include <SpatialCL/query.hpp>

#include <QCL/qcl.hpp>
#include <QCL/qcl_array.hpp>

#include "../../common/environment.hpp"
#include "../../common/random_vectors.hpp"
#include "../../common/verification_knn.hpp"

constexpr std::size_t particle_dimension = 3;

using tree_type = spatialcl::hilbert_bvh_sp3d_tree<particle_dimension>;
using type_system = tree_type::type_system;
using scalar = type_system::scalar;

const std::size_t num_particles = 32000;
const std::size_t num_queries = 2000;
constexpr std::size_t K = 8;

using particle_type = spatialcl::configuration<type_system>::particle_type;
using vector_type = spatialcl::configuration<type_system>::vector_type;
constexpr std::size_t dimension = type_system::dimension;

using strict_dfs_knn_engine =
  spatialcl::query::strict_dfs_knn_query_engine<tree_type, K>;

using relaxed_dfs_knn_engine =
  spatialcl::query::relaxed_dfs_knn_query_engine<tree_type, K>;

template <std::size_t Group_size>
using grouped_dfs_knn_engine =
  spatialcl::query::grouped_dfs_knn_query_engine<tree_type, K, Group_size>;

template<class Query_engine>
std::size_t execute_knn_query_test(const qcl::device_context_ptr& ctx,
                                   const tree_type& tree,
                                   const std::vector<vector_type>& host_queries,
                                   const qcl::device_array<vector_type>& queries,
                                   const std::vector<particle_type>& particles,
                                   qcl::device_array<particle_type>& result)
{
  // ToDo: Reset results

  Query_engine query_engine;
  typename Query_engine::handler_type query_handler {
    queries.get_buffer(),
    result.get_buffer(),
    queries.size()
  };

  std::cout << "Executing query..." << std::endl;
  // Executy query
  query_engine(tree, query_handler);

  cl_int err = ctx->get_command_queue().finish();
  qcl::check_cl_error(err, "Error while executing KNN query");

  // Retrieve results
  std::vector<particle_type> host_results;
  result.read(host_results);

  std::cout << "Verifying results, please wait..." << std::endl;
  // Verify results
  common::verification::naive_cpu_knn_verifier<type_system, K> verifier{
    host_queries
  };

  return verifier(particles, host_results);
}

int main(int argc, char* argv[])
{
  common::environment env;
  qcl::device_context_ptr ctx = env.get_device_context();

  std::vector<particle_type> particles;
  // Create random 3D positions
  common::random_vectors<scalar, particle_dimension> rnd;
  rnd(num_particles, particles);

  tree_type gpu_tree{ctx, particles};

  std::vector<vector_type> query_points;
  rnd(num_queries, query_points);

  qcl::device_array<vector_type> queries{ctx, query_points};
  qcl::device_array<particle_type> result{ctx, K * queries.size()};

  std::size_t num_errors = 0;

  #define RUN_TEST(test_name) \
  num_errors = \
      execute_knn_query_test<test_name>(ctx,          \
                                        gpu_tree,     \
                                        query_points, \
                                        queries,      \
                                        particles,    \
                                        result);      \
  std::cout << BOOST_PP_STRINGIZE(test_name) <<" completed queries with " \
            << num_errors << " errors." << std::endl

  RUN_TEST(strict_dfs_knn_engine);
  RUN_TEST(relaxed_dfs_knn_engine);
  RUN_TEST(grouped_dfs_knn_engine<16>);
  RUN_TEST(grouped_dfs_knn_engine<32>);
  RUN_TEST(grouped_dfs_knn_engine<64>);

  return 0;
}


#include <chrono>
#include <exception>
#include <iostream>
#include <string>
#include <tuple>
#include <vector>

#include <boost/algorithm/string.hpp>
#include <boost/container_hash/hash.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include "nlohmann/json.hpp"
#include "psvt/statistics.hpp"
#include "yhirose/httplib.h"

#include "api/isoline.hpp"                 // IsolineAPI
#include "api/stats.hpp"                   // StatsAPI
#include "distributions/angular_gauss.hpp" // AngularGauss
#include "grids/classic_grid.hpp"          // ClassicGrid
#include "grids/spiral_grid.hpp"           // SpiralGrid
#include "util/cfg.hpp"                    // cfg::kConfig

namespace ublas = boost::numeric::ublas;
using json = nlohmann::json;

// GridsCache stores grids by raw mean and cov parameters.
typedef std::unordered_map<std::tuple<std::array<double, 3>, std::array<double, 6>>,
                           std::tuple<ClassicGrid*, SpiralGrid*>,
                           boost::hash<std::tuple<std::array<double, 3>, std::array<double, 6>>>>
    GridsCache;

GridsCache* cache;

std::tuple<ClassicGrid*, SpiralGrid*> GetGrids(std::array<double, 3>& mean, std::array<double, 6>& cov) {
    auto key = std::make_tuple(mean, cov);
    if (cache->find(key) != cache->end()) {
        std::cout << "debug: got grids from cache" << std::endl;
        return cache->at(key);
    }

    ublas::matrix<double> cov_mat(3, 3);
    cov_mat(0,0) = cov[0], cov_mat(0,1) = cov[1], cov_mat(0,2) = cov[2];
    cov_mat(1,0) = cov[1], cov_mat(1,1) = cov[3], cov_mat(1,2) = cov[4];
    cov_mat(2,0) = cov[2], cov_mat(2,1) = cov[4], cov_mat(2,2) = cov[5];

    std::cout << "debug: going to calculate grids with params: mean=[" << mean[0] << "," << mean[1] << "," << mean[2] << "], cov=["
        << cov[0] << "," << cov[1] << "," << cov[2] << "," << cov[3] << "," << cov[4] << "," << cov[5] << "]\n";

    AngularGauss* distribution = new AngularGauss(mean, cov_mat);

    std::cout << "debug: going to calculate ClassicGrid with bw=" << cfg::kConfig["classic_grid_div"].get<int>() << std::endl;
    auto start = std::chrono::high_resolution_clock::now();
    ClassicGrid* classic_grid = new ClassicGrid(cfg::kConfig["classic_grid_div"].get<int>(), distribution);
    auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now() - start);
    std::cout << "debug: ClassicGrid is ready in " << duration.count() << "ms" << std::endl;

    std::cout << "debug: going to calculate SpiralGrid with n=" << cfg::kConfig["spiral_grid_points"].get<int>() << std::endl;
    start = std::chrono::high_resolution_clock::now();
    SpiralGrid* spiral_grid = new SpiralGrid(cfg::kConfig["spiral_grid_points"].get<int>(), distribution);
    duration = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now() - start);
    std::cout << "debug: SpiralGrid is ready in " << duration.count() << "ms" << std::endl;

    auto result = std::make_tuple(classic_grid, spiral_grid);
    (*cache)[key] = result;

    return result;
}

IsolineAPI* GetIsolineAPI(const httplib::Request& req) {
    if (!req.has_param("mean"))
        throw std::invalid_argument("invalid request: no 'mean' URL param");
    if (!req.has_param("cov"))
        throw std::invalid_argument("invalid request: no 'cov' URL param");
    if (!req.has_param("ratio"))
        throw std::invalid_argument("invalid request: no 'ratio' URL param");

    std::vector<std::string> raw_mean_vec;
    boost::algorithm::split(raw_mean_vec, req.get_param_value("mean"), boost::is_any_of(","));
    if (raw_mean_vec.size() != 3) {
        throw std::invalid_argument("invalid request: wrong 'mean' URL param");
    }

    std::array<double, 3> mean_vec;
    for (size_t i = 0; i < raw_mean_vec.size(); ++i)
        mean_vec[i] = boost::lexical_cast<double>(raw_mean_vec[i]);

    std::vector<std::string> raw_cov_vec;
    boost::algorithm::split(raw_cov_vec, req.get_param_value("cov"), boost::is_any_of(","));
    if (raw_cov_vec.size() != 6) {
        throw std::invalid_argument("invalid request: wrong 'cov' URL param");
    }

    std::array<double, 6> cov_vec;
    for (size_t i = 0; i < raw_cov_vec.size(); ++i)
        cov_vec[i] = boost::lexical_cast<double>(raw_cov_vec[i]);

    std::vector<std::string> raw_ratios;
    boost::algorithm::split(raw_ratios, req.get_param_value("ratio"), boost::is_any_of(","));
    if (raw_ratios.size() == 0) {
        throw std::invalid_argument("invalid request: wrong 'ratio' URL param");
    }

    std::vector<double> ratios_vec(raw_ratios.size());
    for (size_t i = 0; i < raw_ratios.size(); ++i)
        ratios_vec[i] = boost::lexical_cast<double>(raw_ratios[i]);

    ClassicGrid* classic_grid;
    SpiralGrid* spiral_grid;
    std::tie(classic_grid, spiral_grid) = GetGrids(mean_vec, cov_vec);

    IsolineAPI* api = new IsolineAPI(ratios_vec, classic_grid, spiral_grid);
    return api;
}

StatsAPI* GetStatsAPI(const httplib::Request& req) {
    json params = json::parse(req.body);

    if (params.find("mean") == params.end())
        throw std::invalid_argument("invalid request: no 'mean' data");
    if (params.find("cov") == params.end())
        throw std::invalid_argument("invalid request: no 'cov' data");
    if (params.find("points") == params.end())
        throw std::invalid_argument("invalid request: no 'points' data");

    std::vector<std::string> raw_mean_vec;
    boost::algorithm::split(raw_mean_vec, params["mean"].get<std::string>(), boost::is_any_of(","));
    if (raw_mean_vec.size() != 3) {
        throw std::invalid_argument("invalid request: wrong 'mean' data");
    }

    std::array<double, 3> mean_vec;
    for (size_t i = 0; i < raw_mean_vec.size(); ++i)
        mean_vec[i] = boost::lexical_cast<double>(raw_mean_vec[i]);

    std::vector<std::string> raw_cov_vec;
    boost::algorithm::split(raw_cov_vec, params["cov"].get<std::string>(), boost::is_any_of(","));
    if (raw_cov_vec.size() != 6) {
        throw std::invalid_argument("invalid request: wrong 'cov' data");
    }

    std::array<double, 6> cov_vec;
    for (size_t i = 0; i < raw_cov_vec.size(); ++i)
        cov_vec[i] = boost::lexical_cast<double>(raw_cov_vec[i]);

    std::vector<std::string> raw_points;
    boost::algorithm::split(raw_points, params["points"].get<std::string>(), boost::is_any_of(","));
    if (raw_points.size() == 0 || raw_points.size()%3 != 0) {
        throw std::invalid_argument("invalid request: wrong 'points' data");
    }

    std::vector<std::array<double, 3>> points_vec(raw_points.size() / 3);
    for (size_t i = 0; i < raw_points.size(); i += 3) {
        std::array<double, 3> point = {
            boost::lexical_cast<double>(raw_points[i]),
            boost::lexical_cast<double>(raw_points[i+1]),
            boost::lexical_cast<double>(raw_points[i+2])
        };
        points_vec[i/3] = point;
    }

    ClassicGrid* classic_grid;
    SpiralGrid* spiral_grid;
    std::tie(classic_grid, spiral_grid) = GetGrids(mean_vec, cov_vec);

    StatsAPI* api = new StatsAPI(points_vec, classic_grid, spiral_grid);
    return api;
}

void InitServer(void) {
    using namespace httplib;

    Server server;

    server.Get("/api/isoline", [&](const Request& req, Response& res) {
        auto start = std::chrono::high_resolution_clock::now();
        std::cout << "debug: /api/isoline: got new GET request\n";

        res.status = 200;
        res.set_header("Access-Control-Allow-Origin", "*");
        json response;

        try {
            auto api = GetIsolineAPI(req);
            api->Validate();
            auto isolines = api->GetIsolines();
            response["body"] = isolines;
            delete api;
        } catch (std::exception& e) {
            response["code"] = 500;
            response["body"] = e.what();
            res.set_content(response.dump(), "application/json");
            return;
        }

        response["code"] = 200;
        res.set_content(response.dump(), "application/json");

        auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now() - start);
        std::cout << "debug: /api/isoline: response is ready in " << duration.count() << "ms" << std::endl;
    });

    server.Post("/api/stats", [&](const Request& req, Response& res) {
        auto start = std::chrono::high_resolution_clock::now();
        std::cout << "debug: /api/stats: got new POST request\n";

        res.status = 200;
        res.set_header("Access-Control-Allow-Origin", "*");
        res.set_header("Access-Control-Request-Method", "POST");
        res.set_header("Access-Control-Allow-Headers", "*");
        json response;

        try {
            auto api = GetStatsAPI(req);
            api->Validate();
            auto points = api->Points();

            std::vector<double> s_stats;
            s_stats.reserve(points.size());
            int s_skipped_counter = 0;

            std::vector<double> t_stats;
            t_stats.reserve(points.size());
            int t_skipped_counter = 0;

            bool s_stat_with_debug_info = cfg::kConfig["s_stat_with_debug_info"].get<bool>();
            std::vector<json> s_stats_debug_info;
            s_stats_debug_info.reserve(points.size());

            for (int i = 0; i < points.size(); ++i) {
                double value = api->CalculateFunc(points[i]);
                try {
                    t_stats.push_back(api->CalculateTStat(value));
                } catch(std::exception& e) {
                    std::cerr << "error: could not calculate t stat for point #" << i << " (" <<
                        points[i][0] << ", " << points[i][1] << ", " << points[i][2] << "): " << e.what() << std::endl;
                    std::cout << "warning: t stat for point #" << i << " will be skipped" << std::endl;
                    ++t_skipped_counter;
                }
                try {
                    if (!s_stat_with_debug_info) {
                        s_stats.push_back(api->CalculateSStat(points[i], value));
                        continue;
                    }

                    auto result = api->CalculateSStatWithDebugInfo(points[i], value);
                    s_stats.push_back(result["s"].get<double>());
                    s_stats_debug_info.push_back(result);

                } catch(std::exception& e) {
                    std::cerr << "error: could not calculate s stat for point #" << i << " (" <<
                        points[i][0] << ", " << points[i][1] << ", " << points[i][2] << "): " << e.what() << std::endl;
                    std::cout << "warning: s stat for point #" << i << " will be skipped" << std::endl;
                    ++s_skipped_counter;
                }
            }

            if (t_skipped_counter)
                std::cout << "warning: " << t_skipped_counter << " points were skipped for t stat calculation" << std::endl;
            if (s_skipped_counter)
                std::cout << "warning: " << s_skipped_counter << " points were skipped for s stat calculation" << std::endl;

            if (t_stats.size() != 0) {
                auto tests_result = UniformityTests(t_stats);
                response["body"]["t_results"] = json({});
                response["body"]["t_results"]["ks_measure"] = tests_result.KS_measure, response["body"]["t_results"]["ks_est"] = tests_result.KS_estimate;
                response["body"]["t_results"]["ad_measure"] = tests_result.AD_measure, response["body"]["t_results"]["ad_est"] = tests_result.AD_estimate;
                std::cout << "info: t statistics results:" << std::endl;
                std::cout << "info: KS_measure=" << tests_result.KS_measure << " KS_estimate=" << tests_result.KS_estimate << std::endl;
                std::cout << "info: AD_measure=" << tests_result.AD_measure << " AD_estimate=" << tests_result.AD_estimate << std::endl;
            }

            if (s_stats.size() != 0) {
                auto tests_result = UniformityTests(s_stats);
                response["body"]["s_results"] = json({});
                response["body"]["s_results"]["ks_measure"] = tests_result.KS_measure, response["body"]["s_results"]["ks_est"] = tests_result.KS_estimate;
                response["body"]["s_results"]["ad_measure"] = tests_result.AD_measure, response["body"]["s_results"]["ad_est"] = tests_result.AD_estimate;
                std::cout << "info: s statistics results:" << std::endl;
                std::cout << "info: KS_measure=" << tests_result.KS_measure << " KS_estimate=" << tests_result.KS_estimate << std::endl;
                std::cout << "info: AD_measure=" << tests_result.AD_measure << " AD_estimate=" << tests_result.AD_estimate << std::endl;
            }

            response["body"]["t"] = t_stats;
            response["body"]["s"] = s_stats;

            if (s_stat_with_debug_info)
                response["body"]["s_stats_debug_info"] = s_stats_debug_info;

            delete api;

        } catch (std::exception& e) {
            response["code"] = 500;
            response["body"] = e.what();
            res.set_content(response.dump(), "application/json");
            return;
        }

        response["code"] = 200;
        res.set_content(response.dump(), "application/json");

        auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now() - start);
        std::cout << "debug: /api/stats: response is ready in " << duration.count() << "ms" << std::endl;
    });

    server.Options("/api/stats", [](const Request& req, Response& res) {
        res.status = 204;
        // WARNING: for local use only
        res.set_header("Access-Control-Allow-Origin", "*");
        res.set_header("Access-Control-Request-Method", "POST");
        res.set_header("Access-Control-Allow-Headers", "*");
    });

    server.Get("/stop", [&](const Request& req, Response& res) {server.stop();});

    const auto host = cfg::kConfig["host"].get<std::string>();
    const auto port = cfg::kConfig["port"].get<int>();

    server.listen(host.c_str(), port);
}

int main(int argc, char **argv) {
    if (argc != 2) {
        std::cerr << "please specify config path as second argument" << std::endl;
        exit(1);
    }

    cache = new GridsCache;
    cfg::kConfig = cfg::GetConfig(argv[1]);
    InitServer();
}


#ifndef TEST_UNIT_MATH_MIX_MAT_VECTORIZE_BUILD_MIX_MATRIX_HPP
#define TEST_UNIT_MATH_MIX_MAT_VECTORIZE_BUILD_MIX_MATRIX_HPP

#include <stan/math/mix/mat.hpp>
#include <test/unit/math/mix/mat/vectorize/build_mix_vector.hpp>
#include <Eigen/Dense>
#include <vector>

template <typename F, typename T, int R, int C>
static inline Eigen::Matrix<T, R, C> build_mix_matrix(
    const Eigen::Matrix<T, R, C>& x, int seed_index = -1) {
  using Eigen::Matrix;
  using std::vector;

  Matrix<T, R, C> fvar_matrix(x.rows(), x.cols());
  size_t num_inputs = F::valid_inputs().size();
  // Fills matrix with copies of valid_input values
  for (int i = 0; i < x.size(); ++i) {
    vector<T> inputs;
    if (seed_index == i)
      inputs = build_mix_vector<F>(vector<T>(), seed_index % num_inputs);
    else
      inputs = build_mix_vector<F>(vector<T>());
    fvar_matrix(i) = inputs[i % num_inputs];
  }
  return fvar_matrix;
}

#endif


//==================================================================================================
/*!

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/cotpi.hpp>
#include <boost/simd/function/restricted.hpp>
#include <scalar_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>

STF_CASE_TPL (" cotpi",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::cotpi;

  using r_t = decltype(cotpi(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(cotpi(-bs::Zero<T>()), -bs::Inf<r_t>(), 0.5);
  STF_ULP_EQUAL(cotpi(bs::Inf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(cotpi(bs::Minf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(cotpi(bs::Nan<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(cotpi(bs::One<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(cotpi(bs::Zero<T>()), bs::Inf<r_t>(), 0.5);
#endif
  STF_ULP_EQUAL(cotpi(-bs::Quarter<T>()), bs::Mone<r_t>(), 0.5);
  STF_ULP_EQUAL(cotpi(bs::Half<T>()), bs::Zero<r_t>(), 0.5);
  STF_ULP_EQUAL(cotpi(bs::Mhalf<T>()), bs::Zero<r_t>(), 0.5);
  STF_ULP_EQUAL(cotpi(bs::Quarter<T>()), bs::One<r_t>(), 0.5);
}
STF_CASE_TPL (" cotpi restricted_",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::cotpi;

  using r_t = decltype(cotpi(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(bs::restricted_(cotpi)(-bs::Zero<T>()), -bs::Inf<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::restricted_(cotpi)(bs::Inf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::restricted_(cotpi)(bs::Minf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::restricted_(cotpi)(bs::Nan<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::restricted_(cotpi)(bs::One<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::restricted_(cotpi)(bs::Zero<T>()), bs::Inf<r_t>(), 0.5);
#endif
  STF_ULP_EQUAL(bs::restricted_(cotpi)(-bs::Quarter<T>()), bs::Mone<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::restricted_(cotpi)(bs::Half<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::restricted_(cotpi)(bs::Mhalf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::restricted_(cotpi)(bs::Quarter<T>()), bs::One<r_t>(), 0.5);
}


// Copyright (c) 2019 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE Train0TopkTest

#include <../random_util.hpp>
#include <boost/test/unit_test.hpp>
#include <filereader.hpp>
#include <popart/builder.hpp>
#include <popart/dataflow.hpp>
#include <popart/devicemanager.hpp>
#include <popart/inputshapeinfo.hpp>
#include <popart/ndarraywrapper.hpp>
#include <popart/op/identity.hpp>
#include <popart/op/l1.hpp>
#include <popart/session.hpp>
#include <popart/sgd.hpp>
#include <popart/tensorinfo.hpp>
#include <popart/tensornames.hpp>
#include <popart/testdevice.hpp>

#include <algorithm>
#include <map>
#include <tuple>
#include <vector>

BOOST_AUTO_TEST_CASE(Train0TopK) {

  // input tensor X
  // The input tensor will be rank 3, D0 x D1 x D2 x D3
  int D0 = 3;
  int D1 = 7;
  int D2 = 2;
  int D3 = 5;

  // We will perform top-k on axis "axis = 1" and so the output will
  // be of size D0 x top_k x D2 x D3, where top_k <= D1.
  int axis = 1;
  std::vector<int> top_ks{1, 3, D1};

  auto test = [D0, D1, D2, D3, axis](int top_k) {
    // basic differentiation shows that we expect
    // d_topk_out = 2*scale*lossLambda*topk_out

    using namespace popart;

    // we will generate random input data
    int seed = 1013;
    DefaultRandomEngine eng(seed);
    UniformRealDistribution<float> fdis(-4.f, +4.f);

    // prepare to build an onnx model
    auto builder     = Builder::create();
    auto aiOnnx      = builder->aiOnnxOpset9();
    auto aiGraphcore = builder->aiGraphcoreOpset1();

    TensorInfo xInfo{"FLOAT", std::vector<int64_t>{D0, D1, D2, D3}};
    TensorId xId = builder->addInputTensor(xInfo);
    std::vector<float> vXData(xInfo.nelms());
    for (auto &val : vXData) {
      val = fdis(eng);
    }

    // the networt scale((topk)**2) with loss L1.
    float scaleFactor = 3.0f;
    float lossLambda  = 0.26;

    Shape outShape = xInfo.shape();
    outShape[axis] = top_k;
    TensorInfo outValuesInfo{xInfo.dataType(), outShape};

    // Prepare the baseline data, perform D0*D2
    // sorts of vectors of size D1 to get the values and the indices
    std::vector<int> expectedOutputIndices(outValuesInfo.nelms(), -1);
    std::vector<float> expectedOutputValues(outValuesInfo.nelms(), -1.0f);
    std::vector<float> expectedInputGradients(xInfo.nelms(), 0.0f);
    int stride0 = D1 * D2 * D3;
    int stride1 = D2 * D3;
    int stride2 = D3;

    int outStride0 = top_k * D2 * D3;
    int outStride1 = D2 * D3;
    int outStride2 = D3;

    for (int d0 = 0; d0 < D0; ++d0) {
      for (int d2 = 0; d2 < D2; ++d2) {
        for (int d3 = 0; d3 < D3; ++d3) {

          std::vector<std::tuple<float, int>> toSort(D1);
          for (int d1 = 0; d1 < D1; ++d1) {
            toSort[d1] = std::tuple<float, int>(
                vXData[d0 * stride0 + d1 * stride1 + d2 * stride2 + d3], d1);
          }

          // sort, largest to smallest
          std::sort(toSort.rbegin(), toSort.rend());

          // populate expected values
          for (int d1 = 0; d1 < top_k; ++d1) {
            int sortIndex = std::get<1>(toSort[d1]);
            auto sortVal  = std::get<0>(toSort[d1]);

            int index =
                d0 * outStride0 + d1 * outStride1 + d2 * outStride2 + d3;
            expectedOutputIndices[index] = sortIndex;
            expectedOutputValues[index]  = sortVal;
            expectedInputGradients[d0 * stride0 + sortIndex * stride1 +
                                   d2 * stride2 + d3] =
                2 * scaleFactor * lossLambda * std::get<0>(toSort[d1]);
          }
        }
      }
    }

    auto topkOut = aiOnnx.topk({xId}, top_k, axis);

    auto values  = topkOut[0];
    auto indices = topkOut[1];

    auto squaredOut = aiOnnx.mul({values, values});
    auto halvedOut  = aiGraphcore.scale({squaredOut}, scaleFactor);

    auto l1 = builder->aiGraphcoreOpset1().l1loss(
        {halvedOut}, lossLambda, ReductionType::Sum);

    auto proto      = builder->getModelProto();
    auto modelProto = io::getModelFromString(proto);

    // create the IR
    auto art      = AnchorReturnType("All");
    auto dataFlow = DataFlow(1, {{reservedGradientPrefix() + xId, art}});

    auto device = popart::createTestDevice(TEST_TARGET);

    auto opts = SessionOptions();

    float learnRate = 0.1;
    auto optimizer  = ConstSGD(learnRate);

    auto session = popart::TrainingSession::createFromOnnxModel(
        proto,
        dataFlow,
        l1,
        optimizer,
        device,
        popart::InputShapeInfo(),
        opts,
        popart::Patterns(PatternsLevel::Default));

    // prepare the anchors. We test just the
    // gradient of output values.
    std::vector<float> rawXGrad(xInfo.nelms());
    popart::NDArrayWrapper<float> xGrad(rawXGrad.data(), xInfo.shape());

    std::map<popart::TensorId, popart::IArray &> anchors = {
        {reservedGradientPrefix() + xId, xGrad}};

    session->prepareDevice();

    popart::NDArrayWrapper<float> xData(vXData.data(), xInfo);

    std::map<popart::TensorId, popart::IArray &> inputs = {{xId, xData}};

    popart::StepIO stepio(inputs, anchors);

    session->run(stepio);

    // confirm the gradient values agree (exactly...)
    BOOST_CHECK_EQUAL_COLLECTIONS(rawXGrad.begin(),
                                  rawXGrad.end(),
                                  expectedInputGradients.begin(),
                                  expectedInputGradients.end());
  };

  for (int top_k : top_ks) {
    test(top_k);
  }
}


#include <cmath>
#include <boost/serialization/export.hpp>
#include "RecurrentNeuron.hpp"

using namespace std;
using namespace snn;
using namespace internal;

BOOST_CLASS_EXPORT(RecurrentNeuron)

RecurrentNeuron::RecurrentNeuron(NeuronModel model, shared_ptr<NeuralNetworkOptimizer> optimizer)
    : Neuron(model, optimizer)
{
}

#ifdef _MSC_VER
#pragma warning(disable:4701)
#endif
float RecurrentNeuron::output(const vector<float>& inputs, bool temporalReset)
{
    if (temporalReset)
        this->reset();
    this->lastInputs = inputs;
    this->previousSum = this->sum;
    this->previousOutput = this->lastOutput;
    this->sum = 0;
    int w = 0;
    float tmp = 0.0f; // to activate the SIMD optimization
    #pragma omp simd
    for (w = 0; w < (int)inputs.size(); ++w)
    {
        tmp += inputs[w] * this->weights[w];
    }
    this->sum = tmp + this->previousOutput * this->weights[w] + this->bias;
    float output = outputFunction->function(sum);
    this->lastOutput = output;
    return output;
    #ifdef _MSC_VER
    #pragma warning(default:4701)
    #endif
}

std::vector<float>& RecurrentNeuron::backOutput(float error)
{
    error = error * outputFunction->derivative(this->sum);

    #pragma omp simd // seems to do nothing
    for (int w = 0; w < this->numberOfInputs; ++w)
    {
        this->errors[w] = error * this->weights[w];
    }
    this->optimizer->updateWeights(*this, error);
    return this->errors;
}

void RecurrentNeuron::train(float error)
{
    error = error * outputFunction->derivative(this->sum);
    this->optimizer->updateWeights(*this, error);
}

inline
void RecurrentNeuron::reset()
{
    this->previousOutput = 0;
    this->recurrentError = 0;
    this->previousSum = 0;
}

int RecurrentNeuron::isValid() const
{
    if (static_cast<int>(this->weights.size()) != this->numberOfInputs + 1)
        return 304;
    return this->Neuron::isValid();
}

bool RecurrentNeuron::operator==(const RecurrentNeuron& neuron) const
{
        return this->Neuron::operator==(neuron)
            && this->lastOutput == neuron.lastOutput
            && this->previousOutput == neuron.previousOutput
            && this->recurrentError == neuron.recurrentError
            && this->previousSum == neuron.previousSum;
}

bool RecurrentNeuron::operator!=(const RecurrentNeuron& neuron) const
{
    return !(*this == neuron);
}


#include <boost/timer.hpp>
#include <kv/allsol.hpp>

#include "coprin.hpp"

using namespace std;

namespace ub = boost::numeric::ublas;

int main()
{
	int i;
	boost::timer t;
	ub::vector< kv::interval<double> > I;

	std::cout.precision(17);

	Bellido().range(I);
	std::cout << "Bellido\n";
	t.restart();
	allsol(Bellido(), I);
	cout << t.elapsed() << " sec\n";

	Bronstein().range(I);
	std::cout << "Bronstein\n";
	allsol(Bronstein(), I);
	t.restart();
	for (i=0; i<1000; i++) allsol(Bronstein(), I, 0);
	cout << t.elapsed()/1000. << " sec\n";

	Caprasse().range(I);
	std::cout << "Caprasse\n";
	allsol(Caprasse(), I);
	t.restart();
	for (i=0; i<5; i++) allsol(Caprasse(), I, 0);
	cout << t.elapsed()/5. << " sec\n";

	Cyclo().range(I);
	std::cout << "Cyclo\n";
	allsol(Cyclo(), I);
	t.restart();
	for (i=0; i<200; i++) allsol(Cyclo(), I, 0);
	cout << t.elapsed()/200. << " sec\n";

	Celestial().range(I);
	std::cout << "Celestial\n";
	allsol(Celestial(), I);
	t.restart();
	for (i=0; i<10; i++) allsol(Celestial(), I, 0);
	cout << t.elapsed()/10. << " sec\n";

	Eco9().range(I);
	std::cout << "Eco9\n";
	t.restart();
	allsol(Eco9(), I);
	cout << t.elapsed() << " sec\n";

	Freudenstein().range(I);
	std::cout << "Freudenstein\n";
	allsol(Freudenstein(), I);
	t.restart();
	for (i=0; i<10; i++) allsol(Freudenstein(), I, 0);
	cout << t.elapsed()/10. << " sec\n";

	Geneig().range(I);
	for (i=0; i<I.size(); i++) {
		I(i) = kv::interval<double>(-10., 10.);
	}
	std::cout << "Geneig\n";
	t.restart();
	allsol(Geneig(), I);
	cout << t.elapsed() << " sec\n";

	Himmelblau().range(I);
	std::cout << "Himmelblau\n";
	allsol(Himmelblau(), I);
	t.restart();
	for (i=0; i<2000; i++) allsol(Himmelblau(), I, 0);
	cout << t.elapsed()/2000. << " sec\n";

	Kincox().range(I);
	std::cout << "Kincox\n";
	allsol(Kincox(), I);
	t.restart();
	for (i=0; i<1000; i++) allsol(Kincox(), I, 0);
	cout << t.elapsed()/1000. << " sec\n";

	Redeco8().range(I);
	std::cout << "Redeco8\n";
	t.restart();
	allsol(Redeco8(), I);
	cout << t.elapsed() << " sec\n";

	Stenger().range(I);
	std::cout << "Stenger\n";
	allsol(Stenger(), I);
	t.restart();
	for (i=0; i<5000; i++) allsol(Stenger(), I, 0);
	cout << t.elapsed()/5000. << " sec\n";

	Yamamura1().range(7, I);
	std::cout << "Yamamura1(7)\n";
	t.restart();
	allsol(Yamamura1(), I);
	cout << t.elapsed() << " sec\n";

	Math_Maple().range(I);
	std::cout << "Math_Maple\n";
	t.restart();
	allsol(Math_Maple(), I);
	cout << t.elapsed() << " sec\n";

	Collins().range(I);
	std::cout << "Collins\n";
	t.restart();
	allsol(Collins(), I);
	cout << t.elapsed() << " sec\n";

	Math_Num1().range(I);
	std::cout << "Math_Num1\n";
	t.restart();
	allsol(Math_Num1(), I, 1, 1e-8);
	cout << t.elapsed() << " sec\n";

	Xu().range(I);
	std::cout << "Xu\n";
	t.restart();
	allsol(Xu(), I);
	cout << t.elapsed() << " sec\n";

	Box3().range(I);
	std::cout << "Box3\n";
	t.restart();
	allsol(Box3(), I);
	cout << t.elapsed() << " sec\n";

	Rump_univariate().range(I);
	std::cout << "Rump_univariate\n";
	t.restart();
	allsol(Rump_univariate(), I);
	cout << t.elapsed() << " sec\n";

	#if 0
	// too difficult for our system
	AOL_cosh1().range(I);
	std::cout << "AOL_cosh1\n";
	t.restart();
	allsol(AOL_cosh1(), I, 2);
	cout << t.elapsed() << " sec\n";
	#endif

	AOL_log1().range(I);
	std::cout << "AOL_log1\n";
	t.restart();
	allsol(AOL_log1(), I);
	cout << t.elapsed() << " sec\n";

	DiGregorio().range(I);
	std::cout << "DiGregorio\n";
	t.restart();
	allsol(DiGregorio(), I);
	cout << t.elapsed() << " sec\n";

	SMNA90897().range(I);
	std::cout << "SMNA90897\n";
	t.restart();
	allsol(SMNA90897(), I);
	cout << t.elapsed() << " sec\n";

	#if 0
	// too difficult for our system
	SMNA92191().range(I);
	std::cout << "SMNA92191\n";
	t.restart();
	allsol(SMNA92191(), I, 2);
	cout << t.elapsed() << " sec\n";
	#endif

	Sinxx().range(I);
	std::cout << "Sinxx\n";
	t.restart();
	allsol(Sinxx(), I);
	cout << t.elapsed() << " sec\n";

	Sinxx1().range(I);
	std::cout << "Sin1xx\n";
	t.restart();
	allsol(Sinxx1(), I);
	cout << t.elapsed() << " sec\n";

	#if 0
	// very difficult; has 8 solutions
	Sjirk_Boon().range(I);
	std::cout << "Sjirk_Boon\n";
	t.restart();
	allsol(Sjirk_Boon(), I);
	cout << t.elapsed() << " sec\n";
	#endif

	// Bellido().range(I);
	// allsol(Bellido(), I);
	// allsol_list(Bellodo(), pre_allsol(Bellido(), I, 10000000));
}


// URDF to RBDL parser
// Author: Max Schwarz <max.schwarz@uni-bonn.de>
//         Philipp Allgeuer <pallgeuer@ais.uni-bonn.de>

// Includes
#include <rbdl/rbdl_parser.h>
#include <rbdl/Model.h>
#include <urdf/model.h>
#include <Eigen/Geometry>
#include <ros/console.h>
#include <ros/assert.h>

// Debugging defines
#define DISPLAY_DEBUG_INFO 0

// Namespace aliases
namespace Math = RigidBodyDynamics::Math;

// RBDL parser namespace
namespace rbdl_parser
{

//
// Some basic conversions
//

static Math::Vector3d toRBDL(const urdf::Vector3& vec)
{
	return Math::Vector3d(vec.x, vec.y, vec.z);
}

static Math::SpatialTransform toRBDL(const urdf::Pose& pose)
{
	double x, y, z, w;
	pose.rotation.getQuaternion(x, y, z, w);

	Math::Matrix3d mat = Eigen::Quaternion<double>(w, x, y, z).toRotationMatrix();

	return Math::SpatialTransform(mat.transpose(), toRBDL(pose.position));
}

static RigidBodyDynamics::Joint toRBDL(const urdf::Joint& joint)
{
	switch(joint.type)
	{
		case urdf::Joint::FIXED:
			return RigidBodyDynamics::Joint(RigidBodyDynamics::JointTypeFixed);
		case urdf::Joint::CONTINUOUS:
		case urdf::Joint::REVOLUTE:
			return RigidBodyDynamics::Joint(RigidBodyDynamics::JointTypeRevolute, toRBDL(joint.axis));
		case urdf::Joint::PRISMATIC:
			return RigidBodyDynamics::Joint(RigidBodyDynamics::JointTypePrismatic, toRBDL(joint.axis));
		default:
			ROS_WARN("Unknown joint type on joint '%s', using fixed joint", joint.name.c_str());
			return RigidBodyDynamics::Joint(RigidBodyDynamics::JointTypeFixed);
	}
}

//
// URDF_RBDL_Model class
//

URDF_RBDL_Model::URDF_RBDL_Model()
 : m_indexURDFRoot((unsigned int) -1)
{
}

URDF_RBDL_Model::~URDF_RBDL_Model()
{
}

void URDF_RBDL_Model::process(const urdf::Link& link, unsigned int parent, const Math::SpatialTransform& parentJointFrame)
{
	RigidBodyDynamics::Body body;

	if(link.inertial)
	{
		const urdf::Inertial& in = *link.inertial;
		Math::Matrix3d I;
		I << in.ixx, in.ixy, in.ixz,
		     in.ixy, in.iyy, in.iyz,
		     in.ixz, in.iyz, in.izz;

		double mass = link.inertial->mass;
		if(mass == 0)
			mass = 0.000001;
		body = RigidBodyDynamics::Body(mass, toRBDL(link.inertial->origin.position), I);
	}
	else
	{
		body.mCenterOfMass = Math::Vector3d::Zero();
		body.mInertia = Math::Matrix3d::Zero();
		body.mMass = 0.000001;
	}

	Math::SpatialTransform jointFrame = Math::SpatialTransform();
	RigidBodyDynamics::Joint joint = RigidBodyDynamics::Joint(RigidBodyDynamics::JointTypeFixed);

	if(link.parent_joint)
	{
		jointFrame = toRBDL(link.parent_joint->parent_to_joint_origin_transform);
		jointFrame.r = jointFrame.r - parentJointFrame.r;
		joint = toRBDL(*link.parent_joint);
	}

	// Add the body to the RBDL tree (Note: This function merges fixed joints into following non-fixed joints and doesn't create a new id for them)
	unsigned int id = AddBody(parent, jointFrame, joint, body, link.name);
#if DISPLAY_DEBUG_INFO
	ROS_INFO("Added body link '%s' as ID %u, Parent ID %u, Joint type %d, Mass %.4f (now %d DOF)", link.name.c_str(), id, parent, joint.mJointType, body.mMass, dof_count);
#endif

	// Set up the associated joint if required
	if(link.parent_joint && link.parent_joint->type != urdf::Joint::FIXED)
	{
		setupJoint(id, *link.parent_joint, false);
#if DISPLAY_DEBUG_INFO
		ROS_WARN("Added joint %u: '%s' (%s -> %s)", id - 1, link.parent_joint->name.c_str(), link.parent_joint->parent_link_name.c_str(), link.parent_joint->child_link_name.c_str());
#endif
	}

	if(link.name == m_nameURDFRoot)
		m_indexURDFRoot = (dof_count == 0 ? 0 : id);

	for(size_t i = 0; i < link.child_links.size(); ++i)
		process(*link.child_links[i], id, Math::SpatialTransform());
}

void URDF_RBDL_Model::processReverse(const urdf::Link& link, unsigned int parent, const urdf::Link* parentLink, const Math::SpatialTransform& parentJointFrame)
{
	RigidBodyDynamics::Body body;

	// Situation here: In the URDF world, we are the child of link.parent_link.
	//  We want to become the child of parentLink, which is a child of ours in
	//  URDF.

	// Our origin needs to be in the point of the joint from parentLink to us.

	// Determine transform from old origin to the new origin (joint of the parentLink)
	Math::SpatialTransform tf;
	boost::shared_ptr<urdf::Joint> child_joint;

	if(parentLink)
	{
		child_joint = parentLink->parent_joint;
		tf = toRBDL(child_joint->parent_to_joint_origin_transform);
	}
	else
	{
		// Just leave our origin at the child joint
		tf.E = Math::Matrix3d::Identity();
		tf.r = Math::Vector3d::Zero();
	}

	if(link.inertial)
	{
		const urdf::Inertial& in = *link.inertial;
		Math::Matrix3d I;
		I << in.ixx, in.ixy, in.ixz,
		     in.ixy, in.iyy, in.iyz,
		     in.ixz, in.iyz, in.izz;

		Math::Vector3d cog = toRBDL(link.inertial->origin.position);

		// As the coordinate system is not rotated, and the center of mass
		// stays where it is, the inertia tensor does not need to be
		// modified.

		// Transform cog relative to our new origin
		cog = cog - tf.r;

		double mass = link.inertial->mass;
		if(mass == 0)
			mass = 0.000001;
		body = RigidBodyDynamics::Body(mass, cog, I);
	}
	else
	{
		body.mCenterOfMass = Math::Vector3d::Zero();
		body.mInertia = Math::Matrix3d::Zero();
		body.mMass = 0.000001;
	}

	Math::SpatialTransform jointFrame = Math::SpatialTransform();
	RigidBodyDynamics::Joint joint = RigidBodyDynamics::Joint(RigidBodyDynamics::JointTypeFixed);

	if(parentLink && parentLink->parent_joint)
	{
		// Rotate the other way around since we are reversing the joint.
		Math::Matrix3d E = tf.E.transpose();

		// Origin of the joint in parentLink coordinates
		Math::Vector3d r = -parentJointFrame.r;
		jointFrame = Math::SpatialTransform(E, r);

		joint = toRBDL(*parentLink->parent_joint);

		// Flip joint axes
		for(size_t i = 0; i < joint.mDoFCount; ++i)
			joint.mJointAxes[i] = -joint.mJointAxes[i];
	}

	// Add the body to the RBDL tree (Note: This function merges fixed joints into following non-fixed joints and doesn't create a new id for them)
	unsigned int id = AddBody(parent, jointFrame, joint, body, "?reverse_" + link.name);
#if DISPLAY_DEBUG_INFO
	ROS_INFO("Added body link '%s' as ID %u, Parent ID %u, Joint type %d, Mass %.4f (now %d DOF)", link.name.c_str(), id, parent, joint.mJointType, body.mMass, dof_count);
	ROS_INFO(" -> parentLink: %p, parentLink->parentJoint: %p",
		parentLink, parentLink ? parentLink->parent_joint.get() : 0
	);
	ROS_INFO(" -> joint frame was %f, %f, %f", jointFrame.r.x(), jointFrame.r.y(), jointFrame.r.z());
	if(id >= fixed_body_discriminator)
	{
		ROS_INFO(" -> fixed body has movable parent %u now, offset %f, %f, %f",
			mFixedBodies[id - fixed_body_discriminator].mMovableParent,
			mFixedBodies[id - fixed_body_discriminator].mParentTransform.r.x(),
			mFixedBodies[id - fixed_body_discriminator].mParentTransform.r.y(),
			mFixedBodies[id - fixed_body_discriminator].mParentTransform.r.z()
		);
	}
#endif

	// Set up the associated joint if required
	if(parentLink && parentLink->parent_joint && parentLink->parent_joint->type != urdf::Joint::FIXED)
	{
		setupJoint(id, *parentLink->parent_joint, true);
#if DISPLAY_DEBUG_INFO
		ROS_WARN("Added joint %u: '%s' (%s -> %s)", id - 1, parentLink->parent_joint->name.c_str(), parentLink->parent_joint->parent_link_name.c_str(), parentLink->parent_joint->child_link_name.c_str());
#endif
	}

	if(link.name == m_nameURDFRoot)
		m_indexURDFRoot = (dof_count == 0 ? 0 : id);

	// Provide a zero-weight fixed body with the correct origin
	{
		RigidBodyDynamics::Joint fixedJoint(RigidBodyDynamics::JointTypeFixed);
		RigidBodyDynamics::Body fixedBody;

		fixedBody.mMass = 0.0;

		RigidBodyDynamics::Math::SpatialTransform t;
		t.E.setIdentity();
		t.r = -tf.r;

		AddBody(id, t, fixedJoint, fixedBody, link.name);
	}

	for(size_t i = 0; i < link.child_links.size(); ++i)
	{
		const urdf::Link* child = link.child_links[i].get();
		if(child == parentLink)
			continue;
		process(*link.child_links[i], id, tf);
	}

	boost::shared_ptr<urdf::Link> urdfParent = link.getParent();
	if(urdfParent)
		processReverse(*urdfParent, id, &link, tf);
}

bool URDF_RBDL_Model::initFrom(const urdf::Model& model, const std::string& root)
{
	// Display debug information
#if DISPLAY_DEBUG_INFO
	ROS_ERROR("Initialising RBDL model of '%s' with root link '%s'...", model.getName().c_str(), root.c_str());
#endif

	// Save the root link name
	m_nameURDFRoot = model.getRoot()->name;
	m_indexURDFRoot = (unsigned int) -1;

	// Reset root mass to zero and give it the proper name
	mBodies[0].mMass = 0;

	// Set gravity to ROS standards
	gravity << 0, 0, -9.81;

	std::vector<boost::shared_ptr<urdf::Link> > links;
	model.getLinks(links);

	boost::shared_ptr<const urdf::Link> oldRoot = model.getRoot();
	boost::shared_ptr<const urdf::Link> newRoot = model.getLink(root);

	if(oldRoot == newRoot || !newRoot)
		process(*oldRoot, 0, Math::SpatialTransform());
	else
		processReverse(*newRoot, 0, NULL, Math::SpatialTransform());

	// Rename the root body to our URDF root
	mBodyNameMap.erase("ROOT");
	mBodyNameMap[root] = 0;

	// Display debug information
#if DISPLAY_DEBUG_INFO
	ROS_INFO("URDF root link is '%s' and was detected at RBDL body index %u", m_nameURDFRoot.c_str(), m_indexURDFRoot);
#endif

	return true;
}

void URDF_RBDL_Model::setupJoint(unsigned int index, const urdf::Joint& urdf, bool reverse)
{
	m_jointMap[urdf.name] = index;
	if(index >= m_jointNames.size())
		m_jointNames.resize(index + 1);
	m_jointNames[index] = urdf.name;
}

unsigned int URDF_RBDL_Model::jointIndex(const std::string& name)
{
	JointMap::const_iterator it = m_jointMap.find(name);
	ROS_ASSERT_MSG(it != m_jointMap.end(), "Unknown joint '%s'", name.c_str());
	return it->second;
}

int URDF_RBDL_Model::findJointIndex(const std::string& name)
{
	JointMap::const_iterator it = m_jointMap.find(name);
	if(it == m_jointMap.end())
		return -1;

	return it->second;
}

std::string URDF_RBDL_Model::jointName(unsigned int index)
{
	ROS_ASSERT_MSG(index < m_jointNames.size(), "Invalid joint index %u", index);
	return m_jointNames[index];
}

}


/*
 Copyright (C) 2016 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <qle/instruments/crossccyfixfloatswap.hpp>

#include <boost/make_shared.hpp>
#include <ql/cashflows/fixedratecoupon.hpp>
#include <ql/cashflows/iborcoupon.hpp>
#include <ql/cashflows/simplecashflow.hpp>

namespace QuantExt {

CrossCcyFixFloatSwap::CrossCcyFixFloatSwap(
    Type type, Real fixedNominal, const Currency& fixedCurrency, const Schedule& fixedSchedule, Rate fixedRate,
    const DayCounter& fixedDayCount, BusinessDayConvention fixedPaymentBdc, Natural fixedPaymentLag,
    const Calendar& fixedPaymentCalendar, Real floatNominal, const Currency& floatCurrency,
    const Schedule& floatSchedule, const boost::shared_ptr<IborIndex>& floatIndex, Spread floatSpread,
    BusinessDayConvention floatPaymentBdc, Natural floatPaymentLag, const Calendar& floatPaymentCalendar)
    : CrossCcySwap(2), type_(type), fixedNominal_(fixedNominal), fixedCurrency_(fixedCurrency),
      fixedSchedule_(fixedSchedule), fixedRate_(fixedRate), fixedDayCount_(fixedDayCount),
      fixedPaymentBdc_(fixedPaymentBdc), fixedPaymentLag_(fixedPaymentLag), fixedPaymentCalendar_(fixedPaymentCalendar),
      floatNominal_(floatNominal), floatCurrency_(floatCurrency), floatSchedule_(floatSchedule),
      floatIndex_(floatIndex), floatSpread_(floatSpread), floatPaymentBdc_(floatPaymentBdc),
      floatPaymentLag_(floatPaymentLag), floatPaymentCalendar_(floatPaymentCalendar) {

    // Build the float leg
    Leg floatLeg = IborLeg(floatSchedule_, floatIndex_)
                       .withNotionals(floatNominal_)
                       .withSpreads(floatSpread_)
                       .withPaymentAdjustment(floatPaymentBdc_)
                       .withPaymentLag(floatPaymentLag_)
                       .withPaymentCalendar(floatPaymentCalendar_);

    // Register with each floating rate coupon
    for (Leg::const_iterator it = floatLeg.begin(); it < floatLeg.end(); ++it)
        registerWith(*it);

    // Initial notional exchange
    Date aDate = floatSchedule_.dates().front();
    aDate = floatPaymentCalendar_.adjust(aDate, floatPaymentBdc_);
    boost::shared_ptr<CashFlow> aCashflow = boost::make_shared<SimpleCashFlow>(-floatNominal_, aDate);
    floatLeg.insert(floatLeg.begin(), aCashflow);

    // Final notional exchange
    aDate = floatLeg.back()->date();
    aCashflow = boost::make_shared<SimpleCashFlow>(floatNominal_, aDate);
    floatLeg.push_back(aCashflow);

    // Build the fixed rate leg
    Leg fixedLeg = FixedRateLeg(fixedSchedule_)
                       .withNotionals(fixedNominal_)
                       .withCouponRates(fixedRate_, fixedDayCount_)
                       .withPaymentAdjustment(fixedPaymentBdc_)
                       .withPaymentLag(fixedPaymentLag)
                       .withPaymentCalendar(fixedPaymentCalendar);

    // Initial notional exchange
    aDate = fixedSchedule_.dates().front();
    aDate = fixedPaymentCalendar_.adjust(aDate, fixedPaymentBdc_);
    aCashflow = boost::make_shared<SimpleCashFlow>(-fixedNominal_, aDate);
    fixedLeg.insert(fixedLeg.begin(), aCashflow);

    // Final notional exchange
    aDate = fixedLeg.back()->date();
    aCashflow = boost::make_shared<SimpleCashFlow>(fixedNominal_, aDate);
    fixedLeg.push_back(aCashflow);

    // Deriving from cross currency swap where:
    //   First leg should hold the pay flows
    //   Second leg should hold the receive flows
    payer_[0] = -1.0;
    payer_[1] = 1.0;
    switch (type_) {
    case Payer:
        legs_[0] = fixedLeg;
        currencies_[0] = fixedCurrency_;
        legs_[1] = floatLeg;
        currencies_[1] = floatCurrency_;
        break;
    case Receiver:
        legs_[1] = fixedLeg;
        currencies_[1] = fixedCurrency_;
        legs_[0] = floatLeg;
        currencies_[0] = floatCurrency_;
        break;
    default:
        QL_FAIL("Unknown cross currency fix float swap type");
    }
}

void CrossCcyFixFloatSwap::setupArguments(PricingEngine::arguments* a) const {
    CrossCcySwap::setupArguments(a);
    if (CrossCcyFixFloatSwap::arguments* args = dynamic_cast<CrossCcyFixFloatSwap::arguments*>(a)) {
        args->fixedRate = fixedRate_;
        args->spread = floatSpread_;
    }
}

void CrossCcyFixFloatSwap::fetchResults(const PricingEngine::results* r) const {

    CrossCcySwap::fetchResults(r);

    // Depending on the pricing engine used, we may have CrossCcyFixFloatSwap::results
    if (const CrossCcyFixFloatSwap::results* res = dynamic_cast<const CrossCcyFixFloatSwap::results*>(r)) {
        // If we have CrossCcyFixFloatSwap::results from the pricing engine
        fairFixedRate_ = res->fairFixedRate;
        fairSpread_ = res->fairSpread;
    } else {
        // If not, set them to Null to indicate a calculation is needed below
        fairFixedRate_ = Null<Rate>();
        fairSpread_ = Null<Spread>();
    }

    // Calculate fair rate and spread if they are still Null here
    static Spread basisPoint = 1.0e-4;

    Size idxFixed = type_ == Payer ? 0 : 1;
    if (fairFixedRate_ == Null<Rate>() && legBPS_[idxFixed] != Null<Real>())
        fairFixedRate_ = fixedRate_ - NPV_ / (legBPS_[idxFixed] / basisPoint);

    Size idxFloat = type_ == Payer ? 1 : 0;
    if (fairSpread_ == Null<Spread>() && legBPS_[idxFloat] != Null<Real>())
        fairSpread_ = floatSpread_ - NPV_ / (legBPS_[idxFloat] / basisPoint);
}

void CrossCcyFixFloatSwap::setupExpired() const {
    CrossCcySwap::setupExpired();
    fairFixedRate_ = Null<Rate>();
    fairSpread_ = Null<Spread>();
}

void CrossCcyFixFloatSwap::arguments::validate() const {
    CrossCcySwap::arguments::validate();
    QL_REQUIRE(fixedRate != Null<Rate>(), "Fixed rate cannot be null");
    QL_REQUIRE(spread != Null<Spread>(), "Spread cannot be null");
}

void CrossCcyFixFloatSwap::results::reset() {
    CrossCcySwap::results::reset();
    fairFixedRate = Null<Rate>();
    fairSpread = Null<Spread>();
}

} // namespace QuantExt


#define BOOST_SIMD_ASSUME_SSE4_2

#include <algorithm>
#include <array>
#include <boost/simd/algorithm/find.hpp>
#include <boost/simd/algorithm/find_if_not.hpp>
#include <boost/simd/function/plus.hpp>
#include <boost/simd/function/store.hpp>
#include <boost/simd/memory.hpp>
#include <boost/simd/pack.hpp>
#include <celero/Celero.h>
#include <numeric>
#include <vector>
namespace bs = boost::simd;
using TestType = float;
using pack_t = bs::pack<TestType>;

//CELERO_MAIN
int main(int argc, char** argv)
{
	std::cout << pack_t::static_size << std::endl;
	celero::Run(argc, argv);
	return 0;
}

constexpr int g_samples = 200;
constexpr int g_iterations = 10000;

const std::string str = "                                                                                1";

BASELINE(add_multi, simd, g_samples, g_iterations)
{
	pack_t iter_pack;
	std::iota(iter_pack.begin(), iter_pack.begin() + pack_t::static_size, TestType(0));
	iter_pack = pack_t(iter_pack + iter_pack) * 2;
	//std::cout << iter_pack << std::endl;
}

BENCHMARK(add_multi, loop_vector, g_samples, g_iterations)
{
	std::vector<TestType> data(pack_t::static_size);
	std::iota(data.begin(), data.begin() + pack_t::static_size, TestType(0));
	for (size_t i = 0; i < data.size(); ++i) {
		data[i] = (data[i] + data[i]) * 2;
		//std::cout << data[i] << std::endl;
	}
}

BENCHMARK(add_multi, loop_array, g_samples, g_iterations)
{
	std::array<TestType, pack_t::static_size> data;
	std::iota(data.begin(), data.begin() + pack_t::static_size, TestType(0));
	for (size_t i = 0; i < data.size(); ++i) {
		data[i] = (data[i] + data[i]) * 2;
		//std::cout << data[i] << std::endl;
	}
}

BASELINE(find, simd, g_samples, g_iterations)
{
	celero::DoNotOptimizeAway(bs::find(str.data(), str.data() + str.size(), '1'));
}

BENCHMARK(find, iter_while, g_samples, g_iterations)
{
	auto iter = str.begin();
	while (*iter != '1')
		++iter;
	celero::DoNotOptimizeAway(iter);
}

BENCHMARK(find, string, g_samples, g_iterations)
{
	celero::DoNotOptimizeAway(str.find('1'));
}

BENCHMARK(find, std_find, g_samples, g_iterations)
{
	celero::DoNotOptimizeAway(std::find(str.begin(), str.end(), '1'));
}

BASELINE(find_not, simd, g_samples, g_iterations)
{
	celero::DoNotOptimizeAway(bs::find_if_not(str.data(), str.data() + str.size(), [](auto& c) {return c == ' '; }));
}

BENCHMARK(find_not, simd_it_it, g_samples, g_iterations)
{
	celero::DoNotOptimizeAway(bs::find_if_not(&*str.begin(), &*str.end(), [](auto& c) {return c == ' '; }));
}
BENCHMARK(find_not, simd_da_it, g_samples, g_iterations)
{
	celero::DoNotOptimizeAway(bs::find_if_not(str.data(), &*str.end(), [](auto& c) {return c == ' '; }));
}

BENCHMARK(find_not, iter_while, g_samples, g_iterations)
{
	auto iter = str.begin();
	while (*iter == ' ')
		++iter;
	celero::DoNotOptimizeAway(iter);
}

BENCHMARK(find_not, iter_for, g_samples, g_iterations)
{
	auto iter = str.begin();
	for (; *iter == ' '; ++iter);
	celero::DoNotOptimizeAway(iter);
}

BENCHMARK(find_not, string, g_samples, g_iterations)
{
	celero::DoNotOptimizeAway(str.find_first_not_of(' '));
}

BENCHMARK(find_not, std_find, g_samples, g_iterations)
{
	celero::DoNotOptimizeAway(std::find_if_not(str.begin(), str.end(), [](auto& c) {return c == ' '; }));
}


//
//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
//
#ifndef BOOST_GRAPH_MST_KRUSKAL_HPP
#define BOOST_GRAPH_MST_KRUSKAL_HPP

/*
 *Minimum Spanning Tree
 *         Kruskal Algorithm
 *
 *Requirement:
 *      undirected graph
 */

#include <vector>
#include <queue>
#include <functional>

#include <boost/property_map/property_map.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/pending/disjoint_sets.hpp>
#include <boost/pending/indirect_cmp.hpp>
#include <boost/concept/assert.hpp>

namespace boost {

  // Kruskal's algorithm for Minimum Spanning Tree
  //
  // This is a greedy algorithm to calculate the Minimum Spanning Tree
  // for an undirected graph with weighted edges. The output will be a
  // set of edges.
  //

  namespace detail {

    template <class Graph, class OutputIterator,
              class Rank, class Parent, class Weight>
    void
    kruskal_mst_impl(const Graph& G,
                     OutputIterator spanning_tree_edges,
                     Rank rank, Parent parent, Weight weight)
    {
      if (num_vertices(G) == 0) return; // Nothing to do in this case
      typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
      typedef typename graph_traits<Graph>::edge_descriptor Edge;
      BOOST_CONCEPT_ASSERT(( VertexListGraphConcept<Graph> ));
      BOOST_CONCEPT_ASSERT(( EdgeListGraphConcept<Graph> ));
      BOOST_CONCEPT_ASSERT(( OutputIteratorConcept<OutputIterator, Edge> ));
      BOOST_CONCEPT_ASSERT(( ReadWritePropertyMapConcept<Rank, Vertex> ));
      BOOST_CONCEPT_ASSERT(( ReadWritePropertyMapConcept<Parent, Vertex> ));
      BOOST_CONCEPT_ASSERT(( ReadablePropertyMapConcept<Weight, Edge> ));
      typedef typename property_traits<Weight>::value_type W_value;
      typedef typename property_traits<Rank>::value_type R_value;
      typedef typename property_traits<Parent>::value_type P_value;
      BOOST_CONCEPT_ASSERT(( ComparableConcept<W_value> ));
      BOOST_CONCEPT_ASSERT(( ConvertibleConcept<P_value, Vertex> ));
      BOOST_CONCEPT_ASSERT(( IntegerConcept<R_value> ));

      disjoint_sets<Rank, Parent>  dset(rank, parent);

      typename graph_traits<Graph>::vertex_iterator ui, uiend;
      for (boost::tie(ui, uiend) = vertices(G); ui != uiend; ++ui)
        dset.make_set(*ui);

      typedef indirect_cmp<Weight, std::greater<W_value> > weight_greater;
      weight_greater wl(weight);
      std::priority_queue<Edge, std::vector<Edge>, weight_greater> Q(wl);
      /*push all edge into Q*/
      typename graph_traits<Graph>::edge_iterator ei, eiend;
      for (boost::tie(ei, eiend) = edges(G); ei != eiend; ++ei)
        Q.push(*ei);

      while (! Q.empty()) {
        Edge e = Q.top();
        Q.pop();
        Vertex u = dset.find_set(source(e, G));
        Vertex v = dset.find_set(target(e, G));
        if ( u != v ) {
          *spanning_tree_edges++ = e;
          dset.link(u, v);
        }
      }
    }

  } // namespace detail

  // Named Parameters Variants

  template <class Graph, class OutputIterator>
  inline void
  kruskal_minimum_spanning_tree(const Graph& g,
                                OutputIterator spanning_tree_edges)
  {
    typedef typename graph_traits<Graph>::vertices_size_type size_type;
    typedef typename graph_traits<Graph>::vertex_descriptor vertex_t;
    if (num_vertices(g) == 0) return; // Nothing to do in this case
    typename graph_traits<Graph>::vertices_size_type
      n = num_vertices(g);
    std::vector<size_type> rank_map(n);
    std::vector<vertex_t> pred_map(n);

    detail::kruskal_mst_impl
      (g, spanning_tree_edges,
       make_iterator_property_map(rank_map.begin(), get(vertex_index, g), rank_map[0]),
       make_iterator_property_map(pred_map.begin(), get(vertex_index, g), pred_map[0]),
       get(edge_weight, g));
  }

  template <class Graph, class OutputIterator, class P, class T, class R>
  inline void
  kruskal_minimum_spanning_tree(const Graph& g,
                                OutputIterator spanning_tree_edges,
                                const bgl_named_params<P, T, R>& params)
  {
    typedef typename graph_traits<Graph>::vertices_size_type size_type;
    typedef typename graph_traits<Graph>::vertex_descriptor vertex_t;
    if (num_vertices(g) == 0) return; // Nothing to do in this case
    typename graph_traits<Graph>::vertices_size_type n;
    n = is_default_param(get_param(params, vertex_rank))
                                   ? num_vertices(g) : 1;
    std::vector<size_type> rank_map(n);
    n = is_default_param(get_param(params, vertex_predecessor))
                                   ? num_vertices(g) : 1;
    std::vector<vertex_t> pred_map(n);

    detail::kruskal_mst_impl
      (g, spanning_tree_edges,
       choose_param
       (get_param(params, vertex_rank),
        make_iterator_property_map
        (rank_map.begin(),
         choose_pmap(get_param(params, vertex_index), g, vertex_index), rank_map[0])),
       choose_param
       (get_param(params, vertex_predecessor),
        make_iterator_property_map
        (pred_map.begin(),
         choose_const_pmap(get_param(params, vertex_index), g, vertex_index),
         pred_map[0])),
       choose_const_pmap(get_param(params, edge_weight), g, edge_weight));
  }

} // namespace boost

#endif // BOOST_GRAPH_MST_KRUSKAL_HPP


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

#ifndef MTL_COMPLEXITY_INCLUDE
#define MTL_COMPLEXITY_INCLUDE

#include <boost/mpl/int.hpp>
#include <boost/mpl/max.hpp>
#include <boost/mpl/if.hpp>
#include <boost/mpl/less.hpp>
#include <boost/mpl/less_equal.hpp>

// This file contains types to characterize time complexities of operations or traversals
// Different traversals over given collections have different run time
// Algorithms implementable with different traversals can use these complexities to dispatch

// Complexities in a order, which of course will not be changed
// The underlying MPL definitions might be modified to add finer grained distinctions

namespace mtl {
/// Namespace to define complexity classes
namespace complexity_classes {

/// Constant complexity where the value is expected to be in cache with high probability.
/** Special type for traversals to distinguish between strided or random memory access with 'constant'
    (but slow) memory access and consecutive memory access with a good change that only one element
    per cache line must be load from memory
**/
struct cached : boost::mpl::int_<1> {};

/// Constant complexity
struct constant : boost::mpl::int_<2> {};

/// Logarithmic complexity
struct log_n : boost::mpl::int_<4> {};

/// Polynomial logarithm, i.e. log^k n
struct polylog_n : boost::mpl::int_<5> {};

/// Product of linear and cached
struct linear_cached : boost::mpl::int_<21> {};

/// Linear
struct linear : boost::mpl::int_<22> {};

/// Logarithm times linear, i.e. n * log n
struct n_log_n : boost::mpl::int_<24> {};

/// Polynomial logarithm times linear, i.e. n * log^k n
struct n_polylog_n : boost::mpl::int_<25> {};

struct quadratic : boost::mpl::int_<41> {};

/// All complexities larger than quadratic (< infinite) including n^2 log^k n
struct polynomial : boost::mpl::int_<200> {};

/// Infinite time complexity, which usually means that the operation or traversal is not available
struct infinite : boost::mpl::int_<1000> {};

/// Adding complexities of two operations is the maximal complexity of both operations
template <typename X, typename Y>
struct plus : boost::mpl::if_< boost::mpl::less<X, Y>, Y, X> {};

/// Minimal complexity class
template <typename X, typename Y>
struct min : boost::mpl::if_< boost::mpl::less<X, Y>, X, Y> {};

namespace detail
{
    // specializations on first argument

    // polynomial is the most frequent result, otherwise explicit definition later
    template <typename X, typename Y> struct times
    {
	typedef polynomial type;
    };

    template <typename Y> struct times<cached, Y>
    {
	typedef Y type;
    };

    template <typename Y> struct times<constant, Y>
    {
	typedef Y type;
    };

    template <> struct times<log_n, log_n>
    {
	typedef polylog_n type;
    };

    template <> struct times<log_n, polylog_n> : times<log_n, log_n> {};

    template <> struct times<log_n, linear_cached>
    {
	typedef n_log_n type;
    };

    template <> struct times<log_n, linear> : times<log_n, linear_cached> {};

    template <> struct times<log_n, n_log_n>
    {
	typedef n_polylog_n type;
    };

    template <> struct times<log_n, n_polylog_n> : times<log_n, n_log_n> {};

    template <> struct times<polylog_n, polylog_n>
    {
	typedef polylog_n type;
    };

    template <> struct times<polylog_n, linear_cached>
    {
	typedef n_polylog_n type;
    };

    template <> struct times<polylog_n, linear> : times<polylog_n, linear_cached> {};

    template <> struct times<polylog_n, n_log_n> : times<polylog_n, linear_cached> {};

    template <> struct times<polylog_n, n_polylog_n> : times<polylog_n, linear_cached> {};

    template <> struct times<linear_cached, linear_cached>
    {
	typedef quadratic type;
    };

    template <> struct times<linear_cached, linear> : times<linear_cached, linear_cached> {};

    template <> struct times<linear, linear> : times<linear_cached, linear_cached> {};

} // namespace detail

/// Product of complexities
// Multiplication needs to be defined explicitly
// At least is symmetric, so we only consider X <= Y
template <typename X, typename Y>
struct times
    : boost::mpl::if_<
            boost::mpl::less<X, Y>
          , detail::times<X, Y>
          , detail::times<Y, X>
    >::type
{};

// Specializations on second argument (if were ordered)
// Done here to avoid ambiguities

template <typename X>
struct times<X, infinite>
{
    typedef infinite type;
};

template <typename X> struct times<infinite, X> : times<X, infinite> {};

}} // namespace mtl::complexity

#define MTL_PRINT_COMPLEXITY(TYPE, STRING) \
inline std::ostream& operator<< (std::ostream& os, mtl::complexity_classes::TYPE) \
{                                                 \
    return os << STRING;                          \
}

MTL_PRINT_COMPLEXITY(cached, "cached constant complexity")
MTL_PRINT_COMPLEXITY(constant, "constant complexity")
MTL_PRINT_COMPLEXITY(log_n, "logarithmic complexity")
MTL_PRINT_COMPLEXITY(polylog_n, "poly-logarithmic complexity")
MTL_PRINT_COMPLEXITY(linear_cached, "cached linear complexity")
MTL_PRINT_COMPLEXITY(linear, "linear complexity")
MTL_PRINT_COMPLEXITY(n_log_n, "n log n complexity")
MTL_PRINT_COMPLEXITY(n_polylog_n, "n poly-log n complexity")
MTL_PRINT_COMPLEXITY(quadratic, "quadratic complexity")
MTL_PRINT_COMPLEXITY(polynomial, "polynomial complexity")
MTL_PRINT_COMPLEXITY(infinite, "infinite complexity")

#endif // MTL_COMPLEXITY_INCLUDE


// Copyright (c) 2019 Egor Tensin <Egor.Tensin@gmail.com>
// This file is part of the "math-server" project.
// For details, see https://github.com/egor-tensin/math-server.
// Distributed under the MIT License.

#include <parser/error.hpp>
#include <parser/parser.hpp>

#include <boost/test/data/monomorphic.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/unit_test.hpp>

#include <string>
#include <string_view>
#include <vector>

namespace bdata = boost::unit_test::data;
using math::server::Parser;
using math::server::ParserError;

namespace {
namespace exec::valid {

const std::vector<std::string_view> input{
    // Constants and unary operators:
    "1",
    "-1",
    "--1",
    "+--+-2",
    // Basic exponentiation:
    "0^0",
    "4^.5",
    " 4 ^ ---0.5 ",
    // Exponentiation is right-associative:
    "2 ^ 3 ^ 3",
    "(2 ^ 3) ^ 3",
    // Exponentiation has higher precedence than the unary minus:
    "(-2) ^ 2",
    "-2 ^ 2",
    "(.5 ^ -1) ^ 4",
    ".5 ^ -1 ^ 4",
    // Basic binary operators:
    " 1 + 2 ",
    " 2 * 1 + 3 ",
    " 2 * (1 + 3) ",
    " 2 * (1 + 3 * (1 - -3)) ",
    " -2 * (1 + --3 * (1 - -3)) ",
    // Looks weird, but also works in e.g. Python:
    " -2 * -+--- (3 + -100e-1)  ",
    " -2^2 --+- -3^0 / (4 + +12) ^.5^2 ",
    " -2^2 --+- -3^0 / ((4 + +12) ^.5)^2 ",
};

const std::vector<double> expected{
    // Constants and unary operators:
    1,
    -1,
    1,
    -2,
    // Basic exponentiation:
    1,
    2,
    0.5,
    // Exponentiation is right-associative:
    134217728,
    512,
    // Exponentiation has higher precedence than the unary minus:
    4,
    -4,
    16,
    2,
    // Basic binary operators:
    3,
    5,
    8,
    26,
    -26,
    // Looks weird, but also works in e.g. Python:
    14,
    -3.5,
    -3.9375,
};

} // namespace exec::valid

namespace exec::invalid {

const std::vector<std::string_view> input{
    "",
    "-",
    "-+-",
    // Missing operand:
    " 1 + ",
    "-2 + 3 ^ -",
    // Unmatched parentheses:
    " 2 * (1 + 3 ",
    " 2 * (1 + 3) )",
};

const std::vector<std::string> error_msg{
    "server error: parser error: expected '-', '+', '(' or a number",
    "server error: parser error: expected '-', '+', '(' or a number",
    "server error: parser error: expected '-', '+', '(' or a number",
    // Missing operand:
    "server error: parser error: expected '-', '+', '(' or a number",
    "server error: parser error: expected '-', '+', '(' or a number",
    // Unmatched parentheses:
    "server error: parser error: missing closing ')'",
    "server error: parser error: expected a binary operator",
};

} // namespace exec::invalid
} // namespace

BOOST_AUTO_TEST_SUITE(parser_tests)

BOOST_DATA_TEST_CASE(test_exec_valid,
                     bdata::make(exec::valid::input) ^ exec::valid::expected,
                     input,
                     expected) {
    Parser parser{input};
    BOOST_TEST(parser.exec() == expected);
}

BOOST_DATA_TEST_CASE(test_exec_invalid,
                     bdata::make(exec::invalid::input) ^ exec::invalid::error_msg,
                     input,
                     error_msg) {
    BOOST_REQUIRE_THROW(
        do {
            Parser parser{input};
            parser.exec();
        } while (0),
        ParserError);

    try {
        Parser parser{input};
        parser.exec();
    } catch (const ParserError& e) {
        BOOST_TEST(error_msg == e.what());
    }
}

BOOST_AUTO_TEST_SUITE_END()


//==================================================================================================
/*
  Copyright 2017 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
//! [is_less]
#include <boost/simd/predicates.hpp>
#include <boost/simd/pack.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/mindenormal.hpp>
#include <iostream>

namespace bs =  boost::simd;
using pack_ft = bs::pack <float, 8>;

int main()
{
  pack_ft pf = { 0.0f, 1.0f, -1.0f, -2.0f
                , bs::Mindenormal<float>(), bs::Inf<float>(), bs::Minf<float>(), bs::Nan<float>() };
  pack_ft qf = { bs::Mindenormal<float>(), bs::Inf<float>(), bs::Minf<float>(), bs::Nan<float>(),
                 0.0f, 1.0f, -1.0f, -2.0f };

  std::cout
    << "---- simd" << '\n'
    << "<- pf =                  " << pf << '\n'
    << "<- qf =                  " << pf << '\n'
    << "-> bs::is_less(pf, qf) = " << bs::is_less(pf, qf) << '\n';

  float xf = 1.0f;
  float yf = bs::Nan<float>();

  std::cout
    << "---- scalar"  << '\n'
    << "<- xf =                  " << xf << '\n'
    << "<- yf =                  " << yf << '\n'
    << "-> bs::is_less(xf, yf) = " << bs::is_less(xf, yf) << '\n';
  return 0;
}
//! [is_less]


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<float, Eigen::Dynamic, 6> test_float_D_6(const Eigen::Matrix<float, Eigen::Dynamic, 6> & M)
{
	return M;
}
void export_float_D_6()
{
	boost::python::def("test_float_D_6",test_float_D_6);
}


///////////////////////////////////////////////////////////////////////////////
// kernel::bandwidth::include.hpp                                            //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_KERNEL_BANDWIDTH_SELECTION_INCLUDE_H_ER_2009
#define BOOST_STATISTICS_DETAIL_KERNEL_BANDWIDTH_SELECTION_INCLUDE_H_ER_2009

#include <boost/statistics/detail/kernel/bandwidth/normal_distribution.hpp>
#include <boost/statistics/detail/kernel/bandwidth/meta_k_fold_nw.hpp>

// TODO meta_k_fold_rp

#endif


//           Copyright Jeroen Habraken 2011 - 2012.
//
// 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(BOOST_COERCE_DETAIL_PRECISION_HPP)
#define BOOST_COERCE_DETAIL_PRECISION_HPP

#if defined(_MSC_VER) && _MSC_VER >= 1200
    #pragma once
#endif  // defined(_MSC_VER) && _MSC_VER >= 1200

#include <boost/config.hpp>
#include <boost/limits.hpp>
#include <boost/spirit/home/karma/numeric/real.hpp>
#include <boost/spirit/home/karma/numeric/real_policies.hpp>
#include <boost/type_traits/remove_const.hpp>

namespace boost { namespace coerce { namespace detail {

    template <typename T>
    struct precision {
        typedef std::numeric_limits<T> limits;

        BOOST_STATIC_CONSTANT(bool, is_specialized =
            limits::is_specialized && !limits::is_exact);

        BOOST_STATIC_CONSTANT(bool, is_specialized_binary =
            is_specialized && limits::radix == 2 && limits::digits > 0);

        BOOST_STATIC_CONSTANT(unsigned, precision_binary =
            2UL + limits::digits * 30103UL / 100000UL);

        BOOST_STATIC_CONSTANT(bool, is_specialized_decimal =
            is_specialized && limits::radix == 10 && limits::digits10 > 0);

        BOOST_STATIC_CONSTANT(unsigned, precision_decimal =
            limits::digits10 + 1U);

        BOOST_STATIC_CONSTANT(unsigned, value =
            is_specialized_binary ? precision_binary
                : is_specialized_decimal ? precision_decimal : 6);
    };

    template <typename Source>
    struct real_policies
        : spirit::karma::real_policies<
            typename remove_const<Source>::type
        > {
        static inline unsigned
        precision(Source const &) {
            return detail::precision<Source>::value;
        }
    };

    template <typename Source>
    struct real_generator
        : spirit::karma::real_generator<
            Source, detail::real_policies<Source>
        > { };

} } }  // namespace boost::coerce::detail

#endif  // !defined(BOOST_COERCE_DETAIL_PRECISION_HPP)


/* \author Adrian Arroyo - adr.arroyo.perez@gmail.com */

#include <iostream>

#include <boost/thread/thread.hpp>
#include <pcl/range_image/range_image.h>
#include <pcl/io/ply_io.h>
#include <pcl/visualization/range_image_visualizer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/features/range_image_border_extractor.h>
#include <pcl/keypoints/narf_keypoint.h>
#include <pcl/features/narf_descriptor.h>
#include <pcl/console/parse.h>
#include <pcl/point_types.h>
#include <pcl/features/pfh.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/features/normal_3d.h>
#include <pcl/surface/gp3.h>
#include <pcl/features/spin_image.h>

typedef pcl::PointXYZ PointType;

// --------------------
// -----Parameters-----
// --------------------
float angular_resolution = 0.5f;
float support_size = 0.2f;
pcl::RangeImage::CoordinateFrame coordinate_frame =
		pcl::RangeImage::CAMERA_FRAME;
bool setUnseenToMaxRange = false;
bool rotation_invariant = true;

// --------------
// -----Help-----
// --------------
void printUsage(const char* progName) {
	std::cout << "\n\nUsage: " << progName << " [options] <scene.pcd>\n\n"
			<< "Options:\n" << "-------------------------------------------\n"
			<< "-r <float>   angular resolution in degrees (default "
			<< angular_resolution << ")\n"
			<< "-c <int>     coordinate frame (default "
			<< (int) coordinate_frame << ")\n"
			<< "-m           Treat all unseen points to max range\n"
			<< "-s <float>   support size for the interest points (diameter of the used sphere - "
					"default " << support_size << ")\n"
			<< "-o <0/1>     switch rotational invariant version of the feature on/off"
			<< " (default " << (int) rotation_invariant << ")\n"
			<< "-h           this help\n" << "\n\n";
}

void setViewerPose(pcl::visualization::PCLVisualizer& viewer,
		const Eigen::Affine3f& viewer_pose) {
	Eigen::Vector3f pos_vector = viewer_pose * Eigen::Vector3f(0, 0, 0);
	Eigen::Vector3f look_at_vector = viewer_pose.rotation()
			* Eigen::Vector3f(0, 0, 1) + pos_vector;
	Eigen::Vector3f up_vector = viewer_pose.rotation()
			* Eigen::Vector3f(0, -1, 0);
	viewer.setCameraPosition(pos_vector[0], pos_vector[1], pos_vector[2],
			look_at_vector[0], look_at_vector[1], look_at_vector[2],
			up_vector[0], up_vector[1], up_vector[2]);
}

// --------------
// -----Main-----
// --------------
int main(int argc, char** argv) {
	// --------------------------------------
	// -----Parse Command Line Arguments-----
	// --------------------------------------
	if (pcl::console::find_argument(argc, argv, "-h") >= 0) {
		printUsage(argv[0]);
		return 0;
	}
	if (pcl::console::find_argument(argc, argv, "-m") >= 0) {
		setUnseenToMaxRange = true;
		cout
				<< "Setting unseen values in range image to maximum range readings.\n";
	}
	if (pcl::console::parse(argc, argv, "-o", rotation_invariant) >= 0)
		cout << "Switching rotation invariant feature version "
				<< (rotation_invariant ? "on" : "off") << ".\n";
	int tmp_coordinate_frame;
	if (pcl::console::parse(argc, argv, "-c", tmp_coordinate_frame) >= 0) {
		coordinate_frame = pcl::RangeImage::CoordinateFrame(
				tmp_coordinate_frame);
		cout << "Using coordinate frame " << (int) coordinate_frame << ".\n";
	}
	if (pcl::console::parse(argc, argv, "-s", support_size) >= 0)
		cout << "Setting support size to " << support_size << ".\n";
	if (pcl::console::parse(argc, argv, "-r", angular_resolution) >= 0)
		cout << "Setting angular resolution to " << angular_resolution
				<< "deg.\n";
	angular_resolution = pcl::deg2rad(angular_resolution);

	// ------------------------------------------------------------------
	// -----Read ply file-----
	// ------------------------------------------------------------------
	pcl::PointCloud<PointType>::Ptr point_cloud_ptr(
			new pcl::PointCloud<PointType>);
	pcl::PointCloud<PointType>& point_cloud = *point_cloud_ptr;
	pcl::PointCloud<pcl::PointWithViewpoint> far_ranges;
	Eigen::Affine3f scene_sensor_pose(Eigen::Affine3f::Identity());
	std::vector<int> pcd_filename_indices =
			pcl::console::parse_file_extension_argument(argc, argv, "ply");
	std::string filename = argv[pcd_filename_indices[0]];
	if (pcl::io::loadPLYFile(filename, point_cloud) == -1) {
		cerr << "Was not able to open file \"" << filename << "\".\n";
		printUsage(argv[0]);
		return 0;
	}
	scene_sensor_pose = Eigen::Affine3f(
			Eigen::Translation3f(point_cloud.sensor_origin_[0],
					point_cloud.sensor_origin_[1],
					point_cloud.sensor_origin_[2]))
			* Eigen::Affine3f(point_cloud.sensor_orientation_);

	//---------EXTRACTION OF NARF FEATURES------------
	// -----------------------------------------------
	// -----Create RangeImage from the PointCloud-----
	// -----------------------------------------------
	float noise_level = 0.0;
	float min_range = 0.0f;
	int border_size = 1;
	boost::shared_ptr<pcl::RangeImage> range_image_ptr(new pcl::RangeImage);
	pcl::RangeImage& range_image = *range_image_ptr;
	range_image.createFromPointCloud(point_cloud, angular_resolution,
			pcl::deg2rad(360.0f), pcl::deg2rad(180.0f), scene_sensor_pose,
			coordinate_frame, noise_level, min_range, border_size);
	range_image.integrateFarRanges(far_ranges);
	if (setUnseenToMaxRange)
		range_image.setUnseenToMaxRange();

	// --------------------------------------------
	// -----Open 3D viewer and add point cloud-----
	// --------------------------------------------
	pcl::visualization::PCLVisualizer viewer("3D Viewer");
	viewer.setBackgroundColor(1, 1, 1);
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> range_image_color_handler(
			range_image_ptr, 0, 0, 0);
	viewer.addPointCloud(range_image_ptr, range_image_color_handler,
			"range image");
	viewer.setPointCloudRenderingProperties(
			pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "range image");
	//viewer.addCoordinateSystem (1.0f, "global");
	//PointCloudColorHandlerCustom<PointType> point_cloud_color_handler (point_cloud_ptr, 150, 150, 150);
	//viewer.addPointCloud (point_cloud_ptr, point_cloud_color_handler, "original point cloud");
	viewer.initCameraParameters();
	setViewerPose(viewer, range_image.getTransformationToWorldSystem());

	// --------------------------------
	// -----Extract NARF keypoints-----
	// --------------------------------
	pcl::RangeImageBorderExtractor range_image_border_extractor;
	pcl::NarfKeypoint narf_keypoint_detector;
	narf_keypoint_detector.setRangeImageBorderExtractor(
			&range_image_border_extractor);
	narf_keypoint_detector.setRangeImage(&range_image);
	narf_keypoint_detector.getParameters().support_size = support_size;

	pcl::PointCloud<int> keypoint_indices;
	narf_keypoint_detector.compute(keypoint_indices);
	std::cout << "Found " << keypoint_indices.points.size() << " key points.\n";

	// -------------------------------------
	// -----Show keypoints in 3D viewer-----
	// -------------------------------------
	pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints_ptr(
			new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>& keypoints = *keypoints_ptr;
	keypoints.points.resize(keypoint_indices.points.size());
	for (size_t i = 0; i < keypoint_indices.points.size(); ++i)
		keypoints.points[i].getVector3fMap() =
				range_image.points[keypoint_indices.points[i]].getVector3fMap();
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> keypoints_color_handler(
			keypoints_ptr, 0, 255, 0);
	viewer.addPointCloud<pcl::PointXYZ>(keypoints_ptr, keypoints_color_handler,
			"keypoints");
	viewer.setPointCloudRenderingProperties(
			pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "keypoints");

	// ------------------------------------------------------
	// -----Extract NARF descriptors for interest points-----
	// ------------------------------------------------------
	std::vector<int> keypoint_indices2;
	keypoint_indices2.resize(keypoint_indices.points.size());
	for (unsigned int i = 0; i < keypoint_indices.size(); ++i) // This step is necessary to get the right vector type
		keypoint_indices2[i] = keypoint_indices.points[i];
	pcl::NarfDescriptor narf_descriptor(&range_image, &keypoint_indices2);
	narf_descriptor.getParameters().support_size = support_size;
	narf_descriptor.getParameters().rotation_invariant = rotation_invariant;
	pcl::PointCloud<pcl::Narf36> narf_descriptors;
	narf_descriptor.compute(narf_descriptors);
	cout << "Extracted " << narf_descriptors.size() << " descriptors for "
			<< keypoint_indices.points.size() << " keypoints.\n";

	//--------------------
	// -----Main loop-----
	//--------------------
	while (!viewer.wasStopped()) {
		viewer.spinOnce();
		pcl_sleep(0.01);
	}

	// ------------------------------------------------------
	// ---------------Extract PFH features-------------------
	// ------------------------------------------------------
	// Normal estimation*
	/*pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
	 pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
	 pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(
	 new pcl::search::KdTree<pcl::PointXYZ>);
	 tree->setInputCloud(point_cloud_ptr);
	 n.setInputCloud(point_cloud_ptr);
	 n.setSearchMethod(tree);
	 n.setKSearch(20);
	 n.compute(*normals);

	 // Create the PFH estimation class, and pass the input dataset+normals to it
	 /*pcl::PFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::PFHSignature125> pfh;
	 pfh.setInputCloud(point_cloud_ptr);
	 pfh.setInputNormals(normals);

	 // Create an empty kdtree representation, and pass it to the PFH estimation object.
	 // Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).
	 pfh.setSearchMethod(tree);

	 // Output datasets
	 pcl::PointCloud<pcl::PFHSignature125>::Ptr pfhs(
	 new pcl::PointCloud<pcl::PFHSignature125>());

	 // Use all neighbors in a sphere of radius 5cm
	 // IMPORTANT: the radius used here has to be larger than the radius used to estimate the surface normals!!!
	 pfh.setRadiusSearch(0.05);

	 // Check for NaNs in normals
	 for (int i = 0; i < normals->points.size(); i++) {
	 if (!pcl::isFinite<pcl::Normal>(normals->points[i])) {
	 PCL_WARN("normals[%d] is not finite\n", i);
	 }
	 }

	 // Compute the features
	 pfh.compute(*pfhs);

	 cout << "Extracted " << pfh.k_ << "PFH features";*/

	// ------------------------------------------------------
	// ------------Extract Spin Image Estimator--------------
	// ------------------------------------------------------
	// Setup the spin images computation
	/*typedef pcl::Histogram<153> SpinImage;
	pcl::SpinImageEstimation<pcl::PointXYZ, pcl::Normal, SpinImage> spinImageEstimation(
			8, 0.5, 16);
	spinImageEstimation.setInputCloud(point_cloud_ptr);
	spinImageEstimation.setInputNormals(normals);

	// Use the same KdTree from the normal estimation
	spinImageEstimation.setSearchMethod(tree);
	pcl::PointCloud<SpinImage>::Ptr spinImages(new pcl::PointCloud<SpinImage>);
	spinImageEstimation.setRadiusSearch (2);
	//spinImageEstimation.setKSearch(10);

	// Actually compute the spin images
	spinImageEstimation.compute(*spinImages);
	std::cout << "SI output points.size (): " << spinImages->points.size()
			<< std::endl;

	// Display and retrieve the spin image descriptor vector for the first point.
	SpinImage descriptor = spinImages->points[0];
	std::cout << descriptor << std::endl;*/

	return 1;
}


// ------------------------------------------------------------
// Copyright (c) Microsoft Corporation.  All rights reserved.
// Licensed under the MIT License (MIT). See License.txt in the repo root for license information.
// ------------------------------------------------------------

#include "stdafx.h"
#include "Accumulator.h"
#include "AccumulatorWithMinMax.h"
#include "DynamicBitSet.h"

#include <boost/test/unit_test.hpp>
#include "Common/boost-taef.h"
#include "ServiceModel/federation/NodeId.h"
#include "ServiceModel/federation/NodeIdGenerator.h"

namespace PlacementAndLoadBalancingUnitTest
{
    using namespace std;
    using namespace Common;
    using namespace Reliability::LoadBalancingComponent;

    // Tests for auxiliary structures in PLB (accumulators, bit sets, etc.)
    class TestAuxiliaryStructures
    {
    protected:
        void CompareSets(DynamicBitSet const& set, std::set<size_t> const& controlSet);
    };

    BOOST_FIXTURE_TEST_SUITE(TestAuxiliaryStructuresSuite, TestAuxiliaryStructures)

    BOOST_AUTO_TEST_CASE(AccumulatorBasicTest)
    {
        AccumulatorWithMinMax s(false);
        Federation::NodeId id;
        Federation::NodeIdGenerator::GenerateFromString(L"WorkerRole1.0", id);
        VERIFY_ARE_EQUAL(L"6e2eb710b98296decd1fb3d31b6e54df", id.ToString());

        s.AddOneValue(1, 0, Federation::NodeId::MinNodeId);
        s.AddOneValue(2, 0, Federation::NodeId::MaxNodeId);
        s.AddOneValue(3, 0, id);

        VERIFY_ARE_EQUAL(2.0, s.Average);
        VERIFY_ARE_EQUAL(6.0, s.Sum);
        VERIFY_ARE_EQUAL(14.0, s.SquaredSum);
        VERIFY_ARE_EQUAL(1.0, s.Min);
        VERIFY_ARE_EQUAL(3.0, s.Max);
        VERIFY_ARE_EQUAL(Federation::NodeId::MinNodeId, s.MinNode);
        VERIFY_ARE_EQUAL(id, s.MaxNode);

        s.AdjustOneValue(1, 3, 0, Federation::NodeId::MinNodeId);
        s.AdjustOneValue(3, 10, 0, Federation::NodeId::MinNodeId);

        VERIFY_ARE_EQUAL(5.0, s.Average);
        VERIFY_ARE_EQUAL(15.0, s.Sum);
        VERIFY_ARE_EQUAL(113.0, s.SquaredSum);
        VERIFY_ARE_EQUAL(10.0, s.Max);
    }

    BOOST_AUTO_TEST_CASE(EmptyTest)
    {
        AccumulatorWithMinMax s(false);
        VERIFY_ARE_EQUAL(0.0, s.NormStdDev);
        AccumulatorWithMinMax s2(true);
        VERIFY_ARE_EQUAL(0.0, s2.NormStdDev);
    }

    BOOST_AUTO_TEST_CASE(DynamicBitSetBasicTest)
    {
        // This set will grow
        DynamicBitSet set1(0);
        // This set will never grow
        DynamicBitSet set2(1042);

        VERIFY_ARE_EQUAL(set1.Count, 0);
        VERIFY_ARE_EQUAL(set2.Count, 0);

        // Control structures (we trust in std::set)
        std::set<size_t> controlSet;
        std::vector<size_t> elementsToRemove;

        // Random insert of 100 elements between 1 and 1000
        for (size_t count = 0; count < 100; ++count)
        {
            size_t element = (std::rand() % 1000) + 1;
            set1.Add(element);
            set2.Add(element);
            controlSet.insert(element);
            if ((std::rand() % 2) == 1)
            {
                elementsToRemove.push_back(element);
            }
        }

        CompareSets(set1, controlSet);
        CompareSets(set2, controlSet);

        // Remove elements from the set
        for (size_t element : elementsToRemove)
        {
            set1.Delete(element);
            set2.Delete(element);
            controlSet.erase(element);
        }

        CompareSets(set1, controlSet);
        CompareSets(set2, controlSet);
    }

    BOOST_AUTO_TEST_CASE(DynamicBitSetForEachTest)
    {
        // This set will grow
        DynamicBitSet bitSet(0);

        // Control structures (we trust in std::set)
        std::set<size_t> controlSet;

        // Random insert of 100 elements between 1 and 1000
        for (size_t count = 0; count < 100; ++count)
        {
            size_t element = (std::rand() % 1000) + 1;
            bitSet.Add(element);
            controlSet.insert(element);
        }

        // Special case (around the edges of words in bitmap)
        bitSet.Add(31);
        bitSet.Add(32);
        bitSet.Add(63);
        bitSet.Add(64);
        controlSet.insert(31);
        controlSet.insert(32);
        controlSet.insert(63);
        controlSet.insert(64);

        std::set<size_t> forEachOutput;

        bitSet.ForEach([&forEachOutput](size_t element) -> void
        {
            // Sanity check: can't have the same element 2 times
            VERIFY_IS_FALSE(forEachOutput.find(element) != forEachOutput.end());
            forEachOutput.insert(element);
        });

        // All sets must be the same
        CompareSets(bitSet, controlSet);
        CompareSets(bitSet, forEachOutput);
        VERIFY_ARE_EQUAL(controlSet, forEachOutput);
    }

    BOOST_AUTO_TEST_SUITE_END()

    void TestAuxiliaryStructures::CompareSets(DynamicBitSet const& set, std::set<size_t> const& controlSet)
    {
        VERIFY_ARE_EQUAL(set.Count, controlSet.size());
        for (size_t element : controlSet)
        {
            VERIFY_IS_TRUE(set.Check(element));
        }
    }
}


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014   MetaScale SAS
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================

#include <boost/fusion/include/at.hpp>
#include <vector>

#include <nt2/sdk/bench/benchmark.hpp>
#include <nt2/sdk/bench/metric/gflops.hpp>
#include <nt2/sdk/bench/protocol/max_duration.hpp>
#include <nt2/sdk/bench/setup/geometric.hpp>
#include <nt2/sdk/bench/setup/constant.hpp>
#include <nt2/sdk/bench/setup/combination.hpp>
#include <nt2/sdk/bench/stats/median.hpp>

using namespace nt2::bench;
using namespace nt2;

template<typename T> struct axpy_scalar
{
  typedef void experiment_is_immutable;
  template<typename Setup>
  axpy_scalar(Setup const& s)
              :  size_(boost::fusion::at_c<0>(s))
              ,  alpha(boost::fusion::at_c<1>(s))
  {
    X.resize(size_); Y.resize(size_);
    for(std::size_t i = 0; i<size_; ++i)
      X[i] = Y[i] = T(i);
  }

  void operator()()
  {
    for(std::size_t i = 0; i<size_; i++)
      Y[i] = Y[i] + alpha*(X[i]);
  }

  friend std::ostream& operator<<(std::ostream& os, axpy_scalar<T> const& p)
  {
    return os << "(" << p.size() << ")";
  }

  std::size_t size() const { return size_; }
  double flops() const { return 2.*size_; }

  private:
    std::size_t size_;
    T alpha;
    std::vector<T> X, Y;
};

NT2_REGISTER_BENCHMARK_TPL( axpy_scalar, NT2_SIMD_REAL_TYPES )
{
  std::size_t size_min  = args("size_min", 16);
  std::size_t size_max  = args("size_max", 4096);
  std::size_t size_step = args("size_step", 2);
  T alpha = args("alpha", 1.);

  run_during_with< axpy_scalar<T> > ( 1.
                                    , and_( geometric(size_min,size_max,size_step)
                                          , constant(alpha)
                                    )
                                    , gflops<stats::median_>()
                                    );
}


#include <boost/test/unit_test.hpp>

#include "Variable.hpp"
#include "it/Distribution.hpp"
#include "it/VectorCounter.hpp"

using namespace mist;

// test data
auto* data_many_bin2_a{ new Variable::data_t[6]{ 0, 1, 1, 0, 0, 1 } };
;
auto* data_many_bin2_b{ new Variable::data_t[6]{ 1, 1, 0, 0, 1, 0 } };
;

Variable variable_many_bin2_a(Variable::data_ptr(data_many_bin2_a), 6, 0, 2);
Variable variable_many_bin2_b(Variable::data_ptr(data_many_bin2_b), 6, 1, 2);

BOOST_AUTO_TEST_CASE(VectorCounter_default_constructor)
{
  it::VectorCounter pdv;
}

BOOST_AUTO_TEST_CASE(BitsetCounter_count_1)
{
  it::VectorCounter pdb;

  Variable::tuple vars;
  vars.push_back(variable_many_bin2_a);

  it::Distribution pd0;
  it::Distribution pd1;
  it::Distribution pd2;
  pdb.count(vars, { 0 }, pd0);
  pdb.count(vars, pd1);
  pdb.count(variable_many_bin2_a, pd2);

  BOOST_TEST(pd0(std::vector<Variable::data_t>{ 0 }) == 3);
  BOOST_TEST(pd0(std::vector<Variable::data_t>{ 1 }) == 3);
  BOOST_TEST(pd1(std::vector<Variable::data_t>{ 0 }) == 3);
  BOOST_TEST(pd1(std::vector<Variable::data_t>{ 1 }) == 3);
  BOOST_TEST(pd2(std::vector<Variable::data_t>{ 0 }) == 3);
  BOOST_TEST(pd2(std::vector<Variable::data_t>{ 1 }) == 3);

  BOOST_TEST(pd0 == pd2);
  BOOST_TEST(pd1 == pd2);
}

BOOST_AUTO_TEST_CASE(VectorCounter_count_bins_2)
{
  it::VectorCounter pdv;

  Variable::tuple vars;
  vars.push_back(variable_many_bin2_a);
  vars.push_back(variable_many_bin2_b);

  it::Distribution pd;
  pdv.count(vars, pd);

  BOOST_TEST(pd(std::vector<Variable::data_t>{ 0, 0 }) == 1);
  BOOST_TEST(pd(std::vector<Variable::data_t>{ 0, 1 }) == 2);
  BOOST_TEST(pd(std::vector<Variable::data_t>{ 1, 0 }) == 2);
  BOOST_TEST(pd(std::vector<Variable::data_t>{ 1, 1 }) == 1);

  vars.push_back(variable_many_bin2_b);
  pdv.count(vars, pd);

  BOOST_TEST(pd(std::vector<Variable::data_t>{ 0, 0, 0 }) == 1);
  BOOST_TEST(pd(std::vector<Variable::data_t>{ 0, 0, 1 }) == 0);
  BOOST_TEST(pd(std::vector<Variable::data_t>{ 0, 1, 0 }) == 0);
  BOOST_TEST(pd(std::vector<Variable::data_t>{ 0, 1, 1 }) == 2);
}

static it::Distribution
polymorphCount(it::Counter& pdc, Variable::tuple const& vars)
{
  it::Distribution dist;
  pdc.count(vars, dist);
  return dist;
}

BOOST_AUTO_TEST_CASE(VectorCounter_polymorph)
{
  it::VectorCounter pdv;

  Variable::tuple vars;
  vars.push_back(variable_many_bin2_b);
  vars.push_back(variable_many_bin2_a);

  it::Distribution pd = polymorphCount(pdv, vars);

  BOOST_TEST(pd(std::vector<Variable::data_t>{ 0, 0 }) == 1);
  BOOST_TEST(pd(std::vector<Variable::data_t>{ 0, 1 }) == 2);
  BOOST_TEST(pd(std::vector<Variable::data_t>{ 1, 0 }) == 2);
  BOOST_TEST(pd(std::vector<Variable::data_t>{ 1, 1 }) == 1);
}