#include <boost/test/unit_test.hpp>
#include <array>
#include "utils/random.h"
#include "utils/noise.h"

namespace utf = boost::unit_test;
using namespace utils;

BOOST_AUTO_TEST_SUITE(noise_tests)

// checks that noise arrays with same seed contain the same values
// even if random generator instances are different
BOOST_AUTO_TEST_CASE(same_rand_different_obj, *utf::tolerance(0.00001))
{
    const int size = 256;
    std::array<float, size> noise_2d;
    std::array<float, size> noise_2d_second;
    std::array<float, size> noise_2d_third;

    random rand(1);
    for (int i = 0; i < size; i++)
    {
        noise_2d[i] = rand.get_float();
    }

    random rand2(1);
    for (int i = 0; i < size; i++)
    {
        noise_2d_second[i] = rand2.get_float();
        BOOST_TEST(noise_2d[i] == noise_2d_second[i]);
    }

    random rand3(1);
    for (int i = 0; i < size; i++)
    {
        noise_2d_third[i] = rand3.get_float();
        BOOST_TEST(noise_2d[i] == noise_2d_third[i]);
    }
}

// checks that noise arrays with same seed contain the same values when updating
// the seed via the class method
BOOST_AUTO_TEST_CASE(same_rand_same_obj, *utf::tolerance(0.00001))
{
    const int size = 256;
    std::array<float, size> noise_2d;
    std::array<float, size> noise_2d_second;
    std::array<float, size> noise_2d_third;

    random rand(1);
    for (int i = 0; i < size; i++)
    {
        noise_2d[i] = rand.get_float();
    }
    rand.seed(2);
    for (int i = 0; i < size; i++)
    {
        noise_2d_second[i] = rand.get_float();
        BOOST_TEST(noise_2d[i] != noise_2d_second[i]);
    }

    rand.seed(1);
    for (int i = 0; i < size; i++)
    {
        noise_2d_third[i] = rand.get_float();
        BOOST_TEST(noise_2d[i] == noise_2d_third[i]);
    }
}

// checks that perlin_1d method with same parameters returns the same values
BOOST_AUTO_TEST_CASE(same_noise_1d)
{
    const int size = 256;
    std::array<float, size> noise_1d;
    std::array<float, size> noise_1d_second;

    utils::perlin_noise_1d(size, 5, 0.2f, 0, noise_1d.data());
    utils::perlin_noise_1d(size, 5, 0.2f, 0, noise_1d_second.data());

    for(int i = 0; i < size; i++)
    {
        BOOST_TEST(noise_1d[i] == noise_1d_second[i]);
    }
}

// checks that perlin_2d method with same parameters returns the same values
BOOST_AUTO_TEST_CASE(same_noise_2d)
{
    const int size = 256;
    std::array<float, size * size> noise_2d;
    std::array<float, size * size> noise_2d_second;

    utils::perlin_noise_2d(size, 5, 0.2f, 0, noise_2d.data());
    utils::perlin_noise_2d(size, 5, 0.2f, 0, noise_2d_second.data());

    for(int i = 0; i < size * size; i++)
    {
        BOOST_TEST(noise_2d[i] == noise_2d_second[i]);
    }
}

// checks that perlin_2d method with different parameters does not return the same values
BOOST_AUTO_TEST_CASE(noise_2d_changed)
{
    const int size = 256;
    std::array<float, size * size> noise_2d;
    std::array<float, size * size> noise_2d_second;

    utils::perlin_noise_2d(size, 5, 0.2f, 0, noise_2d.data());
    utils::perlin_noise_2d(size, 7, 0.3f, 0, noise_2d_second.data());

    for(int i = 0; i < size * size; i++)
    {
        BOOST_TEST(noise_2d[i] != noise_2d_second[i]);
    }

    utils::perlin_noise_2d(size, 5, 0.2f, 0, noise_2d_second.data());

    for(int i = 0; i < size * size; i++)
    {
        BOOST_TEST(noise_2d[i] == noise_2d_second[i]);
    }
}

BOOST_AUTO_TEST_SUITE_END()


//==================================================================================================
/*!
  @file

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

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

#include <boost/simd/constant/zero.hpp>
#include <boost/simd/function/is_eqz.hpp>
#include <boost/simd/logical.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( if_zero_else_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::scalar_< logical_<A0> >
                          , bd::scalar_< bd::unspecified_<A1> >
                          )
  {
    BOOST_FORCEINLINE A1 operator() ( A0 a0, A1 const& a1) const BOOST_NOEXCEPT
    {
      return is_eqz(a0) ? a1 : Zero<A1>();
    }
  };

  BOOST_DISPATCH_OVERLOAD ( if_zero_else_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::scalar_< bd::arithmetic_<A0> >
                          , bd::scalar_< bd::arithmetic_<A1> >
                          )
  {
    BOOST_FORCEINLINE A1 operator() ( A0 a0, A1  a1) const BOOST_NOEXCEPT
    {
      return a0 ? Zero<A1>() : a1;
    }
  };

  BOOST_DISPATCH_OVERLOAD ( if_zero_else_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::bool_<A0> >
                          , bd::scalar_< bd::bool_<A0> >
                          )
  {
    BOOST_FORCEINLINE bool operator() ( A0 a0, A0  a1) const BOOST_NOEXCEPT
    {
      return a0 ? false : a1;
    }
  };
} } }

#endif


#define BOOST_TEST_MODULE "test_read_excluded_volume_wall_potential"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <mjolnir/input/read_external_potential.hpp>
#include <tuple>

using test_types = std::tuple<double, float>;

constexpr inline float  tolerance_value(float)  noexcept {return 1e-4;}
constexpr inline double tolerance_value(double) noexcept {return 1e-8;}

template<typename Real>
decltype(boost::test_tools::tolerance(std::declval<Real>()))
tolerance() {return boost::test_tools::tolerance(tolerance_value(Real()));}

BOOST_AUTO_TEST_CASE_TEMPLATE(read_excluded_volume_wall_noenv, T, test_types)
{
    mjolnir::LoggerManager::set_default_logger("test_read_excluded_volume_wall.log");

    using real_type = T;
    {
        using namespace toml::literals;
        const toml::value v = u8R"(
            interaction    = "Distance"
            potential      = "ExcludedVolumeWall"
            shape.name     = "AxisAlignedPlane"
            shape.axis     = "-X"
            shape.position = 1.0
            shape.margin   = 0.5
            epsilon        = 3.14
            parameters     = [
                {index = 0, radius = 2.0},
                {index = 1, radius = 3.0},
                {index = 5, radius = 4.0},
            ]
        )"_toml;

        const auto g = mjolnir::read_excluded_volume_wall_potential<real_type>(v);

        BOOST_TEST(g.participants().size() == 3u);
        BOOST_TEST(g.participants().at(0) == 0u);
        BOOST_TEST(g.participants().at(1) == 1u);
        BOOST_TEST(g.participants().at(2) == 5u);
        BOOST_TEST(g.parameters().at(0)  == real_type(2.0 ), tolerance<real_type>());
        BOOST_TEST(g.parameters().at(1)  == real_type(3.0 ), tolerance<real_type>());
        BOOST_TEST(g.parameters().at(5)  == real_type(4.0 ), tolerance<real_type>());
        BOOST_TEST(g.epsilon()           == real_type(3.14), tolerance<real_type>());
        BOOST_TEST(g.cutoff()/*default*/ == real_type(2.0),  tolerance<real_type>());
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(read_excluded_volume_wall_env, T, test_types)
{
    mjolnir::LoggerManager::set_default_logger("test_read_excluded_volume_wall.log");

    using real_type = T;
    {
        using namespace toml::literals;
        const toml::value v = u8R"(
            interaction    = "Distance"
            potential      = "ExcludedVolumeWall"
            shape.name     = "AxisAlignedPlane"
            shape.axis     = "-X"
            shape.position = 1.0
            shape.margin   = 0.5
            epsilon        = 3.14
            cutoff         = 2.71
            env.large = 10.0
            env.small = 0.01
            parameters     = [
                {index = 0, radius = "large"},
                {index = 1, radius = "small"},
                {index = 5, radius = "large"},
            ]
        )"_toml;

        const auto g = mjolnir::read_excluded_volume_wall_potential<real_type>(v);

        BOOST_TEST(g.participants().size() == 3u);
        BOOST_TEST(g.participants().at(0) == 0u);
        BOOST_TEST(g.participants().at(1) == 1u);
        BOOST_TEST(g.participants().at(2) == 5u);

        BOOST_TEST(g.parameters().at(0)  == real_type(10.0), tolerance<real_type>());
        BOOST_TEST(g.parameters().at(1)  == real_type(0.01), tolerance<real_type>());
        BOOST_TEST(g.parameters().at(5)  == real_type(10.0), tolerance<real_type>());
        BOOST_TEST(g.epsilon()           == real_type(3.14), tolerance<real_type>());
        BOOST_TEST(g.cutoff()            == real_type(2.71), tolerance<real_type>());
    }
}


#include <graphlab.hpp>
#include <vector>
#include <algorithm>
#include <boost/iterator/counting_iterator.hpp>
#include <graphlab/macros_def.hpp>

namespace pagerank {
  typedef std::vector<float> gather_type;
}

namespace std {
  // The gather type is vector of float = numdocs * <topic samples>
  inline pagerank::gather_type&
    operator+=(pagerank::gather_type& lvalue, const pagerank::gather_type& rvalue) {
      for (size_t i = 0; i < lvalue.size(); ++i)
        lvalue[i] += rvalue[i];
      return lvalue;
    }
}

namespace pagerank {
// Global random reset probability
float RESET_PROB = 0.15;
float TOLERANCE = 1.0E-2;
int DEFAULT_NDOCS = 3;
int DEFAULT_TOPICVAL = 1;
int NPAGES = 0;
int NTOPICS = 50;
size_t TOPK = 50;
bool HAS_DOC_COUNT = false;
bool JOIN_ON_ID = true;
std::vector<float> w_personal; // personalization weights for topics

struct top_pages_type {
  graphlab::mutex lock;
  std::string json_string;
  top_pages_type() :
    json_string("{\n" + json_header_string() + "\tvalues: [] \n }") { }
  inline std::string json_header_string() const {
    return
      "\t\"npages\": " + graphlab::tostr(NPAGES) + ",\n" +
      "\t\"ntopics\": " + graphlab::tostr(NTOPICS) + ",\n" +
      "\t\"default_ndocs\": " + graphlab::tostr(DEFAULT_NDOCS) + ",\n" +
      "\t\"default_topicval\": " + graphlab::tostr(DEFAULT_TOPICVAL) + ",\n";
  } // end of json header string
} TOP_PAGES;

std::pair<std::string, std::string>
pagerank_callback(std::map<std::string, std::string>& varmap) {
  TOP_PAGES.lock.lock();
  const std::pair<std::string, std::string>
    pair("text/html",TOP_PAGES.json_string);
  TOP_PAGES.lock.unlock();
  return pair;
}

void set_w_personal(std::vector<float> new_w_personal) {
  w_personal = new_w_personal;
}

std::pair<std::string, std::string>
weight_update_callback(std::map<std::string, std::string>& varmap) {
  std::string c = varmap["topic"];
  size_t topick = atoi(c.c_str());
  std::vector<float> curwp = w_personal;
  if (topick < curwp.size()) {
    curwp[topick] *= 1.2;
    if (curwp[topick] < 0.1) curwp[topick] = 0.1;
    double s = 0;
    for (size_t i = 0;i < curwp.size(); ++i) s += curwp[i];
    for (size_t i = 0;i < curwp.size(); ++i) curwp[i] /= s;

    graphlab::procid_t nprocs = graphlab::dc_impl::get_last_dc()->numprocs();
    graphlab::dc_impl::get_last_dc()->remote_call(boost::counting_iterator<graphlab::procid_t>(0),
                                        boost::counting_iterator<graphlab::procid_t>(nprocs),
                                        set_w_personal, curwp);

  }
  std::pair<std::string, std::string> pair("text/plain","");
  std::stringstream strm;
  strm.precision(3);
  for (size_t i = 0;i < curwp.size(); ++i) {
    strm << curwp[i] << " ";
  }
  strm.flush();
  pair.second = strm.str();
  // add a boost to the topic
  return pair;
}

// Graph Types
// ============================================================================
struct user_feature {
   size_t join_key; // hash value of the user name, used as vertex join key
   float rank;
   int numdocs;
   std::vector<float> topics;
   std::vector<float> out_normalizer;
   user_feature() : join_key(-1), rank(1.0),
                              numdocs(DEFAULT_NDOCS),
                              topics(std::vector<float>(NTOPICS, 1.0/NTOPICS)),
                              out_normalizer(topics) { }
   user_feature(size_t join_key) : join_key(join_key), rank(1.0),
                              numdocs(DEFAULT_NDOCS),
                              topics(std::vector<float>(NTOPICS, 1.0/NTOPICS)),
                              out_normalizer(topics) { }
   std::string topics_tostr() const {
     std::stringstream strm;
     for (size_t i = 0; i < topics.size(); ++i) {
       strm << topics[i];
       if (i < topics.size()-1)
         strm << ", ";
     }
     return strm.str();
   }
  void save(graphlab::oarchive& oarc) const {
    oarc << join_key << rank << numdocs << topics << out_normalizer;
  }
  void load(graphlab::iarchive& iarc) {
    iarc >> join_key >> rank >> numdocs >> topics >> out_normalizer;
  }
};

// The vertex data is its pagerank
typedef user_feature vertex_data_type;
// The edge data is its the marginal transition probability
typedef float edge_data_type;
// The gather type (for compute_transit vertex program) is the user topic vector.
typedef std::vector<float> gather_type;

// The graph type is determined by the vertex and edge data types
typedef graphlab::distributed_graph<vertex_data_type, edge_data_type> graph_type;

// void init_vertex(graph_type::vertex_type& vertex) {
//   vertex.data().numdocs = DEFAULT_NDOCS;
//   std::fill(vertex.data().topics.begin(), vertex.data().topics.end(), 1);
//   vertex.data().rank = 1.0;
// }

/*
 * Load a table of userid -> username, used for create vertex_join key
 */
bool vertex_line_parser(graph_type& graph,
                        const std::string& filename,
                        const std::string& textline) {
  ASSERT_FALSE(textline.empty());
  namespace qi = boost::spirit::qi;
  namespace ascii = boost::spirit::ascii;
  namespace phoenix = boost::phoenix;

  graphlab::vertex_id_type vid(-1);
  std::string name;
  int numdocs = DEFAULT_NDOCS;

  if (HAS_DOC_COUNT) {
    const bool success = qi::phrase_parse
      (textline.begin(), textline.end(),
       //  Begin grammar
       (
        qi::ulong_[phoenix::ref(vid) = qi::_1] >> -qi::char_(',') >>
        qi::int_[phoenix::ref(numdocs) = qi::_1] >> -qi::char_(',') >>
        +qi::char_[phoenix::ref(name) = qi::_1]
        )
       ,
       //  End grammar
       ascii::space);
    if(!success) return false;
  } else {
    const bool success = qi::phrase_parse
      (textline.begin(), textline.end(),
       //  Begin grammar
       (
        qi::ulong_[phoenix::ref(vid) = qi::_1] >> -qi::char_(',') >>
        +qi::char_[phoenix::ref(name) = qi::_1]
        )
       ,
       //  End grammar
       ascii::space);
    if(!success) return false;
  }

  size_t join_key = boost::hash<std::string>()(name);

  // std::cout << vid << ", " << name << "(" << join_key << ")" << std::endl;

  user_feature vdata(join_key);
  vdata.numdocs = numdocs;
  ASSERT_FALSE(join_key == size_t(-1)); // -1 is reserved for non_join_vertex
  graph.add_vertex(vid, vdata);
  return true;
}

/* Global thread func for join vertices and recompute transit probs */
size_t left_emit_key (const graph_type::vertex_type& vertex) {
  return JOIN_ON_ID ? vertex.id() : vertex.data().join_key;
}
inline bool is_join(const graph_type::vertex_type& vertex) {
  return left_emit_key(vertex) != size_t(-1);
}

bool load_and_initialize_graph(graphlab::distributed_control& dc,
                               graph_type& graph,
                               const std::string& edge_dir,
                               const std::string& vertex_dir) {
  dc.cout() << "Loading pagerank graph." << std::endl;
  graphlab::timer timer; timer.start();

  graph.load_format(edge_dir, "snap");
  graph.load(vertex_dir, vertex_line_parser);

  dc.cout() << "Loading pagerank graph. Finished in "
            << timer.current_time() << " seconds." << std::endl;

  dc.cout() << "Finalizing pagerank graph." << std::endl;
  timer.start();
  graph.finalize();
  dc.cout() << "Finalizing pagerank graph. Finished in "
            << timer.current_time() << " seconds." << std::endl;

  // must call finalize before querying the graph
  dc.cout() << " #vertices: " << graph.num_vertices()
            << " #edges:" << graph.num_edges()
            << std::endl;

  size_t isjoin = graph.map_reduce_vertices<size_t>(is_join);
  ASSERT_EQ(isjoin, graph.num_vertices());
  NPAGES = graph.num_vertices();
  TOP_PAGES.lock.lock();
  TOP_PAGES.json_string = "{\n" + TOP_PAGES.json_header_string() +
    "\t\"values\": [] \n }";
  TOP_PAGES.lock.unlock();
  return true;
}

////////// Vertex Program 1 //////////////
// One pass trasformation to initialize the topic-specific jump probability
class compute_transit_prob :
  public graphlab::ivertex_program<graph_type, gather_type>
  {

    std::vector<float> normalizer;

public:
    edge_dir_type gather_edges(icontext_type& context, const vertex_type& vertex) const {
      return graphlab::ALL_EDGES;
    }

    void save(graphlab::oarchive& arc) const {
      arc << normalizer;
    }

    void load(graphlab::iarchive& arc) {
      arc >> normalizer;
    }

    // gather_type is vector<float> of length = #topics.
    gather_type gather(icontext_type& context, const vertex_type& vertex, edge_type& edge) const {
      vertex_type other = edge.target().id() == vertex.id() ? edge.source() : edge.target();
      int numdocs = other.data().numdocs;
      const std::vector<float>& topics = other.data().topics;
      std::vector<float> ret(topics);
      return ret;
    }

    void apply(icontext_type& context, vertex_type& vertex, const gather_type& total) {
      vertex.data().out_normalizer = total;
    }

    edge_dir_type scatter_edges(icontext_type& context, const vertex_type& vertex) const {
      return graphlab::NO_EDGES;
    }

    void scatter(icontext_type& context, const vertex_type& vertex, edge_type& edge) const {
      vertex_type other = edge.target().id() == vertex.id() ? edge.source() : edge.target();
      double pij = 0.0; // the marginal (over topics) jump probability from this vertex to its target
      std::vector<float>& pk_ij = other.data().topics;
      int numdocs = other.data().numdocs;
      ASSERT_EQ(pk_ij.size(), w_personal.size());
      for (size_t k = 0; k < vertex.data().topics.size(); ++k) {
        if (normalizer[k] > 0)
        pij +=  (w_personal[k] * pk_ij[k] / (float)normalizer[k]);  // p(i -> j | k)
        // pij +=  (1.0/NTOPICS)*(pk_ij[k] / ((float)normalizer[k]) );  // p(i -> j | k)
      }
      edge.data() = (float)pij;
    }
  }; // end of compute_transit_prob

/////////////// Vertex Program 2 //////////////////
class compute_pagerank :
  public graphlab::ivertex_program<graph_type, float>,
  public graphlab::IS_POD_TYPE {
public:
    edge_dir_type gather_edges(icontext_type& context, const vertex_type& vertex) const {
      return graphlab::ALL_EDGES;
    }

  /* Gather the weighted rank of the adjacent page   */
  float gather(icontext_type& context, const vertex_type& vertex,
               edge_type& edge) const {
    vertex_type other = edge.target().id() == vertex.id() ? edge.source() : edge.target();

    float pij = 0;
    for (size_t k = 0; k < vertex.data().topics.size(); ++k) {
        if (other.data().out_normalizer[k] > 0) {
          pij +=  (w_personal[k] * vertex.data().topics[k] / (float)other.data().out_normalizer[k]);
        }
    }

    return ((1.0 - RESET_PROB) * pij * other.data().rank);
    // return ((1.0 - RESET_PROB) * edge.source().data().rank / edge.source().num_out_edges());
  }

  /* Use the total rank of adjacent pages to update this page */
  void apply(icontext_type& context, vertex_type& vertex,
             const float& total) {
    const double newval = total + RESET_PROB;
    vertex.data().rank = std::isnan(newval) ? 1 : newval;
    context.signal(vertex);
  }

  /* The scatter edges depend on whether the pagerank has converged */
  edge_dir_type scatter_edges(icontext_type& context,
                              const vertex_type& vertex) const {
    return graphlab::NO_EDGES;
  }

  /* The scatter function just signal adjacent pages */
  void scatter(icontext_type& context, const vertex_type& vertex,
               edge_type& edge) const {
    //context.signal(edge.target());
  }
}; // end of factorized_pagerank update functor

typedef compute_pagerank::icontext_type icontext_type;
// ========================================================
// Aggregators
struct vinfo_type {
  graphlab::vertex_id_type vid;
  user_feature features;
  void save(graphlab::oarchive& arc) const {
    arc << vid << features;
  }
  void load(graphlab::iarchive& arc) {
    arc >> vid >> features;
  }
  bool operator<(const vinfo_type& other) const {
      if (features.rank < other.features.rank)
        return true;
      else if (features.rank == other.features.rank)
        return (vid < other.vid);
      else
        return false;
  }
};

/** Periodically compute top k rank pages. */
class topk_aggregator {
private:
  std::set<vinfo_type> top_pages;
public:
  topk_aggregator() {}
  void save(graphlab::oarchive& arc) const { arc << top_pages; }
  void load(graphlab::iarchive& arc) { arc >> top_pages; }

  topk_aggregator& operator+=(const topk_aggregator& other) {
    if(other.top_pages.empty()) return *this;
      // Merge the topk
      top_pages.insert(other.top_pages.begin(),
                          other.top_pages.end());
      // Remove excess elements
      while(top_pages.size() > TOPK)
        top_pages.erase(top_pages.begin());
    return *this;
  } // end of operator +=

  static topk_aggregator map(icontext_type& context,
                             const graph_type::vertex_type& vertex) {
    topk_aggregator ret_value;
    vinfo_type vinfo;
    vinfo.vid = vertex.id();
    vinfo.features = vertex.data();
    ret_value.top_pages.insert(vinfo);
    return ret_value;
  } // end of map function

  static void finalize(icontext_type& context,
                       const topk_aggregator& total) {
    if(context.procid() != 0) return;
    std::string json = "{\n"+ TOP_PAGES.json_header_string() +
      "\t\"values\": [\n";
      size_t counter = 0;
      std::set<vinfo_type>::iterator iter = total.top_pages.begin();
      while(iter != total.top_pages.end())  {
        json += "\t[\"" + graphlab::tostr(iter->vid) + "\", " +
          graphlab::tostr(iter->features.rank) + ", " +
          graphlab::tostr(iter->features.numdocs) + ", " +
          "[" + iter->features.topics_tostr() + "]"+
          "]";
        if(++counter < total.top_pages.size()) json += ", ";
        json += '\n';
        std::cout << graphlab::tostr(iter->vid)
                  << ": " << iter->features.rank
                  << " (" << iter->features.topics_tostr() << ")" << "\n";
        ++iter;
      }
      json += '\n';
      std::cout << std::endl;
    json += "]}";
    // Post the change to the global variable
    TOP_PAGES.lock.lock();
    TOP_PAGES.json_string.swap(json);
    TOP_PAGES.lock.unlock();
  } // end of finalize
}; // end of topk_aggregator
} // end of namespace pagerank
#include <graphlab/macros_undef.hpp>


/*
 *
 * Copyright (c) 2002, 2003 Kresimir Fresl, Toon Knapen and Karl Meerbergen
 *
 * 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)
 *
 * KF acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_TRAITS_UBLAS_VECTOR_H
#define BOOST_NUMERIC_BINDINGS_TRAITS_UBLAS_VECTOR_H

#include <boost/numeric/bindings/traits/vector_traits.hpp>

#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS

#ifndef BOOST_UBLAS_HAVE_BINDINGS
#  include <boost/numeric/ublas/vector.hpp>
#endif

namespace boost { namespace numeric { namespace bindings { namespace traits {

  // ublas::vector<>
  template <typename T, typename ArrT, typename V>
  struct vector_detail_traits< boost::numeric::ublas::vector<T, ArrT>, V >
  : default_vector_traits< V, T >
  {
#ifndef BOOST_NUMERIC_BINDINGS_NO_SANITY_CHECK
    BOOST_STATIC_ASSERT( (boost::is_same< boost::numeric::ublas::vector<T, ArrT>, typename boost::remove_const<V>::type >::value) );
#endif

    typedef boost::numeric::ublas::vector<T, ArrT>          identifier_type;
    typedef V                                               vector_type;
    typedef typename default_vector_traits< V, T >::pointer pointer;

    static pointer storage (vector_type& v) {
      typedef typename detail::generate_const<V,ArrT>::type array_type ;
      return vector_traits<array_type>::storage (v.data());
    }
  };

  // ublas::vector_reference<>
  template <typename V, typename VR>
  struct vector_detail_traits< boost::numeric::ublas::vector_reference<V>, VR >
  : default_vector_traits< VR, typename V::value_type >
  {
#ifndef BOOST_NUMERIC_BINDINGS_NO_SANITY_CHECK
    BOOST_STATIC_ASSERT( (boost::is_same< boost::numeric::ublas::vector_reference<V>, typename boost::remove_const<VR>::type >::value) );
#endif

    typedef boost::numeric::ublas::vector_reference<V>             identifier_type;
    typedef VR                                                     vector_type;
    typedef typename V::value_type                                 value_type ;
    typedef typename default_vector_traits<VR,value_type>::pointer pointer;

  private:
    typedef typename detail::generate_const<VR,V>::type vct_t;

  public:
    static pointer storage (vector_type& v) {
      return vector_traits<vct_t>::storage (v.expression());
    }
    static std::ptrdiff_t stride (vector_type& v) {
      return vector_traits<vct_t>::stride (v.expression());
    }
  };

  // ublas::vector_range<>
  template <typename V, typename VR>
  struct vector_detail_traits< boost::numeric::ublas::vector_range<V>, VR >
  : default_vector_traits< VR, typename V::value_type >
  {
#ifndef BOOST_NUMERIC_BINDINGS_NO_SANITY_CHECK
    BOOST_STATIC_ASSERT( (boost::is_same< boost::numeric::ublas::vector_range<V>, typename boost::remove_const<VR>::type >::value) );
#endif

    typedef boost::numeric::ublas::vector_range<V>                 identifier_type;
    typedef VR                                                     vector_type;
    typedef typename V::value_type                                 value_type ;
    typedef typename default_vector_traits<VR,value_type>::pointer pointer;

  private:
    typedef typename detail::generate_const<VR, typename VR::vector_closure_type>::type v_type;

  public:
    static pointer storage (vector_type& vr) {
      pointer ptr = vector_traits<v_type>::storage (vr.data());
      ptr += vr.start() * vector_traits<v_type>::stride (vr.data());
      return ptr;
    }
    static std::ptrdiff_t stride (vector_type& vr) {
      return vector_traits<v_type>::stride (vr.data());
    }
  };

  // ublas::vector_slice<>
  template <typename V, typename VS>
  struct vector_detail_traits<boost::numeric::ublas::vector_slice<V>, VS >
  : default_vector_traits< VS, typename V::value_type >
  {
#ifndef BOOST_NUMERIC_BINDINGS_NO_SANITY_CHECK
    BOOST_STATIC_ASSERT( (boost::is_same< boost::numeric::ublas::vector_slice<V>, typename boost::remove_const<VS>::type >::value) );
#endif

    typedef boost::numeric::ublas::vector_slice<V>                 identifier_type;
    typedef VS                                                     vector_type;
    typedef typename V::value_type                                 value_type ;
    typedef typename default_vector_traits<VS,value_type>::pointer pointer;

  private:
    typedef typename detail::generate_const<VS, typename VS::vector_closure_type>::type v_type;

  public:
    static pointer storage (vector_type& vs) {
      pointer ptr = vector_traits<v_type>::storage (vs.data());
      ptr += vs.start() * vector_traits<v_type>::stride (vs.data());
      return ptr;
    }
    static std::ptrdiff_t stride (vector_type& vs) {
      return vs.stride() * vector_traits<v_type>::stride (vs.data());
    }
  };

#ifndef BOOST_NUMERIC_BINDINGS_FORTRAN

  // (undocumented) ublas::c_vector<>
  template <typename T, std::size_t N, typename V>
  struct vector_detail_traits< boost::numeric::ublas::c_vector<T, N>, V >
  : default_vector_traits< V, T >
  {
#ifndef BOOST_NUMERIC_BINDINGS_NO_SANITY_CHECK
    BOOST_STATIC_ASSERT( (boost::is_same< boost::numeric::ublas::c_vector<T,N>, typename boost::remove_const<V>::type >::value) );
#endif

    typedef boost::numeric::ublas::c_vector<T,N>         identifier_type;
    typedef V                                            vector_type;
    typedef typename default_vector_traits<V,T>::pointer pointer;

    static pointer storage (vector_type& v) { return v.data(); }
  };

#endif // BOOST_NUMERIC_BINDINGS_FORTRAN

  // ublas::bounded_vector<>
  template <typename T, std::size_t N, typename V>
  struct vector_detail_traits< boost::numeric::ublas::bounded_vector<T, N>, V >
  : default_vector_traits< V, T >
  {
#ifndef BOOST_NUMERIC_BINDINGS_NO_SANITY_CHECK
    BOOST_STATIC_ASSERT( (boost::is_same< boost::numeric::ublas::bounded_vector<T, N>, typename boost::remove_const<V>::type >::value) );
#endif

    typedef boost::numeric::ublas::bounded_vector<T, N>     identifier_type;
    typedef V                                               vector_type;
    typedef typename default_vector_traits< V, T >::pointer pointer;

    static pointer storage (vector_type& v) {
      typedef typename detail::generate_const<V,typename identifier_type::array_type>::type array_type ;
      return vector_traits<array_type>::storage (v.data());
    }
  };

}}}}

#endif // BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS

#endif // BOOST_NUMERIC_BINDINGS_TRAITS_UBLAS_VECTOR_H


/**
 * \file dcs/math/stats/distribution/base_distribution.hpp
 *
 * \brief Base class for probability distributions.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright 2009 Marco Guazzone (marco.guazzone@gmail.com)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef DCS_MATH_STATS_DISTRIBUTION_BASE_DISTRIBUTION_HPP
#define DCS_MATH_STATS_DISTRIBUTION_BASE_DISTRIBUTION_HPP

#include <boost/utility/enable_if.hpp>
#include <boost/type_traits/is_base_of.hpp>
//#include <dcs/math/random/any_generator.hpp>
#include <dcs/math/random/base_generator.hpp>
#include <dcs/math/random/uniform_01_adaptor.hpp>
//#include <iosfwd>

namespace dcs { namespace math { namespace stats {

/**
 * \brief Interface for the \c ProbabilityDistribution concept.
 *
 * \tparam ValueT The type used for real numbers.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 */
template <typename ValueT>
class base_distribution
{
	public: typedef ValueT value_type;
	public: typedef value_type support_type;

	public: virtual ~base_distribution() { }

//	public: template <typename UniformRandomGeneratorT>
//		typename ::boost::disable_if<
//			::boost::is_base_of< ::dcs::math::random::base_generator<typename UniformRandomGeneratorT::result_type>, UniformRandomGeneratorT>,
//			support_type
//		>::type rand(UniformRandomGeneratorT& rng) const
//	{
//		::dcs::math::random::any_generator<value_type> any_rng;
//
//		any_rng = ::dcs::math::random::make_any_generator<UniformRandomGeneratorT&>(rng);
//
//		return do_rand(any_rng);
//	}

	public: template <typename UniformRandomGeneratorT>
		support_type rand(UniformRandomGeneratorT& rng) const
	{
		typedef UniformRandomGeneratorT base_generator_type;
		typedef ::dcs::math::random::uniform_01_adaptor<base_generator_type&, value_type> adapted_generator_type;
		//::dcs::math::random::uniform_01_adaptor<UniformRandomGeneratorT&,value_type> u01_rng = ::dcs::math::random::uniform_01_adaptor<UniformRandomGeneratorT&,value_type>(rng)
		adapted_generator_type u01_rng(rng);

		return do_rand(u01_rng);
	}

	public: support_type rand(::dcs::math::random::base_generator<value_type>& rng) const
	{
		return do_rand(rng);
	}

//	public: void print(::std::ostream& os) const
//	{
//		do_print(os);
//	}

	private: virtual support_type do_rand(::dcs::math::random::base_generator<value_type>& rng) const = 0;

//	private: virtual support_type do_rand(::dcs::math::random::any_generator<value_type>& rng) const = 0;

//	private: virtual ::std::ostream& do_print(::std::ostream& os) const
//	{
//		return os;
//	}

//	friend template <
//				typename CharT,
//				typename CharTraitsT
//			>
//		::std::basic_ostream<CharT,CharTraitsT>& operator<<(::std::basic_ostream<CharT,CharTraitsT>& os, base_distribution const& distr)
//	{
//		return distr.do_print(os);
//	}
};

//template <
//	typename CharT,
//	typename CharTraitsT,
//	typename ValueT
//>
//::std::basic_ostream<CharT,CharTraitsT>& operator<<(::std::basic_ostream<CharT,CharTraitsT>& os, base_distribution<ValueT> const& dist)
//{
//	return dist.print(os);
//}

}}} // Namaespace dcs::math::stats

#endif // DCS_MATH_STATS_DISTRIBUTION_BASE_DISTRIBUTION_HPP


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-bolean
    Function object implementing if_allbits_else_zero capabilities

    Returns a mask of bits. All ones if the
    input element is non @ref Zero else all zeros.

    This is a convenient alias of @ref genmask
  **/
  const boost::dispatch::functor<tag::if_allbits_else_zero_> if_allbits_else_zero = {};
} }
#endif

#include <boost/simd/function/scalar/genmask.hpp>
#include <boost/simd/function/simd/if_allbits_else_zero.hpp>

#endif


// Copyright (C) 2016 Internet Systems Consortium, Inc. ("ISC")
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include <config.h>

#include <asiolink/io_address.h>
#include <dhcp/dhcp6.h>
#include <dhcp/option6_pdexclude.h>
#include <exceptions/exceptions.h>
#include <util/encode/hex.h>
#include <util/io_utilities.h>

#include <boost/dynamic_bitset.hpp>
#include <iostream>
#include <stdint.h>

using namespace std;
using namespace isc;
using namespace isc::dhcp;
using namespace isc::asiolink;
using namespace isc::util;

namespace isc {
namespace dhcp {

Option6PDExclude::Option6PDExclude(const isc::asiolink::IOAddress& delegated_prefix,
                                   const uint8_t delegated_prefix_length,
                                   const isc::asiolink::IOAddress& excluded_prefix,
                                   const uint8_t excluded_prefix_length)
    : Option(V6, D6O_PD_EXCLUDE),
      excluded_prefix_length_(excluded_prefix_length),
      subnet_id_() {

    // Expecting v6 prefixes of sane length.
    if (!delegated_prefix.isV6() || !excluded_prefix.isV6() ||
        (delegated_prefix_length > 128) || (excluded_prefix_length_ > 128)) {
        isc_throw(BadValue, "invalid delegated or excluded prefix values specified: "
                  << delegated_prefix << "/"
                  << static_cast<int>(delegated_prefix_length) << ", "
                  << excluded_prefix << "/"
                  << static_cast<int>(excluded_prefix_length_));
    }

    // Excluded prefix must be longer than the delegated prefix.
    if (excluded_prefix_length_ <= delegated_prefix_length) {
        isc_throw(BadValue, "length of the excluded prefix "
                  << excluded_prefix << "/"
                  << static_cast<int>(excluded_prefix_length_)
                  << " must be greater than the length of the"
                  " delegated prefix " << delegated_prefix << "/"
                  << static_cast<int>(delegated_prefix_length));
    }

    // Both prefixes must share common part with a length equal to the
    // delegated prefix length.
    std::vector<uint8_t> delegated_prefix_bytes = delegated_prefix.toBytes();
    boost::dynamic_bitset<uint8_t> delegated_prefix_bits(delegated_prefix_bytes.rbegin(),
                                                         delegated_prefix_bytes.rend());

    std::vector<uint8_t> excluded_prefix_bytes = excluded_prefix.toBytes();
    boost::dynamic_bitset<uint8_t> excluded_prefix_bits(excluded_prefix_bytes.rbegin(),
                                                        excluded_prefix_bytes.rend());

    // See RFC6603, section 4.2: assert(p1>>s == p2>>s)
    const uint8_t delta = 128 - delegated_prefix_length;

    if ((delegated_prefix_bits >> delta) != (excluded_prefix_bits >> delta)) {
        isc_throw(BadValue, "excluded prefix "
                  << excluded_prefix << "/"
                  << static_cast<int>(excluded_prefix_length_)
                  << " must have the same common prefix part of "
                  << static_cast<int>(delegated_prefix_length)
                  << " as the delegated prefix "
                  << delegated_prefix << "/"
                  << static_cast<int>(delegated_prefix_length));
    }

    // Shifting prefix by delegated prefix length leaves us with only a
    // subnet id part of the excluded prefix.
    excluded_prefix_bits <<= delegated_prefix_length;

    // Calculate subnet id length.
    const uint8_t subnet_id_length = getSubnetIDLength(delegated_prefix_length,
                                                       excluded_prefix_length);
    for (uint8_t i = 0; i < subnet_id_length; ++i) {
        // Retrieve bit representation of the current byte.
        const boost::dynamic_bitset<uint8_t> first_byte = excluded_prefix_bits >> 120;

        // Convert it to a numeric value.
        uint8_t val = static_cast<uint8_t>(first_byte.to_ulong());

        // Zero padded excluded_prefix_bits follow when excluded_prefix_length_ is
        // not divisible by 8.
        if (i == subnet_id_length - 1) {
            uint8_t length_delta = excluded_prefix_length_ - delegated_prefix_length;
            if (length_delta % 8 != 0) {
                uint8_t mask = 0xFF;
                mask <<= (8 - (length_delta % 8));
                val &= mask;
            }
        }
        // Store calculated value in a buffer.
        subnet_id_.push_back(val);

        // Go to the next byte.
        excluded_prefix_bits <<= 8;
    }
}

Option6PDExclude::Option6PDExclude(OptionBufferConstIter begin,
                                   OptionBufferConstIter end)
    : Option(V6, D6O_PD_EXCLUDE),
      excluded_prefix_length_(0),
      subnet_id_() {
    unpack(begin, end);
}

OptionPtr
Option6PDExclude::clone() const {
    return (cloneInternal<Option6PDExclude>());
}

void
Option6PDExclude::pack(isc::util::OutputBuffer& buf) const {
    // Make sure that the subnet identifier is valid. It should never
    // be empty.
    if ((excluded_prefix_length_ == 0) || subnet_id_.empty()) {
        isc_throw(BadValue, "subnet identifier of a Prefix Exclude option"
                  " must not be empty");
    }

    // Header = option code and length.
    packHeader(buf);

    // Excluded prefix length is always 1 byte long field.
    buf.writeUint8(excluded_prefix_length_);

    // Write the subnet identifier.
    buf.writeData(static_cast<const void*>(&subnet_id_[0]), subnet_id_.size());
}

void
Option6PDExclude::unpack(OptionBufferConstIter begin,
                         OptionBufferConstIter end) {

    // At this point we don't know the excluded prefix length, but the
    // minimum requirement is that reminder of this option includes the
    // excluded prefix length and at least 1 byte of the IPv6 subnet id.
    if (std::distance(begin, end) < 2) {
        isc_throw(BadValue, "truncated Prefix Exclude option");
    }

    // We can safely read the excluded prefix length and move forward.
    uint8_t excluded_prefix_length = *begin++;
    if (excluded_prefix_length == 0) {
        isc_throw(BadValue, "excluded prefix length must not be 0");
    }

    std::vector<uint8_t> subnet_id_bytes(begin, end);

    // Subnet id parsed, proceed to the end of the option.
    begin = end;

    uint8_t last_bits_num = excluded_prefix_length % 8;
    if (last_bits_num > 0) {
        *subnet_id_bytes.rbegin() = (*subnet_id_bytes.rbegin() >> (8 - last_bits_num)
                                     << (8 - (last_bits_num)));
    }

    excluded_prefix_length_ = excluded_prefix_length;
    subnet_id_.swap(subnet_id_bytes);
}

uint16_t
Option6PDExclude::len() const {
    return (getHeaderLen() + sizeof(excluded_prefix_length_) + subnet_id_.size());
}

std::string
Option6PDExclude::toText(int indent) const {
    std::ostringstream s;
    s << headerToText(indent) << ": ";
    s << "excluded-prefix-len=" << static_cast<unsigned>(excluded_prefix_length_)
      << ", subnet-id=0x" << util::encode::encodeHex(subnet_id_);
    return (s.str());
}

asiolink::IOAddress
Option6PDExclude::getExcludedPrefix(const IOAddress& delegated_prefix,
                                    const uint8_t delegated_prefix_length) const {
    // Get binary representation of the delegated prefix.
    std::vector<uint8_t> delegated_prefix_bytes = delegated_prefix.toBytes();
    //  We need to calculate how many bytes include the useful data and assign
    // zeros to remaining bytes (beyond the prefix length).
    const uint8_t bytes_length = (delegated_prefix_length / 8) +
        static_cast<uint8_t>(delegated_prefix_length % 8 != 0);
    std::fill(delegated_prefix_bytes.begin() + bytes_length,
              delegated_prefix_bytes.end(), 0);

    // Convert the delegated prefix to bit format.
    boost::dynamic_bitset<uint8_t> bits(delegated_prefix_bytes.rbegin(),
                                        delegated_prefix_bytes.rend());

    boost::dynamic_bitset<uint8_t> subnet_id_bits(subnet_id_.rbegin(),
                                                  subnet_id_.rend());

    // Concatenate the delegated prefix with subnet id. The resulting prefix
    // is an excluded prefix in bit format.
    for (int i = subnet_id_bits.size() - 1; i >= 0; --i) {
        bits.set(128 - delegated_prefix_length - subnet_id_bits.size() + i,
                 subnet_id_bits.test(i));
    }

    // Convert the prefix to binary format.
    std::vector<uint8_t> bytes(V6ADDRESS_LEN);
    boost::to_block_range(bits, bytes.rbegin());

    // And create a prefix object from bytes.
    return (IOAddress::fromBytes(AF_INET6, &bytes[0]));
}

uint8_t
Option6PDExclude::getSubnetIDLength(const uint8_t delegated_prefix_length,
                                    const uint8_t excluded_prefix_length) const {
    uint8_t subnet_id_length_bits = excluded_prefix_length -
        delegated_prefix_length - 1;
    uint8_t subnet_id_length = (subnet_id_length_bits / 8) + 1;
    return (subnet_id_length);
}

} // end of namespace isc::dhcp
} // end of namespace isc


//  Copyright John Maddock 2006.
//  Use, modification and distribution are subject to 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_STATS_WEIBULL_HPP
#define BOOST_STATS_WEIBULL_HPP

// http://www.itl.nist.gov/div898/handbook/eda/section3/eda3668.htm
// http://mathworld.wolfram.com/WeibullDistribution.html

#include <boost/math/distributions/fwd.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/log1p.hpp>
#include <boost/math/special_functions/expm1.hpp>
#include <boost/math/distributions/detail/common_error_handling.hpp>
#include <boost/math/distributions/complement.hpp>

#include <utility>

namespace boost{ namespace math
{
namespace detail{

template <class RealType, class Policy>
inline bool check_weibull_shape(
      const char* function,
      RealType shape,
      RealType* result, const Policy& pol)
{
   if((shape < 0) || !(boost::math::isfinite)(shape))
   {
      *result = policies::raise_domain_error<RealType>(
         function,
         "Shape parameter is %1%, but must be > 0 !", shape, pol);
      return false;
   }
   return true;
}

template <class RealType, class Policy>
inline bool check_weibull_x(
      const char* function,
      RealType const& x,
      RealType* result, const Policy& pol)
{
   if((x < 0) || !(boost::math::isfinite)(x))
   {
      *result = policies::raise_domain_error<RealType>(
         function,
         "Random variate is %1% but must be >= 0 !", x, pol);
      return false;
   }
   return true;
}

template <class RealType, class Policy>
inline bool check_weibull(
      const char* function,
      RealType scale,
      RealType shape,
      RealType* result, const Policy& pol)
{
   return check_scale(function, scale, result, pol) && check_weibull_shape(function, shape, result, pol);
}

} // namespace detail

template <class RealType = double, class Policy = policies::policy<> >
class weibull_distribution
{
public:
   typedef RealType value_type;
   typedef Policy policy_type;

   weibull_distribution(RealType shape, RealType scale = 1)
      : m_shape(shape), m_scale(scale)
   {
      RealType result;
      detail::check_weibull("boost::math::weibull_distribution<%1%>::weibull_distribution", scale, shape, &result, Policy());
   }

   RealType shape()const
   {
      return m_shape;
   }

   RealType scale()const
   {
      return m_scale;
   }
private:
   //
   // Data members:
   //
   RealType m_shape;     // distribution shape
   RealType m_scale;     // distribution scale
};

typedef weibull_distribution<double> weibull;

template <class RealType, class Policy>
inline const std::pair<RealType, RealType> range(const weibull_distribution<RealType, Policy>& /*dist*/)
{ // Range of permissible values for random variable x.
   using boost::math::tools::max_value;
   return std::pair<RealType, RealType>(0, max_value<RealType>());
}

template <class RealType, class Policy>
inline const std::pair<RealType, RealType> support(const weibull_distribution<RealType, Policy>& /*dist*/)
{ // Range of supported values for random variable x.
   // This is range where cdf rises from 0 to 1, and outside it, the pdf is zero.
   using boost::math::tools::max_value;
   using boost::math::tools::min_value;
   return std::pair<RealType, RealType>(min_value<RealType>(),  max_value<RealType>());
   // A discontinuity at x == 0, so only support down to min_value.
}

template <class RealType, class Policy>
inline RealType pdf(const weibull_distribution<RealType, Policy>& dist, const RealType& x)
{
   BOOST_MATH_STD_USING  // for ADL of std functions

   static const char* function = "boost::math::pdf(const weibull_distribution<%1%>, %1%)";

   RealType shape = dist.shape();
   RealType scale = dist.scale();

   RealType result;
   if(false == detail::check_weibull(function, scale, shape, &result, Policy()))
      return result;
   if(false == detail::check_weibull_x(function, x, &result, Policy()))
      return result;

   if(x == 0)
   { // Special case, but x == min, pdf = 1 for shape = 1,
      return 0;
   }
   result = exp(-pow(x / scale, shape));
   result *= pow(x / scale, shape) * shape / x;

   return result;
}

template <class RealType, class Policy>
inline RealType cdf(const weibull_distribution<RealType, Policy>& dist, const RealType& x)
{
   BOOST_MATH_STD_USING  // for ADL of std functions

   static const char* function = "boost::math::cdf(const weibull_distribution<%1%>, %1%)";

   RealType shape = dist.shape();
   RealType scale = dist.scale();

   RealType result;
   if(false == detail::check_weibull(function, scale, shape, &result, Policy()))
      return result;
   if(false == detail::check_weibull_x(function, x, &result, Policy()))
      return result;

   result = -boost::math::expm1(-pow(x / scale, shape), Policy());

   return result;
}

template <class RealType, class Policy>
inline RealType quantile(const weibull_distribution<RealType, Policy>& dist, const RealType& p)
{
   BOOST_MATH_STD_USING  // for ADL of std functions

   static const char* function = "boost::math::quantile(const weibull_distribution<%1%>, %1%)";

   RealType shape = dist.shape();
   RealType scale = dist.scale();

   RealType result;
   if(false == detail::check_weibull(function, scale, shape, &result, Policy()))
      return result;
   if(false == detail::check_probability(function, p, &result, Policy()))
      return result;

   if(p == 1)
      return policies::raise_overflow_error<RealType>(function, 0, Policy());

   result = scale * pow(-boost::math::log1p(-p, Policy()), 1 / shape);

   return result;
}

template <class RealType, class Policy>
inline RealType cdf(const complemented2_type<weibull_distribution<RealType, Policy>, RealType>& c)
{
   BOOST_MATH_STD_USING  // for ADL of std functions

   static const char* function = "boost::math::cdf(const weibull_distribution<%1%>, %1%)";

   RealType shape = c.dist.shape();
   RealType scale = c.dist.scale();

   RealType result;
   if(false == detail::check_weibull(function, scale, shape, &result, Policy()))
      return result;
   if(false == detail::check_weibull_x(function, c.param, &result, Policy()))
      return result;

   result = exp(-pow(c.param / scale, shape));

   return result;
}

template <class RealType, class Policy>
inline RealType quantile(const complemented2_type<weibull_distribution<RealType, Policy>, RealType>& c)
{
   BOOST_MATH_STD_USING  // for ADL of std functions

   static const char* function = "boost::math::quantile(const weibull_distribution<%1%>, %1%)";

   RealType shape = c.dist.shape();
   RealType scale = c.dist.scale();
   RealType q = c.param;

   RealType result;
   if(false == detail::check_weibull(function, scale, shape, &result, Policy()))
      return result;
   if(false == detail::check_probability(function, q, &result, Policy()))
      return result;

   if(q == 0)
      return policies::raise_overflow_error<RealType>(function, 0, Policy());

   result = scale * pow(-log(q), 1 / shape);

   return result;
}

template <class RealType, class Policy>
inline RealType mean(const weibull_distribution<RealType, Policy>& dist)
{
   BOOST_MATH_STD_USING  // for ADL of std functions

   static const char* function = "boost::math::mean(const weibull_distribution<%1%>)";

   RealType shape = dist.shape();
   RealType scale = dist.scale();

   RealType result;
   if(false == detail::check_weibull(function, scale, shape, &result, Policy()))
      return result;

   result = scale * boost::math::tgamma(1 + 1 / shape, Policy());
   return result;
}

template <class RealType, class Policy>
inline RealType variance(const weibull_distribution<RealType, Policy>& dist)
{
   RealType shape = dist.shape();
   RealType scale = dist.scale();

   static const char* function = "boost::math::variance(const weibull_distribution<%1%>)";

   RealType result;
   if(false == detail::check_weibull(function, scale, shape, &result, Policy()))
   {
      return result;
   }
   result = boost::math::tgamma(1 + 1 / shape, Policy());
   result *= -result;
   result += boost::math::tgamma(1 + 2 / shape, Policy());
   result *= scale * scale;
   return result;
}

template <class RealType, class Policy>
inline RealType mode(const weibull_distribution<RealType, Policy>& dist)
{
   BOOST_MATH_STD_USING  // for ADL of std function pow.

   static const char* function = "boost::math::mode(const weibull_distribution<%1%>)";

   RealType shape = dist.shape();
   RealType scale = dist.scale();

   RealType result;
   if(false == detail::check_weibull(function, scale, shape, &result, Policy()))
   {
      return result;
   }
   if(shape <= 1)
      return 0;
   result = scale * pow((shape - 1) / shape, 1 / shape);
   return result;
}

template <class RealType, class Policy>
inline RealType median(const weibull_distribution<RealType, Policy>& dist)
{
   BOOST_MATH_STD_USING  // for ADL of std function pow.

   static const char* function = "boost::math::median(const weibull_distribution<%1%>)";

   RealType shape = dist.shape(); // Wikipedia k
   RealType scale = dist.scale(); // Wikipedia lambda

   RealType result;
   if(false == detail::check_weibull(function, scale, shape, &result, Policy()))
   {
      return result;
   }
   using boost::math::constants::ln_two;
   result = scale * pow(ln_two<RealType>(), 1 / shape);
   return result;
}

template <class RealType, class Policy>
inline RealType skewness(const weibull_distribution<RealType, Policy>& dist)
{
   BOOST_MATH_STD_USING  // for ADL of std functions

   static const char* function = "boost::math::skewness(const weibull_distribution<%1%>)";

   RealType shape = dist.shape();
   RealType scale = dist.scale();

   RealType result;
   if(false == detail::check_weibull(function, scale, shape, &result, Policy()))
   {
      return result;
   }
   RealType g1, g2, g3, d;

   g1 = boost::math::tgamma(1 + 1 / shape, Policy());
   g2 = boost::math::tgamma(1 + 2 / shape, Policy());
   g3 = boost::math::tgamma(1 + 3 / shape, Policy());
   d = pow(g2 - g1 * g1, RealType(1.5));

   result = (2 * g1 * g1 * g1 - 3 * g1 * g2 + g3) / d;
   return result;
}

template <class RealType, class Policy>
inline RealType kurtosis_excess(const weibull_distribution<RealType, Policy>& dist)
{
   BOOST_MATH_STD_USING  // for ADL of std functions

   static const char* function = "boost::math::kurtosis_excess(const weibull_distribution<%1%>)";

   RealType shape = dist.shape();
   RealType scale = dist.scale();

   RealType result;
   if(false == detail::check_weibull(function, scale, shape, &result, Policy()))
      return result;

   RealType g1, g2, g3, g4, d, g1_2, g1_4;

   g1 = boost::math::tgamma(1 + 1 / shape, Policy());
   g2 = boost::math::tgamma(1 + 2 / shape, Policy());
   g3 = boost::math::tgamma(1 + 3 / shape, Policy());
   g4 = boost::math::tgamma(1 + 4 / shape, Policy());
   g1_2 = g1 * g1;
   g1_4 = g1_2 * g1_2;
   d = g2 - g1_2;
   d *= d;

   result = -6 * g1_4 + 12 * g1_2 * g2 - 3 * g2 * g2 - 4 * g1 * g3 + g4;
   result /= d;
   return result;
}

template <class RealType, class Policy>
inline RealType kurtosis(const weibull_distribution<RealType, Policy>& dist)
{
   return kurtosis_excess(dist) + 3;
}

} // namespace math
} // namespace boost

// This include must be at the end, *after* the accessors
// for this distribution have been defined, in order to
// keep compilers that support two-phase lookup happy.
#include <boost/math/distributions/detail/derived_accessors.hpp>

#endif // BOOST_STATS_WEIBULL_HPP


/* This file is part of the Tomographer project, which is distributed under the
 * terms of the MIT license.
 *
 * The MIT License (MIT)
 *
 * Copyright (c) 2016 ETH Zurich, Institute for Theoretical Physics, Philippe Faist
 * Copyright (c) 2017 Caltech, Institute for Quantum Information and Matter, Philippe Faist
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#include <cmath>

#include <string>
#include <iostream>
#include <random>

#include <boost/math/constants/constants.hpp>

// definitions for Tomographer test framework -- this must be included before any
// <Eigen/...> or <tomographer/...> header
#include "test_tomographer.h"

#include <tomographer/tools/loggers.h>
#include <tomographer/mhrw.h>
#include <tomographer/mhrw_bin_err.h>
#include <tomographer/mhrwstatscollectors.h>
#include <tomographer/densedm/densellh.h>
#include <tomographer/densedm/indepmeasllh.h>
#include <tomographer/densedm/tspacefigofmerit.h>
#include <tomographer/densedm/tspacellhwalker.h>

#include <tomographer/tools/boost_test_logger.h>

// -----------------------------------------------------------------------------
// fixture(s)

struct test_norm_value_calculator
{
  typedef double ValueType;

  template<typename Derived>
  inline ValueType getValue(const Eigen::MatrixBase<Derived> & pt)
  {
    return pt.norm();
  }
};

template<typename RealScalar_, int Dim = 4>
struct test_hypercube_mhwalker
{
  typedef RealScalar_ RealScalar;
  typedef RealScalar_ StepRealType;
  typedef Eigen::Matrix<RealScalar, Dim, 1> PointType;

  typedef float FnValueType;

  enum { UseFnSyntaxType = Tomographer::MHUseFnValue };

  // random number generator, etc.
  std::mt19937 rng;
  std::uniform_real_distribution<double> dist;

  // constructor
  test_hypercube_mhwalker() : rng(0), dist(-1.0, 1.0) { }

  inline void init() { }

  auto startPoint() -> decltype(PointType::Zero()) { return PointType::Zero(); }

  inline void thermalizingDone() { }
  inline void done() { }
  inline PointType jumpFn(const PointType & curpt, RealScalar step_size)
  {
    PointType newpt;
    newpt = curpt + step_size*Tomographer::Tools::denseRandom<PointType>(rng, dist);
    // do the random walk on the torus, i.e. modulo 1.0
    for (int i = 0; i < Dim; ++i) {
      if (newpt(i) < 0) { newpt(i) += 1.0; }
      if (newpt(i) > 1) { newpt(i) -= 1.0; }
    }
    return newpt;
  }

  inline FnValueType fnVal(const PointType & )
  {
    return 1.0;
  }
};

// -----------------------------------------------------------------------------
// test suites

BOOST_AUTO_TEST_SUITE(test_zzzcombinations)

// =============================================================================

BOOST_AUTO_TEST_SUITE(valuehistogramwithbinning)

BOOST_AUTO_TEST_CASE(simple1)
{
  //<<<<<<<<<<<
  typedef Tomographer::Logger::BoostTestLogger LoggerType;
  LoggerType buflog(Tomographer::Logger::DEBUG);
  //-----------
  //  typedef Tomographer::Logger::FileLogger LoggerType;
  //  LoggerType buflog(stderr, Tomographer::Logger::LONGDEBUG) ;
  //>>>>>>>>>>>

  typedef Tomographer::ValueHistogramWithBinningMHRWStatsCollector<
    Tomographer::ValueHistogramWithBinningMHRWStatsCollectorParams<test_norm_value_calculator,
                                                                   int,
                                                                   float>,
    LoggerType
    > ValWBinningMHRWStatsCollectorType;

  typedef ValWBinningMHRWStatsCollectorType::HistogramParams HistogramParams;
  typedef test_hypercube_mhwalker<
    double,
    3 // Dim
    > MHWalkerType;
  typedef Tomographer::MHRandomWalk<std::mt19937, MHWalkerType, ValWBinningMHRWStatsCollectorType,
				    LoggerType, int> MHRandomWalkType;

  test_norm_value_calculator vcalc;

  // N levels -> samples_size = 2^N,  2^10 == 1024
  const int num_levels = 10;
  //const int num_levels = 4;
  ValWBinningMHRWStatsCollectorType vhist(HistogramParams(0.f, 2.0f, 20), vcalc, num_levels, buflog);

  std::mt19937 rng(0); // seeded rng, deterministic results

  MHWalkerType mhwalker;
  MHRandomWalkType rwalk(0.03, 5, 50, 500000, mhwalker, vhist, rng, buflog);

  rwalk.run();

  const ValWBinningMHRWStatsCollectorType::ResultType & result = vhist.getResult();

  // Todo: Fixme: Why are the error bar curves going like crazy?
  // ----> ok if we have enough samples it seems.

  // TODO: CHECK THAT THE VALUES ARE CORRECT, ALONG WITH THE ERRORS.

  MY_BOOST_CHECK_EIGEN_EQUAL(result.converged_status,
                             Eigen::ArrayXi::Constant(
                                 vhist.getBinningAnalysis().numTrackValues(),
                                 ValWBinningMHRWStatsCollectorType::BinningAnalysisType::CONVERGED
                                 ),
                             tol) ;

}

BOOST_AUTO_TEST_SUITE_END()

// =============================================================================

BOOST_AUTO_TEST_SUITE(integrator_tests)

BOOST_AUTO_TEST_CASE(basic1)
{
  typedef Tomographer::DenseDM::DMTypes<2> DMTypes;
  DMTypes dmt;

  typedef Tomographer::DenseDM::IndepMeasLLH<DMTypes> DenseLLH;
  DenseLLH llh(dmt);

  DenseLLH::VectorParamListType Exn(6, dmt.dim2());
  Exn <<
    0.5, 0.5,  0.707107,  0,
    0.5, 0.5, -0.707107,  0,
    0.5, 0.5,  0,         0.707107,
    0.5, 0.5,  0,        -0.707107,
    1,   0,    0,         0,
    0,   1,    0,         0
    ;
  DenseLLH::FreqListType Nx(6);
  Nx << 0, 0, 0, 0, 250, 0;

  llh.setMeas(Exn, Nx, false);

  // now, prepare the integrator.
  std::mt19937 rng(0); // seeded random number generator

  typedef Tomographer::Logger::BoostTestLogger LoggerType;
  LoggerType logger(Tomographer::Logger::DEBUG);

  DMTypes::MatrixType start_T(dmt.initMatrixType());
  start_T << 1.0/sqrt(2.0), 0,
              0, 1.0/sqrt(2.0);

  typedef Tomographer::DenseDM::TSpace::FidelityToRefCalculator<DMTypes> OurValueCalculator;
  typedef Tomographer::UniformBinsHistogram<typename OurValueCalculator::ValueType> OurHistogramType;
  typedef Tomographer::ValueHistogramMHRWStatsCollector<
    OurValueCalculator,
    LoggerType,
    OurHistogramType
    >
    OurValMHRWStatsCollector;
  typedef Tomographer::MultipleMHRWStatsCollectors<
    OurValMHRWStatsCollector,
    OurValMHRWStatsCollector
    >
    OurMultiMHRWStatsCollector;

  DMTypes::MatrixType ref_T(dmt.initMatrixType());
  ref_T <<
    1, 0,
    0, 0;

  OurValueCalculator fidcalc(ref_T);
  OurValMHRWStatsCollector fidstats(OurHistogramType::Params(0.98, 1.0, 50), fidcalc, logger);
  OurValMHRWStatsCollector fidstats2(OurHistogramType::Params(0.96, 0.98, 10), fidcalc, logger);

  OurMultiMHRWStatsCollector multistats(fidstats, fidstats2);

  typedef Tomographer::DenseDM::TSpace::LLHMHWalker<DenseLLH,std::mt19937,LoggerType>
    MyMHWalker;

  MyMHWalker mhwalker(start_T, llh, rng, logger);

  //  std::cout << "About to create the randomwalk object ...\n";
  Tomographer::MHRandomWalk<std::mt19937,MyMHWalker,OurMultiMHRWStatsCollector,LoggerType,long>
    rwalk(0.05, 20, 300, 5000, mhwalker, multistats, rng, logger);

  //  std::cout << "About to run the randomwalk object ...\n";

  rwalk.run();

  // because we used a seeded RNG, we should get exactly reproducible results, i.e. the
  // exact same histograms.

  const auto & hist1 = fidstats.histogram();
  BOOST_MESSAGE("FINAL HISTOGRAM(1):\n" << hist1.prettyPrint(100));

  boost::test_tools::output_test_stream output1(
      TOMOGRAPHER_TEST_PATTERNS_DIR "test_densedm_tspacellhwalker/hist1.txt",
      true // true = match mode, false = write mode
      );
  dump_histogram_test(output1, hist1);
  BOOST_CHECK(output1.match_pattern());

  const auto & hist2 = fidstats2.histogram();
  BOOST_MESSAGE("FINAL HISTOGRAM(2):\n" << hist2.prettyPrint(100));

  boost::test_tools::output_test_stream output2(
      TOMOGRAPHER_TEST_PATTERNS_DIR "test_densedm_tspacellhwalker/hist2.txt",
      true // true = match mode, false = write mode
      );
  dump_histogram_test(output2, hist2);
  BOOST_CHECK(output2.match_pattern());
}

BOOST_AUTO_TEST_CASE(with_binning_analysis)
{
  typedef Tomographer::DenseDM::DMTypes<2> DMTypes;
  DMTypes dmt;
  typedef Tomographer::DenseDM::IndepMeasLLH<DMTypes> DenseLLH;
  DenseLLH llh(dmt);

  DenseLLH::VectorParamListType Exn(2, dmt.dim2());
  Exn <<
    1,   0,    0,         0,
    0,   1,    0,         0
    ;
  DenseLLH::FreqListType Nx(2);
  Nx << 250, 0;

  llh.setMeas(Exn, Nx);

  // --------

  typedef Tomographer::Logger::BoostTestLogger LoggerType;
  LoggerType logger(Tomographer::Logger::DEBUG);

  typedef Tomographer::DenseDM::TSpace::FidelityToRefCalculator<DMTypes> OurValueCalculator;
  typedef Tomographer::ValueHistogramWithBinningMHRWStatsCollector<
    Tomographer::ValueHistogramWithBinningMHRWStatsCollectorParams<OurValueCalculator,
                                                                   int,
                                                                   float>,
    LoggerType
    > ValWBinningMHRWStatsCollectorType;

  typedef ValWBinningMHRWStatsCollectorType::HistogramParams HistogramParams;
  typedef Tomographer::DenseDM::TSpace::LLHMHWalker<DenseLLH,std::mt19937,LoggerType> MHWalkerType;

  DMTypes::MatrixType ref_T(dmt.initMatrixType());
  ref_T << 1, 0, 0, 0;
  OurValueCalculator fidcalc(ref_T);

  // N levels -> samples_size = 2^N
  const int num_levels = 5;

  ValWBinningMHRWStatsCollectorType vhist(HistogramParams(0.98f, 1.0f, 20), fidcalc, num_levels, logger);

  std::mt19937 rng(0); // seeded rng, deterministic results

  DMTypes::MatrixType start_T = dmt.initMatrixType();
  start_T << 1.0/sqrt(2.0), 0, 0, 1.0/sqrt(2.0);

  MHWalkerType mhwalker(start_T, llh, rng, logger);

  //  std::cout << "About to create the randomwalk object ...\n";
  Tomographer::MHRandomWalk<std::mt19937,MHWalkerType,ValWBinningMHRWStatsCollectorType,LoggerType,unsigned long>
    rwalk(0.05, 20, 300, 8192, mhwalker, vhist, rng, logger);

  rwalk.run();

  const ValWBinningMHRWStatsCollectorType::ResultType & result = vhist.getResult();

  // all error bars should have converged with these settings
  MY_BOOST_CHECK_EIGEN_EQUAL(
      result.converged_status,
      Eigen::ArrayXi::Constant(result.hist.numBins(),
			       ValWBinningMHRWStatsCollectorType::BinningAnalysisParamsType::CONVERGED),
      tol_f
      );

  std::string conv_analysis = result.dumpConvergenceAnalysis();
  BOOST_MESSAGE("Convergence Analysis:\n" << conv_analysis);

  boost::test_tools::output_test_stream output_conv_analysis(
      TOMOGRAPHER_TEST_PATTERNS_DIR "test_densedm_tspacellhwalker/binning_convergence_analysis.txt",
      true // true = compare mode, false = write mode
      );
  output_conv_analysis << conv_analysis;
  BOOST_CHECK(output_conv_analysis.match_pattern());

  boost::test_tools::output_test_stream output_error_bars(
      TOMOGRAPHER_TEST_PATTERNS_DIR "test_densedm_tspacellhwalker/binning_error_bars.txt",
      true // true = compare mode, false = write mode
      );
  output_error_bars
    << "--------------------------------------------------------------------------------\n";
  dump_histogram_test(output_error_bars, result.hist, 2);
  output_error_bars
    << "--------------------------------------------------------------------------------\n"
    << "ERROR BARS @ BINNING LEVELS = \n"
    << std::setprecision(2) << std::scientific << std::left << std::setfill('0')
    << result.error_levels
    << "\n";
  BOOST_CHECK(output_error_bars.match_pattern());
}

BOOST_AUTO_TEST_SUITE_END()

// =============================================================================

BOOST_AUTO_TEST_SUITE_END()


#ifndef FRENET_STATE_FILTER_H
#define FRENET_STATE_FILTER_H

#include <Eigen/Dense>
#include <cmath>
#include <deque>
#include <iostream>
#include <numeric>
#include "mpc_tracker/FrenetCoordinate.hpp"
#include "mpc_tracker/Pose.hpp"
#include "mpc_tracker/Twist.hpp"

namespace pathtrack_tools {
///
/// @class FrenetStateFilter
/// @brief Estimation of state quantities of Frenet coordinate system that is impossible to observe.
///

class FrenetStateFilter {
 public:
  /**
   * @brief Default constructor
   *
   */
  FrenetStateFilter(const double& sampling_time);

  /**
   * @brief Destroy the FrenetStateFilter object
   *
   */
  ~FrenetStateFilter();
  /**
   * @brief Set the initial pose object
   *
   * @param pose_f
   */
  void set_initial_pose(const FrenetCoordinate& pose_f, const Twist& twist);

  /**
   * @brief Estimated from y_f of the past few points
   *
   * @param y_f
   * @return double dy_f
   */
  double estimate_dy_f(const double& current_y_f);

  /**
   * @brief Calculate the projection in the reference path direction
   *
   * @param current_pose_f
   * @param current_twist
   * @return double dy_f
   */
  double estimate_dy_f(const FrenetCoordinate& current_pose_f, const Twist& current_twist) const;

 private:
  const double sampling_time_;
  double prev_y_f_ = 0.0;
  const size_t dy_f_queue_size_ = 10;
  std::deque<double> dy_f_queue_;
};

}  // namespace pathtrack_tools

#endif


#ifndef STAN_MATH_PRIM_PROB_POISSON_BINOMIAL_RNG_HPP
#define STAN_MATH_PRIM_PROB_POISSON_BINOMIAL_RNG_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/err.hpp>
#include <stan/math/prim/fun/size.hpp>
#include <boost/random/bernoulli_distribution.hpp>
#include <boost/random/variate_generator.hpp>

namespace stan {
namespace math {

/** \ingroup prob_dists
 * Return a pseudorandom Poisson binomial random variable for the given vector
 * of success parameters using the specified random number
 * generator.
 *
 * @tparam RNG class of rng
 * @param theta (Sequence of) chance of success parameter(s)
 * @param rng random number generator
 * @return a Poisson binomial distribution random variable
 * @throw std::domain_error if theta is not a valid probability
 */
template <typename T_theta, typename RNG,
          require_eigen_vt<std::is_arithmetic, T_theta>* = nullptr>
inline int poisson_binomial_rng(const T_theta& theta, RNG& rng) {
  static const char* function = "poisson_binomial_rng";
  check_finite(function, "Probability parameters", theta);
  check_bounded(function, "Probability parameters", value_of(theta), 0.0, 1.0);

  int y = 0;
  for (size_t i = 0; i < theta.size(); ++i) {
    boost::variate_generator<RNG&, boost::bernoulli_distribution<> >
        bernoulli_rng(rng, boost::bernoulli_distribution<>(theta(i)));
    y += bernoulli_rng();
  }

  return y;
}

}  // namespace math
}  // namespace stan
#endif


#include "benchmark.h"
#include "classification/alexnet.h"
#include "classification/darknet.h"
#include "classification/densenet.h"
#include "classification/googlenet.h"
#include "classification/resnet.h"
#include "classification/squeezenet.h"
#include "classification/vggnet.h"
#include "classification/vovnet.h"
#include "classification/repvgg.h"

#include <dlib/cmd_line_parser.h>

#define DNN_BENCH_ALEXNET 1
#define DNN_BENCH_VGGNET 1
#define DNN_BENCH_GOOGLENET 1
#define DNN_BENCH_RESNET 1
#define DNN_BENCH_DARKNET 1
#define DNN_BENCH_DENSENET 1
#define DNN_BENCH_VOVNET 1
#define DNN_BENCH_SQUEEZENET 1
#define DNN_BENCH_REPVGG 1

int main(const int argc, const char** argv)
try
{

    dlib::command_line_parser parser;
    parser.add_option("batch-size", "set the batch size (default: 1)", 1);
    parser.add_option("image-size", "set the image size (default: 224)", 1);
    parser.add_option("num-outputs", "set the number of fc outputs (default: 1000)", 1);
    parser.add_option("num-iters", "set the number of iterations (default: 100)", 1);
    parser.add_option("cuda-blocking", "disable cuda synchronization");
    parser.set_group_name("Help Options");
    parser.add_option("h", "alias for --help");
    parser.add_option("help", "display this message and exit");
    parser.parse(argc, argv);

    if (parser.option("h") or parser.option("help"))
    {
        parser.print_options();
        return EXIT_SUCCESS;
    }

    const std::string cuda_blocking = parser.option("cuda-blocking") ? "1" : "0";
    const size_t batch_size = dlib::get_option(parser, "batch-size", 1);
    const size_t image_size = dlib::get_option(parser, "image-size", 224);
    const size_t num_outputs = dlib::get_option(parser, "num-outputs", 1000);
    const int num_iters = dlib::get_option(parser, "num-iters", 100);
    setenv("CUDA_LAUNCH_BLOCKING", cuda_blocking.c_str(), 1);
    std::cout << std::fixed << std::setprecision(3);

#if DNN_BENCH_ALEXNET
    {
        alexnet::train tnet;
        dlib::disable_duplicative_biases(tnet);
        alexnet::infer net(tnet);
        net.subnet().layer_details().set_num_outputs(num_outputs);
        benchmark("alexnet  ", net, batch_size, image_size, num_iters);
    }
#endif

#if DNN_BENCH_SQUEEZENET
    {
        squeezenet::train_v1_0 tnet;
        dlib::disable_duplicative_biases(tnet);
        squeezenet::infer_v1_0 net(tnet);
        net.subnet().subnet().subnet().layer_details().set_num_filters(num_outputs);
        benchmark("sqznet1.0", net, batch_size, image_size, num_iters);
    }
    {
        squeezenet::train_v1_1 tnet;
        dlib::disable_duplicative_biases(tnet);
        squeezenet::infer_v1_1 net(tnet);
        net.subnet().subnet().subnet().layer_details().set_num_filters(num_outputs);
        benchmark("sqznet1.1", net, batch_size, image_size, num_iters);
    }
#endif

#if DNN_BENCH_VGGNET
    {
        vggnet::train_11 tnet;
        dlib::disable_duplicative_biases(tnet);
        vggnet::infer_11 net(tnet);
        net.subnet().layer_details().set_num_outputs(num_outputs);
        benchmark("vggnet11 ", net, batch_size, image_size, num_iters);
    }
    {
        vggnet::train_13 tnet;
        dlib::disable_duplicative_biases(tnet);
        vggnet::infer_13 net(tnet);
        net.subnet().layer_details().set_num_outputs(num_outputs);
        benchmark("vggnet13 ", net, batch_size, image_size, num_iters);
    }
    {
        vggnet::train_16 tnet;
        dlib::disable_duplicative_biases(tnet);
        vggnet::infer_16 net(tnet);
        net.subnet().layer_details().set_num_outputs(num_outputs);
        benchmark("vggnet16 ", net, batch_size, image_size, num_iters);
    }
    {
        vggnet::train_19 tnet;
        dlib::disable_duplicative_biases(tnet);
        vggnet::infer_19 net(tnet);
        net.subnet().layer_details().set_num_outputs(num_outputs);
        benchmark("vggnet19 ", net, batch_size, image_size, num_iters);
    }
#endif

#if DNN_BENCH_GOOGLENET
    {
        googlenet::train tnet;
        dlib::disable_duplicative_biases(tnet);
        googlenet::infer net(tnet);
        net.subnet().layer_details().set_num_outputs(num_outputs);
        benchmark("googlenet", net, batch_size, image_size, num_iters);
    }
#endif

#if DNN_BENCH_RESNET
    {
        resnet::train_18 tnet;
        dlib::disable_duplicative_biases(tnet);
        resnet::infer_18 net(tnet);
        net.subnet().layer_details().set_num_outputs(num_outputs);
        benchmark("resnet18 ", net, batch_size, image_size, num_iters);
    }
    {
        resnet::train_34 tnet;
        dlib::disable_duplicative_biases(tnet);
        resnet::infer_34 net(tnet);
        net.subnet().layer_details().set_num_outputs(num_outputs);
        benchmark("resnet34 ", net, batch_size, image_size, num_iters);
    }
    {
        resnet::train_50 tnet;
        dlib::disable_duplicative_biases(tnet);
        resnet::infer_50 net(tnet);
        net.subnet().layer_details().set_num_outputs(num_outputs);
        benchmark("resnet50 ", net, batch_size, image_size, num_iters);
    }
    {
        resnet::train_101 tnet;
        dlib::disable_duplicative_biases(tnet);
        resnet::infer_101 net(tnet);
        net.subnet().layer_details().set_num_outputs(num_outputs);
        benchmark("resnet101", net, batch_size, image_size, num_iters);
    }
    {
        resnet::train_152 tnet;
        dlib::disable_duplicative_biases(tnet);
        resnet::infer_152 net(tnet);
        net.subnet().layer_details().set_num_outputs(num_outputs);
        benchmark("resnet152", net, batch_size, image_size, num_iters);
    }
#endif

#if DNN_BENCH_DARKNET
    {
        darknet::train_19 tnet;
        dlib::disable_duplicative_biases(tnet);
        darknet::infer_19 net(tnet);
        net.subnet().layer_details().set_num_outputs(num_outputs);
        benchmark("darknet19", net, batch_size, image_size, num_iters);
    }
    {
        darknet::train_53 tnet;
        dlib::disable_duplicative_biases(tnet);
        darknet::infer_53 net(tnet);
        net.subnet().layer_details().set_num_outputs(num_outputs);
        benchmark("darknet53", net, batch_size, image_size, num_iters);
    }
    {
        darknet::train_53csp tnet;
        dlib::disable_duplicative_biases(tnet);
        darknet::infer_53csp net(tnet);
        net.subnet().layer_details().set_num_outputs(num_outputs);
        benchmark("darknet53csp", net, batch_size, image_size, num_iters);
    }
#endif

#if DNN_BENCH_DENSENET
    {
        densenet::train_121 tnet;
        dlib::visit_layers(tnet, visitor_con_disable_bias());
        densenet::infer_121 net(tnet);
        net.subnet().layer_details().set_num_outputs(num_outputs);
        benchmark("densenet121", net, batch_size, image_size, num_iters);
    }
    {
        densenet::train_169 tnet;
        dlib::visit_layers(tnet, visitor_con_disable_bias());
        densenet::infer_169 net(tnet);
        net.subnet().layer_details().set_num_outputs(num_outputs);
        benchmark("densenet169", net, batch_size, image_size, num_iters);
    }
    {
        densenet::train_201 tnet;
        dlib::visit_layers(tnet, visitor_con_disable_bias());
        densenet::infer_201 net(tnet);
        net.subnet().layer_details().set_num_outputs(num_outputs);
        benchmark("densenet201", net, batch_size, image_size, num_iters);
    }
    {
        densenet::train_265 tnet;
        dlib::visit_layers(tnet, visitor_con_disable_bias());
        densenet::infer_265 net(tnet);
        net.subnet().layer_details().set_num_outputs(num_outputs);
        benchmark("densenet265", net, batch_size, image_size, num_iters);
    }
    {
        densenet::train_161 tnet;
        dlib::visit_layers(tnet, visitor_con_disable_bias());
        densenet::infer_161 net(tnet);
        net.subnet().layer_details().set_num_outputs(num_outputs);
        benchmark("densenet161", net, batch_size, image_size, num_iters);
    }
#endif

#if DNN_BENCH_VOVNET
    {
        vovnet::train_19_slim tnet;
        dlib::visit_layers(tnet, visitor_con_disable_bias());
        vovnet::infer_19_slim net(tnet);
        net.subnet().layer_details().set_num_outputs(num_outputs);
        benchmark("vovnet19s", net, batch_size, image_size, num_iters);
    }
    {
        vovnet::train_19 tnet;
        dlib::visit_layers(tnet, visitor_con_disable_bias());
        vovnet::infer_19 net(tnet);
        net.subnet().layer_details().set_num_outputs(num_outputs);
        benchmark("vovnet19 ", net, batch_size, image_size, num_iters);
    }
    {
        vovnet::train_27_slim tnet;
        dlib::visit_layers(tnet, visitor_con_disable_bias());
        vovnet::infer_27_slim net(tnet);
        net.subnet().layer_details().set_num_outputs(num_outputs);
        benchmark("vovnet27s", net, batch_size, image_size, num_iters);
    }
    {
        vovnet::train_27 tnet;
        dlib::visit_layers(tnet, visitor_con_disable_bias());
        vovnet::infer_27 net(tnet);
        net.subnet().layer_details().set_num_outputs(num_outputs);
        benchmark("vovnet27 ", net, batch_size, image_size, num_iters);
    }
    {
        vovnet::train_39 tnet;
        dlib::visit_layers(tnet, visitor_con_disable_bias());
        vovnet::infer_39 net(tnet);
        net.subnet().layer_details().set_num_outputs(num_outputs);
        benchmark("vovnet39 ", net, batch_size, image_size, num_iters);
    }
    {
        vovnet::train_57 tnet;
        dlib::visit_layers(tnet, visitor_con_disable_bias());
        vovnet::infer_57 net(tnet);
        net.subnet().layer_details().set_num_outputs(num_outputs);
        benchmark("vovnet57 ", net, batch_size, image_size, num_iters);
    }
    {
        vovnet::train_99 tnet;
        dlib::visit_layers(tnet, visitor_con_disable_bias());
        vovnet::infer_99 net(tnet);
        net.subnet().layer_details().set_num_outputs(num_outputs);
        benchmark("vovnet99 ", net, batch_size, image_size, num_iters);
    }
#endif

#if DNN_BENCH_REPVGG
    {
        repvgg::infer_a0 net;
        net.subnet().layer_details().set_num_outputs(num_outputs);
        benchmark("repvgg_a0 ", net, batch_size, image_size, num_iters);
    }
    {
        repvgg::infer_a1 net;
        net.subnet().layer_details().set_num_outputs(num_outputs);
        benchmark("repvgg_a1 ", net, batch_size, image_size, num_iters);
    }
    {
        repvgg::infer_a2 net;
        net.subnet().layer_details().set_num_outputs(num_outputs);
        benchmark("repvgg_a2 ", net, batch_size, image_size, num_iters);
    }
    {
        repvgg::infer_b0 net;
        net.subnet().layer_details().set_num_outputs(num_outputs);
        benchmark("repvgg_b0 ", net, batch_size, image_size, num_iters);
    }
    {
        repvgg::infer_b1 net;
        net.subnet().layer_details().set_num_outputs(num_outputs);
        benchmark("repvgg_b1 ", net, batch_size, image_size, num_iters);
    }
    {
        repvgg::infer_b2 net;
        net.subnet().layer_details().set_num_outputs(num_outputs);
        benchmark("repvgg_b2 ", net, batch_size, image_size, num_iters);
    }
    {
        repvgg::infer_b3 net;
        net.subnet().layer_details().set_num_outputs(num_outputs);
        benchmark("repvgg_b3 ", net, batch_size, image_size, num_iters);
    }
#endif

    return EXIT_SUCCESS;
}
catch (const std::exception& e)
{
    std::cout << e.what() << '\n';
    return EXIT_FAILURE;
}


#ifndef HGaussianProcessBaseClass
#define HGaussianProcessBaseClass

#include "GaussianProcessKernelBaseClass.hpp"
#include <Eigen/Dense>
#include <vector>

//! Gaussian process regression
/*!
 Interface for Gaussian process regression
 \see GaussianProcessKernelBaseClasss
*/
class GaussianProcessBaseClass : GaussianProcessKernelBaseClass {
  public:
    //! Constructor
    GaussianProcessBaseClass ( ) { }
    //! Destructor
    ~GaussianProcessBaseClass ( ) { }
    //! Build the Gaussian process
    virtual void build ( std::vector< std::vector<double> > const&,
                         std::vector<double> const&,
                         std::vector<double> const& ) = 0;
    //! Update the Gaussian process
    virtual void update ( std::vector<double> const&,
                          double&, double& ) = 0;
    //! Evaluate Gaussian process
    virtual void evaluate ( std::vector<double> const&,
                            double&, double& ) = 0;
};

#endif


#include <burst/container/make_list.hpp>
#include <burst/container/make_vector.hpp>
#include <burst/integer/divceil.hpp>
#include <burst/range/buffered_chunks.hpp>
#include <burst/range/istream_range.hpp>

#include <doctest/doctest.h>

#include <boost/range/iterator_range.hpp>

#include <iterator>
#include <sstream>
#include <vector>

TEST_SUITE("buffered_chunks")
{
    TEST_CASE("Диапазон кусков пустого диапазона — пустой диапазон")
    {
        const auto v = std::vector<int>{};

        auto chunks = burst::buffered_chunks(v, 10);

        CHECK(chunks.empty());
    }

    TEST_CASE("Кусков столько, сколь нужно, чтобы разместить на них все элементы исходного "
        "диапазона")
    {
        const auto v = burst::make_vector(100u, 'q');
        const auto n = 13;

        auto chunks = burst::buffered_chunks(v, n);

        CHECK(std::distance(chunks.begin(), chunks.end()) == burst::divceil(v.size(), n));
    }

    TEST_CASE("Работает с однопроходным диапазоном")
    {
        auto s = std::stringstream("5 4 3 2 1");

        auto chunks = burst::buffered_chunks(burst::make_istream_range<int>(s), 3);

        const auto expected_first = {5, 4, 3};
        CHECK(chunks.front() == expected_first);

        chunks.advance_begin(1);
        const auto expected_second = {2, 1};
        CHECK(chunks.front() == expected_second);
    }

    TEST_CASE("Изменяемость кусков не зависит от изменяемости исходного диапазона")
    {
        const auto v = burst::make_list({'q', 'w', 'e', 'r'});
        const auto n = 2;

        auto chunks = burst::buffered_chunks(v, n);

        for (auto & x: chunks.front())
        {
            x = 17;
        }

        for (const auto & x: chunks.front())
        {
            CHECK(x == 17);
        }
    }
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          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
//==============================================================================
/*!
 * \file
**/
#ifndef BOOST_SIMD_TOOLBOX_BITWISE_FUNCTIONS_ILOG2_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_BITWISE_FUNCTIONS_ILOG2_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

/*!
 * \ingroup boost_simd_bitwise
 * \defgroup boost_simd_bitwise_ilog2 ilog2
 *
 * \par Description
 * The function return the integer part of log2 of the argument
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/ilog2.hpp>
 * \endcode
 *
 *
 * \synopsis
 *
 * \code
 * namespace boost::simd
 * {
 *   template <class A0>
 *     meta::call<tag::ilog2_(A0)>::type
 *     ilog2(const A0 & a0);
 * }
 * \endcode
 *
 * \param a0 the unique parameter of ilog2
 *
 * \return always returns an integer value
 *
 * \par Notes
 * In SIMD mode, this function acts elementwise on the inputs vectors elements
 * \par
 *
**/

namespace boost { namespace simd { namespace tag
  {
    /*!
     * \brief Define the tag ilog2_ of functor ilog2
     *        in namespace boost::simd::tag for toolbox boost.simd.bitwise
    **/
    struct ilog2_ : ext::elementwise_<ilog2_> { typedef ext::elementwise_<ilog2_> parent; };
  }
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::ilog2_, ilog2, 1)
} }

#endif

// modified by jt the 25/12/2010


//---------------------------------------------------------------------------//
//!
//! \file   tstAngstromUnit.cpp
//! \author Alex Robinson
//! \brief  The Angstrom unit unit tests
//!
//---------------------------------------------------------------------------//

// Boost Includes
#include <boost/units/quantity.hpp>
#include <boost/units/systems/cgs/length.hpp>
#include <boost/units/systems/si/length.hpp>

// Trilinos Includes
#include <Teuchos_UnitTestHarness.hpp>

// FRENSIE Includes
#include "Utility_AngstromUnit.hpp"

using namespace Utility::Units;
using boost::units::quantity;
namespace si = boost::units::si;
namespace cgs = boost::units::cgs;

//---------------------------------------------------------------------------//
// Tests
//---------------------------------------------------------------------------//
// Check that the angstrom unit can be initialized
TEUCHOS_UNIT_TEST( AngstromUnit, initialize )
{
  quantity<Angstrom> angstrom_length( 1.0*angstrom );

  TEST_EQUALITY_CONST( angstrom_length.value(), 1.0 );

  angstrom_length = 2.0*angstrom;

  TEST_EQUALITY_CONST( angstrom_length.value(), 2.0 );
}

//---------------------------------------------------------------------------//
// Check that the angstrom unit can be initialized from cgs and si units
TEUCHOS_UNIT_TEST( AngstromUnit, initialize_from_cgs_si )
{
  quantity<Angstrom> angstrom_length_cgs( 1.0*cgs::centimeter );

  quantity<Angstrom> angstrom_length_si( 1.0*si::meter );

  TEST_EQUALITY_CONST( angstrom_length_cgs.value(), 1e8 );
  TEST_EQUALITY_CONST( angstrom_length_si.value(), 1e10 );
}

//---------------------------------------------------------------------------//
// Check that the cgs and si units can be initialized from the angstrom unit
TEUCHOS_UNIT_TEST( AngstromUnit, initialize_cgs_si )
{
  quantity<cgs::length> centimeter_length( 1.0*angstrom );

  quantity<si::length> meter_length( 1.0*angstrom );

  TEST_EQUALITY_CONST( centimeter_length.value(), 1e-8 );
  TEST_EQUALITY_CONST( meter_length.value(), 1e-10 );
}

//---------------------------------------------------------------------------//
// end tstAngstromUnit.cpp
//---------------------------------------------------------------------------//


/**
 * @file
 * @license License BSD-3-Clause
 * @copyright Copyright (c) 2019, New York University and Max Planck
 * Gesellshaft.
 *
 * @brief Defines the drivers of the test_bench
 */

#pragma once

#include <Eigen/Eigen>

#include "master_board_sdk/master_board_interface.h"

namespace Eigen
{
typedef Eigen::Matrix<double, 12, 1> Vector12d;
}

namespace test_bench
{
typedef std::array<double, 12> Array12d;

struct TestBenchData
{
    /*
     * Joint data
     */

    /** @brief Joint positions [rad]. */
    Eigen::Vector12d joint_positions;

    /** @brief Joint velocities [rad/s]. */
    Eigen::Vector12d joint_velocities;

    /** @brief Desired motor current. */
    Eigen::Vector12d desired_motor_current;

    /*
     * Hardware data
     */

    /** @brief Joint gear ratios. */
    Eigen::Vector12d joint_gear_ratios;

    /** @brief motor_torque_constants_ are the motor torque constants. */
    Eigen::Vector12d motor_torque_constants;

    /** @brief Max motor current. */
    Eigen::Vector12d max_motor_current;

    /** Joint rotation direction. negative means we invert it. */
    Eigen::Vector12d polarities;

    /**
     * Additional data
     */

    /** @brief slider_positions_ is the position of the linear potentiometer.
     * Can be used as a joystick input.
     */
    Eigen::Vector12d slider_positions_;
};

enum TestBenchState
{
    initial,
    getting_ready,
    ready
};

class TestBench
{
public:
    /**
     * @brief DGMTeststand is the constructor.
     */
    TestBench();

    /**
     * @brief ~DGMTeststand is the destructor.
     */
    ~TestBench(){};

    /**
     * @brief initialize is the function that initialize the hardware.
     */
    void initialize(const std::string& network_id);

    /**
     * @brief Fills in the internal sensors data.
     */
    void acquire_sensors();

    /**
     * @brief send_target_torques sends the target currents to the motors
     */
    void send_target_joint_torque(
        Eigen::Ref<const Eigen::Vector12d> target_joint_torque);

    /**
     * @brief Check if everything is setup (motor aligned, no timeout, etc).
     *
     * @return true
     * @return false
     */
    bool ready();

    /**
     * @brief get_joint_positions
     * WARNING !!!! The method acquire_sensors() has to be called prior to
     * any getter to have up to date data.
     *
     * @return  the joint angle of each module
     */
    const TestBenchData& get_data()
    {
        return data_;
    }

private:
    /**
     * @brief Calibrate the robot joint position
     *
     * @param soft or mechanical calibration?
     * @param zero_to_index_angle is the angle between the theoretical zero and
     * the next positive angle.
     * @param index_angle is the positition of the next index.
     */
    void calibrate_joint_position(bool mechanical_calibration,
                                  Array12d& zero_to_index_angle,
                                  Array12d& index_angle);

    /**
     * Entries for the real hardware.
     */

    /** @brief Robot driver sdk object. Allowing communication with the robot.
     */
    std::shared_ptr<MasterBoardInterface> robot_drivers;

    /** @brief Sensor and internal robot data. */
    TestBenchData data_;

    /** @brief State of the state machine managing the initialization procedure.
     */
    TestBenchState state_;
};

}  // namespace test_bench