#include <MFModel.h>
#include <Utils.h>
#include <MlUtils.h>
#include <Eigen/Dense>
#include <Checksum.h>
#include <algorithm>
#include <ModelGradient.h>

//#define DEBUG

namespace cirrus {

MFModel::MFModel(uint64_t users, uint64_t items, uint64_t nfactors) {
    initialize_data(users, items, nfactors);
}

MFModel::MFModel(
    const void* data, uint64_t /*nusers*/, uint64_t /*nitems*/, uint64_t /*nfactors*/) {
  loadSerialized(data);
}

void MFModel::initialize_reg_params() {
  item_fact_reg_ = 0.01;
  user_fact_reg_ = 0.01;

  user_bias_reg_ = 0.01;
  item_bias_reg_ = 0.01;
}

// FORMAT
// Number of users (32bits)
// Number of factors (32bits)
// Sample1: factor 1 (FEATURE_TYPE) | factor 2 | factor 3
// Sample2 : ...
// ....

void MFModel::initialize_data(uint64_t users, uint64_t items, uint64_t nfactors) {
  user_weights_.resize(users * nfactors);
  item_weights_.resize(items * nfactors);
  global_bias_ = 3.604;

  user_bias_.resize(users);
  item_bias_.resize(items);

  initialize_reg_params();

  nusers_ = users;
  nitems_ = items;
  nfactors_ = nfactors;

  randomize();

  std::cout << "Initializing MFModel nusers: " << nusers_
            << " nitems: " << nitems_
            << std::endl;
}

uint64_t MFModel::size() const {
  throw std::runtime_error("Not implemented");
  return 0;
}

/**
  *
  * Serialization / deserialization
  *
  */

std::unique_ptr<CirrusModel> MFModel::deserialize(void* /*data*/, uint64_t /*size*/) const {
  throw std::runtime_error("Not implemented");
}

std::pair<std::unique_ptr<char[]>, uint64_t>
MFModel::serialize() const {
    std::pair<std::unique_ptr<char[]>, uint64_t> res;
    uint64_t size = getSerializedSize();

    res.first.reset(new char[size]);
    res.second = size;

    serializeTo(res.first.get());
    return res;
}

uint64_t MFModel::getSerializedSize() const {
    return sizeof(uint64_t) * 3 + // nusers + nitem + nfactors
      user_bias_.size() * sizeof(FEATURE_TYPE) +
      item_bias_.size() * sizeof(FEATURE_TYPE) +
      user_weights_.size() * sizeof(FEATURE_TYPE) +
      item_weights_.size() * sizeof(FEATURE_TYPE);
}

void MFModel::serializeTo(void* mem) const {
    char* data = reinterpret_cast<char*>(mem);

#ifdef DEBUG
    std::cout
      << "SerializeTo"
      << " nusers: " << nusers_
      << " nitems_: " << nitems_
      << " nfactors_: " << nfactors_
      << std::endl;
#endif

    store_value<uint64_t>(data, nusers_);
    store_value<uint64_t>(data, nitems_);
    store_value<uint64_t>(data, nfactors_);

    for (const auto& v : user_bias_) {
      store_value<FEATURE_TYPE>(data, v);
    }
    for (const auto& v : item_bias_) {
      store_value<FEATURE_TYPE>(data, v);
    }
    for (const auto& v : user_weights_) {
      store_value<FEATURE_TYPE>(data, v);
    }
    for (const auto& v : item_weights_) {
      store_value<FEATURE_TYPE>(data, v);
    }
}

void MFModel::loadSerialized(const void* data) {
  // Read number of samples, number of factors
  nusers_ = load_value<uint64_t>(data);
  nitems_ = load_value<uint64_t>(data);
  nfactors_ = load_value<uint64_t>(data);

#ifdef DEBUG
  std::cout << "loadSerialized"
    << " nusers: " << nusers_
    << " nitems_: " << nitems_
    << " nfactors_: " << nfactors_
    << std::endl;
#endif

  global_bias_ = 3.604;
  user_weights_.resize(nusers_ * nfactors_);
  item_weights_.resize(nitems_ * nfactors_);
  user_bias_.resize(nusers_);
  item_bias_.resize(nitems_);

  // read user bias
  for (uint32_t i = 0; i < nusers_; ++i) {
    FEATURE_TYPE user_bias = load_value<FEATURE_TYPE>(data);
    user_bias_[i] = user_bias;
  }
  // read item bias
  for (uint32_t i = 0; i < nitems_; ++i) {
    FEATURE_TYPE item_bias = load_value<FEATURE_TYPE>(data);
    item_bias_[i] = item_bias;
  }
  // read user weights
  for (uint32_t i = 0; i < nusers_; ++i) {
    for (uint32_t j = 0; j < nfactors_; ++j) {
      FEATURE_TYPE user_weight = load_value<FEATURE_TYPE>(data);
      get_user_weights(i, j) = user_weight;
    }
  }
  // read item weights
  for (uint32_t i = 0; i < nitems_; ++i) {
    for (uint32_t j = 0; j < nfactors_; ++j) {
      FEATURE_TYPE item_weight = load_value<FEATURE_TYPE>(data);
      get_item_weights(i, j) = item_weight;
    }
  }
}

/**
  * We probably want to put 0 in the values we don't know
  */
void MFModel::randomize() {
  std::default_random_engine generator;
  std::normal_distribution<FEATURE_TYPE> distribution(0, 1.0 / nfactors_); // mean 0 and stddev=1
  for (uint64_t i = 0; i < nusers_; ++i) {
    for (uint64_t j = 0; j < nfactors_; ++j) {
      get_user_weights(i, j) = distribution(generator);
    }
  }
  for (uint64_t i = 0; i < nitems_; ++i) {
    for (uint64_t j = 0; j < nfactors_; ++j) {
      get_item_weights(i, j) = distribution(generator);
    }
  }
}

std::unique_ptr<CirrusModel> MFModel::copy() const {
    std::unique_ptr<MFModel> new_model =
        std::make_unique<MFModel>(nusers_, nitems_, nfactors_);
    return new_model;
}

void MFModel::sgd_update(double learning_rate,
        const ModelGradient* gradient) {
  const MFSparseGradient* grad_ptr = dynamic_cast<const MFSparseGradient*>(gradient);
  assert(grad_ptr);

  // apply grad to users_bias_grad
  for (const auto& v : grad_ptr->users_bias_grad) {
      user_bias_[v.first] += v.second;
  }
  for (const auto& v : grad_ptr->items_bias_grad) {
      item_bias_[v.first] += v.second;
  }
  for (const auto& v : grad_ptr->users_weights_grad) {
    int user_id = v.first;
    assert(v.second.size() == NUM_FACTORS);
    for (uint32_t i = 0; i < v.second.size(); ++i) {
      get_user_weights(user_id, i) += v.second[i];
    }
  }
  for (const auto& v : grad_ptr->items_weights_grad) {
    int item_id = v.first;
    assert(v.second.size() == NUM_FACTORS);
    for (uint32_t i = 0; i < v.second.size(); ++i) {
      get_item_weights(item_id, i) += v.second[i];
    }
  }
}

FEATURE_TYPE MFModel::predict(uint32_t userId, uint32_t itemId) const {
#ifdef DEBUG
  FEATURE_TYPE res = global_bias_ + user_bias_.at(userId) + item_bias_.at(itemId);
#else
  FEATURE_TYPE res = global_bias_ + user_bias_[userId] + item_bias_[itemId];
#endif
  for (uint32_t i = 0; i < nfactors_; ++i) {
    res += get_user_weights(userId, i) * get_item_weights(itemId, i);
#ifdef DEBUG
    if (std::isnan(res) || std::isinf(res)) {
      std::cout << "userId: " << userId << " itemId: " << itemId
        << " get_user_weights(userId, i): " << get_user_weights(userId, i)
        << " get_item_weights(itemId, i): " << get_item_weights(itemId, i)
        << std::endl;
      throw std::runtime_error("nan error in predict");
    }
#endif
  }
  return res;
}

std::unique_ptr<ModelGradient> MFModel::minibatch_grad(
        const Matrix&,
        FEATURE_TYPE*,
        uint64_t,
        double) const {
  throw std::runtime_error("Not implemented");
}

FEATURE_TYPE& MFModel::get_user_weights(uint64_t userId, uint64_t factor) {
  return user_weights_.at(userId * nfactors_ + factor);
}

FEATURE_TYPE& MFModel::get_item_weights(uint64_t itemId, uint64_t factor) {
  if (itemId >= nitems_) {
    std::cout << "itemId: " << itemId << " nitems_: " << nitems_ << std::endl;
  }
  return item_weights_.at(itemId * nfactors_ + factor);
}

const FEATURE_TYPE& MFModel::get_user_weights(uint64_t userId, uint64_t factor) const {
  return user_weights_.at(userId * nfactors_ + factor);
}

const FEATURE_TYPE& MFModel::get_item_weights(uint64_t itemId, uint64_t factor) const {
  return item_weights_.at(itemId * nfactors_ + factor);
}

void MFModel::sgd_update(
            double learning_rate,
            uint64_t base_user,
            const SparseDataset& dataset,
            double /*epsilon*/) {
  // iterate all pairs user rating
  for (uint64_t i = 0; i < dataset.data_.size(); ++i) {
    for (uint64_t j = 0; j < dataset.data_[i].size(); ++j) {
      uint64_t user = base_user + i;
      uint64_t itemId = dataset.data_[i][j].first;
      FEATURE_TYPE rating = dataset.data_[i][j].second;

      FEATURE_TYPE pred = predict(user, itemId);
      FEATURE_TYPE error = rating - pred;

#ifdef DEBUG
      std::cout
        << "user: " << user
        << "itemId: " << itemId
        << "rating: " << rating
        << " prediction: " << pred
        << " error: " << error
        << std::endl;
#endif

      if (itemId >= nitems_ || user >= nusers_) {
        std::cout
          << "itemId: " << itemId
          << " nitems_: " << nitems_
          << " user: " << user
          << " nusers_: " << nusers_
          << std::endl;
        throw std::runtime_error("Wrong value here");
      }
      user_bias_[user] += learning_rate * (error - user_bias_reg_ * user_bias_[user]);
      item_bias_[itemId] += learning_rate * (error - item_bias_reg_ * item_bias_[itemId]);

#ifdef DEBUG
      if (std::isnan(user_bias_[user]) || std::isnan(item_bias_[itemId]) ||
          std::isinf(user_bias_[user]) || std::isinf(item_bias_[itemId]))
        throw std::runtime_error("nan in user_bias or item_bias");
#endif

      // update user latent factors
      for (uint64_t k = 0; k < nfactors_; ++k) {
        FEATURE_TYPE delta_user_w =
          learning_rate * (error * get_item_weights(itemId, k) - user_fact_reg_ * get_user_weights(user, k));
        //std::cout << "delta_user_w: " << delta_user_w << std::endl;
        get_user_weights(user, k) += delta_user_w;
#ifdef DEBUG
        if (std::isnan(get_user_weights(user, k)) || std::isinf(get_user_weights(user, k))) {
          throw std::runtime_error("nan in user weight");
        }
#endif
      }

      // update item latent factors
      for (uint64_t k = 0; k < nfactors_; ++k) {
        FEATURE_TYPE delta_item_w =
          learning_rate * (error * get_user_weights(user, k) - item_fact_reg_ * get_item_weights(itemId, k));
        //std::cout << "delta_item_w: " << delta_item_w << std::endl;
        get_item_weights(itemId, k) += delta_item_w;
#ifdef DEBUG
        if (std::isnan(get_item_weights(itemId, k)) || std::isinf(get_item_weights(itemId, k))) {
          std::cout << "error: " << error << std::endl;
          std::cout << "user weight: " << get_user_weights(user, k) << std::endl;
          std::cout << "item weight: " << get_item_weights(itemId, k) << std::endl;
          std::cout << "learning_rate: " << learning_rate << std::endl;
          throw std::runtime_error("nan in item weight");
        }
#endif
      }
    }
  }
}

std::pair<double, double> MFModel::calc_loss(Dataset& /*dataset*/) const {
  throw std::runtime_error("Not implemented");
  return std::make_pair(0.0, 0.0);
}

std::pair<double, double> MFModel::calc_loss(SparseDataset& dataset, uint32_t start_index) const {
  double error = 0;
  uint64_t count = 0;

#ifdef DEBUG
  std::cout
    << "calc_loss() starting"
    << std::endl;
#endif

  for (uint64_t userId = 0; userId < dataset.data_.size(); ++userId) {
    uint64_t off_userId = userId + start_index;
#ifdef DEBUG
      std::cout
        << "off_userId: " << off_userId
        << " userId: " << userId
        << " dataset.data_.size(): " << dataset.data_.size()
        << std::endl;
#endif
    for (uint64_t j = 0; j < dataset.data_.at(userId).size(); ++j) {
      uint64_t movieId = dataset.data_.at(userId).at(j).first;
#ifdef DEBUG
      std::cout
        << " movieId: " << movieId
        << std::endl;
#endif
      FEATURE_TYPE rating = dataset.data_.at(userId).at(j).second;

      FEATURE_TYPE prediction = predict(off_userId, movieId);
      FEATURE_TYPE e = rating - prediction;

      FEATURE_TYPE e_pow_2 = pow(e, 2);
      error += e_pow_2;
#ifdef DEBUG
      std::cout
        << "prediction: " << prediction
        << " rating: " << rating
        << " e: " << e
        << " e_pow_2: " << pow(e, 2)
        << " error: " << error
        << " count: " << count
        << std::endl;
#endif
      if (std::isnan(e) || std::isnan(error)) {
        std::string error = std::string("nan in calc_loss rating: ") + std::to_string(rating) +
          " prediction: " + std::to_string(prediction);
        throw std::runtime_error(error);
      }
      count++;
    }
  }

#ifdef DEBUG
  std::cout << "error: " << error << " count: " << count << std::endl;
#endif

  //error = error / count;
  //error = std::sqrt(error);
  if (std::isnan(error)) {
    throw std::runtime_error("error isnan");
  }
  return std::make_pair(error, count);
}

uint64_t MFModel::getSerializedGradientSize() const {
    return size() * sizeof(FEATURE_TYPE);
}

std::unique_ptr<ModelGradient> MFModel::loadGradient(void* mem) const {
    auto grad = std::make_unique<MFGradient>(10, 10);
    grad->loadSerialized(mem);
    return grad;
}

double MFModel::checksum() const {
  return crc32(user_weights_.data(), user_weights_.size() * sizeof(FEATURE_TYPE));
}

void MFModel::print() const {
    std::cout << "MODEL user weights: ";
    for (uint64_t i = 0; i < user_weights_.size(); ++i) {
      std::cout << " " << user_weights_[i];
    }
    std::cout << std::endl;
}

FEATURE_TYPE& MFModel::get_user_bias(uint64_t userId) {
  if (userId >= user_bias_.size()) {
    throw std::runtime_error("User bias index too large");
  }
  return user_bias_.at(userId);
}

FEATURE_TYPE& MFModel::get_item_bias(uint64_t itemId) {
  if (itemId >= item_bias_.size()) {
    throw std::runtime_error("Item bias index too large");
  }
  return item_bias_.at(itemId);
}

}  // namespace cirrus


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

    @copyright 2016 NumScale SAS

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

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

  /*!
    @ingroup group-boolean
    This function object conditionally returns @c Nan (respectively @c x)
    if @c c is @ref True (respectively  @ref False)

    @par Header <boost/simd/function/if_nan_else.hpp>

    @par Example:

      @snippet if_nan_else.cpp if_nan_else

    @par Possible output:

      @snippet if_nan_else.txt if_nan_else

  **/
  Value1 if_nan_else(Value0 const& c, Value1 const& x);
} }
#endif

#include <boost/simd/function/scalar/if_allbits_else.hpp>
#include <boost/simd/function/simd/if_nan_else.hpp>

#endif


#include <El.hpp>
#include <boost/mpi.hpp>
#include <boost/format.hpp>
#include <boost/program_options.hpp>

#define SKYLARK_NO_ANY
#include <skylark.hpp>

#include <iostream>
#include <unordered_map>
#include <string>
#include <vector>

namespace bpo = boost::program_options;

template<typename VertexType>
struct simple_parallel_graph_t {

    typedef VertexType vertex_type;
    typedef skylark::base::sparse_vc_star_matrix_t<vertex_type> adjacency_type;

    simple_parallel_graph_t(const std::string &gf) {
        skylark::utility::io::ReadArcList(gf, _adj_matrix, _world, true);
        std::fill(_adj_matrix.values(),
            _adj_matrix.values() + _adj_matrix.local_nonzeros(), 1.0);
    }

#if SKYLARK_HAVE_LIBHDFS
    simple_parallel_graph_t(const hdfsFS &fs, const std::string &gf) {
        SKYLARK_THROW_EXCEPTION(skylark::base::io_exception() <<
            skylark::base::error_msg("No HDFS support yet!"));
    }
#endif

    size_t num_vertices() const { return _adj_matrix.num_rows(); }
    size_t num_edges()    const { return _adj_matrix.num_nnz(); }

    size_t degree(const vertex_type &vertex) const {
        assert(vertex < _adj_matrix.num_rows());
        return _adj_matrix.indptr[vertex + 1] - _adj_matrix.indptr[vertex];
    }

    void adjacency_matrix(
            adjacency_type &A,
            std::vector<vertex_type> &indexmap) const {
        _adj_matrix.view(A);
    }

private:
    skylark::base::sparse_vc_star_matrix_t<vertex_type> _adj_matrix;
    boost::mpi::communicator _world;
};

template<typename VertexType>
struct simple_unweighted_graph_t {

    typedef VertexType vertex_type;

    typedef typename std::vector<vertex_type>::const_iterator iterator_type;
    typedef typename std::unordered_map<vertex_type,
                                        std::vector<vertex_type> >::const_iterator
    vertex_iterator_type;

    simple_unweighted_graph_t(const std::string &gf);
#if SKYLARK_HAVE_LIBHDFS
    simple_unweighted_graph_t(const hdfsFS &fs, const std::string &gf);
#endif

    size_t num_vertices() const { return _nodemap.size(); }
    size_t num_edges() const { return _num_edges; }

    size_t degree(const vertex_type &vertex) const {
        return _nodemap.at(vertex).size();
    }

    iterator_type adjanct_begin(const vertex_type &vertex) const {
        return _nodemap.at(vertex).begin();
    }

    iterator_type adjanct_end(const vertex_type &vertex) const {
        return _nodemap.at(vertex).end();
    }

    vertex_iterator_type vertex_begin() const {
        return _nodemap.begin();
    }

    vertex_iterator_type vertex_end() const {
        return _nodemap.end();
    }

    template<typename T>
    void adjacency_matrix(skylark::base::sparse_matrix_t<T> &A,
        std::vector<vertex_type> &indexmap) const;

private:
    std::unordered_map<vertex_type, std::vector<vertex_type> > _nodemap;
    size_t _num_edges;
};

template<typename VertexType>
simple_unweighted_graph_t<VertexType>::simple_unweighted_graph_t(const std::string &gf) {

    std::ifstream in(gf);
    std::string line, token;
    vertex_type u, v;

    std::unordered_set<std::pair<vertex_type, vertex_type>,
                       skylark::utility::pair_hasher_t> added;

    _num_edges = 0;
    while(true) {
        getline(in, line);
        if (in.eof())
            break;
        if (line[0] == '#')
            continue;

        std::istringstream tokenstream(line);
        tokenstream >> u;
        tokenstream >> v;

        if (u == v)
            continue;

        if (added.count(std::make_pair(u, v)) > 0)
            continue;

        added.insert(std::make_pair(u, v));
        added.insert(std::make_pair(v, u));
        _num_edges += 2;

        _nodemap[u].push_back(v);
        _nodemap[v].push_back(u);
    }

    in.close();
}

#if SKYLARK_HAVE_LIBHDFS
template<typename VertexType>
simple_unweighted_graph_t<VertexType>::
simple_unweighted_graph_t(const hdfsFS &fs, const std::string &gf) {

    skylark::utility::hdfs_line_streamer_t in(fs, gf, 1000);
    std::string line, token;
    vertex_type u, v;

    std::unordered_set<std::pair<vertex_type, vertex_type>,
                       skylark::utility::pair_hasher_t> added;

    _num_edges = 0;
    while(true) {
        in.getline(line);
        if (in.eof())
            break;
        if (line[0] == '#')
            continue;

        std::istringstream tokenstream(line);
        tokenstream >> u;
        tokenstream >> v;

        if (u == v)
            continue;

        if (added.count(std::make_pair(u, v)) > 0)
            continue;

        added.insert(std::make_pair(u, v));
        added.insert(std::make_pair(v, u));
        _num_edges += 2;

        _nodemap[u].push_back(v);
        _nodemap[v].push_back(u);
    }

    in.close();
}
#endif

template<typename VertexType>
template<typename T>
void simple_unweighted_graph_t<VertexType>::
adjacency_matrix(skylark::base::sparse_matrix_t<T> &A,
    std::vector<vertex_type> &indexmap) const {

    typedef typename skylark::base::sparse_matrix_t<T>::index_type index_type;
    typedef typename skylark::base::sparse_matrix_t<T>::value_type value_type;

    index_type n = num_vertices();
    index_type nnz = num_edges();

    // Allocate space for the matrix
    index_type *colptr = new index_type[n + 1];
    index_type *rowind = new index_type[nnz];
    value_type *values = new value_type[nnz];

    // Create index & node map and initial population of colptr
    std::unordered_map<vertex_type, index_type> nodemap;
    indexmap.resize(n);
    n = 0;
    index_type c = 0;
    for (vertex_iterator_type vit = vertex_begin();
         vit != vertex_end(); vit++) {
        nodemap[vit->first] = n;
        indexmap[n] = vit->first;
        colptr[n] = c;
        c += degree(vit->first);
        n++;
    }
    colptr[n] = c;

    for(index_type j = 0; j < n; j++) {
        index_type i = 0;
        for(iterator_type it = adjanct_begin(indexmap[j]);
            it != adjanct_end(indexmap[j]); it++) {
            values[colptr[j] + i] = 1.0;
            rowind[colptr[j] + i] = nodemap[*it];
            i++;
        }
    }

    A.attach(colptr, rowind, values, nnz, n, n, true);
}

template<>
template<typename T>
void simple_unweighted_graph_t<int>::
adjacency_matrix(skylark::base::sparse_matrix_t<T> &A,
    std::vector<vertex_type> &indexmap) const {

    typedef typename skylark::base::sparse_matrix_t<T>::index_type index_type;
    typedef typename skylark::base::sparse_matrix_t<T>::value_type value_type;

    index_type nnz = num_edges();
    int n = 0;
    for (vertex_iterator_type vit = vertex_begin();
         vit != vertex_end(); vit++)
        n = std::max(vit->first + 1, n);

    // Allocate space for the matrix
    index_type *colptr = new index_type[n + 1];
    index_type *rowind = new index_type[nnz];
    value_type *values = new value_type[nnz];

    indexmap.resize(n);
    colptr[0] = 0;
    for(index_type j = 0; j < n; j++) {
        indexmap[j] = j;
        colptr[j + 1] = colptr[j] + degree(j);
        index_type i = 0;
        for(iterator_type it = adjanct_begin(j);
            it != adjanct_end(j); it++) {
            values[colptr[j] + i] = 1.0;
            rowind[colptr[j] + i] = *it;
            i++;
        }
    }

    A.attach(colptr, rowind, values, nnz, n, n, true);
}

int seed, k, powerits, port;
int oversampling_ratio, oversampling_additive;
std::string graphfile, indexfile, prefix, hdfs;
bool use_single, skipqr, directory, numeric;

template<typename graph_type, typename embeddings_type,
         typename s_type = embeddings_type>
void execute() {

    skylark::base::context_t context(seed);
    graph_type *G;

    boost::mpi::communicator world;
    int rank = world.rank();

    boost::mpi::timer timer;

    if (rank == 0) {
        std::cout << "Reading the graph... ";
        std::cout.flush();
        timer.restart();
    }

    if (!hdfs.empty()) {
#       if SKYLARK_HAVE_LIBHDFS

        hdfsFS fs;
        if (rank == 0)
            fs = hdfsConnect(hdfs.c_str(), port);

        G = new graph_type(fs, graphfile);

#       else

        SKYLARK_THROW_EXCEPTION(skylark::base::io_exception() <<
            skylark::base::error_msg("Install libhdfs for HDFS support!"));

#       endif
    } else {
        G = new graph_type(graphfile);
    }

    if (rank == 0)
        std::cout <<"took " << boost::format("%.2e") % timer.elapsed()
                  << " sec\n";

    if (rank == 0) {
        std::cout << "Computing embeddings... ";
        std::cout.flush();
        timer.restart();
    }

    std::vector<typename graph_type::vertex_type> indexmap;
    embeddings_type X;

    skylark::ml::approximate_ase_params_t params;
    params.skip_qr = skipqr;
    params.num_iterations = powerits;
    params.oversampling_ratio = oversampling_ratio;
    params.oversampling_additive = oversampling_additive;

    skylark::ml::ApproximateASE(*G, k, indexmap, X, context, params);

    if (rank == 0)
        std::cout <<"took " << boost::format("%.2e") % timer.elapsed()
                  << " sec\n";

    if (rank == 0) {
        std::cout << "Writing results... ";
        std::cout.flush();
        timer.restart();
    }

    El::Write(X, prefix + ".vec", El::ASCII);

    // the index map is the same for all procs
    if (rank == 0 && !numeric) {
        std::ofstream of(prefix + ".index.txt");
        for (size_t i = 0; i < indexmap.size(); i++)
            of << i << "\t" << indexmap[i] << std::endl;
        of.close();
    }

    if (rank == 0)
        std::cout << "took " << boost::format("%.2e") % timer.elapsed()
                  << " sec\n";

    world.barrier();
}

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

    El::Initialize(argc, argv);

    boost::mpi::communicator world;
    int rank = world.rank();

    // Parse options
    bpo::options_description
        desc("Options");
    desc.add_options()
        ("help,h", "produce a help message")
        ("graphfile,g",
            bpo::value<std::string>(&graphfile),
            "File holding the graph. REQUIRED.")
        //("directory,d", "Whether inputfile is a directory of files whose"
        //    " concatination is the input.")
        ("seed,s",
            bpo::value<int>(&seed)->default_value(38734),
            "Seed for random number generation. OPTIONAL.")
        ("hdfs",
            bpo::value<std::string>(&hdfs)->default_value(""),
            "If not empty, will assume file is in an HDFS. "
            "Parameter is filesystem name.")
        ("port",
            bpo::value<int>(&port)->default_value(0),
            "For HDFS: port to use.")
        ("rank,k",
            bpo::value<int>(&k)->default_value(10),
            "Target rank. OPTIONAL.")
        ("powerits,i",
            bpo::value<int>(&powerits)->default_value(2),
            "Number of power iterations. OPTIONAL.")
        ("skipqr", "Whether to skip QR in each iteration. Higher than one power"
            " iterations is not recommended in this mode.")
        ("ratio,r",
            bpo::value<int>(&oversampling_ratio)->default_value(2),
            "Ratio of oversampling of rank. OPTIONAL.")
        ("additive,a",
            bpo::value<int>(&oversampling_additive)->default_value(0),
            "Additive factor for oversampling of rank. OPTIONAL.")
        ("single", "Whether to use single precision instead of double.")
        ("numeric,n",
            "If present, node labels are numeric and the code exploits that.")
        ("prefix",
            bpo::value<std::string>(&prefix)->default_value("out"),
            "Prefix for output files (prefix.vec.txt and prefix.index.txt)."
            "  OPTIONAL.");

    bpo::positional_options_description positional;
    positional.add("graphfile", 1);

    bpo::variables_map vm;
    try {
        bpo::store(bpo::command_line_parser(argc, argv)
            .options(desc).positional(positional).run(), vm);

        if (vm.count("help")) {
            if (rank == 0) {
                std::cout << "Usage: " << argv[0]
                          << " [options] input-file-name" << std::endl;
                std::cout << desc;
            }
            world.barrier();
            return 0;
        }

        use_single = vm.count("single");
        skipqr = vm.count("skipqr");
        //directory = vm.count("directory");
        numeric = vm.count("numeric");

        if (!vm.count("graphfile")) {
            std::cout << "Input graph-file is required." << std::endl;
            return -1;
        }

        bpo::notify(vm);
    } catch(bpo::error& e) {
        std::cerr << e.what() << std::endl;
        std::cerr << desc << std::endl;
        return -1;
    }

    SKYLARK_BEGIN_TRY()

        if (numeric) {
            if (use_single) {
                if(world.size() > 1)
                    execute<simple_parallel_graph_t<float>,
                            El::DistMatrix<float, El::VC, El::STAR> >();
                else
                    execute<simple_unweighted_graph_t<int>,
                            El::Matrix<float> >();
            } else {
                if(world.size() > 1)
                    execute<simple_parallel_graph_t<double>,
                            El::DistMatrix<double, El::VC, El::STAR> >();
                else
                    execute<simple_unweighted_graph_t<int>,
                            El::Matrix<double> >();
            }
        } else {
            if (world.size() > 1)
                SKYLARK_THROW_EXCEPTION(
                    skylark::base::unsupported_matrix_distribution()
                        << skylark::base::error_msg(
                            "Non-numeric indexes are not yet"
                            "supported for parallel computation."));

            if (use_single)
                execute<simple_unweighted_graph_t<std::string>,
                        El::Matrix<float> >();
            else
                execute<simple_unweighted_graph_t<std::string>,
                        El::Matrix<double> >();
        }

    SKYLARK_END_TRY() SKYLARK_CATCH_AND_PRINT((rank == 0))

    return 0;
}


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

#include <boost/simd/memory/functions/slide.hpp>
#include <boost/dispatch/functor/preprocessor/call.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/dispatch/meta/mpl.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( slide_
                                    , boost::simd::tag::cpu_
                                    , (A0)(N)(X)
                                    , ((simd_<arithmetic_<A0>,X>))
                                      (mpl_integral_< scalar_< integer_<N> > >)
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0, N const&) const
    {
      return eval(a0,boost::mpl::bool_<(N::value>=0)>());
    }

    BOOST_FORCEINLINE
    result_type eval(A0 const& a0, boost::mpl::true_ const&) const
    {
      return slide<N::value>(a0,Zero<A0>());
    }

    BOOST_FORCEINLINE
    result_type eval(A0 const& a0, boost::mpl::false_ const&) const
    {
      return slide<meta::cardinal_of<A0>::value+N::value>(Zero<A0>(),a0);
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( slide_
                                    , boost::simd::tag::cpu_
                                    , (A0)(N)(X)
                                    , ((simd_<arithmetic_<A0>,X>))
                                      ((simd_<arithmetic_<A0>, X>))
                                      (mpl_integral_< scalar_< integer_<N> > >)
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE
    result_type operator()(A0 const& a0, A0 const& a1, N const&) const
    {
      return side ( a0, a1
                  , boost::mpl::bool_< (N::value>=0) >()
                  );
    }

    BOOST_FORCEINLINE
    result_type side(A0 const& a0, A0 const& a1, boost::mpl::true_ const&) const
    {
      return eval ( a0, a1
                  , boost::mpl::bool_<N::value==0>()
                  , boost::mpl::bool_<N::value==A0::static_size>()
                  );
    }

    BOOST_FORCEINLINE
    result_type side(A0 const& a0, A0 const& a1, boost::mpl::false_ const&) const
    {
      return slide<A0::static_size + N::value>(a1,a0);
    }

    BOOST_FORCEINLINE
    result_type eval( A0 const& a0, A0 const&
                    , boost::mpl::true_ const&, boost::mpl::false_ const&
                    ) const
    {
      return a0;
    }

    BOOST_FORCEINLINE
    result_type eval( A0 const&, A0 const& a1
                    , boost::mpl::false_ const&, boost::mpl::true_ const&
                    ) const
    {
      return a1;
    }

    BOOST_FORCEINLINE
    result_type eval( A0 const& a0, A0 const& a1
                    , boost::mpl::false_ const&, boost::mpl::false_ const&
                    ) const
    {
      result_type that;
      static const int limit = A0::static_size-N::value;

      for(int i=0;i<limit;++i) that[i] =a0[i+N::value];
      for(int i=limit;i<A0::static_size;++i) that[i] =a1[i-limit];

      return that;
    }
  };
} } }

#endif


/* Author: Mincheul Kang */

#include <harmonious_sampling/HarmoniousLazyPRMstarMulti.h>
#include <ompl/base/objectives/PathLengthOptimizationObjective.h>
#include <ompl/base/goals/GoalSampleableRegion.h>
#include <ompl/util/GeometricEquations.h>
#include <ompl/geometric/planners/prm/ConnectionStrategy.h>
#include <ompl/tools/config/SelfConfig.h>
#include <ompl/base/spaces/RealVectorStateSpace.h>

#include <boost/lambda/bind.hpp>
#include <boost/graph/astar_search.hpp>
#include <boost/graph/lookup_edge.hpp>
#include <boost/foreach.hpp>
#include <fstream>
#include <queue>
#include <stack>
#include <utility>
#include <numeric>
#include <stdio.h>
#include <iostream>

#define foreach BOOST_FOREACH

namespace ompl {
    namespace magic {
/** \brief The number of nearest neighbors to consider by
      default in the construction of the PRM roadmap */
        static const unsigned int DEFAULT_NEAREST_NEIGHBORS_LAZY = 5;

/** \brief When optimizing solutions with lazy planners, this is the minimum
      number of path segments to add before attempting a new optimized solution
          extraction */
        static const unsigned int MIN_ADDED_SEGMENTS_FOR_LAZY_OPTIMIZATION = 5;
    }
}

ompl::geometric::HarmoniousLazyPRMstarMulti::HarmoniousLazyPRMstarMulti(const base::SpaceInformationPtr &si,
                                                                        base::HarmoniousSampler &hs,
                                                                        const std::vector<bool> &isContinuous,
                                                                        bool starStrategy) :
        base::Planner(si, "HarmoniousLazyPRMstarMulti"),
        starStrategy_(starStrategy),
        userSetConnectionStrategy_(false),
        maxDistance_(0.0),
        indexProperty_(boost::get(boost::vertex_index_t(), g_)),
        stateProperty_(boost::get(vertex_state_t(), g_)),
        radiusProperty_(boost::get(vertex_radius_t(), g_)),
        witnessProperty_(boost::get(vertex_witness_t(), g_)),
        costProperty_(boost::get(vertex_cost_t(), g_)),
        childrenProperty_(boost::get(vertex_children_t(), g_)),
        predecessorProperty_(boost::get(boost::vertex_predecessor_t(), g_)),
        colorProperty_(boost::get(boost::vertex_color_t(), g_)),
        weightProperty_(boost::get(boost::edge_weight_t(), g_)),
        vertexValidityProperty_(boost::get(vertex_flags_t(), g_)),
        edgeValidityProperty_(boost::get(edge_flags_t(), g_)),
        bestCost_(std::numeric_limits<double>::quiet_NaN()),
        iterations_(0),
        increaseIterations_(0),
        BisectionCC_(true),
        rewireFactor_(1.5),
        hs_(hs),
        isContinuous_(isContinuous)
{
    specs_.recognizedGoal = base::GOAL_SAMPLEABLE_REGION;
    specs_.approximateSolutions = false;
    specs_.optimizingPaths = true;

    Planner::declareParam<bool>("BisectionCC", this,
                                &HarmoniousLazyPRMstarMulti::setBisectionCC, std::string("."));
    Planner::declareParam<double>("RewireFactor", this,
                                  &HarmoniousLazyPRMstarMulti::setRewireFactor, std::string("."));

    addPlannerProgressProperty("iterations INTEGER",
                               std::bind(&HarmoniousLazyPRMstarMulti::getIterationCount, this));
    addPlannerProgressProperty("best cost REAL",
                               std::bind(&HarmoniousLazyPRMstarMulti::getBestCost, this));
    addPlannerProgressProperty("milestone count INTEGER",
                               std::bind(&HarmoniousLazyPRMstarMulti::getMilestoneCountString, this));
    addPlannerProgressProperty("edge count INTEGER",
                               std::bind(&HarmoniousLazyPRMstarMulti::getEdgeCountString, this));
}

ompl::geometric::HarmoniousLazyPRMstarMulti::~HarmoniousLazyPRMstarMulti() {
}

void ompl::geometric::HarmoniousLazyPRMstarMulti::setup() {
    Planner::setup();
    tools::SelfConfig sc(si_, getName());
    sc.configurePlannerRange(maxDistance_);

    if (!nn_) {
        nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(this));
        nn_->setDistanceFunction(std::bind(&HarmoniousLazyPRMstarMulti::distanceFunctionHarmonious, this, std::placeholders::_1, std::placeholders::_2));
    }

    if (!nnB_) {
        nnB_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(this));
        nnB_->setDistanceFunction(std::bind(&HarmoniousLazyPRMstarMulti::distanceFunctionHarmonious, this, std::placeholders::_1, std::placeholders::_2));
    }

    if (!nnM_) {
        nnM_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(this));
        nnM_->setDistanceFunction(std::bind(&HarmoniousLazyPRMstarMulti::distanceFunctionHarmonious, this, std::placeholders::_1, std::placeholders::_2));
    }

    if (!connectionStrategy_) {
//        if (starStrategy_)
//            connectionStrategy_ = KStarStrategy<Vertex>(std::bind(&HarmoniousLazyPRMstarMulti::milestoneCount, this), nn_, si_->getStateDimension());
//        else
//            connectionStrategy_ = KBoundedStrategy<Vertex>(magic::DEFAULT_NEAREST_NEIGHBORS_LAZY, maxDistance_, nn_);
    }

    double dimDbl = static_cast<double>(si_->getStateDimension());
    // double prunedMeasure_ = si_->getSpaceMeasure();

    k_rrgConstant_ = rewireFactor_ * boost::math::constants::e<double>() + (boost::math::constants::e<double>() / dimDbl);

    // Setup optimization objective
    //
    // If no optimization objective was specified, then default to
    // optimizing path length as computed by the distance() function
    // in the state space.
    if (pdef_) {
        if (pdef_->hasOptimizationObjective()) {
            opt_ = pdef_->getOptimizationObjective();
        }
        else {
            opt_.reset(new base::PathLengthOptimizationObjective(si_));

            if (!starStrategy_) {
                opt_->setCostThreshold(opt_->infiniteCost());
            }
        }
    }
    else {
        OMPL_ERROR("%s: problem definition is not set, deferring setup completion...\n", getName().c_str());
        setup_ = false;
    }
    sampler_ = si_->allocStateSampler();
}

void ompl::geometric::HarmoniousLazyPRMstarMulti::setRange(double distance) {
    maxDistance_ = distance;

    if (!userSetConnectionStrategy_) {
        connectionStrategy_.clear();
    }

    if (isSetup()) {
        setup();
    }
}

void ompl::geometric::HarmoniousLazyPRMstarMulti::setMaxNearestNeighbors(unsigned int k) {
    if (starStrategy_) {
        throw Exception("Cannot set the maximum nearest neighbors for " + getName());
    }

    if (!nn_) {
        nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(this));
        nn_->setDistanceFunction(std::bind(&HarmoniousLazyPRMstarMulti::distanceFunctionHarmonious, this,std::placeholders::_1, std::placeholders::_2));
    }

    if (!nnB_) {
        nnB_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(this));
        nnB_->setDistanceFunction(std::bind(&HarmoniousLazyPRMstarMulti::distanceFunctionHarmonious, this, std::placeholders::_1, std::placeholders::_2));
    }

    if (!nnM_) {
        nnM_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(this));
        nnM_->setDistanceFunction(std::bind(&HarmoniousLazyPRMstarMulti::distanceFunctionHarmonious, this, std::placeholders::_1, std::placeholders::_2));
    }

    if (!userSetConnectionStrategy_) {
        connectionStrategy_.clear();
    }

    if (isSetup()) {
        setup();
    }
}

void ompl::geometric::HarmoniousLazyPRMstarMulti::setProblemDefinition(const base::ProblemDefinitionPtr &pdef) {
    Planner::setProblemDefinition(pdef);
    clearQuery();
}

void ompl::geometric::HarmoniousLazyPRMstarMulti::clearQuery() {
    startM_.clear();
    goalM_.clear();
    pis_.restart();
}

void ompl::geometric::HarmoniousLazyPRMstarMulti::clear() {
    Planner::clear();
    freeMemory();

    if (nn_) {
        nn_->clear();
    }

    if (nnB_) {
        nnB_->clear();
    }

    if (nnM_) {
        nnM_->clear();
    }

    clearQuery();

    iterations_ = 0;
    bestCost_ = base::Cost(std::numeric_limits<double>::quiet_NaN());
}

void ompl::geometric::HarmoniousLazyPRMstarMulti::freeMemory() {
    foreach (Vertex v, boost::vertices(g_)) {
        si_->freeState(stateProperty_[v]);
    }

    g_.clear();
}

// Add newly sampled vertex and its adjancency edges connected to neigh neighbors.
ompl::geometric::HarmoniousLazyPRMstarMulti::Vertex ompl::geometric::HarmoniousLazyPRMstarMulti::addMilestone(base::State *state, bool isM, bool isChecked) {
    Vertex m = boost::add_vertex(g_);
    stateProperty_[m] = state;
    radiusProperty_[m] = std::numeric_limits<double>::infinity();
    costProperty_[m] = std::numeric_limits<double>::infinity();
    childrenProperty_[m] = new std::vector<Vertex>();
    witnessProperty_[m] = NULL;
    predecessorProperty_[m] = NULL;
    colorProperty_[m] = 0;
    vertexValidityProperty_[m] = (isChecked) ? VALIDITY_TRUE : VALIDITY_UNKNOWN;

    std::vector<Vertex> neighbors;
    std::vector<double> neighbors_costs;

    if(isM){
        // manipulation regions
        unsigned long int bSize = nnB_->size();
        unsigned int max_number_of_neighbors = std::ceil(k_rrgConstant_ * log(static_cast<double>(milestoneCount()-bSize) + 1u));
        neighbors.reserve(max_number_of_neighbors);
        neighbors_costs.reserve(max_number_of_neighbors);

        nnM_->nearestK(m, max_number_of_neighbors, neighbors);

        foreach (Vertex n, neighbors) {
            const double weight = distanceFunctionHarmonious(n, m);

            ompl::base::Cost cost_weight(weight);
            const Graph::edge_property_type properties(cost_weight);

            neighborProperty_[m].push_back(type_neighbor(n, weight));
            neighborProperty_[n].push_back(type_neighbor(m, weight));

            // If collision-free or optimized well,
            const Edge &e = boost::add_edge(n, m, properties, g_).first;
            edgeValidityProperty_[e] = VALIDITY_UNKNOWN;
        }

        // base regions
        max_number_of_neighbors = std::ceil(k_rrgConstant_ * log(static_cast<double>(bSize) + 1u));

        neighbors.reserve(max_number_of_neighbors);
        neighbors_costs.reserve(max_number_of_neighbors);

        nnB_->nearestK(m, max_number_of_neighbors, neighbors);

        foreach (Vertex n, neighbors) {
            const double weight = distanceFunctionHarmonious(n, m);

            ompl::base::Cost cost_weight(weight);
            const Graph::edge_property_type properties(cost_weight);

            neighborProperty_[m].push_back(type_neighbor(n, weight));
            neighborProperty_[n].push_back(type_neighbor(m, weight));

            // If collision-free or optimized well,
            const Edge &e = boost::add_edge(n, m, properties, g_).first;
            edgeValidityProperty_[e] = VALIDITY_UNKNOWN;
        }

        nnM_->add(m);
    }
    else{
        unsigned int max_number_of_neighbors = std::ceil(k_rrgConstant_ * log(static_cast<double>(milestoneCount()) + 1u));

        neighbors.reserve(max_number_of_neighbors);
        neighbors_costs.reserve(max_number_of_neighbors);

        nn_->nearestK(m, max_number_of_neighbors, neighbors);

        foreach (Vertex n, neighbors) {
            const double weight = distanceFunctionHarmonious(n, m);

            ompl::base::Cost cost_weight(weight);
            const Graph::edge_property_type properties(cost_weight);

            neighborProperty_[m].push_back(type_neighbor(n, weight));
            neighborProperty_[n].push_back(type_neighbor(m, weight));

            // If collision-free or optimized well,
            const Edge &e = boost::add_edge(n, m, properties, g_).first;
            edgeValidityProperty_[e] = VALIDITY_UNKNOWN;
        }

        nnB_->add(m);
    }

    nn_->add(m);

    return m;
}

ompl::base::PlannerStatus ompl::geometric::HarmoniousLazyPRMstarMulti::solve(const base::PlannerTerminationCondition &ptc) {
    // Initial checkup for start/goal configurations.
    checkValidity();

    // Add the valid start states as milestones
    while (const base::State *st = pis_.nextStart()) {
        Vertex st_vert = addMilestone(si_->cloneState(st), false);
        costProperty_[st_vert] = 0.0; // Initialize with 0 cost.
        startM_.push_back(st_vert);
    }

    if (startM_.size() == 0) {
        OMPL_ERROR("error-%s: There are no valid initial states!", getName().c_str());
        return base::PlannerStatus::INVALID_START;
    }

    unsigned long int nrStartStates = boost::num_vertices(g_);
    OMPL_INFORM("%s: Starting planning with %lu states already in datastructure", getName().c_str(), nrStartStates);

    bestCost_ = opt_->infiniteCost();
    base::State *workState = si_->allocState();
    bool fullyOptimized = false;
    base::PathPtr bestSolution;

    base::RealVectorStateSpace::StateType *rstate = static_cast<base::RealVectorStateSpace::StateType*>(workState);

    Vertex startV = startM_[0];

    do{
        if(hs_.makeGoal(si_, workState)){
            goalM_.push_back(addMilestone(si_->cloneState(workState), true));
        }
    } while(goalM_.size() == 0 && ptc == false);

    bool isM = false;
    while (ptc == false) {
        ++iterations_;

        int r_val = std::rand() % 100;
        if(r_val < hs_.params_.prob_goal_ && goalM_.size() < hs_.params_.max_num_goal_){
            if(hs_.makeGoal(si_, workState)){
                goalM_.push_back(addMilestone(si_->cloneState(workState), true));
            }
            else{
                continue;
            }
        }
        else if(r_val < hs_.params_.prob_uniform_){
            hs_.uniformSampling(workState);
            if(hs_.getReachable(workState)){
                isM = true;
            }
            else{
                isM = false;
            }
        }
        else{
            hs_.biasedSampling(workState);

            if(hs_.getReachable(workState)){
                hs_.jointSampling(workState);
                isM = true;
            }
            else{
                hs_.setBasicPose(workState);
                isM = false;
            }
        }

        if (!si_->isValid(workState)) {
            continue;
        }

        // Add collision-free vertices.
        Vertex addedVertex = addMilestone(si_->cloneState(workState), isM);

        // DSPT update.
        Decrease(addedVertex);

        // Only support a single pair of start and goal node.
        base::PathPtr solution;

        for (int i = 0; i < (int)goalM_.size(); i++) {
            Vertex goalV = goalM_[i];
            do {
                if (predecessorProperty_[goalV] == NULL || bestCost_.value() <= costProperty_[goalV])
                    break;
                solution = constructSolution(startV, goalV);
            } while (!solution);

            if (solution) {
                base::Cost c(costProperty_[goalV]);

                if (opt_->isCostBetterThan(c, bestCost_)) {
                    bestSolution = solution;

                    bestCost_ = c;
                }
            }
        }
    }
    if (goalM_.empty()) {
        OMPL_ERROR("%s: Unable to find any valid goal states", getName().c_str());
        return base::PlannerStatus::INVALID_GOAL;
    }

    si_->freeState(workState);

    if (bestSolution) {
        base::PlannerSolution psol(bestSolution);
        psol.setPlannerName(getName());
        // If the solution was optimized, we mark it as such
        psol.setOptimized(opt_, bestCost_, fullyOptimized);
        pdef_->addSolutionPath(psol);
    }

    OMPL_INFORM("%s: Created %lu vertices and %lu edges.", getName().c_str(), boost::num_vertices(g_) - 1, boost::num_edges(g_));
    OMPL_INFORM("%s: Goalstates %lu ", getName().c_str(), goalM_.size());
    OMPL_INFORM("Cost: %.6f", bestCost_.value());

    return bestSolution ? base::PlannerStatus::EXACT_SOLUTION :
           base::PlannerStatus::TIMEOUT;
}

// outedge, inedge? - doesn't matter, need to scan all the neighbors.
void ompl::geometric::HarmoniousLazyPRMstarMulti::Decrease(const Vertex &v) {
    typedef std::pair<double, Vertex> weight_vertex;
    std::priority_queue<weight_vertex> pq;

    // Initialize cost of v, i.e., finding best parent vertex in G(g_).
    BGL_FORALL_OUTEDGES(v, e, g_, Graph) {
        Vertex w = target(e, g_);
        double weight = weightProperty_[e].value();

        if (costProperty_[v] > costProperty_[w] + weight) {
            predecessorProperty_[v] = w;
            costProperty_[v] = costProperty_[w] + weight;
        }
    }

    // No need to invoke cancelAdoption since v is newly sampled.
    if (predecessorProperty_[v] != NULL) {
        childrenProperty_[predecessorProperty_[v]]->push_back(v);
    }

    // At this point, v has a best parent. From now on construct its subtree of descendants.

    pq.push(weight_vertex(-costProperty_[v], v)); // Invert the cost value for mimicking min-heap.

    while (!pq.empty()) {
        weight_vertex top = pq.top();
        pq.pop();

        double cost = -top.first; // Invert the cost value to be like min-heap.
        Vertex vert = top.second;

        if (cost > costProperty_[vert]) {
            continue;
        }

        BGL_FORALL_OUTEDGES(vert, e, g_, Graph) {
            Vertex w = target(e, g_);
            double weight = weightProperty_[e].value();
            double cost_w = costProperty_[w];

            if (cost_w > cost + weight) {
                costProperty_[w] = cost + weight;
                cancelAdoption(w);

                predecessorProperty_[w] = vert;
                childrenProperty_[vert]->push_back(w);
                pq.push(weight_vertex(-costProperty_[w], w));
            }
        }
    }

    // Now, DSPT is stable.
}

#define RED (increaseIterations_ + 1) // I know, it's bad #define.

void ompl::geometric::HarmoniousLazyPRMstarMulti::Increase(const Vertex vs) {
    // <Step 1. Preparation.
    // white is used for color of each vertex without initialization.
    // For each iteration, white is increased by 1, thus we can use it as
    // equal to or less than 'white' means 'white' color, 'red' otherwise.
    increaseIterations_ += 1;

    std::vector<Vertex> reds;
    typedef std::pair<double, Vertex> weight_vertex;
    std::priority_queue<weight_vertex> pq; //  Max-heap by default.

    pq.push(weight_vertex(-costProperty_[vs], vs));  // It works as if it is min-heap.

    // <Step 2. Coloring
    while (!pq.empty()) {
        weight_vertex top = pq.top();
        pq.pop();

        double cost = -top.first;
        Vertex vert = top.second;

        if (cost > costProperty_[vert]) {
            continue;  // Instead of heap-improve
        }

        // Probability to get a pink node? almost impossible!
        /*
        // If there exist a non-red neighbor q of z such that Dist(q) + w_(q, z) = D(z)
        // set pink, that means it can keep the current cost, thus it is not necessary to
        // iterate its children.
        // Otherwise, set red and enqueue all the children of z.
        bool pink_flag = false;
        BGL_FORALL_OUTEDGES(vert, e, g_, Graph) {
          Vertex w = target(e, g_);
          double weight = weightProperty_[e].value();

          if (colorProperty_[w] != RED && costProperty_[w] + weight == cost) {
            // Actually, '<' should not be happened all the time.
            // And even '==' would very rarely occur, but possible.
            pink_flag = true;

            cancelAdoption(vert);
            predecessorProperty_[vert] = w;
            childrenProperty_[w]->push_back(vert);
            break; // If there exsits, take anyone among them.
          }
        }

        if (pink_flag) {
          continue;
        }*/

        colorProperty_[vert] = RED; // Set to 'red'
        reds.push_back(vert);
        // Even with multiple starting red nodes, there will be no re-visit since each starting node is
        // a root node of sub'tree' in DSPT. That is, if statement within for loop might be useless.
        // Someone would be curious, e.g., then why do we have to use priority queue in step2 ?
        // Just for 'pink' case. Yeap. We need to identify all the other parent candidates are red or not
        // prior to checking current node.
        std::vector<Vertex> *children = childrenProperty_[vert];

        for (unsigned int i = 0; i < children->size(); i++) {
            pq.push(weight_vertex(-costProperty_[(*children)[i]], (*children)[i]));
        }
    }

    // 'pq' is empty at here.

    // <Step 3-a. Find best non-red parent for each red node.
    for (unsigned int i = 0; i < reds.size(); i++) {
        // TODO : need to be verified
        // Cost/predecessor initialization.
        costProperty_[reds[i]] = std::numeric_limits<double>::infinity();
        cancelAdoption(reds[i]);

        BGL_FORALL_OUTEDGES(reds[i], e, g_, Graph) {
            Vertex w = target(e, g_);
            double weight = weightProperty_[e].value();

            if (colorProperty_[w] == RED) {
                continue;  // If red, put aside for a while.
            }

            if (costProperty_[reds[i]] > costProperty_[w] + weight) {
                costProperty_[reds[i]] = costProperty_[w] + weight;
                predecessorProperty_[reds[i]] = w;
            }
        }

        if (predecessorProperty_[reds[i]] != NULL) {
            childrenProperty_[predecessorProperty_[reds[i]]]->push_back(reds[i]);
        }

        pq.push(weight_vertex(-costProperty_[reds[i]], reds[i]));
    }

    // <Step 3-b. Propagate the changes; rewiring for 'red' nodes whether it can replace
    // existing parent node of near neighbors.
    while (!pq.empty()) {
        weight_vertex top = pq.top();
        pq.pop();

        double cost = -top.first;
        Vertex vert = top.second;

        if (costProperty_[vert] < cost) {
            continue;
        }

        BGL_FORALL_OUTEDGES(vert, e, g_, Graph) {
            Vertex w = target(e, g_);
            double weight = weightProperty_[e].value();

            if (colorProperty_[w] != RED) {
                continue;  // If not red, then skip.
            }

            if (cost + weight < costProperty_[w]) {
                costProperty_[w] = cost + weight;

                cancelAdoption(w);

                predecessorProperty_[w] = vert;
                childrenProperty_[vert]->push_back(w);
                pq.push(weight_vertex(-costProperty_[w], w));
            }
        }
    }

    // The end!
    // colorProperty_ is not necessary to be cleansed out, just increase variable, 'RED'.
}

// TODO : sync between children & edges.
void ompl::geometric::HarmoniousLazyPRMstarMulti::cancelAdoption(const Vertex &child) {
    if (predecessorProperty_[child] == NULL)
        return;

    std::vector<Vertex> *children = childrenProperty_[predecessorProperty_[child]];

    for (unsigned int i = 0; i < children->size(); i++) if ((*children)[i] == child) {
            std::swap((*children)[i], children->back());
            children->pop_back();
            break;
        }

    predecessorProperty_[child] = NULL;
}

// Vertex first.
ompl::base::PathPtr ompl::geometric::HarmoniousLazyPRMstarMulti::constructSolution(const Vertex &start, const Vertex &goal) {
    std::vector<Vertex> solution_path;

    // Construct a solution from DSPT.
    for (Vertex vert = goal; vert != NULL; vert = predecessorProperty_[vert])
        solution_path.push_back(vert);

    if (solution_path.empty() || solution_path.size() == 1)
        return base::PathPtr();

    // Goal = 0, Start = n - 1.
    // TODO : From goal or start ? which one is better?

    // auto from = solution_path.rbegin();
    std::vector<Vertex>::reverse_iterator from = solution_path.rbegin();
    for (std::vector<Vertex>::reverse_iterator to = from + 1; to != solution_path.rend(); ++to) {
        Edge e = boost::lookup_edge(*from, *to, g_).first; // Exhaustive search O(E) at worst case.
        unsigned int &evd = edgeValidityProperty_[e];

        if ((evd & VALIDITY_TRUE) == 0) { // Unknown
            bool result = true;
            // double weight = weightProperty_[e].value();

            result &= checkMotion(stateProperty_[*from], stateProperty_[*to]);

            if (result) {
                evd |= VALIDITY_TRUE;
            }
            else {
                boost::remove_edge(e, g_); // O(log(E/V)) time...

                cancelAdoption(*to);
                Increase(*to);
                return base::PathPtr();
            }
        }

        from = to;
    }

    PathGeometric *p = new PathGeometric(si_);

    // Feasible path is found, fetch optimized edges if possible.

    for (std::vector<Vertex>::const_reverse_iterator sol = solution_path.rbegin(); sol != solution_path.rend(); ++sol) {
        p->append(stateProperty_[*sol]);
    }

    return base::PathPtr(p);
}

void ompl::geometric::HarmoniousLazyPRMstarMulti::getPlannerData(base::PlannerData &data) const {
    Planner::getPlannerData(data);
    // Caution : it handles directional information regardless of the search graph setting
    //           which is undirectional graph.
}

double ompl::geometric::HarmoniousLazyPRMstarMulti::distanceFunction(const base::State *a, const base::State *b) const {
    return si_->distance(a, b);
}

double ompl::geometric::HarmoniousLazyPRMstarMulti::distanceFunctionHarmonious(const Vertex a, const Vertex b) const {
    double base = distanceFunctionBase(stateProperty_[a], stateProperty_[b]);
    double arm =  distanceFunctionJoints(stateProperty_[a], stateProperty_[b]);

    return base + arm;
}

double ompl::geometric::HarmoniousLazyPRMstarMulti::distanceFunctionBase(const base::State *a, const base::State *b) const {
    double dist = 0.0;
    int dim = si_->getStateDimension();

    std::vector<double> ca, cb;
    si_->getStateSpace()->copyToReals(ca, a);
    si_->getStateSpace()->copyToReals(cb, b);

    for (int i = 0; i < NUM_BASE_DOF; i++) {
        double dd = fabs(ca[i]-cb[i]);

        if(isContinuous_[i]) {
            if (dd > boost::math::constants::pi<double>()) {
                dd = 2.0 * boost::math::constants::pi<double>() - dd;
            }
        }

        dist += hs_.params_.weights_[i] * dd * hs_.params_.weights_[i] * dd;
    }

    return std::sqrt(dist);
}

double ompl::geometric::HarmoniousLazyPRMstarMulti::distanceFunctionJoints(const base::State *a, const base::State *b) const {
    double dist = 0.0;

    std::vector<double> ca, cb;
    si_->getStateSpace()->copyToReals(ca, a);
    si_->getStateSpace()->copyToReals(cb, b);

    for (int i = NUM_BASE_DOF; i < ca.size(); i++) {
        double dd = fabs(ca[i]-cb[i]);

        if(isContinuous_[i+3]){
            if (dd > boost::math::constants::pi<double>()) {
                dd = 2.0 * boost::math::constants::pi<double>() - dd;
            }
        }

        dist += hs_.params_.weights_[i] * dd * hs_.params_.weights_[i] * dd;
    }

    return std::sqrt(dist);
}

bool ompl::geometric::HarmoniousLazyPRMstarMulti::checkMotion(base::State *s1, base::State *s2) const {
//    /* Assume motion starts/ends in a valid configuration so v1/v2 are valid */
    bool result = true;
    int dim = si_->getStateDimension();
    int nd;

    int nd_j = validSegmentCount_compare(distanceFunctionJoints(s1, s2), hs_.params_.factor_joint_);
    int nd_b = validSegmentCount_compare(distanceFunctionBase(s1, s2), hs_.params_.factor_base_);
    if(nd_j > nd_b){
        nd = nd_j;
    }
    else{
        nd = nd_b;
    }

    if (nd > 1) {
        /* Temporary storage for the checked state */
        base::State *test = si_->allocState();
        std::queue<std::pair<unsigned int, unsigned int> > q;
        q.push(std::make_pair(1, nd - 1));

        while (!q.empty()) {
            std::pair<unsigned int, unsigned int> range = q.front();
            unsigned int mid;

            mid = (range.first + range.second) / 2;
            interpolate(s1, s2, (double)mid / (double)nd, test);
//            si_->getStateSpace()->interpolate(s1, s2, (double)mid / (double)nd, test);

            if (!si_->isValid(test)) {
                result = false;
                break;
            }

            q.pop();
            if (range.first < mid)
                q.push(std::make_pair(range.first, mid - 1));
            if (mid < range.second) {
                q.push(std::make_pair(mid + 1, range.second));
            } // if mid == first, no more recursion.
        }
    }

    return result;
}

unsigned int ompl::geometric::HarmoniousLazyPRMstarMulti::validSegmentCount_compare(const double dist,
                                                                                const double longestValidSegment) const {
    return si_->getStateSpace()->getValidSegmentCountFactor() * (unsigned int)ceil(dist / longestValidSegment);
}

void ompl::geometric::HarmoniousLazyPRMstarMulti::interpolate(const base::State *from, const base::State *to, const double t, base::State *state) const
{
    const base::RealVectorStateSpace::StateType *rfrom = static_cast<const base::RealVectorStateSpace::StateType*>(from);
    const base::RealVectorStateSpace::StateType *rto = static_cast<const base::RealVectorStateSpace::StateType*>(to);
    const base::RealVectorStateSpace::StateType *rstate = static_cast<base::RealVectorStateSpace::StateType*>(state);
    for (unsigned int i = 0 ; i < isContinuous_.size() ; ++i){
        if(isContinuous_[i]){
            double diff = rto->values[i] - rfrom->values[i];
            if (fabs(diff) <= boost::math::constants::pi<double>())
                rstate->values[i] = rfrom->values[i] + diff * t;
            else
            {
                double &v = rstate->values[i];
                if (diff > 0.0)
                    diff = 2.0 * boost::math::constants::pi<double>() - diff;
                else
                    diff = -2.0 * boost::math::constants::pi<double>() - diff;
                v = rfrom->values[i] - diff * t;
                // input states are within bounds, so the following check is sufficient
                if (v > boost::math::constants::pi<double>())
                    v -= 2.0 * boost::math::constants::pi<double>();
                else
                if (v < -boost::math::constants::pi<double>())
                    v += 2.0 * boost::math::constants::pi<double>();
            }
        }
        else{
            rstate->values[i] = rfrom->values[i] + (rto->values[i] - rfrom->values[i]) * t;
        }
    }
}


/**
 * Copyright Soramitsu Co., Ltd. All Rights Reserved.
 * SPDX-License-Identifier: Apache-2.0
 */

#ifndef IROHA_SHARED_MODEL_AMOUNT_HPP
#define IROHA_SHARED_MODEL_AMOUNT_HPP

#include "interfaces/base/model_primitive.hpp"

#include <boost/multiprecision/cpp_int.hpp>
#include "interfaces/common_objects/types.hpp"

namespace shared_model {
  namespace interface {

    /**
     * Representation of fixed point number
     */
    class Amount final : public ModelPrimitive<Amount> {
     public:
      explicit Amount(const std::string &amount);
      explicit Amount(std::string &&amount);

      Amount(const Amount &o);
      Amount(Amount &&o) noexcept;

      /**
       * Gets integer representation value, which ignores precision
       * @return amount represented as integer value, which ignores precision
       */
      const boost::multiprecision::uint256_t &intValue() const;

      /**
       * Gets the position of precision
       * @return the position of precision
       */
      types::PrecisionType precision() const;

      /**
       * String representation.
       * @return string representation of the asset.
       */
      std::string toStringRepr() const;

      /**
       * Checks equality of objects inside
       * @param rhs - other wrapped value
       * @return true, if wrapped objects are same
       */
      bool operator==(const ModelType &rhs) const override;

      /**
       * Stringify the data.
       * @return the content of asset.
       */
      std::string toString() const override;

      Amount *clone() const override;

     private:
      const std::string amount_;

      interface::types::PrecisionType precision_;
      const boost::multiprecision::uint256_t multiprecision_repr_;
    };
  }  // namespace interface
}  // namespace shared_model
#endif  // IROHA_SHARED_MODEL_AMOUNT_HPP


///////////////////////////////////////////////////////////////
//  Copyright 2011 John Maddock. 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_

#define BOOST_CHRONO_HEADER_ONLY

#ifdef _MSC_VER
#  define _SCL_SECURE_NO_WARNINGS
#endif

#if !defined(TEST_MPF) && !defined(TEST_MPZ) && \
   !defined(TEST_CPP_DEC_FLOAT) && !defined(TEST_MPFR) && !defined(TEST_MPQ) \
   && !defined(TEST_TOMMATH) && !defined(TEST_TOMMATH_BOOST_RATIONAL) && !defined(TEST_MPZ_BOOST_RATIONAL)\
   && !defined(TEST_CPP_INT) && !defined(TEST_CPP_INT_RATIONAL) && !defined(TEST_CPP_BIN_FLOAT)
#  define TEST_MPF
#  define TEST_MPZ
#  define TEST_MPQ
#  define TEST_MPFR
#  define TEST_CPP_DEC_FLOAT
#  define TEST_MPQ
#  define TEST_TOMMATH
#  define TEST_CPP_INT
#  define TEST_CPP_INT_RATIONAL
#  define TEST_CPP_BIN_FLOAT

#ifdef _MSC_VER
#pragma message("CAUTION!!: No backend type specified so testing everything.... this will take some time!!")
#endif
#ifdef __GNUC__
#pragma warning "CAUTION!!: No backend type specified so testing everything.... this will take some time!!"
#endif

#endif

#if defined(TEST_MPF) || defined(TEST_MPZ) || defined(TEST_MPQ) || defined(TEST_MPZ_BOOST_RATIONAL)
#include <boost/multiprecision/gmp.hpp>
#include <boost/multiprecision/rational_adaptor.hpp>
#endif
#ifdef TEST_CPP_DEC_FLOAT
#include <boost/multiprecision/cpp_dec_float.hpp>
#endif
#ifdef TEST_CPP_BIN_FLOAT
#include <boost/multiprecision/cpp_bin_float.hpp>
#endif
#if defined(TEST_MPFR)
#include <boost/multiprecision/mpfr.hpp>
#endif
#if defined(TEST_TOMMATH) || defined(TEST_TOMMATH_BOOST_RATIONAL)
#include <boost/multiprecision/tommath.hpp>
#include <boost/multiprecision/rational_adaptor.hpp>
#endif
#if defined(TEST_CPP_INT) || defined(TEST_CPP_INT_RATIONAL)
#include <boost/multiprecision/cpp_int.hpp>
#endif

#include <boost/chrono.hpp>
#include <vector>
#include <map>
#include <string>
#include <cstring>
#include <cctype>
#include <iostream>
#include <iomanip>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int.hpp>

template <class Clock>
struct stopwatch
{
   typedef typename Clock::duration duration;
   stopwatch()
   {
      m_start = Clock::now();
   }
   duration elapsed()
   {
      return Clock::now() - m_start;
   }
   void reset()
   {
      m_start = Clock::now();
   }

private:
   typename Clock::time_point m_start;
};

unsigned bits_wanted; // for integer types

template <class T, int Type>
struct tester
{
   tester()
   {
      a.assign(500, 0);
      for(int i = 0; i < 500; ++i)
      {
         b.push_back(generate_random());
         c.push_back(generate_random());
         small.push_back(gen());
      }
   }
   double test_add()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for(unsigned i = 0; i < 1000; ++i)
      {
         for(unsigned i = 0; i < b.size(); ++i)
            a[i] = b[i] + c[i];
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_subtract()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for(unsigned i = 0; i < 1000; ++i)
      {
         for(unsigned i = 0; i < b.size(); ++i)
            a[i] = b[i] - c[i];
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_add_int()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for(unsigned i = 0; i < 1000; ++i)
      {
         for(unsigned i = 0; i < b.size(); ++i)
            a[i] = b[i] + 1;
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_subtract_int()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for(unsigned i = 0; i < 1000; ++i)
      {
         for(unsigned i = 0; i < b.size(); ++i)
            a[i] = b[i] - 1;
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_multiply()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for(unsigned i = 0; i < 1000; ++i)
      {
         for(unsigned k = 0; k < b.size(); ++k)
            a[k] = b[k] * c[k];
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_multiply_int()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for(unsigned i = 0; i < 1000; ++i)
      {
         for(unsigned i = 0; i < b.size(); ++i)
            a[i] = b[i] * 3;
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_divide()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for(unsigned i = 0; i < 1000; ++i)
      {
         for(unsigned i = 0; i < b.size(); ++i)
            a[i] = b[i] / c[i] + b[i] / small[i];
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_divide_int()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for(unsigned i = 0; i < 1000; ++i)
      {
         for(unsigned i = 0; i < b.size(); ++i)
            a[i] = b[i] / 3;
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_str(const boost::mpl::false_&)
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for(unsigned i = 0; i < b.size(); ++i)
         a[i] = boost::lexical_cast<T>(boost::lexical_cast<std::string>(b[i]));
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_str(const boost::mpl::true_&)
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for(unsigned i = 0; i < b.size(); ++i)
         a[i].assign(b[i].str());
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_str()
   {
      return test_str(boost::is_class<T>());
   }
   //
   // The following tests only work for integer types:
   //
   double test_mod()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for(unsigned i = 0; i < 1000; ++i)
      {
         for(unsigned i = 0; i < b.size(); ++i)
            a[i] = b[i] % c[i] + b[i] % small[i];
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_mod_int()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for(unsigned i = 0; i < 1000; ++i)
      {
         for(unsigned i = 0; i < b.size(); ++i)
            a[i] = b[i] % 254;
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_or()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for(unsigned i = 0; i < 1000; ++i)
      {
         for(unsigned i = 0; i < b.size(); ++i)
            a[i] = b[i] | c[i];
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_or_int()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for(unsigned i = 0; i < 1000; ++i)
      {
         for(unsigned i = 0; i < b.size(); ++i)
            a[i] = b[i] | 234;
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_and()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for(unsigned i = 0; i < 1000; ++i)
      {
         for(unsigned i = 0; i < b.size(); ++i)
            a[i] = b[i] & c[i];
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_and_int()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for(unsigned i = 0; i < 1000; ++i)
      {
         for(unsigned i = 0; i < b.size(); ++i)
            a[i] = b[i] & 234;
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_xor()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for(unsigned i = 0; i < 1000; ++i)
      {
         for(unsigned i = 0; i < b.size(); ++i)
            a[i] = b[i] ^ c[i];
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_xor_int()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for(unsigned i = 0; i < 1000; ++i)
      {
         for(unsigned i = 0; i < b.size(); ++i)
            a[i] = b[i] ^ 234;
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_complement()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for(unsigned i = 0; i < 1000; ++i)
      {
         for(unsigned i = 0; i < b.size(); ++i)
            a[i] = ~b[i];
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_left_shift()
   {
      int shift = std::numeric_limits<T>::is_bounded ? std::numeric_limits<T>::digits : bits_wanted;
      shift /= 2;
      stopwatch<boost::chrono::high_resolution_clock> w;
      for(unsigned i = 0; i < 1000; ++i)
      {
         for(unsigned i = 0; i < b.size(); ++i)
            a[i] = b[i] << shift;
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_right_shift()
   {
      int shift = std::numeric_limits<T>::is_bounded ? std::numeric_limits<T>::digits : bits_wanted;
      shift /= 2;
      stopwatch<boost::chrono::high_resolution_clock> w;
      for(unsigned i = 0; i < 1000; ++i)
      {
         for(unsigned i = 0; i < b.size(); ++i)
            a[i] = b[i] >> shift;
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_gcd()
   {
      using boost::math::gcd;
      stopwatch<boost::chrono::high_resolution_clock> w;
      for(unsigned i = 0; i < 1000; ++i)
      {
         for(unsigned i = 0; i < b.size(); ++i)
            a[i] = gcd(b[i], c[i]);
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_powm()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for(unsigned i = 0; i < 25; ++i)
      {
         for(unsigned i = 0; i < b.size(); ++i)
            a[i] = powm(b[i], b[i] / 2, c[i]);
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_construct()
   {
      std::allocator<T> a;
      T* pt = a.allocate(1000);
      stopwatch<boost::chrono::high_resolution_clock> w;
      for(unsigned i = 0; i < 1000; ++i)
      {
         for(unsigned i = 0; i < 1000; ++i)
            new(pt+i) T();
         for(unsigned i = 0; i < 1000; ++i)
            a.destroy(pt+i);
      }
      double result = boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
      a.deallocate(pt, 1000);
      return result;
   }
   double test_construct_unsigned()
   {
      std::allocator<T> a;
      T* pt = a.allocate(1000);
      stopwatch<boost::chrono::high_resolution_clock> w;
      for(unsigned i = 0; i < 1000; ++i)
      {
         for(unsigned i = 0; i < 1000; ++i)
            new(pt+i) T(i);
         for(unsigned i = 0; i < 1000; ++i)
            a.destroy(pt+i);
      }
      double result = boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
      a.deallocate(pt, 1000);
      return result;
   }
   double test_construct_unsigned_ll()
   {
      std::allocator<T> a;
      T* pt = a.allocate(1000);
      stopwatch<boost::chrono::high_resolution_clock> w;
      for(unsigned i = 0; i < 1000; ++i)
      {
         for(unsigned long long j = 0; j < 1000; ++j)
            new(pt+j) T(j);
         for(unsigned j = 0; j < 1000; ++j)
            a.destroy(pt+j);
      }
      double result = boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
      a.deallocate(pt, 1000);
      return result;
   }

   //
   // Hetero operations:
   //
   template <class U>
   static U get_hetero_test_value(boost::mpl::false_ const&)
   {
      return U(2) / 3;
   }
   template <class U>
   static U get_hetero_test_value(boost::mpl::true_ const&)
   {
      return (std::numeric_limits<U>::max)() >> 4;
   }
   template <class U>
   static U get_hetero_test_value()
   {
      return get_hetero_test_value<U>(boost::is_integral<U>());
   }
   template <class U>
   double test_multiply_hetero()
   {
      static const U val = get_hetero_test_value<U>();
      stopwatch<boost::chrono::high_resolution_clock> w;
      for(unsigned i = 0; i < 1000; ++i)
      {
         for(unsigned i = 0; i < b.size(); ++i)
            a[i] = b[i] * val;
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   template <class U>
   double test_inplace_multiply_hetero()
   {
      static const U val = get_hetero_test_value<U>();
      for(unsigned i = 0; i < b.size(); ++i)
         a[i] = b[i];
      stopwatch<boost::chrono::high_resolution_clock> w;
      for(unsigned i = 0; i < 1000; ++i)
      {
         for(unsigned i = 0; i < b.size(); ++i)
            a[i] *= val;
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   template <class U>
   double test_add_hetero()
   {
      static const U val = get_hetero_test_value<U>();
      stopwatch<boost::chrono::high_resolution_clock> w;
      for(unsigned i = 0; i < 1000; ++i)
      {
         for(unsigned i = 0; i < b.size(); ++i)
            a[i] = b[i] + val;
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   template <class U>
   double test_inplace_add_hetero()
   {
      static const U val = get_hetero_test_value<U>();
      for(unsigned i = 0; i < b.size(); ++i)
         a[i] = b[i];
      stopwatch<boost::chrono::high_resolution_clock> w;
      for(unsigned i = 0; i < 1000; ++i)
      {
         for(unsigned i = 0; i < b.size(); ++i)
            a[i] += val;
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   template <class U>
   double test_subtract_hetero()
   {
      static const U val = get_hetero_test_value<U>();
      stopwatch<boost::chrono::high_resolution_clock> w;
      for(unsigned i = 0; i < 1000; ++i)
      {
         for(unsigned i = 0; i < b.size(); ++i)
            a[i] = b[i] - val;
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   template <class U>
   double test_inplace_subtract_hetero()
   {
      static const U val = get_hetero_test_value<U>();
      for(unsigned i = 0; i < b.size(); ++i)
         a[i] = b[i];
      stopwatch<boost::chrono::high_resolution_clock> w;
      for(unsigned i = 0; i < 1000; ++i)
      {
         for(unsigned i = 0; i < b.size(); ++i)
            a[i] -= val;
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   template <class U>
   double test_divide_hetero()
   {
      static const U val = get_hetero_test_value<U>();
      stopwatch<boost::chrono::high_resolution_clock> w;
      for(unsigned i = 0; i < 1000; ++i)
      {
         for(unsigned i = 0; i < b.size(); ++i)
            a[i] = b[i] / val;
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   template <class U>
   double test_inplace_divide_hetero()
   {
      static const U val = get_hetero_test_value<U>();
      for(unsigned i = 0; i < b.size(); ++i)
         a[i] = b[i];
      stopwatch<boost::chrono::high_resolution_clock> w;
      for(unsigned i = 0; i < 1000; ++i)
      {
         for(unsigned i = 0; i < b.size(); ++i)
            a[i] /= val;
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
private:
   T generate_random()
   {
      return generate_random(boost::mpl::int_<Type>());
   }
   T generate_random(const boost::mpl::int_<boost::multiprecision::number_kind_floating_point>&)
   {
      T val = gen();
      T prev_val = -1;
      while(val != prev_val)
      {
         val *= (gen.max)();
         prev_val = val;
         val += gen();
      }
      int e;
      val = frexp(val, &e);

      typedef typename T::backend_type::exponent_type e_type;
      static boost::random::uniform_int_distribution<e_type> ui(-30, 30);
      return ldexp(val, static_cast<int>(ui(gen)));
   }
   T generate_random(const boost::mpl::int_<boost::multiprecision::number_kind_integer>&)
   {
      typedef boost::random::mt19937::result_type random_type;

      T max_val;
      unsigned digits;
      if(std::numeric_limits<T>::is_bounded)
      {
         max_val = (std::numeric_limits<T>::max)();
         digits = std::numeric_limits<T>::digits;
      }
      else
      {
         max_val = T(1) << bits_wanted;
         digits = bits_wanted;
      }

      unsigned bits_per_r_val = std::numeric_limits<random_type>::digits - 1;
      while((random_type(1) << bits_per_r_val) > (gen.max)()) --bits_per_r_val;

      unsigned terms_needed = digits / bits_per_r_val + 1;

      T val = 0;
      for(unsigned i = 0; i < terms_needed; ++i)
      {
         val *= (gen.max)();
         val += gen();
      }
      val %= max_val;
      return val;
   }
   T generate_random(const boost::mpl::int_<boost::multiprecision::number_kind_rational>&)
   {
      typedef boost::random::mt19937::result_type random_type;
      typedef typename boost::multiprecision::component_type<T>::type IntType;

      IntType max_val;
      unsigned digits;
      if(std::numeric_limits<IntType>::is_bounded)
      {
         max_val = (std::numeric_limits<IntType>::max)();
         digits = std::numeric_limits<IntType>::digits;
      }
      else
      {
         max_val = IntType(1) << bits_wanted;
         digits = bits_wanted;
      }

      unsigned bits_per_r_val = std::numeric_limits<random_type>::digits - 1;
      while((random_type(1) << bits_per_r_val) > (gen.max)()) --bits_per_r_val;

      unsigned terms_needed = digits / bits_per_r_val + 1;

      IntType val = 0;
      IntType denom = 0;
      for(unsigned i = 0; i < terms_needed; ++i)
      {
         val *= (gen.max)();
         val += gen();
      }
      for(unsigned i = 0; i < terms_needed; ++i)
      {
         denom *= (gen.max)();
         denom += gen();
      }
      if(denom == 0)
         denom = 1;
      val %= max_val;
      denom %= max_val;
      return T(val, denom);
   }
   std::vector<T> a, b, c, small;
   static boost::random::mt19937 gen;
};

template <class N, int V>
boost::random::mt19937 tester<N, V>::gen;

const char* category_name(const boost::mpl::int_<boost::multiprecision::number_kind_integer>&)
{
   return "integer";
}
const char* category_name(const boost::mpl::int_<boost::multiprecision::number_kind_floating_point>&)
{
   return "float";
}
const char* category_name(const boost::mpl::int_<boost::multiprecision::number_kind_rational>&)
{
   return "rational";
}

//
// Keys in order are:
// Category
// Operator
// Type
// Precision
// Time
//
std::map<std::string, std::map<std::string, std::map<std::string, std::map<int, double> > > > result_table;

void report_result(const char* cat, const char* type, const char* op, unsigned precision, double time)
{
   std::cout << std::left << std::setw(15) << type << std::setw(10) << precision << std::setw(35) << op << time << std::endl;
   result_table[cat][op][type][precision] = time;
}

template <class Number, int N>
void test_int_ops(tester<Number, N>& t, const char* type, unsigned precision, const boost::mpl::int_<boost::multiprecision::number_kind_integer>&)
{
   const char* cat = "integer";
   report_result(cat, type, "%", precision, t.test_mod());
   report_result(cat, type, "|", precision, t.test_or());
   report_result(cat, type, "&", precision, t.test_and());
   report_result(cat, type, "^", precision, t.test_xor());
   //report_result(cat, type, "~", precision, t.test_complement());
   report_result(cat, type, "<<", precision, t.test_left_shift());
   report_result(cat, type, ">>", precision, t.test_right_shift());
   // integer ops:
   report_result(cat, type, "%(int)", precision, t.test_mod_int());
   report_result(cat, type, "|(int)", precision, t.test_or_int());
   report_result(cat, type, "&(int)", precision, t.test_and_int());
   report_result(cat, type, "^(int)", precision, t.test_xor_int());
   report_result(cat, type, "gcd", precision, t.test_gcd());
   report_result(cat, type, "powm", precision, t.test_powm());
}
template <class Number, int N, class U>
void test_int_ops(tester<Number, N>& t, const char* type, unsigned precision, const U&)
{
}

template <class Number>
void test(const char* type, unsigned precision)
{
   bits_wanted = precision;
   tester<Number, boost::multiprecision::number_category<Number>::value> t;
   const char* cat = category_name(typename boost::multiprecision::number_category<Number>::type());
   //
   // call t.test_multiply() first so that the destination operands are
   // forced to perform whatever memory allocation may be needed.  That way
   // we measure only algorithm performance, and not memory allocation effects.
   //
   t.test_multiply();
   //
   // Now the actual tests:
   //
   report_result(cat, type, "+", precision, t.test_add());
   report_result(cat, type, "-", precision, t.test_subtract());
   report_result(cat, type, "*", precision, t.test_multiply());
   report_result(cat, type, "/", precision, t.test_divide());
   report_result(cat, type, "str", precision, t.test_str());
   // integer ops:
   report_result(cat, type, "+(int)", precision, t.test_add_int());
   report_result(cat, type, "-(int)", precision, t.test_subtract_int());
   report_result(cat, type, "*(int)", precision, t.test_multiply_int());
   report_result(cat, type, "/(int)", precision, t.test_divide_int());
   // construction and destruction:
   report_result(cat, type, "construct", precision, t.test_construct());
   report_result(cat, type, "construct(unsigned)", precision, t.test_construct_unsigned());
   report_result(cat, type, "construct(unsigned long long)", precision, t.test_construct_unsigned_ll());
   test_int_ops(t, type, precision, typename boost::multiprecision::number_category<Number>::type());
   // Hetero ops:
   report_result(cat, type, "+(unsigned long long)", precision, t.template test_add_hetero<unsigned long long>());
   report_result(cat, type, "-(unsigned long long)", precision, t.template test_subtract_hetero<unsigned long long>());
   report_result(cat, type, "*(unsigned long long)", precision, t.template test_multiply_hetero<unsigned long long>());
   report_result(cat, type, "/(unsigned long long)", precision, t.template test_divide_hetero<unsigned long long>());
   report_result(cat, type, "+=(unsigned long long)", precision, t.template test_inplace_add_hetero<unsigned long long>());
   report_result(cat, type, "-=(unsigned long long)", precision, t.template test_inplace_subtract_hetero<unsigned long long>());
   report_result(cat, type, "*=(unsigned long long)", precision, t.template test_inplace_multiply_hetero<unsigned long long>());
   report_result(cat, type, "/=(unsigned long long)", precision, t.template test_inplace_divide_hetero<unsigned long long>());
}

void quickbook_results()
{
   //
   // Keys in order are:
   // Category
   // Operator
   // Type
   // Precision
   // Time
   //
   typedef std::map<std::string, std::map<std::string, std::map<std::string, std::map<int, double> > > >::const_iterator  category_iterator;
   typedef std::map<std::string, std::map<std::string, std::map<int, double> > >::const_iterator                          operator_iterator;
   typedef std::map<std::string, std::map<int, double> >::const_iterator                                                  type_iterator;
   typedef std::map<int, double>::const_iterator                                                                          precision_iterator;

   for(category_iterator i = result_table.begin(); i != result_table.end(); ++i)
   {
      std::string cat = i->first;
      cat[0] = std::toupper(cat[0]);
      std::cout << "[section:" << i->first << "_performance " << cat << " Type Perfomance]" << std::endl;

      for(operator_iterator j = i->second.begin(); j != i->second.end(); ++j)
      {
         std::string op = j->first;
         std::cout << "[table Operator " << op << std::endl;
         std::cout << "[[Backend]";

         for(precision_iterator k = j->second.begin()->second.begin(); k != j->second.begin()->second.end(); ++k)
         {
            std::cout << "[" << k->first << " Bits]";
         }
         std::cout << "]\n";

         std::vector<double> best_times(j->second.begin()->second.size(), (std::numeric_limits<double>::max)());
         for(unsigned m = 0; m < j->second.begin()->second.size(); ++m)
         {
            for(type_iterator k = j->second.begin(); k != j->second.end(); ++k)
            {
               precision_iterator l = k->second.begin();
               std::advance(l, m);
               if(best_times[m] > l->second)
                  best_times[m] = l->second ? l->second : best_times[m];
            }
         }

         for(type_iterator k = j->second.begin(); k != j->second.end(); ++k)
         {
            std::cout << "[[" << k->first << "]";

            unsigned m = 0;
            for(precision_iterator l = k->second.begin(); l != k->second.end(); ++l)
            {
               double rel_time = l->second / best_times[m];
               if(rel_time == 1)
                  std::cout << "[[*" << rel_time << "]";
               else
                  std::cout << "[" << rel_time;
               std::cout << " (" << l->second << "s)]";
               ++m;
            }

            std::cout << "]\n";
         }

         std::cout << "]\n";
      }

      std::cout << "[endsect]" << std::endl;
   }
}

int main()
{
#ifdef TEST_INT64
   test<boost::uint64_t>("boost::uint64_t", 64);
#endif
#ifdef TEST_MPF
   test<boost::multiprecision::mpf_float_50>("gmp_float", 50);
   test<boost::multiprecision::mpf_float_100>("gmp_float", 100);
   test<boost::multiprecision::mpf_float_500>("gmp_float", 500);
#endif
#ifdef TEST_MPZ
   test<boost::multiprecision::mpz_int>("gmp_int", 128);
   test<boost::multiprecision::mpz_int>("gmp_int", 256);
   test<boost::multiprecision::mpz_int>("gmp_int", 512);
   test<boost::multiprecision::mpz_int>("gmp_int", 1024);
#endif
#ifdef TEST_CPP_INT
   //test<boost::multiprecision::number<boost::multiprecision::cpp_int_backend<64, 64, boost::multiprecision::unsigned_magnitude, boost::multiprecision::unchecked, void>, boost::multiprecision::et_off> >("cpp_int(unsigned, fixed)", 64);
   //test<boost::multiprecision::number<boost::multiprecision::cpp_int_backend<64, 64, boost::multiprecision::signed_magnitude, boost::multiprecision::unchecked, void>, boost::multiprecision::et_off> >("cpp_int(fixed)", 64);
   test<boost::multiprecision::number<boost::multiprecision::cpp_int_backend<128, 128, boost::multiprecision::signed_magnitude, boost::multiprecision::unchecked, void>, boost::multiprecision::et_off> >("cpp_int(fixed)", 128);
   test<boost::multiprecision::number<boost::multiprecision::cpp_int_backend<256, 256, boost::multiprecision::signed_magnitude, boost::multiprecision::unchecked, void>, boost::multiprecision::et_off> >("cpp_int(fixed)", 256);
   test<boost::multiprecision::number<boost::multiprecision::cpp_int_backend<512, 512, boost::multiprecision::signed_magnitude, boost::multiprecision::unchecked, void>, boost::multiprecision::et_off> >("cpp_int(fixed)", 512);
   test<boost::multiprecision::number<boost::multiprecision::cpp_int_backend<1024, 1024, boost::multiprecision::signed_magnitude, boost::multiprecision::unchecked, void>, boost::multiprecision::et_off> >("cpp_int(fixed)", 1024);

   test<boost::multiprecision::cpp_int>("cpp_int", 128);
   test<boost::multiprecision::cpp_int>("cpp_int", 256);
   test<boost::multiprecision::cpp_int>("cpp_int", 512);
   test<boost::multiprecision::cpp_int>("cpp_int", 1024);
#endif
#ifdef TEST_CPP_INT_RATIONAL
   test<boost::multiprecision::cpp_rational>("cpp_rational", 128);
   test<boost::multiprecision::cpp_rational>("cpp_rational", 256);
   test<boost::multiprecision::cpp_rational>("cpp_rational", 512);
   test<boost::multiprecision::cpp_rational>("cpp_rational", 1024);
#endif
#ifdef TEST_MPQ
   test<boost::multiprecision::mpq_rational>("mpq_rational", 128);
   test<boost::multiprecision::mpq_rational>("mpq_rational", 256);
   test<boost::multiprecision::mpq_rational>("mpq_rational", 512);
   test<boost::multiprecision::mpq_rational>("mpq_rational", 1024);
#endif
#ifdef TEST_TOMMATH
   test<boost::multiprecision::tom_int>("tommath_int", 128);
   test<boost::multiprecision::tom_int>("tommath_int", 256);
   test<boost::multiprecision::tom_int>("tommath_int", 512);
   test<boost::multiprecision::tom_int>("tommath_int", 1024);
   /*
   //
   // These are actually too slow to test!!!
   //
   test<boost::multiprecision::tom_rational>("tom_rational", 128);
   test<boost::multiprecision::tom_rational>("tom_rational", 256);
   test<boost::multiprecision::tom_rational>("tom_rational", 512);
   test<boost::multiprecision::tom_rational>("tom_rational", 1024);
   */
#endif
#ifdef TEST_CPP_DEC_FLOAT
   test<boost::multiprecision::cpp_dec_float_50>("cpp_dec_float", 50);
   test<boost::multiprecision::cpp_dec_float_100>("cpp_dec_float", 100);
   test<boost::multiprecision::number<boost::multiprecision::cpp_dec_float<500> > >("cpp_dec_float", 500);
#endif
#ifdef TEST_CPP_BIN_FLOAT
   test<boost::multiprecision::cpp_bin_float_50>("cpp_bin_float", 50);
   test<boost::multiprecision::cpp_bin_float_100>("cpp_bin_float", 100);
   test<boost::multiprecision::number<boost::multiprecision::cpp_bin_float<500> >::digits> > >("cpp_bin_float", 500);
#endif
#ifdef TEST_MPFR
   test<boost::multiprecision::mpfr_float_50>("mpfr_float", 50);
   test<boost::multiprecision::mpfr_float_100>("mpfr_float", 100);
   test<boost::multiprecision::mpfr_float_500>("mpfr_float", 500);
#endif
   quickbook_results();
   return 0;
}