#include <sstream>
#include <iostream>
#include <vector>
#include <cassert>
#include <cmath>

#include "config.h"
#ifdef HAVE_MPI
#include <boost/mpi/timer.hpp>
#include <boost/mpi.hpp>
namespace mpi = boost::mpi;
#endif

#include <boost/shared_ptr.hpp>
#include <boost/program_options.hpp>
#include <boost/program_options/variables_map.hpp>

#include "verbose.h"
#include "hg.h"
#include "prob.h"
#include "inside_outside.h"
#include "ff_register.h"
#include "decoder.h"
#include "filelib.h"
#include "optimize.h"
#include "fdict.h"
#include "weights.h"
#include "sparse_vector.h"

using namespace std;
using boost::shared_ptr;
namespace po = boost::program_options;

void SanityCheck(const vector<double>& w) {
  for (int i = 0; i < w.size(); ++i) {
    assert(!isnan(w[i]));
    assert(!isinf(w[i]));
  }
}

struct FComp {
  const vector<double>& w_;
  FComp(const vector<double>& w) : w_(w) {}
  bool operator()(int a, int b) const {
    return fabs(w_[a]) > fabs(w_[b]);
  }
};

void ShowLargestFeatures(const vector<double>& w) {
  vector<int> fnums(w.size());
  for (int i = 0; i < w.size(); ++i)
    fnums[i] = i;
  vector<int>::iterator mid = fnums.begin();
  mid += (w.size() > 10 ? 10 : w.size());
  partial_sort(fnums.begin(), mid, fnums.end(), FComp(w));
  cerr << "TOP FEATURES:";
  for (vector<int>::iterator i = fnums.begin(); i != mid; ++i) {
    cerr << ' ' << FD::Convert(*i) << '=' << w[*i];
  }
  cerr << endl;
}

bool InitCommandLine(int argc, char** argv, po::variables_map* conf) {
  po::options_description opts("Configuration options");
  opts.add_options()
        ("input_weights,w",po::value<string>(),"Input feature weights file")
        ("training_data,t",po::value<string>(),"Training data")
        ("decoder_config,d",po::value<string>(),"Decoder configuration file")
        ("sharded_input,s",po::value<string>(), "Corpus and grammar files are 'sharded' so each processor loads its own input and grammar file. Argument is the directory containing the shards.")
        ("output_weights,o",po::value<string>()->default_value("-"),"Output feature weights file")
        ("optimization_method,m", po::value<string>()->default_value("lbfgs"), "Optimization method (sgd, lbfgs, rprop)")
	("correction_buffers,M", po::value<int>()->default_value(10), "Number of gradients for LBFGS to maintain in memory")
        ("gaussian_prior,p","Use a Gaussian prior on the weights")
        ("means,u", po::value<string>(), "File containing the means for Gaussian prior")
        ("sigma_squared", po::value<double>()->default_value(1.0), "Sigma squared term for spherical Gaussian prior");
  po::options_description clo("Command line options");
  clo.add_options()
        ("config", po::value<string>(), "Configuration file")
        ("help,h", "Print this help message and exit");
  po::options_description dconfig_options, dcmdline_options;
  dconfig_options.add(opts);
  dcmdline_options.add(opts).add(clo);

  po::store(parse_command_line(argc, argv, dcmdline_options), *conf);
  if (conf->count("config")) {
    ifstream config((*conf)["config"].as<string>().c_str());
    po::store(po::parse_config_file(config, dconfig_options), *conf);
  }
  po::notify(*conf);

  if (conf->count("help") || !conf->count("input_weights") || !(conf->count("training_data") | conf->count("sharded_input")) || !conf->count("decoder_config")) {
    cerr << dcmdline_options << endl;
    return false;
  }
  if (conf->count("training_data") && conf->count("sharded_input")) {
    cerr << "Cannot specify both --training_data and --sharded_input\n";
    return false;
  }
  return true;
}

void ReadTrainingCorpus(const string& fname, int rank, int size, vector<string>* c) {
  ReadFile rf(fname);
  istream& in = *rf.stream();
  string line;
  int lc = 0;
  while(in) {
    getline(in, line);
    if (!in) break;
    if (lc % size == rank) c->push_back(line);
    ++lc;
  }
}

static const double kMINUS_EPSILON = -1e-6;

struct TrainingObserver : public DecoderObserver {
  void Reset() {
    acc_grad.clear();
    acc_obj = 0;
    total_complete = 0;
  }

  void SetLocalGradientAndObjective(vector<double>* g, double* o) const {
    *o = acc_obj;
    for (SparseVector<prob_t>::const_iterator it = acc_grad.begin(); it != acc_grad.end(); ++it)
      (*g)[it->first] = it->second;
  }

  virtual void NotifyDecodingStart(const SentenceMetadata& smeta) {
    cur_model_exp.clear();
    cur_obj = 0;
    state = 1;
  }

  // compute model expectations, denominator of objective
  virtual void NotifyTranslationForest(const SentenceMetadata& smeta, Hypergraph* hg) {
    assert(state == 1);
    state = 2;
    const prob_t z = InsideOutside<prob_t,
                                   EdgeProb,
                                   SparseVector<prob_t>,
                                   EdgeFeaturesAndProbWeightFunction>(*hg, &cur_model_exp);
    cur_obj = log(z);
    cur_model_exp /= z;
  }

  // compute "empirical" expectations, numerator of objective
  virtual void NotifyAlignmentForest(const SentenceMetadata& smeta, Hypergraph* hg) {
    assert(state == 2);
    state = 3;
    SparseVector<prob_t> ref_exp;
    const prob_t ref_z = InsideOutside<prob_t,
                                       EdgeProb,
                                       SparseVector<prob_t>,
                                       EdgeFeaturesAndProbWeightFunction>(*hg, &ref_exp);
    ref_exp /= ref_z;

    double log_ref_z;
#if 0
    if (crf_uniform_empirical) {
      log_ref_z = ref_exp.dot(feature_weights);
    } else {
      log_ref_z = log(ref_z);
    }
#else
    log_ref_z = log(ref_z);
#endif

    // rounding errors means that <0 is too strict
    if ((cur_obj - log_ref_z) < kMINUS_EPSILON) {
      cerr << "DIFF. ERR! log_model_z < log_ref_z: " << cur_obj << " " << log_ref_z << endl;
      exit(1);
    }
    assert(!isnan(log_ref_z));
    ref_exp -= cur_model_exp;
    acc_grad -= ref_exp;
    acc_obj += (cur_obj - log_ref_z);
  }

  virtual void NotifyDecodingComplete(const SentenceMetadata& smeta) {
    if (state == 3) {
      ++total_complete;
    } else {
    }
  }

  int total_complete;
  SparseVector<prob_t> cur_model_exp;
  SparseVector<prob_t> acc_grad;
  double acc_obj;
  double cur_obj;
  int state;
};

void ReadConfig(const string& ini, vector<string>* out) {
  ReadFile rf(ini);
  istream& in = *rf.stream();
  while(in) {
    string line;
    getline(in, line);
    if (!in) continue;
    out->push_back(line);
  }
}

void StoreConfig(const vector<string>& cfg, istringstream* o) {
  ostringstream os;
  for (int i = 0; i < cfg.size(); ++i) { os << cfg[i] << endl; }
  o->str(os.str());
}

template <typename T>
struct VectorPlus : public binary_function<vector<T>, vector<T>, vector<T> >  {
  vector<T> operator()(const vector<int>& a, const vector<int>& b) const {
    assert(a.size() == b.size());
    vector<T> v(a.size());
    transform(a.begin(), a.end(), b.begin(), v.begin(), plus<T>());
    return v;
  }
};

int main(int argc, char** argv) {
#ifdef HAVE_MPI
  mpi::environment env(argc, argv);
  mpi::communicator world;
  const int size = world.size();
  const int rank = world.rank();
#else
  const int size = 1;
  const int rank = 0;
#endif
  SetSilent(true);  // turn off verbose decoder output
  register_feature_functions();

  po::variables_map conf;
  if (!InitCommandLine(argc, argv, &conf)) return 1;

  string shard_dir;
  if (conf.count("sharded_input")) {
    shard_dir = conf["sharded_input"].as<string>();
    if (!DirectoryExists(shard_dir)) {
      if (rank == 0) cerr << "Can't find shard directory: " << shard_dir << endl;
      return 1;
    }
    if (rank == 0)
      cerr << "Shard directory: " << shard_dir << endl;
  }

  // load initial weights
  Weights weights;
  if (rank == 0) { cerr << "Loading weights...\n"; }
  weights.InitFromFile(conf["input_weights"].as<string>());
  if (rank == 0) { cerr << "Done loading weights.\n"; }

  // freeze feature set (should be optional?)
  const bool freeze_feature_set = true;
  if (freeze_feature_set) FD::Freeze();

  // load cdec.ini and set up decoder
  vector<string> cdec_ini;
  ReadConfig(conf["decoder_config"].as<string>(), &cdec_ini);
  if (shard_dir.size()) {
    if (rank == 0) {
      for (int i = 0; i < cdec_ini.size(); ++i) {
        if (cdec_ini[i].find("grammar=") == 0) {
          cerr << "!!! using sharded input and " << conf["decoder_config"].as<string>() << " contains a grammar specification:\n" << cdec_ini[i] << "\n  VERIFY THAT THIS IS CORRECT!\n";
        }
      }
    }
    ostringstream g;
    g << "grammar=" << shard_dir << "/grammar." << rank << "_of_" << size << ".gz";
    cdec_ini.push_back(g.str());
  }
  istringstream ini;
  StoreConfig(cdec_ini, &ini);
  if (rank == 0) cerr << "Loading grammar...\n";
  Decoder* decoder = new Decoder(&ini);
  if (decoder->GetConf()["input"].as<string>() != "-") {
    cerr << "cdec.ini must not set an input file\n";
    return 1;
  }
  if (rank == 0) cerr << "Done loading grammar!\n";

  const int num_feats = FD::NumFeats();
  if (rank == 0) cerr << "Number of features: " << num_feats << endl;
  const bool gaussian_prior = conf.count("gaussian_prior");
  vector<double> means(num_feats, 0);
  if (conf.count("means")) {
    if (!gaussian_prior) {
      cerr << "Don't use --means without --gaussian_prior!\n";
      exit(1);
    }
    Weights wm;
    wm.InitFromFile(conf["means"].as<string>());
    if (num_feats != FD::NumFeats()) {
      cerr << "[ERROR] Means file had unexpected features!\n";
      exit(1);
    }
    wm.InitVector(&means);
  }
  shared_ptr<BatchOptimizer> o;
  if (rank == 0) {
    const string omethod = conf["optimization_method"].as<string>();
    if (omethod == "rprop")
      o.reset(new RPropOptimizer(num_feats));  // TODO add configuration
    else
      o.reset(new LBFGSOptimizer(num_feats, conf["correction_buffers"].as<int>()));
    cerr << "Optimizer: " << o->Name() << endl;
  }
  double objective = 0;
  vector<double> lambdas(num_feats, 0.0);
  weights.InitVector(&lambdas);
  if (lambdas.size() != num_feats) {
    cerr << "Initial weights file did not have all features specified!\n  feats="
         << num_feats << "\n  weights file=" << lambdas.size() << endl;
    lambdas.resize(num_feats, 0.0);
  }
  vector<double> gradient(num_feats, 0.0);
  vector<double> rcv_grad(num_feats, 0.0);
  bool converged = false;

  vector<string> corpus;
  if (shard_dir.size()) {
    ostringstream os; os << shard_dir << "/corpus." << rank << "_of_" << size;
    ReadTrainingCorpus(os.str(), 0, 1, &corpus);
    cerr << os.str() << " has " << corpus.size() << " training examples. " << endl;
    if (corpus.size() > 500) { corpus.resize(500); cerr << "  TRUNCATING\n"; }
  } else {
    ReadTrainingCorpus(conf["training_data"].as<string>(), rank, size, &corpus);
  }
  assert(corpus.size() > 0);

  TrainingObserver observer;
  while (!converged) {
    observer.Reset();
#ifdef HAVE_MPI
    mpi::timer timer;
    world.barrier();
#endif
    if (rank == 0) {
      cerr << "Starting decoding... (~" << corpus.size() << " sentences / proc)\n";
    }
    decoder->SetWeights(lambdas);
    for (int i = 0; i < corpus.size(); ++i)
      decoder->Decode(corpus[i], &observer);
    cerr << "  process " << rank << '/' << size << " done\n";
    fill(gradient.begin(), gradient.end(), 0);
    fill(rcv_grad.begin(), rcv_grad.end(), 0);
    observer.SetLocalGradientAndObjective(&gradient, &objective);

    double to = 0;
#ifdef HAVE_MPI
    mpi::reduce(world, &gradient[0], gradient.size(), &rcv_grad[0], plus<double>(), 0);
    mpi::reduce(world, objective, to, plus<double>(), 0);
    swap(gradient, rcv_grad);
    objective = to;
#endif

    if (rank == 0) {  // run optimizer only on rank=0 node
      if (gaussian_prior) {
        const double sigsq = conf["sigma_squared"].as<double>();
        double norm = 0;
        for (int k = 1; k < lambdas.size(); ++k) {
          const double& lambda_k = lambdas[k];
          if (lambda_k) {
            const double param = (lambda_k - means[k]);
            norm += param * param;
            gradient[k] += param / sigsq;
          }
        }
        const double reg = norm / (2.0 * sigsq);
        cerr << "REGULARIZATION TERM: " << reg << endl;
        objective += reg;
      }
      cerr << "EVALUATION #" << o->EvaluationCount() << " OBJECTIVE: " << objective << endl;
      double gnorm = 0;
      for (int i = 0; i < gradient.size(); ++i)
        gnorm += gradient[i] * gradient[i];
      cerr << "  GNORM=" << sqrt(gnorm) << endl;
      vector<double> old = lambdas;
      int c = 0;
      while (old == lambdas) {
        ++c;
        if (c > 1) { cerr << "Same lambdas, repeating optimization\n"; }
        o->Optimize(objective, gradient, &lambdas);
        assert(c < 5);
      }
      old.clear();
      SanityCheck(lambdas);
      ShowLargestFeatures(lambdas);
      weights.InitFromVector(lambdas);

      converged = o->HasConverged();
      if (converged) { cerr << "OPTIMIZER REPORTS CONVERGENCE!\n"; }

      string fname = "weights.cur.gz";
      if (converged) { fname = "weights.final.gz"; }
      ostringstream vv;
      vv << "Objective = " << objective << "  (eval count=" << o->EvaluationCount() << ")";
      const string svv = vv.str();
      weights.WriteToFile(fname, true, &svv);
    }  // rank == 0
    int cint = converged;
#ifdef HAVE_MPI
    mpi::broadcast(world, &lambdas[0], lambdas.size(), 0);
    mpi::broadcast(world, cint, 0);
    if (rank == 0) { cerr << "  ELAPSED TIME THIS ITERATION=" << timer.elapsed() << endl; }
#endif
    converged = cint;
  }
  return 0;
}


// Copyright (c) 2000-2020, Heiko Bauke
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//
//   * Redistributions of source code must retain the above copyright
//     notice, this list of conditions and the following disclaimer.
//
//   * Redistributions in binary form must reproduce the above
//     copyright notice, this list of conditions and the following
//     disclaimer in the documentation and/or other materials provided
//     with the distribution.
//
//   * Neither the name of the copyright holder nor the names of its
//     contributors may be used to endorse or promote products derived
//     from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
// COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
// INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
// OF THE POSSIBILITY OF SUCH DAMAGE.

#include <vector>
#include <string>
#include <sstream>
#include <tuple>
#include <ciso646>

#define BOOST_MPL_CFG_NO_PREPROCESSED_HEADERS
#define BOOST_MPL_LIMIT_LIST_SIZE 30
#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include <boost/mpl/list.hpp>

#include <trng/lcg64.hpp>
#include <trng/lcg64_shift.hpp>
#include <trng/mrg2.hpp>
#include <trng/mrg3.hpp>
#include <trng/mrg3s.hpp>
#include <trng/mrg4.hpp>
#include <trng/mrg5.hpp>
#include <trng/mrg5s.hpp>
#include <trng/yarn2.hpp>
#include <trng/yarn3.hpp>
#include <trng/yarn3s.hpp>
#include <trng/yarn4.hpp>
#include <trng/yarn5.hpp>
#include <trng/yarn5s.hpp>
#include <trng/lagfib2xor.hpp>
#include <trng/lagfib4xor.hpp>
#include <trng/lagfib2plus.hpp>
#include <trng/lagfib4plus.hpp>
#include <trng/mt19937.hpp>
#include <trng/mt19937_64.hpp>
#include <trng/xoshiro256plus.hpp>

#include "type_names.hpp"

using engines =
    boost::mpl::list<trng::lcg64, trng::lcg64_shift,  //
                     trng::mrg2, trng::mrg3, trng::mrg3s, trng::mrg4, trng::mrg5,
                     trng::mrg5s,  //
                     trng::yarn2, trng::yarn3, trng::yarn3s, trng::yarn4, trng::yarn5,
                     trng::yarn5s,                                        //
                     trng::lagfib2xor_521_64, trng::lagfib4xor_521_32,    //
                     trng::lagfib2plus_521_32, trng::lagfib4plus_521_64,  //
                     trng::mt19937, trng::mt19937_64,                     //
                     trng::xoshiro256plus>;

using parallel_engines =
    boost::mpl::list<trng::lcg64, trng::lcg64_shift,  //
                     trng::mrg2, trng::mrg3, trng::mrg3s, trng::mrg4, trng::mrg5,
                     trng::mrg5s,  //
                     trng::yarn2, trng::yarn3, trng::yarn3s, trng::yarn4, trng::yarn5,
                     trng::yarn5s>;

using floats = boost::mpl::list<float, double, long double>;

template<typename R>
void advance_engine(R &r1, const long N) {
  for (long i{0}; i < N; ++i)
    r1();
}

template<typename R>
void advance_engine(R &r1, R &r2, const long N) {
  for (long i{0}; i < N; ++i) {
    r1();
    r2();
  }
}

template<typename R>
std::tuple<std::vector<typename R::result_type>, std::vector<typename R::result_type>>
generate_list(R &r1, R &r2, const long N, const long skip1, const long skip2) {
  std::vector<typename R::result_type> v1, v2;
  if (N > 0) {
    v1.reserve(N);
    v2.reserve(N);
  }
  for (long i{0}; i < N; ++i) {
    v1.push_back(r1());
    v2.push_back(r2());
    for (long j{0}; j < skip1; ++j)
      r1();
    for (long j{0}; j < skip2; ++j)
      r2();
  }
  return {v1, v2};
}

template<typename R>
std::tuple<std::vector<typename R::result_type>, std::vector<typename R::result_type>>
generate_list(R &r1, R &r2, const long N) {
  return generate_list(r1, r2, N, 0, 0);
}

//-----------------------------------------------------------------------------------------

BOOST_AUTO_TEST_SUITE(test_suite_engines)

//-----------------------------------------------------------------------------------------

BOOST_AUTO_TEST_SUITE(test_suite_advance)
BOOST_AUTO_TEST_CASE_TEMPLATE(test_advance, R, engines) {
  // two engines with equal state
  R r1, r2;
  advance_engine(r1, 271828l);  // advance engine r1
  BOOST_TEST(r1 != r2, "engines have different state");
}
BOOST_AUTO_TEST_SUITE_END()

//-----------------------------------------------------------------------------------------

BOOST_AUTO_TEST_SUITE(test_suite_restore)
BOOST_AUTO_TEST_CASE_TEMPLATE(test_restore, R, engines) {
  // three engines with equal state
  R r1, r2, r3;
  advance_engine(r1, r2, 271828l);  // advance engines ra and rb
  r3 = r1;                          // backup r1 in r3
  // advance ra such that ra and rb have different states
  advance_engine(r1, 314159l);
  r1 = r3;  // restore ra
  BOOST_TEST(r1 == r2, "engine state restored successfully");
  // when state has been restored correctly, r1 and r2 must yield the same values
  auto v = generate_list(r1, r2, 32);
  BOOST_TEST(std::get<0>(v) == std::get<1>(v), "engines yield same values after restore");
}
BOOST_AUTO_TEST_SUITE_END()

//-----------------------------------------------------------------------------------------

BOOST_AUTO_TEST_SUITE(test_suite_status_io)
BOOST_AUTO_TEST_CASE_TEMPLATE(test_status_io, R, engines) {
  // two engines with equal state
  R r1, r2;
  advance_engine(r1, 271828l);  // advance engine r1
  std::stringstream str;
  str << r1;
  str >> r2;
  BOOST_TEST(r1 == r2, "engine state restored from stream successfully");
  // when state has been restored correctly, r1 and r2 must yield the same values
  auto v = generate_list(r1, r2, 32);
  BOOST_TEST(std::get<0>(v) == std::get<1>(v),
             "engines yield same values after restore from stream");
}
BOOST_AUTO_TEST_SUITE_END()

//-----------------------------------------------------------------------------------------

template<typename R>
struct generator_min_max {
  typedef typename R::result_type result_type;
  static constexpr result_type min() { return R::min(); }
  static constexpr result_type max() { return R::max(); }
};

template<typename R>
struct generator_min : public generator_min_max<R> {
  using base = generator_min_max<R>;
  typename base::result_type operator()() const { return base::min(); }
};

template<typename R>
struct generator_max : public generator_min_max<R> {
  using base = generator_min_max<R>;
  typename base::result_type operator()() const { return base::max(); }
};

template<typename R, typename T>
void test_ranges_impl() {
  const std::string &name{type_name<T>::name};
  generator_min<R> r_min;
  generator_max<R> r_max;
  using u01xx_min = trng::utility::u01xx_traits<T, 1, generator_min<R>>;
  using u01xx_max = trng::utility::u01xx_traits<T, 1, generator_max<R>>;
  T x_min{}, x_max{};
  x_min = u01xx_min::cc(r_min);
  x_max = u01xx_max::cc(r_max);
  BOOST_TEST((x_min >= 0 && x_max <= 1), "[0, 1] bounds ok for " + name);
  x_min = u01xx_min::co(r_min);
  x_max = u01xx_max::co(r_max);
  BOOST_TEST((x_min >= 0 and x_max < 1), "[0, 1) bounds ok for " + name);
  x_min = u01xx_min::oc(r_min);
  x_max = u01xx_max::oc(r_max);
  BOOST_TEST((x_min > 0 and x_max <= 1), "(0, 1] bounds ok for " + name);
  x_min = u01xx_min::oo(r_min);
  x_max = u01xx_max::oo(r_max);
  BOOST_TEST((x_min > 0 and x_max < 1), "(0, 1) bounds ok for " + name);
}

BOOST_AUTO_TEST_SUITE(test_suite_ranges)
BOOST_AUTO_TEST_CASE_TEMPLATE(test_ranges, R, engines) {
  test_ranges_impl<R, float>();
  test_ranges_impl<R, double>();
  test_ranges_impl<R, long double>();
}
BOOST_AUTO_TEST_SUITE_END()

//-----------------------------------------------------------------------------------------

BOOST_AUTO_TEST_SUITE(test_suite_discard)
BOOST_AUTO_TEST_CASE_TEMPLATE(test_discard, R, engines) {
  // two engines with equal state
  R r1, r2;
  const unsigned long long throw_away_1{2ull * 3 * 5 * 7 * 11 * 13 * 17 * 19 + 0x10000000ull};
  const unsigned long long throw_away_2{2ull * 3 * 5 * 7 * 11 * 13 * 17 * 19};
  r1.discard(throw_away_1);
  for (unsigned long long i{0}; i < throw_away_1; ++i)
    r2();
  {
    std::stringstream message;
    message << "engines have equal state after discard(" << throw_away_1 << ")";
    BOOST_TEST(r1 == r2, message.str().c_str());
  }
  r1.discard(throw_away_2);
  for (unsigned long long i{0}; i < throw_away_2; ++i)
    r2();
  {
    std::stringstream message;
    message << "engines have equal state after discard(" << throw_away_2 << ")";
    BOOST_TEST(r1 == r2, message.str().c_str());
  }
}
BOOST_AUTO_TEST_SUITE_END()

//-----------------------------------------------------------------------------------------

BOOST_AUTO_TEST_SUITE(test_suite_jump2)
BOOST_AUTO_TEST_CASE_TEMPLATE(test_jump2, R, parallel_engines) {
  // two engines with equal state
  R r1, r2;
  long n{1l};
  // jump ahead in steps of 2^i or step forwards in unit steps
  for (long i{0l}; i < 20l; ++i) {
    r1.jump2(i);
    for (long j{0l}; j < n; ++j)
      r2();
    std::stringstream message;
    message << "engines have equal state after jump2(" << i << ")";
    BOOST_TEST(r1 == r2, message.str().c_str());
    n <<= 1;
  }
}
BOOST_AUTO_TEST_SUITE_END()

//-----------------------------------------------------------------------------------------

BOOST_AUTO_TEST_SUITE(test_suite_split)
BOOST_AUTO_TEST_CASE_TEMPLATE(test_split, R, parallel_engines) {
  for (long i{2l}; i <= 20l; ++i) {
    for (long j{0l}; j < i; ++j) {
      // two engines with equal state
      R r1, r2;
      // split or step forwards in j unit steps
      for (long k{0l}; k < j; ++k)
        r1();
      r2.split(i, j);  // split into i streams, pick stream j
      {
        auto v = generate_list(r1, r2, 32, i - 1, 0);
        std::stringstream message;
        message << "engines yield same values after split(" << i << ", " << j << ")";
        BOOST_TEST(std::get<0>(v) == std::get<1>(v), message.str().c_str());
      }
    }
  }
}
BOOST_AUTO_TEST_SUITE_END()

//-----------------------------------------------------------------------------------------

BOOST_AUTO_TEST_SUITE_END()


/**
 * Copyright (C) Omar Thor <omarthoro@gmail.com> - All Rights Reserved
 * Unauthorized copying of this file, via any medium is strictly prohibited
 * Proprietary and confidential
 *
 * Written by Omar Thor <omarthoro@gmail.com>, 2017
 */

#include <iostream>
#include "sp/algo/stats.hpp"
#include "sp/algo/gen.hpp"
#include "sp/algo/gen/parser.hpp"
#include "sp/util/units.hpp"

#include "sp/util/timing.hpp"
#include "sp/util/query.hpp"
#include "sp/util/typename.hpp"

#define BOOST_TEST_MODULE sp_algo_gen_parser
#include <boost/test/unit_test.hpp>
using namespace sp::algo::gen;
using namespace sp::algo;
using namespace sp::util;

BOOST_AUTO_TEST_CASE( parser_detail_sequence_collect_types_produces_flat_structure) {
    using namespace parser;
    auto result = dummy() >> dummy() >> dummy() >> dummy() >> dummy();
    size_t seq_len = ::parser::detail::sequence_size_v<decltype(result)>;
    BOOST_REQUIRE_EQUAL(seq_len, 5);
    using result_tuple_expected_type = parser::detail::collect_binary_parser_types_t<decltype(result)>;
    BOOST_REQUIRE((std::is_same_v<std::tuple<dv, dv, dv, dv, dv>, result_tuple_expected_type>));
}

BOOST_AUTO_TEST_CASE( parser_seq_parser_detail_sequence_attribute_strategy_size_3) {

    using reader_type = chromosome_bit_reader;
    chromosome ch{};
    reader_type reader(&ch);

    using namespace parser;

    auto left1 = dummy(1) >> dummy(2);
    auto right1 = dummy(3);

    std::tuple<dv, dv, dv> t;
    using msp_type = parser::detail::sequence_attribute_strategy<decltype(left1), decltype(right1), decltype(t)>;

    auto lattr1 = msp_type::left(t);
    auto rattr1 = msp_type::right(t);

    BOOST_REQUIRE(left1.parse(reader, lattr1));
    BOOST_REQUIRE(right1.parse(reader, rattr1));

    msp_type::left_assign(t, lattr1);
    msp_type::right_assign(t, rattr1);

    BOOST_REQUIRE_EQUAL(std::get<0>(t).value, 1);
    BOOST_REQUIRE_EQUAL(std::get<1>(t).value, 2);
    BOOST_REQUIRE_EQUAL(std::get<2>(t).value, 3);
}

BOOST_AUTO_TEST_CASE( parser_sequence_flatten_into_tuple_of_2) {

    using reader_type = chromosome_bit_reader;
    chromosome ch{};
    reader_type reader(&ch);

    using namespace parser;

    std::tuple<dv, dv, dv, dv> t;
    auto expr = dummy(4) >> dummy(3) >> dummy(2) >> dummy(1);
    BOOST_REQUIRE(expr.parse(reader, t));
    BOOST_REQUIRE_EQUAL(std::get<0>(t).value, 4);
    BOOST_REQUIRE_EQUAL(std::get<1>(t).value, 3);
    BOOST_REQUIRE_EQUAL(std::get<2>(t).value, 2);
    BOOST_REQUIRE_EQUAL(std::get<3>(t).value, 1);
}

BOOST_AUTO_TEST_CASE( parse_seq_with_two_literals ) {

    using namespace parser;
    using reader_type = chromosome_bit_reader;
    chromosome ch{0,1,0,1,1, 0,0,0,0};

    reader_type reader(&ch);

    auto expr = (lit<0, 1, 0, 1, 1>() >> lit<0, 0, 0, 0>());

    BOOST_REQUIRE(expr.parse(reader));
    BOOST_REQUIRE_EQUAL(reader.pos(), ch.size());

    reader.reset();

    ch[8] = 1;

    BOOST_REQUIRE(!expr.parse(reader));
    BOOST_REQUIRE_EQUAL(reader.pos(), 0);

}

BOOST_AUTO_TEST_CASE( parse_action_return_false_fails_valid_expression ) {

    using namespace parser;
    using reader_type = chromosome_bit_reader;
    chromosome ch{0,1,0,1,1,0,0,0,0};

    reader_type reader(&ch);

    auto expr = lit<0, 1, 0, 1, 1>() >> lit<0, 0, 0, 0>();
    auto fail_action = [&](auto& ctx) { return false; };

    BOOST_REQUIRE(expr.parse(reader));
    BOOST_REQUIRE_EQUAL(reader.pos(), ch.size());
    auto act_expr = expr[fail_action];
    reader.reset();
    BOOST_REQUIRE(!act_expr.parse(reader));

    reader.reset();

    ch[8] = 1;

    BOOST_REQUIRE(!expr.parse(reader));
    BOOST_REQUIRE_EQUAL(reader.pos(), 0);

}

BOOST_AUTO_TEST_CASE( parse_action_operator_return_false_fails_valid_expression ) {

    using namespace parser;
    using reader_type = chromosome_bit_reader;
    chromosome ch{0,1,0,1,1,0,0,0,0};

    reader_type reader(&ch);

    auto expr = lit<0, 1, 0, 1, 1>() >> lit<0, 0, 0, 0>();
    auto fail_action = [&](auto& ctx) { return false; };

    BOOST_REQUIRE(expr.parse(reader));
    BOOST_REQUIRE_EQUAL(reader.pos(), ch.size());
    auto act_expr = expr / fail_action;
    reader.reset();
    BOOST_REQUIRE(!act_expr.parse(reader));

    reader.reset();

    ch[8] = 1;

    BOOST_REQUIRE(!expr.parse(reader));
    BOOST_REQUIRE_EQUAL(reader.pos(), 0);

}

BOOST_AUTO_TEST_CASE( parse_literal_with_action_is_called ) {

    using namespace parser;
    using reader_type = chromosome_bit_reader;
    chromosome ch{0,1,0,1,1,0,0,0,0};

    reader_type reader(&ch);
    int action_counter = 0;
    auto test_action = [&](auto& ctx) { ++action_counter; };
    auto expr = (lit<0, 1, 0, 1, 1>()[test_action] >> lit<0, 0, 0, 0>()[test_action]);

    BOOST_REQUIRE(expr.parse(reader));
    BOOST_REQUIRE_EQUAL(reader.pos(), ch.size());
    BOOST_REQUIRE_EQUAL(action_counter, 2);

}

BOOST_AUTO_TEST_CASE( parser_seq_adapts_to_vector ) {

    using namespace parser;
    using reader_type = chromosome_bit_reader;
    chromosome ch{0,1,0,1,1,0,0,0,0};

    reader_type reader(&ch);

    auto expr = (lit<0, 1, 0, 1, 1>() >> lit<0, 0 , 0, 0>());

    std::vector<int> bit_sequences;
    BOOST_REQUIRE(expr.parse(reader, bit_sequences));
    BOOST_REQUIRE_EQUAL(reader.pos(), ch.size());
    BOOST_REQUIRE_EQUAL(bit_sequences.size(), 2);
}

BOOST_AUTO_TEST_CASE( parser_seq_attribute_not_added_on_failure) {

    using namespace parser;
    using reader_type = chromosome_bit_reader;
    chromosome ch{0,1,0,1,1, 0,0,0,0, 0,0,0,1};

    reader_type reader(&ch);

    auto expr = (lit<0, 1, 0, 1, 1>() >> lit<0, 0, 0, 1>() >> lit<0, 0, 0, 1>());

    std::vector<int> bit_sequences;
    BOOST_REQUIRE(!expr.parse(reader, bit_sequences));
    BOOST_REQUIRE_EQUAL(reader.pos(), 0);
    BOOST_REQUIRE_EQUAL(bit_sequences.size(), 0);
}

BOOST_AUTO_TEST_CASE( parser_seq_adapts_to_binary_tuple) {

    using namespace parser;
    using reader_type = chromosome_bit_reader;
    chromosome ch{0,1,0,1,1, 1,0,1,0};

    reader_type reader(&ch);

    auto expr = lit<0, 1, 0, 1, 1>() >> lit<1, 0, 1, 0>();

    std::tuple<int, int> bit_seqs;
    BOOST_REQUIRE(expr.parse(reader, bit_seqs));
    BOOST_REQUIRE_EQUAL(reader.pos(), ch.size());

    BOOST_REQUIRE_EQUAL(std::get<0>(bit_seqs), 11);
    BOOST_REQUIRE_EQUAL(std::get<1>(bit_seqs), 10);
}

BOOST_AUTO_TEST_CASE( parser_seq_adapts_to_tuple_of_three) {

    using namespace parser;
    using reader_type = chromosome_bit_reader;
    chromosome ch{0,1,0,1,1, 1,0,1,0, 0,1,0,1};

    reader_type reader(&ch);

    auto expr = (lit<0, 1, 0, 1, 1>() >> lit<1, 0, 1, 0>() >> lit<0, 1, 0, 1>());

    std::tuple<unsigned char,unsigned char,unsigned char> bit_seq;
    BOOST_REQUIRE(expr.parse(reader, bit_seq));
    BOOST_REQUIRE_EQUAL(reader.pos(), ch.size());

    BOOST_REQUIRE_EQUAL(std::get<0>(bit_seq), 11);

    BOOST_REQUIRE_EQUAL(std::get<1>(bit_seq), 10);

    BOOST_REQUIRE_EQUAL(std::get<2>(bit_seq), 5);
}

BOOST_AUTO_TEST_CASE( parser_seq_adapts_to_tuple_of_four) {

    using namespace parser;
    using reader_type = chromosome_bit_reader;
    chromosome ch{0,1,0,1,1, 1,0,1,0, 0,0,1,0,  0,0,0,1};

    reader_type reader(&ch);

    auto expr = lit<0,1,0,1,1>() >> lit<1,0,1,0>() >> lit<0,0,1,0>() >> lit<0,0,0,1>();

    std::tuple<int, int, int, int> bit_sequences;

    BOOST_REQUIRE(expr.parse(reader, bit_sequences));
    BOOST_REQUIRE_EQUAL(reader.pos(), ch.size());

    BOOST_REQUIRE_EQUAL(std::get<0>(bit_sequences), 11);

    BOOST_REQUIRE_EQUAL(std::get<1>(bit_sequences), 10);

    BOOST_REQUIRE_EQUAL(std::get<2>(bit_sequences), 2);

    BOOST_REQUIRE_EQUAL(std::get<3>(bit_sequences), 1);
}

BOOST_AUTO_TEST_CASE( parser_seq_adapts_to_vector_of_three) {

    using namespace parser;
    using reader_type = chromosome_bit_reader;
    chromosome ch{0,1,0,1,1, 1,0,1,0, 0, 1, 0, 1};

    reader_type reader(&ch);

    auto expr = (lit<0, 1, 0, 1, 1>() >> lit<1, 0, 1, 0>() >> lit<0, 1, 0, 1>());

    std::vector<int> bit_vec;
    BOOST_REQUIRE(expr.parse(reader, bit_vec));
    BOOST_REQUIRE_EQUAL(reader.pos(), ch.size());

    BOOST_REQUIRE_EQUAL(bit_vec.size(), 3);
    BOOST_REQUIRE_EQUAL(bit_vec[0], 11);
    BOOST_REQUIRE_EQUAL(bit_vec[1], 10);
    BOOST_REQUIRE_EQUAL(bit_vec[2], 5);
}

BOOST_AUTO_TEST_CASE( parser_alt_adapts_to_variant_of_four) {
    using namespace parser;
    using reader_type = chromosome_bit_reader;
    chromosome ch{0,0,0,0,0,0,0,1};

    reader_type reader(&ch);

    auto expr = uint64_val(0) | uint32_val(0) | uint16_val(0) | uint8_val(1);
    std::variant<std::monostate, uint64_t, uint16_t, uint8_t, uint32_t> var;
    BOOST_REQUIRE(expr.parse(reader, var));
    BOOST_REQUIRE_EQUAL(reader.pos(), ch.size());
    BOOST_REQUIRE_EQUAL(std::get<uint8_t>(var), uint8_t(1));
}

BOOST_AUTO_TEST_CASE( parser_kleene_adapts_to_vector) {

    using namespace parser;
    using reader_type = chromosome_bit_reader;
    chromosome ch{1,0,1,0,1,0,1,0};

    reader_type reader(&ch);

    auto expr = *lit<1,0>();
    std::vector<int> vec;
    BOOST_REQUIRE(expr.parse(reader, vec));
    BOOST_REQUIRE_EQUAL(reader.pos(), ch.size());
    BOOST_REQUIRE_EQUAL(vec.size(), 4);
}

BOOST_AUTO_TEST_CASE( parser_not) {

    using namespace parser;
    using reader_type = chromosome_bit_reader;
    chromosome ch{1,0};

    reader_type reader(&ch);
    auto expr = !lit<1,0>();
    BOOST_REQUIRE(!expr.parse(reader));
    BOOST_REQUIRE_EQUAL(reader.pos(), 0);
}

BOOST_AUTO_TEST_CASE( parser_plus_and_adapts_to_vector) {

    using namespace parser;
    using reader_type = chromosome_bit_reader;
    chromosome ch{1,0, 1,0, 1,1};

    reader_type reader(&ch);
    auto expr = +lit<1,0>();
    std::vector<int> vec;
    BOOST_REQUIRE(expr.parse(reader, vec));
    BOOST_REQUIRE_EQUAL(reader.pos(), 4);
    BOOST_REQUIRE_EQUAL(vec.size(), 2);
    BOOST_REQUIRE_EQUAL(vec[0], 2);
    BOOST_REQUIRE_EQUAL(vec[1], 2);
}

BOOST_AUTO_TEST_CASE( parser_optional_adapts_to_std_optional_tuple) {

    using namespace parser;
    using reader_type = chromosome_bit_reader;
    chromosome ch{1,0, 1,1};

    reader_type reader(&ch);

    auto expr = (-lit<1,0>()) >> (-lit<1,0>());

    std::tuple<std::optional<int>, std::optional<int>> opt;

    BOOST_REQUIRE(expr.parse(reader, opt));
    BOOST_REQUIRE_EQUAL(reader.pos(), 2);
    BOOST_REQUIRE(std::get<0>(opt).has_value());
    BOOST_REQUIRE(!std::get<1>(opt).has_value());
}

BOOST_AUTO_TEST_CASE( parser_composite_tuple_of_sequences_and_alternatives_is_adapted_into_variant) {

    using namespace parser;
    using reader_type = chromosome_bit_reader;
    chromosome ch{0,1,1,1};

    reader_type reader(&ch);

    auto seq = bool_val(true) >> bool_any() >> bool_any() >> bool_val(false);
    auto seq2 = uint8_any<4>();

    auto expr = seq | seq2;

    std::variant<std::tuple<bool, bool, bool, bool>, int> var;

    BOOST_REQUIRE(expr.parse(reader, var));

    BOOST_REQUIRE_EQUAL(reader.pos(), 4);
    BOOST_REQUIRE(std::holds_alternative<int>(var));

    BOOST_REQUIRE_EQUAL(std::get<1>(var), 7);
}

BOOST_AUTO_TEST_CASE( parser_composite_tuple_of_multiple_sequences_and_alternatives_is_adapted_into_kleene_variant) {

    using namespace parser;
    using reader_type = chromosome_bit_reader;
    chromosome ch{0,1,1,1, 1,1,1,0};

    reader_type reader(&ch);

    auto seq = bool_val(true) >> bool_any() >> bool_any() >> bool_val(false);
    auto seq2 = uint8_any<4>();
    auto expr = +(seq | seq2);

    std::vector<
        std::variant<
            std::tuple<bool, bool, bool, bool>,
            int
        >
    > var;

    BOOST_REQUIRE(expr.parse(reader, var));
    BOOST_REQUIRE_EQUAL(reader.pos(), 8);

    BOOST_REQUIRE_EQUAL(var.size(), 2);
    BOOST_REQUIRE(std::holds_alternative<int>(var[0]));
    BOOST_REQUIRE((std::holds_alternative<std::tuple<bool, bool, bool, bool>>(var[1])));
    BOOST_REQUIRE_EQUAL(std::get<1>(var[0]), 7);
    BOOST_REQUIRE(std::get<0>(var[1]) == std::make_tuple(1,1,1,0));
}

BOOST_AUTO_TEST_CASE( parser_composite_tuple_of_sequences_and_alternatives_is_adapted_into_variant_with_explict_rules) {

    using namespace parser;
    using r_t = chromosome_bit_reader;
    chromosome ch{0,1,1,1};

    r_t reader(&ch);

    using t4 =  std::tuple<bool, bool, bool, bool>;
    rule<r_t, t4> seq;
    rule<r_t, int> seq2;
    rule<r_t, std::variant<std::monostate, t4, int>> expr;

    seq     %= bool_val(true) >> bool_any() >> bool_any() >> bool_val(false);
    seq2    %= uint8_any<4>();
    expr    %= seq | seq2;

    std::variant<std::monostate, std::tuple<bool, bool, bool, bool>, int> var;

    BOOST_REQUIRE(expr.parse(reader, var));

    BOOST_REQUIRE_EQUAL(reader.pos(), 4);
    BOOST_REQUIRE((!std::holds_alternative<std::tuple<bool, bool, bool, bool>>(var)));
    BOOST_REQUIRE(std::holds_alternative<int>(var));

    BOOST_REQUIRE_EQUAL(std::get<2>(var), 7);
}

BOOST_AUTO_TEST_CASE( parser_rule_attribute_conversion) {

    using namespace parser;
    using r_t = chromosome_bit_reader;
    chromosome ch{1,1,1,1};

    r_t reader(&ch);

    struct result {
        result() {}

        result(std::tuple<bool, bool, bool, bool>&& t) {
            values.push_back(std::get<0>(t));
            values.push_back(std::get<1>(t));
            values.push_back(std::get<2>(t));
            values.push_back(std::get<3>(t));
        }

        std::vector<bool> values;
    };

    rule<r_t, result> expr;

    expr    %= bool_val(true) >> bool_any() >> bool_any() >> bool_val(true);

    result res;

    BOOST_REQUIRE(expr.parse(reader, res));
    BOOST_REQUIRE_EQUAL(res.values.size(), 4);
    BOOST_REQUIRE_EQUAL(res.values[0], 1);
    BOOST_REQUIRE_EQUAL(res.values[1], 1);
    BOOST_REQUIRE_EQUAL(res.values[2], 1);
    BOOST_REQUIRE_EQUAL(res.values[3], 1);
}


//
// Created by yche on 1/25/18.
//

#include <iostream>
#include <fstream>
#include <chrono>

#include <boost/graph/adjacency_list.hpp>

#include "../ground_truth/graph_yche.h"
#include "../util/sample_num.h"
#include "../algorithm/readsrq.h"

using namespace std;
using namespace boost;
using namespace std::chrono;
typedef boost::adjacency_list<vecS, vecS, bidirectionalS> DirectedG;

void load_graph(string path, DirectedG &g) {
    // load graph from edge_list file
    if (file_exists(path)) {
        int a, b;
        cout << "loading " << path << endl;;
        ifstream edgeFile(path, ios::in);
        while (edgeFile >> a >> b) {
            add_edge(a, b, g);
        }
        edgeFile.close();
        return;
    } else {
        cout << "file doesn't exit" << endl;
        return;
    }
}

int main(int argc, char *argv[]) {
    DirectedG g;
    string data_name = argv[1];
    load_graph(get_edge_list_path(data_name), g);
    size_t n = num_vertices(g);

    double eps = 0.01;
    double delta = 0.01;
    double c = 0.6;
    int t = 10;
    int r = 100;
    int rq = compute_reads_rq_num(eps, delta, c, r);
    cout << "sample num:" << r << ", on-line rand-walk:" << rq << endl;

    // 1st: generate edges
    int num_updates = 1000;
    cout << "begin generate edges..." << endl;
    vector<pair<unsigned int, unsigned int>> ins_edges;
    random_device rd;
    mt19937 gen(rd());
    uniform_int_distribution<int> distribution(0, static_cast<int>(n - 1));
    while (ins_edges.size() < num_updates) {
        auto e1 = distribution(gen) % n;
        auto e2 = distribution(gen) % n;
        if (!boost::edge(e1, e2, g).second && boost::in_degree(e1, g) > 0 && boost::in_degree(e2, g) > 0) {
            ins_edges.emplace_back(e1, e2);
        }
    }

    // construct the reads-rq algorithm object
    readsrq algorithm(data_name, n, r, rq, c, t);

    // 2nd: dynamic update statistics
    cout << "reads-d begin dynamic update..." << endl;
    auto start = std::chrono::high_resolution_clock::now();
    for (auto &edge:ins_edges) {
        algorithm.insEdge(edge.first, edge.second);
    }
    auto finish = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> elapsed = finish - start;
    cout << "total time: " << elapsed.count() << endl;
    cout << "avg time: " << elapsed.count() / num_updates << endl;
}


//  Copyright (c) 2018 Robert Ramey
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <boost/safe_numerics/checked_result.hpp>
#include <boost/safe_numerics/checked_result_operations.hpp>
#include <boost/safe_numerics/checked_integer.hpp>

// note: T should be of tyme checked_result<R> for some integer type R
template<class T>
constexpr bool test_checked_and(
    const T & v1,
    const T & v2,
    char expected_result
){
    using namespace boost::safe_numerics;
    const T result = v1 & v2;
    switch(expected_result){
    case '.':
        return ! result.exception();
    case '-':
        return safe_numerics_error::negative_overflow_error == result.m_e;
    case '+':
        return safe_numerics_error::positive_overflow_error == result.m_e;
    case '!':
        return safe_numerics_error::range_error == result.m_e;
    }
    return false;
}

#include "test_checked_and.hpp"

template<typename T, typename First, typename Second>
struct test_signed_pair {
    static const std::size_t i = First();
    static const std::size_t j = Second();
    // note: is constexpr really required here?  compilers disagree!
    constexpr static const bool value = test_checked_and(
        signed_values<T>[i],
        signed_values<T>[j],
        signed_and_results[i][j]
    );
};

template<typename T, typename First, typename Second>
struct test_unsigned_pair {
    static const std::size_t i = First();
    static const std::size_t j = Second();
    // note: is constexpr really required here?  compilers disagree!
    constexpr static const bool value = test_checked_and(
        unsigned_values<T>[i],
        unsigned_values<T>[j],
        unsigned_and_results[i][j]
    );
};

#include "check_symmetry.hpp"
#include <boost/mp11/algorithm.hpp>

int main(){
    using namespace boost::mp11;
    check_symmetry(signed_and_results);
    check_symmetry(unsigned_and_results);

    static_assert(
        mp_all_of<
            mp_product<
                test_signed_pair,
                signed_test_types,
                signed_value_indices, signed_value_indices
            >,
            mp_to_bool
        >(),
        "all values for all signed types correctly anded"
    );

    static_assert(
        mp_all_of<
            mp_product<
                test_unsigned_pair,
                unsigned_test_types,
                unsigned_value_indices, unsigned_value_indices
            >,
            mp_to_bool
        >(),
        "all values for all unsigned types correctly anded"
    );
    return 0;
}


#include "file_io.hpp"
#include <boost/filesystem.hpp>
#include <cmath>

size_t get_cnt_lines(const std::string &name_file)
{
    throw (std::runtime_error("not yet implemented"));
    return 0;
}

size_t read_int(const std::string &name_file)
{
    std::ifstream is(name_file, std::ifstream::in);
    size_t res;
    is >> res;
    return res;
}

size_t get_filesize(const std::string & name_file)
{
    std::ifstream in(name_file, std::ifstream::ate | std::ifstream::binary);
    if (!in.is_open())
    {
        std::string message="get_filesize() can not open file ";
        throw std::runtime_error (message+name_file);
    }
    return in.tellg();
}

double get_PMI(Index cnt,Index id1,Index id2,Vocabulary const & vocab)
{
    return log2((static_cast<double>(cnt)*vocab.cnt_words_processed)/(vocab.freq_per_id[id1]*vocab.freq_per_id[id2]));
}

void dump_crs(std::string path_out,std::vector<Accumulator> const & counters,Vocabulary const & vocab, bool binary)
{
    std::ofstream file;
    auto mode=binary? std::ios::out | std::ios::binary : std::ios::out;
    std::string str_path = (boost::filesystem::path(path_out) / boost::filesystem::path(binary?"bigrams.data.bin":"bigrams.data")).string();
    file.open (str_path,mode);
    if(!file) throw  std::runtime_error("can not open output file " + str_path + " , check the path");
    for (size_t first=0;first<counters.size();first++)
        for (const auto& second : counters[first])
        {
            float v=get_PMI(second.second,first,second.first,vocab);
            if (binary)
                file.write( reinterpret_cast<const char*>(&v),sizeof(v));
            else
                file<<v<<"\n";
        }
    file.close();

    str_path = (boost::filesystem::path(path_out) / boost::filesystem::path(binary?"bigrams.col_ind.bin":"bigrams.col_ind")).string();
    file.open (str_path,mode);
    for (size_t first=0;first<counters.size();first++)
        for (const auto& second : counters[first])
        {
            size_t v=second.first;
            if (binary)
               file.write( reinterpret_cast<const char*>(&v),sizeof(v));
            else
                file<<v<<"\n";
        }
    file.close();

    str_path = (boost::filesystem::path(path_out) / boost::filesystem::path(binary?"bigrams.row_ptr.bin":"bigrams.row_ptr")).string();
    file.open (str_path,mode);
    Index row_ptr=0;
    Index id_last=0;
    for (size_t first=0;first<counters.size();first++)
    {
        if (first==0)
            {
                if (binary)
                    file.write( reinterpret_cast<const char*>(&row_ptr),sizeof(row_ptr));
                else
                    file<<row_ptr<<"\n";
            }
        else
            for (size_t k=id_last;k<first;k++)
                if (binary)
                    file.write( reinterpret_cast<const char*>(&row_ptr),sizeof(row_ptr));
                else
                    file<<row_ptr<<"\n";
            id_last=first;
            row_ptr+=counters[first].size();
        }
        for (size_t k=id_last;k<vocab.cnt_words;k++)
            if (binary)
                file.write( reinterpret_cast<const char*>(&row_ptr),sizeof(row_ptr));
            else
                file<<row_ptr<<"\n";
       file.close();
   }

void write_cooccurrence_text(std::string name_file,std::vector<Accumulator> const & counters,Vocabulary const & vocab)
{
std::ofstream file;
file.open (name_file);
if(!file) throw  std::runtime_error("can not open output file "+name_file+" , check the path");
for (size_t first=0;first<counters.size();first++)
{
    for (const auto& second : counters[first])
    {
      //if (t.second>0)
       // file<<first.first<<"\t"<<second.first<<"\t"<<second.second<<"\n";
        double v=log2((static_cast<double>(second.second)*vocab.cnt_words_processed)/(vocab.freq_per_id[first]*vocab.freq_per_id[second.first]));
        //file<<first<<"\t"<<second.first<<"\t"<<v<<"\n";
        file<<vocab.lst_id2word[first]<<"\t"<<vocab.lst_id2word[second.first]<<"\t"<<second.second<<"\t"<<v<<"\n";
    }
}

file.close();
}


#define BOOST_TEST_MODULE "test_read_isolf_potential"

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

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

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

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

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

BOOST_AUTO_TEST_CASE_TEMPLATE(read_isolf_noenv, T, test_types)
{
    mjolnir::LoggerManager::set_default_logger("test_read_isolf.log");

    using real_type = T;
    using traits_type = mjolnir::SimulatorTraits<real_type, mjolnir::UnlimitedBoundary>;
    {
        using namespace toml::literals;
        const auto v = u8R"(
            interaction = "Pair"
            potential   = "iSoLFAttractive"
            spatial_partition.type  = "Naive"
            ignore.molecule         = "Nothing"
            ignore.particles_within.bond  = 1
            ignore.particles_within.angle = 1
            env.large = 100.0
            parameters = [
                {index =   0, sigma =     2.0, epsilon = 1.5, omega = 1.0},
                {index =   1, sigma =     2.0, epsilon = 1.5, omega = 1.0},
                {index =   2, sigma =     5.0, epsilon = 0.5, omega = 1.0},
                {index =   7, sigma =     7.0, epsilon = 0.7, omega = 1.0},
                {index = 100, sigma = "large", epsilon = 0.1, omega = 1.0},
            ]
        )"_toml;

        const auto pot_para = mjolnir::read_isolf_potential<traits_type>(v);
        const auto& para = dynamic_cast<
            mjolnir::iSoLFAttractiveParameterList<traits_type> const&
            >(pot_para.second.cref());

        const auto ignore_within = para.exclusion_list().ignore_topology();
        const std::map<std::string, std::size_t> within(
                ignore_within.begin(), ignore_within.end());

        BOOST_TEST(within.size() == 2u);
        BOOST_TEST(within.at("bond")  == 1ul);
        BOOST_TEST(within.at("angle") == 1ul);

        BOOST_TEST(!para.exclusion_list().is_ignored_molecule(0, 0));
        BOOST_TEST(!para.exclusion_list().is_ignored_molecule(0, 1));
        BOOST_TEST(!para.exclusion_list().is_ignored_molecule(1, 1));

        BOOST_TEST(para.participants().size() ==   5u);
        BOOST_TEST(para.participants().at(0)  ==   0u);
        BOOST_TEST(para.participants().at(1)  ==   1u);
        BOOST_TEST(para.participants().at(2)  ==   2u);
        BOOST_TEST(para.participants().at(3)  ==   7u);
        BOOST_TEST(para.participants().at(4)  == 100u);

        BOOST_TEST(para.parameters().at(  0).sigma == real_type(  2.0), tolerance<real_type>());
        BOOST_TEST(para.parameters().at(  1).sigma == real_type(  2.0), tolerance<real_type>());
        BOOST_TEST(para.parameters().at(  2).sigma == real_type(  5.0), tolerance<real_type>());
        BOOST_TEST(para.parameters().at(  7).sigma == real_type(  7.0), tolerance<real_type>());
        BOOST_TEST(para.parameters().at(100).sigma == real_type(100.0), tolerance<real_type>());

        BOOST_TEST(para.parameters().at(  0).epsilon == real_type(1.5), tolerance<real_type>());
        BOOST_TEST(para.parameters().at(  1).epsilon == real_type(1.5), tolerance<real_type>());
        BOOST_TEST(para.parameters().at(  2).epsilon == real_type(0.5), tolerance<real_type>());
        BOOST_TEST(para.parameters().at(  7).epsilon == real_type(0.7), tolerance<real_type>());
        BOOST_TEST(para.parameters().at(100).epsilon == real_type(0.1), tolerance<real_type>());

        BOOST_TEST(para.parameters().at(  0).omega == real_type(1.0), tolerance<real_type>());
        BOOST_TEST(para.parameters().at(  1).omega == real_type(1.0), tolerance<real_type>());
        BOOST_TEST(para.parameters().at(  2).omega == real_type(1.0), tolerance<real_type>());
        BOOST_TEST(para.parameters().at(  7).omega == real_type(1.0), tolerance<real_type>());
        BOOST_TEST(para.parameters().at(100).omega == real_type(1.0), tolerance<real_type>());
    }
}


#include <fstream>
#include <unordered_map>

#include <boost/format.hpp>

#include "local_push.h"

using boost::format;

double cal_rmax(double c, double epsilon) {
    return (1 - c) * epsilon;
}

void Full_LocalPush::push(NodePair &pab, double inc) {
    // the actually action of push
    n_push++;
    R[pab] += inc;
    // cout << pab.first << " " << pab.second << endl ;
    if (fabs(R[pab]) > r_max) {
        // Q.insert(pab);
        if (!marker[pab]) {
            Q.push(pab);
            marker[pab] = true;
        }
    }
}

void Reduced_LocalPush::push(NodePair &pab, double inc) {
    // the actually action of push
    n_push++;
    R[pab] += inc;
    if (fabs(R[pab]) / sqrt(2) > r_max) { // the criteria for reduced linear system
        // Q.insert(pab);
        if (!marker[pab]) {
            Q.push(pab);
            marker[pab] = true;
        }
    }
}

void LocalPush::init_PR() {
    string data_path = get_file_path_base() + ".P";
    cout << "data path " << data_path << endl;
    if (file_exists(data_path)) { // local push from start
        cout << "file exisis.." << endl;
        load();
    } else { // file exists
        /* init the P and R */
        P.add(n);
        R.add(n);
        marker.add(n);

        cout << "file not exists, compute from scratch" << endl;
        for (int i = 0; i < n; i++) {
            NodePair np(i, i);
            // R.insert({np,1});
            R[np] = 1;
            Q.push(np);
            marker[np] = true;
        }
    }
}

LocalPush::LocalPush(DirectedG &g, string gName_, double c_, double epsilon_, size_t n_) {
    // init data members
    /* init data memebrs */
    g_name = gName_;
    c = c_;
    // r_max = r_max_;
    epsilon = epsilon_;
    r_max = cal_rmax(c, epsilon);
    n = n_;
    string path = get_edge_list_path(gName_);
    n_push = 0;
    cpu_time = -1; // set the init value
}

Full_LocalPush::Full_LocalPush(DirectedG &g, string name, double c_, double r_max_, size_t n_) :
        LocalPush(g, name, c_, r_max_, n_) {
    init_PR();
}

Reduced_LocalPush::Reduced_LocalPush(DirectedG &g, string name, double c_, double r_max_, size_t n_) :
        LocalPush(g, name, c_, r_max_, n_) {
    init_PR();
}

void Reduced_LocalPush::push_to_neighbors(DirectedG &g, NodePair &np, double current_residual) {
    // the push method using reduced linear system
    // out-neighbors of a,b
    DirectedG::out_edge_iterator outi_iter, outi_end, outj_iter, outj_end;
    // /* only push to partial pairs*/

    tie(outi_iter, outi_end) = out_edges(np.first, g);
    tie(outj_iter, outj_end) = out_edges(np.second, g);// init the iterator

    tie(outi_iter, outi_end) = out_edges(np.first, g);
    if (np.first == np.second) {
        /* starting push for singleton nodes*/
        for (; outi_iter != outi_end; outi_iter++) {
            tie(outj_iter, outj_end) = out_edges(np.second, g);// init the iterator
            auto out_a = target(*outi_iter, g); // out-neighbor
            for (; outj_iter != outj_end; outj_iter++) {
                auto out_b = target(*outj_iter, g);
                auto indegree_a = in_degree(out_a, g);
                auto indegree_b = in_degree(out_b, g);
                auto total_in = indegree_a * indegree_b;
                if (out_a < out_b) { // only push to partial pairs for a < b
                    NodePair pab(out_a, out_b); // the node-pair to be pushed to
                    double inc = c * current_residual / total_in; //
                    push(pab, sqrt(2) * inc); // do the push action, sqrt(2) factor is because it is from singleton node
                }
            }
        }
    } else {
        auto i_begin_iter = outi_iter; // mark the begining iterator
        for (; outi_iter != outi_end; outi_iter++) {
            tie(outj_iter, outj_end) = out_edges(np.second, g);// init the iterator
            auto j_begin = outj_iter;
            for (; outj_iter != outj_end; outj_iter++) {
                auto out_a = target(*outi_iter, g); // out-neighbor
                auto out_b = target(*outj_iter, g);
                auto indegree_a = in_degree(out_a, g);
                auto indegree_b = in_degree(out_b, g);
                auto total_in = indegree_a * indegree_b;
                double inc = c * current_residual / total_in;
                if (out_a == out_b) { //don't push to singleton nodes
                    continue;
                } else { //  a, b is normal in-neighbors of out_a and out_b
                    if (out_a > out_b) {
                        swap(out_a, out_b);
                    }
                    NodePair pab(out_a, out_b);
                    push(pab, 1 * inc);
                }
            }
        }
    }
}

void Full_LocalPush::push_to_neighbors(DirectedG &g, NodePair &np, double current_residual) {
    DirectedG::out_edge_iterator outi_iter, outi_end, outj_iter, outj_end;

    tie(outi_iter, outi_end) = out_edges(np.first, g);
    for (; outi_iter != outi_end; outi_iter++) {
        auto out_a = target(*outi_iter, g); // out-neighbor
        tie(outj_iter, outj_end) = out_edges(np.second, g);
        for (; outj_iter != outj_end; outj_iter++) {
            auto out_b = target(*outj_iter, g);
            if (out_a == out_b) {
                continue;
            }
            auto indegree_a = in_degree(out_a, g);
            auto indegree_b = in_degree(out_b, g);
            auto total_in = indegree_a * indegree_b;
            NodePair pab(out_a, out_b);
            double inc = c * current_residual / total_in;
            push(pab, inc);
        }
    }
}

double Full_LocalPush::how_much_residual_to_push(DirectedG &g, NodePair &np) {
    return R[np];
}

double Reduced_LocalPush::how_much_residual_to_push(DirectedG &g, NodePair &np) {
    // determine the residual value for current pair to push
    double r = R[np];
    if (np.first == np.second) { //singleton node
        return r - r_max / (1 - c); // singleton nodes do not need to push all residual as 1
    }
    // /* check whether np forms a self-loop */
    // if(edge(np.first,np.second,g).second == true && edge(np.second, np.first,g).second == true){ // check whether exists reverse edge
    //     auto in_deg_a = in_degree(np.first,g);
    //     auto in_deg_b = in_degree(np.second,g);
    //     double alpha = c / (in_deg_a * in_deg_b);
    //     int k = ceil( log(r_max / fabs(r)) / log(alpha) );
    //     double residual_to_push=(1 - pow(alpha,k)) * r / (1-alpha);
    //     return residual_to_push;
    // }else{
    //     auto push_residual = r;
    //     return push_residual;
    // }
    return r;
}

void LocalPush::local_push(DirectedG &g) { // local push given current P and R
    // cout << r_max << endl;
    auto start = std::chrono::high_resolution_clock::now();
    double sum_of_est = 0;

    while (!Q.empty()) {
        NodePair np = Q.front();
        Q.pop();
        marker[np] = false;
        // cout << "Current: " << np.first << " " << np.second << ": " << R[np] << endl;
        double residual_to_push = how_much_residual_to_push(g, np);
        sum_of_est += residual_to_push;
        // cout << " node pair " << np.first << " " << np.second << endl;
        // R.erase(np); //  remove from residual
        R[np] -= residual_to_push;
        P[np] += residual_to_push;
        push_to_neighbors(g, np, residual_to_push); // push residuals to neighbros of np

    }
    auto finish = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> elapsed = finish - start;
    if (cpu_time == -1) {
        cpu_time = elapsed.count();
        mem_size = getValue();
    }
    // cout << "n: " << n << " m: " << num_edges(g) << " r_max: " << r_max << " time: " << elapsed.count() <<"s"<< endl;
    // cout << sum_of_est * 2 - n << endl;
    // cout << "number of push: " << n_push << endl;
    // save the time to files

    // save time info to file
    // string time_file_name = get_file_path_base() + ".txt"; // the time file
    // fstream time_file(time_file_name, ios::out);
    // cout <<"timing " << "saving to " << time_file_name << endl;
    // time_file << elapsed.count() << endl;
    // time_file.close();
    return;
}

void LocalPush::save() {
    // save data to disk
    // save P
    string p_path = get_file_path_base() + ".P";
    P.save(p_path);
    // FILE *outP = fopen(p_path.c_str(), "wb");
    // cout << "saving P to " << p_path << endl;
    // P.serialize(FileSerializer(), outP);
    // fclose(outP);
    //save R
    string r_path = get_file_path_base() + ".R";
    R.save(r_path);
    // FILE *outR = fopen(r_path.c_str(), "wb");
    // cout << "saving R to " << r_path << endl;
    // R.serialize(FileSerializer(), outR);
    // fclose(outR);

    ofstream out;
    // // save meta info
    string meta_path = get_file_path_base() + ".meta";
    out.open(meta_path);
    out << g_name << endl;
    out << n << endl;
    out << epsilon << endl;
    out << c << endl;
    out << cpu_time << endl;
    out << mem_size << endl;
    auto p_size = P.size();
    auto r_size = R.size();
    out << p_size << endl;
    out << r_size << endl;
    out << double(p_size) / (n * n) << endl;
    out << double(r_size) / (n * n) << endl;
    out.close();

    // // save the experimental data
    // string exp_path =get_file_path_base()+".exp";
    // out.open(exp_path);
    // out << n << endl;
    // cout << c << endl;
    // cout << epsilon << endl;
    // out.close();
    // save exp data
    cout << "save complete" << endl;

}

void LocalPush::load() {
    // load existing local push data
    // load P
    string p_path = get_file_path_base() + ".P";
    P.load(p_path);
    // FILE *inP = fopen(p_path.c_str(), "rb");
    // cout << "loading P to " << p_path << endl;
    // P.unserialize(FileSerializer(), inP);
    // fclose(inP);
    //load R
    string r_path = get_file_path_base() + ".R";
    R.load(r_path);
    // FILE *inR = fopen(r_path.c_str(), "rb");
    // cout << "loading R to " << r_path << endl;
    // R.unserialize(FileSerializer(), inR);
    // fclose(inR);

    string meta_path = get_file_path_base() + ".meta";
    ifstream in;
    in.open(meta_path);
    in >> g_name >> n >> epsilon >> c >> cpu_time >> mem_size;
    r_max = cal_rmax(c, epsilon);
    marker.add(n); // initialize marker
    in.close();
}

void LocalPush::show() {
    // cout << P.size() << endl;
    // for(auto &item:P){
    //     cout << "(" << item.first.first << "," << item.first.second << "): " << item.second << endl;
    // }
}

string Reduced_LocalPush::get_file_path_base() {
    // return the file path, exluding the suffix
    return LOCAL_PUSH_DIR + str(format("RLP_%s-%.3f-%.6f") %
                                g_name % c % epsilon);
}

string Full_LocalPush::get_file_path_base() {
    // return the file path, exluding the suffix
    return LOCAL_PUSH_DIR + str(format("FLP_%s-%.3f-%.6f") %
                                g_name % c % epsilon);
}

void LocalPush::insert(DirectedG::vertex_descriptor u, DirectedG::vertex_descriptor v,
                       DirectedG &g) {
    // insert edge(u,v) to g, noted we assume g is already updated, and we just update P and R
    DirectedG::vertex_iterator v_it, v_end;
    tie(v_it, v_end) = vertices(g);
    for (; v_it != v_end; v_it++) {
        auto a = *v_it;
        auto in_deg_v = in_degree(v, g); // the new degree of v
        if (a > v) {
            update_residual(g, v, a);
        } else if (a < v) {
            update_residual(g, a, v);
        } else {
            // a == v
            R[NodePair(a, a)] = 1 - P[NodePair(a, a)];
        }
    }
    local_push(g);
}

void LocalPush::remove(DirectedG::vertex_descriptor u, DirectedG::vertex_descriptor v,
                       DirectedG &g) {
    // remove edge(a,b) to g, noted we assume g is already updated, and we just update P and R
}

double Reduced_LocalPush::query_P(DirectedG::vertex_descriptor a, DirectedG::vertex_descriptor b) {
    if (a == b) {
        return P[NodePair(a, b)];
    } else if (a > b) {
        return P[NodePair(b, a)] / sqrt(2);
    } else {
        return P[NodePair(a, b)] / sqrt(2);
    }
}

double Full_LocalPush::query_P(DirectedG::vertex_descriptor a, DirectedG::vertex_descriptor b) {
    return P[NodePair(a, b)];
}

double LocalPush::query_R(DirectedG::vertex_descriptor a, DirectedG::vertex_descriptor b) {
    if (a > b) {
        return R[NodePair(b, a)];
    } else {
        return R[NodePair(a, b)];
    }
}

void Reduced_LocalPush::update_residual(DirectedG &g, DirectedG::vertex_descriptor a, DirectedG::vertex_descriptor b) {
    // cout << "-----" << endl;
    // cout << "updating residual "<< a << " " << b << endl;
    NodePair np(a, b); // a < b
    PairMarker indicator; // the indicator of in-neighbors of (a,b)
    DirectedG::in_edge_iterator in_a_it, in_a_end, in_b_it, in_b_end;
    tie(in_a_it, in_a_end) = in_edges(a, g);
    auto in_deg_a = in_degree(a, g);
    auto in_deg_b = in_degree(b, g);
    if (in_deg_a * in_deg_b > 0) {
        double sum_neighbor_residuals = 0;
        for (; in_a_it != in_a_end; in_a_it++) {
            tie(in_b_it, in_b_end) = in_edges(b, g);
            for (; in_b_it != in_b_end; in_b_it++) {
                auto ina = source(*in_a_it, g);
                auto inb = source(*in_b_it, g);
                // indicator[NodePair(min(ina,inb), max(ina,inb))] = true;
                sum_neighbor_residuals += P[NodePair(min(ina, inb), max(ina, inb))];
            }
        }
        // for(auto& item:indicator){
        //     // cout << "collect neighbor P (" << item.first.first
        //     //     << "," << item.first.second <<"): " << P[item.first]  << endl;
        //     sum_neighbor_residuals += P[item.first]; // item.first is the node pair
        // }
        R[np] = c * sum_neighbor_residuals / (in_deg_a * in_deg_b) - P[np];
        // cout << "neighbros residuals " << sum_neighbor_residuals << endl;
        // cout << " new residual " << R[np] <<  " its current estimates " << P[np] << endl;
        if (fabs(R[NodePair(a, b)]) > r_max) {
            if (marker[np] == false) {
                Q.push(np);
                marker[np] = true;
            }
        }
    }
}


//////////////////////////////////////////////////////////////////////////////////////////////
/// \file SteamTrajInterface.hpp
///
/// \author Sean Anderson, ASRL
//////////////////////////////////////////////////////////////////////////////////////////////

#ifndef STEAM_TRAJECTORY_INTERFACE_HPP
#define STEAM_TRAJECTORY_INTERFACE_HPP

#include <Eigen/Core>

#include <steam/common/Time.hpp>

#include <steam/trajectory/SteamTrajVar.hpp>

#include <steam/problem/WeightedLeastSqCostTerm.hpp>
#include <steam/problem/ParallelizedCostTermCollection.hpp>

#include <steam/solver/GaussNewtonSolverBase.hpp>

namespace steam {
namespace se3 {

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief The trajectory class wraps a set of state variables to provide an interface
///        that allows for continuous-time pose interpolation.
//////////////////////////////////////////////////////////////////////////////////////////////
class SteamTrajInterface
{
 public:

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Constructor
  ///        Note that the weighting matrix, Qc, should be provided if prior factors are needed
  ///        for estimation. Without Qc the interpolation methods can be used safely.
  //////////////////////////////////////////////////////////////////////////////////////////////
  SteamTrajInterface(bool allowExtrapolation = false);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Constructor
  //////////////////////////////////////////////////////////////////////////////////////////////
  SteamTrajInterface(const Eigen::Matrix<double,6,6>& Qc_inv, bool allowExtrapolation = false);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Add a new knot
  //////////////////////////////////////////////////////////////////////////////////////////////
  void add(const SteamTrajVar::Ptr& knot);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Add a new knot
  //////////////////////////////////////////////////////////////////////////////////////////////
  void add(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0,
           const VectorSpaceStateVar::Ptr& w_0k_ink);

  void add(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0,
           const VectorSpaceStateVar::Ptr& w_0k_ink,
           const Eigen::Matrix<double,12,12> cov);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Add a new knot
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual void add(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0,
           const VectorSpaceStateVar::Ptr& w_0k_ink,
           const VectorSpaceStateVar::Ptr& acceleration);

  virtual void add(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0,
           const VectorSpaceStateVar::Ptr& w_0k_ink,
           const VectorSpaceStateVar::Ptr& acceleration,
           const Eigen::Matrix<double,18,18> cov);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Get transform evaluator
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual TransformEvaluator::ConstPtr getInterpPoseEval(const steam::Time& time) const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Get velocity evaluator
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual Eigen::VectorXd getVelocity(const steam::Time& time);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Add a unary pose prior factor at a knot time. Note that only a single pose prior
  ///        should exist on a trajectory, adding a second will overwrite the first.
  //////////////////////////////////////////////////////////////////////////////////////////////
  void addPosePrior(const steam::Time& time, const lgmath::se3::Transformation& T_k0,
                    const Eigen::Matrix<double,6,6>& cov);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Add a unary velocity prior factor at a knot time. Note that only a single velocity
  ///        prior should exist on a trajectory, adding a second will overwrite the first.
  //////////////////////////////////////////////////////////////////////////////////////////////
  void addVelocityPrior(const steam::Time& time, const Eigen::Matrix<double,6,1>& w_0k_ink,
                        const Eigen::Matrix<double,6,6>& cov);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Get binary cost terms associated with the prior for active parts of the trajectory
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual void appendPriorCostTerms(const ParallelizedCostTermCollection::Ptr& costTerms) const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Get active state variables in the trajectory
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual void getActiveStateVariables(
      std::map<unsigned int, steam::StateVariableBase::Ptr>* outStates) const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Get pose prior cost
  //////////////////////////////////////////////////////////////////////////////////////////////
  double getPosePriorCost();

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Get velocity prior cost
  //////////////////////////////////////////////////////////////////////////////////////////////
  double getVelocityPriorCost();

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Store solver in trajectory, needed for querying covariances later
  //////////////////////////////////////////////////////////////////////////////////////////////
  void setSolver(std::shared_ptr<GaussNewtonSolverBase> solver);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Get interpolated/extrapolated covariance at given time
  //////////////////////////////////////////////////////////////////////////////////////////////
  Eigen::MatrixXd getCovariance(const steam::Time& time) const;

 protected:

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Ordered map of knots
  //////////////////////////////////////////////////////////////////////////////////////////////
  Eigen::Matrix<double,6,6,Eigen::DontAlign> Qc_inv_;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Allow for extrapolation
  //////////////////////////////////////////////////////////////////////////////////////////////
  bool allowExtrapolation_;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Pose prior
  //////////////////////////////////////////////////////////////////////////////////////////////
  steam::WeightedLeastSqCostTerm<6,6>::Ptr posePriorFactor_;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Velocity prior
  //////////////////////////////////////////////////////////////////////////////////////////////
  steam::WeightedLeastSqCostTerm<6,6>::Ptr velocityPriorFactor_;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Ordered map of knots
  //////////////////////////////////////////////////////////////////////////////////////////////
  std::map<int64_t, SteamTrajVar::Ptr> knotMap_;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Solver used, we use this to query covariances
  //////////////////////////////////////////////////////////////////////////////////////////////
  std::shared_ptr<GaussNewtonSolverBase> solver_;

};

} // se3
} // steam

#endif // STEAM_TRAJECTORY_INTERFACE_HPP


#ifndef BINDINGS_POINT_HPP_
#define BINDINGS_POINT_HPP_

#include "imgen/point.hpp"

#include <boost/python.hpp>

namespace py = boost::python;

namespace imgen { namespace bindings {

point_t::coord_t (*distance_point_point)(const point_t&, const point_t&)
    = &distance<point_t::coord_t>;

point_t::coord_t distance_point_line(const point_t& point, const py::tuple& line)
{
    return distance(point, {
        py::extract<point_t>(line[0]),
        py::extract<point_t>(line[1])
    });
}

} } // namespace imgen::bindings

#endif // BINDINGS_POINT_HPP_


#include <benchmark/benchmark.h>
#include <random>
#include "uuid_v4.h"
#include "basic.h"
#include <uuid/uuid.h>
#include <boost/uuid/uuid.hpp>
#include <boost/uuid/uuid_generators.hpp>
#include <boost/uuid/uuid_io.hpp>

#define SCALE 100
#define LOOP_START for(int i=0;i<SCALE;i++) {
#define LOOP_END }

static void Basic(benchmark::State& state) {
  std::string uuid;
  for (auto _ : state) {
    LOOP_START
    uuid = MathUtils::generateUUID();
    LOOP_END
  }
}
BENCHMARK(Basic);

static void Libuuid(benchmark::State& state) {
  char str[37] = {0};
  uuid_t uuid;
  for (auto _ : state) {
    LOOP_START
    uuid_generate_random(uuid);
    //uuid_unparse(uuid, str);
    LOOP_END
  }
}
BENCHMARK(Libuuid);

static void BoostUUID(benchmark::State& state) {
  boost::uuids::uuid uuid;
  std::string str;
  for (auto _ : state) {
    LOOP_START
    uuid = boost::uuids::random_generator()();
    //str = to_string(uuid);
    LOOP_END
  }
}
BENCHMARK(BoostUUID);

static void BoostUUIDmt19937(benchmark::State& state) {
  boost::uuids::uuid uuid;
  boost::uuids::random_generator_mt19937 gen;

  std::string str;
  for (auto _ : state) {
    LOOP_START
    uuid = gen();
    //str = to_string(uuid);
    LOOP_END
  }
}
BENCHMARK(BoostUUIDmt19937);

static void UUID_v4(benchmark::State& state) {
  UUID::UUIDGenerator<std::mt19937_64> uuidGenerator;
  UUID::UUID uuid;
  for (auto _ : state) {
    LOOP_START
    uuid = uuidGenerator.getUUID();
    LOOP_END
  }
}
BENCHMARK(UUID_v4);

BENCHMARK_MAIN();


///////////////////////////////////////////////////////////////////////////
//
// Copyright (c) 1998-2011, Industrial Light & Magic, a division of Lucas
// Digital Ltd. LLC
//
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
// *       Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// *       Redistributions in binary form must reproduce the above
// copyright notice, this list of conditions and the following disclaimer
// in the documentation and/or other materials provided with the
// distribution.
// *       Neither the name of Industrial Light & Magic nor the names of
// its contributors may be used to endorse or promote products derived
// from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
///////////////////////////////////////////////////////////////////////////

#include "PyIlmBaseConfigInternal.h"

#include "PyImathRandom.h"
#include "PyImathDecorators.h"
#include <Python.h>
#include <boost/python.hpp>
#include <boost/format.hpp>
#include <boost/python/make_constructor.hpp>
#include "PyImath.h"
#include "PyImathMathExc.h"
#include "PyImathFixedArray.h"

namespace PyImath{
using namespace boost::python;

template <class Rand, class T>
static T
nextf2 (Rand &rand, T min, T max)
{
    MATH_EXC_ON;
    return rand.nextf(min, max);
}

template <class Rand>
static float
nextGauss (Rand &rand)
{
    MATH_EXC_ON;
    return gaussRand(rand);
}

template <class T, class Rand>
static IMATH_NAMESPACE::Vec3<T> nextGaussSphere(Rand &rand, const IMATH_NAMESPACE::Vec3<T> &v)
{
    MATH_EXC_ON;
    return IMATH_NAMESPACE::gaussSphereRand<IMATH_NAMESPACE::Vec3<T>,Rand>(rand);
}
template <class T, class Rand>
static IMATH_NAMESPACE::Vec2<T> nextGaussSphere(Rand &rand, const IMATH_NAMESPACE::Vec2<T> &v)
{
    MATH_EXC_ON;
    return IMATH_NAMESPACE::gaussSphereRand<IMATH_NAMESPACE::Vec2<T>,Rand>(rand);
}

template <class T, class Rand>
static IMATH_NAMESPACE::Vec3<T> nextHollowSphere(Rand &rand, const IMATH_NAMESPACE::Vec3<T> &v)
{
    MATH_EXC_ON;
    return IMATH_NAMESPACE::hollowSphereRand<IMATH_NAMESPACE::Vec3<T>,Rand>(rand);
}

template <class T, class Rand>
static IMATH_NAMESPACE::Vec2<T> nextHollowSphere(Rand &rand, const IMATH_NAMESPACE::Vec2<T> &v)
{
    MATH_EXC_ON;
    return IMATH_NAMESPACE::hollowSphereRand<IMATH_NAMESPACE::Vec2<T>,Rand>(rand);
}

template <class T, class Rand>
static IMATH_NAMESPACE::Vec3<T> nextSolidSphere(Rand &rand, const IMATH_NAMESPACE::Vec3<T> &v)
{
    MATH_EXC_ON;
    return IMATH_NAMESPACE::solidSphereRand<IMATH_NAMESPACE::Vec3<T>,Rand>(rand);
}

template <class T, class Rand>
static IMATH_NAMESPACE::Vec2<T> nextSolidSphere(Rand &rand, const IMATH_NAMESPACE::Vec2<T> &v)
{
    MATH_EXC_ON;
    return IMATH_NAMESPACE::solidSphereRand<IMATH_NAMESPACE::Vec2<T>,Rand>(rand);
}

template <class Rand>
static Rand *Rand_constructor1(unsigned long int seed)
{
    return new Rand(seed);
}

template <class Rand>
static Rand *Rand_constructor2(Rand rand)
{
    Rand *r = new Rand();
    *r = rand;

    return r;
}

template <class T, class Rand>
static PyImath::FixedArray<IMATH_NAMESPACE::Vec3<T> >
hollowSphereRand(Rand &rand, int num)
{
    MATH_EXC_ON;
    PyImath::FixedArray<IMATH_NAMESPACE::Vec3<T> >  retval(num);
    for (int i=0; i<num; ++i) {
        retval[i] = IMATH_NAMESPACE::hollowSphereRand<IMATH_NAMESPACE::Vec3<T>,Rand>(rand);
    }
    return retval;
}

template <class T, class Rand>
static PyImath::FixedArray<IMATH_NAMESPACE::Vec3<T> >
solidSphereRand(Rand &rand, int num)
{
    MATH_EXC_ON;
    PyImath::FixedArray<IMATH_NAMESPACE::Vec3<T> >  retval(num);
    for (int i=0; i<num; ++i) {
        retval[i] = IMATH_NAMESPACE::solidSphereRand<IMATH_NAMESPACE::Vec3<T>,Rand>(rand);
    }
    return retval;
}

PYIMATH_EXPORT
class_<IMATH_NAMESPACE::Rand32>
register_Rand32()
{
    float (IMATH_NAMESPACE::Rand32::*nextf1)(void) = &IMATH_NAMESPACE::Rand32::nextf;

    IMATH_NAMESPACE::Vec3<float> (*nextGaussSphere1)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec3<float> &v) = &nextGaussSphere<float,IMATH_NAMESPACE::Rand32>;
    IMATH_NAMESPACE::Vec3<double> (*nextGaussSphere2)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec3<double> &v) = &nextGaussSphere<double,IMATH_NAMESPACE::Rand32>;
    IMATH_NAMESPACE::Vec2<float> (*nextGaussSphere3)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec2<float> &v) = &nextGaussSphere<float,IMATH_NAMESPACE::Rand32>;
    IMATH_NAMESPACE::Vec2<double> (*nextGaussSphere4)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec2<double> &v) = &nextGaussSphere<double,IMATH_NAMESPACE::Rand32>;

    IMATH_NAMESPACE::Vec3<float> (*nextHollowSphere1)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec3<float> &v) = &nextHollowSphere<float,IMATH_NAMESPACE::Rand32>;
    IMATH_NAMESPACE::Vec3<double> (*nextHollowSphere2)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec3<double> &v) = &nextHollowSphere<double,IMATH_NAMESPACE::Rand32>;
    IMATH_NAMESPACE::Vec2<float> (*nextHollowSphere3)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec2<float> &v) = &nextHollowSphere<float,IMATH_NAMESPACE::Rand32>;
    IMATH_NAMESPACE::Vec2<double> (*nextHollowSphere4)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec2<double> &v) = &nextHollowSphere<double,IMATH_NAMESPACE::Rand32>;

    IMATH_NAMESPACE::Vec3<float> (*nextSolidSphere1)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec3<float> &v) = &nextSolidSphere<float,IMATH_NAMESPACE::Rand32>;
    IMATH_NAMESPACE::Vec3<double> (*nextSolidSphere2)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec3<double> &v) = &nextSolidSphere<double,IMATH_NAMESPACE::Rand32>;
    IMATH_NAMESPACE::Vec2<float> (*nextSolidSphere3)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec2<float> &v) = &nextSolidSphere<float,IMATH_NAMESPACE::Rand32>;
    IMATH_NAMESPACE::Vec2<double> (*nextSolidSphere4)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec2<double> &v) = &nextSolidSphere<double,IMATH_NAMESPACE::Rand32>;

    class_< IMATH_NAMESPACE::Rand32 > rand32_class("Rand32");
    rand32_class
        .def(init<>("default construction"))
        .def("__init__", make_constructor(Rand_constructor1<IMATH_NAMESPACE::Rand32>))
        .def("__init__", make_constructor(Rand_constructor2<IMATH_NAMESPACE::Rand32>))
        .def("init", &IMATH_NAMESPACE::Rand32::init,
             "r.init(i) -- initialize with integer "
			 "seed i")

        .def("nexti", &IMATH_NAMESPACE::Rand32::nexti,
        	 "r.nexti() -- return the next integer "
			 "value in the uniformly-distributed "
			 "sequence")
        .def("nextf", nextf1,
        	 "r.nextf() -- return the next floating-point "
			 "value in the uniformly-distributed "
			 "sequence\n"

        	 "r.nextf(float, float) -- return the next floating-point "
			 "value in the uniformly-distributed "
			 "sequence")
        .def("nextf", &nextf2 <IMATH_NAMESPACE::Rand32, float>)

        .def("nextb", &IMATH_NAMESPACE::Rand32::nextb,
	 	     "r.nextb() -- return the next boolean "
			 "value in the uniformly-distributed "
			 "sequence")

        .def("nextGauss", &nextGauss<IMATH_NAMESPACE::Rand32>,
        	 "r.nextGauss() -- returns the next "
			 "floating-point value in the normally "
			 "(Gaussian) distributed sequence")

        .def("nextGaussSphere", nextGaussSphere1,
	 		 "r.nextGaussSphere(v) -- returns the next "
			 "point whose distance from the origin "
			 "has a normal (Gaussian) distribution with "
			 "mean 0 and variance 1.  The vector "
			 "argument, v, specifies the dimension "
			 "and number type.")
        .def("nextGaussSphere", nextGaussSphere2)
        .def("nextGaussSphere", nextGaussSphere3)
        .def("nextGaussSphere", nextGaussSphere4)

        .def("nextHollowSphere", nextHollowSphere1,
        	 "r.nextHollowSphere(v) -- return the next "
	 		 "point uniformly distributed on the surface "
	 		 "of a sphere of radius 1 centered at the "
	 		 "origin.  The vector argument, v, specifies "
			 "the dimension and number type.")
        .def("nextHollowSphere", nextHollowSphere2)
        .def("nextHollowSphere", nextHollowSphere3)
        .def("nextHollowSphere", nextHollowSphere4)

        .def("nextSolidSphere", nextSolidSphere1,
        	 "r.nextSolidSphere(v) -- return the next "
			 "point uniformly distributed in a sphere "
			 "of radius 1 centered at the origin.  The "
			 "vector argument, v, specifies the "
			 "dimension and number type.")
        .def("nextSolidSphere", nextSolidSphere2)
        .def("nextSolidSphere", nextSolidSphere3)
        .def("nextSolidSphere", nextSolidSphere4)
        ;

    def("hollowSphereRand",&hollowSphereRand<float,IMATH_NAMESPACE::Rand32>,"hollowSphereRand(randObj,num) return XYZ vectors uniformly "
        "distributed across the surface of a sphere generated from the given Rand32 object",
        args("randObj","num"));

    def("solidSphereRand",&solidSphereRand<float,IMATH_NAMESPACE::Rand32>,"solidSphereRand(randObj,num) return XYZ vectors uniformly "
        "distributed through the volume of a sphere generated from the given Rand32 object",
        args("randObj","num"));

    decoratecopy(rand32_class);

    return rand32_class;
}

PYIMATH_EXPORT
class_<IMATH_NAMESPACE::Rand48>
register_Rand48()
{
    double (IMATH_NAMESPACE::Rand48::*nextf1)(void) = &IMATH_NAMESPACE::Rand48::nextf;

    IMATH_NAMESPACE::Vec3<float> (*nextGaussSphere1)(IMATH_NAMESPACE::Rand48 &, const IMATH_NAMESPACE::Vec3<float> &v) = &nextGaussSphere<float,IMATH_NAMESPACE::Rand48>;
    IMATH_NAMESPACE::Vec3<double> (*nextGaussSphere2)(IMATH_NAMESPACE::Rand48 &, const IMATH_NAMESPACE::Vec3<double> &v) = &nextGaussSphere<double,IMATH_NAMESPACE::Rand48>;
    IMATH_NAMESPACE::Vec2<float> (*nextGaussSphere3)(IMATH_NAMESPACE::Rand48&, const IMATH_NAMESPACE::Vec2<float> &v) = &nextGaussSphere<float,IMATH_NAMESPACE::Rand48>;
    IMATH_NAMESPACE::Vec2<double> (*nextGaussSphere4)(IMATH_NAMESPACE::Rand48 &, const IMATH_NAMESPACE::Vec2<double> &v) = &nextGaussSphere<double,IMATH_NAMESPACE::Rand48>;

    IMATH_NAMESPACE::Vec3<float> (*nextHollowSphere1)(IMATH_NAMESPACE::Rand48 &, const IMATH_NAMESPACE::Vec3<float> &v) = &nextHollowSphere<float,IMATH_NAMESPACE::Rand48>;
    IMATH_NAMESPACE::Vec3<double> (*nextHollowSphere2)(IMATH_NAMESPACE::Rand48 &, const IMATH_NAMESPACE::Vec3<double> &v) = &nextHollowSphere<double,IMATH_NAMESPACE::Rand48>;
    IMATH_NAMESPACE::Vec2<float> (*nextHollowSphere3)(IMATH_NAMESPACE::Rand48 &, const IMATH_NAMESPACE::Vec2<float> &v) = &nextHollowSphere<float,IMATH_NAMESPACE::Rand48>;
    IMATH_NAMESPACE::Vec2<double> (*nextHollowSphere4)(IMATH_NAMESPACE::Rand48 &, const IMATH_NAMESPACE::Vec2<double> &v) = &nextHollowSphere<double,IMATH_NAMESPACE::Rand48>;

    IMATH_NAMESPACE::Vec3<float> (*nextSolidSphere1)(IMATH_NAMESPACE::Rand48 &, const IMATH_NAMESPACE::Vec3<float> &v) = &nextSolidSphere<float,IMATH_NAMESPACE::Rand48>;
    IMATH_NAMESPACE::Vec3<double> (*nextSolidSphere2)(IMATH_NAMESPACE::Rand48 &, const IMATH_NAMESPACE::Vec3<double> &v) = &nextSolidSphere<double,IMATH_NAMESPACE::Rand48>;
    IMATH_NAMESPACE::Vec2<float> (*nextSolidSphere3)(IMATH_NAMESPACE::Rand48&, const IMATH_NAMESPACE::Vec2<float> &v) = &nextSolidSphere<float,IMATH_NAMESPACE::Rand48>;
    IMATH_NAMESPACE::Vec2<double> (*nextSolidSphere4)(IMATH_NAMESPACE::Rand48 &, const IMATH_NAMESPACE::Vec2<double> &v) = &nextSolidSphere<double,IMATH_NAMESPACE::Rand48>;

    class_< IMATH_NAMESPACE::Rand48 > rand48_class("Rand48");
    rand48_class
        .def(init<>("default construction"))
        .def("__init__", make_constructor(Rand_constructor1<IMATH_NAMESPACE::Rand48>))
        .def("__init__", make_constructor(Rand_constructor2<IMATH_NAMESPACE::Rand48>))
        .def("init", &IMATH_NAMESPACE::Rand48::init,
             "r.init(i) -- initialize with integer "
			 "seed i")

        .def("nexti", &IMATH_NAMESPACE::Rand48::nexti,
        	 "r.nexti() -- return the next integer "
			 "value in the uniformly-distributed "
			 "sequence")

        .def("nextf", nextf1,
        	 "r.nextf() -- return the next double "
			 "value in the uniformly-distributed "
			 "sequence\n"

        	 "r.nextf(double,double) -- return the next double "
			 "value in the uniformly-distributed "
			 "sequence")
        .def("nextf", &nextf2 <IMATH_NAMESPACE::Rand48, double>)

        .def("nextb", &IMATH_NAMESPACE::Rand48::nextb,
	 	     "r.nextb() -- return the next boolean "
			 "value in the uniformly-distributed "
			 "sequence")

        .def("nextGauss", &nextGauss<IMATH_NAMESPACE::Rand48>,
        	 "r.nextGauss() -- returns the next "
			 "floating-point value in the normally "
			 "(Gaussian) distributed sequence")

        .def("nextGaussSphere", nextGaussSphere1,
	 		 "r.nextGaussSphere(v) -- returns the next "
			 "point whose distance from the origin "
			 "has a normal (Gaussian) distribution with "
			 "mean 0 and variance 1.  The vector "
			 "argument, v, specifies the dimension "
			 "and number type.")
        .def("nextGaussSphere", nextGaussSphere2)
        .def("nextGaussSphere", nextGaussSphere3)
        .def("nextGaussSphere", nextGaussSphere4)

        .def("nextHollowSphere", nextHollowSphere1,
        	 "r.nextHollowSphere(v) -- return the next "
	 		 "point uniformly distributed on the surface "
	 		 "of a sphere of radius 1 centered at the "
	 		 "origin.  The vector argument, v, specifies "
			 "the dimension and number type.")
        .def("nextHollowSphere", nextHollowSphere2)
        .def("nextHollowSphere", nextHollowSphere3)
        .def("nextHollowSphere", nextHollowSphere4)

        .def("nextSolidSphere", nextSolidSphere1,
        	 "r.nextSolidSphere(v) -- return the next "
			 "point uniformly distributed in a sphere "
			 "of radius 1 centered at the origin.  The "
			 "vector argument, v, specifies the "
			 "dimension and number type.")
        .def("nextSolidSphere", nextSolidSphere2)
        .def("nextSolidSphere", nextSolidSphere3)
        .def("nextSolidSphere", nextSolidSphere4)
        ;

    decoratecopy(rand48_class);

    return rand48_class;
}

//

PyObject *
Rand32::wrap (const IMATH_NAMESPACE::Rand32 &r)
{
    boost::python::return_by_value::apply <IMATH_NAMESPACE::Rand32>::type converter;
    PyObject *p = converter (r);
    return p;
}

PyObject *
Rand48::wrap (const IMATH_NAMESPACE::Rand48 &r)
{
    boost::python::return_by_value::apply <IMATH_NAMESPACE::Rand48>::type converter;
    PyObject *p = converter (r);
    return p;
}

} //namespace PyIMath


// Distributed under the MIT License.
// See LICENSE.txt for details.

#pragma once

#include <boost/preprocessor/arithmetic/dec.hpp>
#include <boost/preprocessor/arithmetic/inc.hpp>
#include <boost/preprocessor/control/expr_iif.hpp>
#include <boost/preprocessor/list/adt.hpp>
#include <boost/preprocessor/repetition/for.hpp>
#include <boost/preprocessor/repetition/repeat.hpp>
#include <boost/preprocessor/tuple/to_list.hpp>
#include <limits>
#include <pup.h>

#include "DataStructures/Tensor/TypeAliases.hpp"
#include "Options/Options.hpp"
#include "Parallel/CharmPupable.hpp"
#include "PointwiseFunctions/Hydro/EquationsOfState/EquationOfState.hpp"  // IWYU pragma: keep
#include "Utilities/TMPL.hpp"

/// \cond
class DataVector;
/// \endcond

namespace EquationsOfState {
/*!
 * \ingroup EquationsOfStateGroup
 * \brief Equation of state for a dark energy fluid
 *
 * A dark energy fluid equation of state:
 * \f[
 * p = w(z) \rho ( 1.0 + \epsilon)
 * \f]
 * where \f$\rho\f$ is the rest mass density, \f$\epsilon\f$ is the specific
 * internal energy, and \f$w(z) > 0\f$ is a parameter depending on the redshift
 * \f$z\f$.
 */
template <bool IsRelativistic>
class DarkEnergyFluid : public EquationOfState<IsRelativistic, 2> {
 public:
  static_assert(IsRelativistic,
                "Dark energy fluid equation of state only makes sense in a "
                "relativistic setting.");

  struct ParameterW {
    using type = double;
    static constexpr OptionString help = {"Parameter w(z)"};
  };

  static constexpr OptionString help = {
      "A dark energy fluid equation of state.\n"
      "The pressure is related to the rest mass density by "
      "p = w(z) * rho * (1 + epsilon), where p is the pressure, rho is the "
      "rest mass density, epsilon is the specific internal energy, and w(z) is "
      "a parameter."};

  using options = tmpl::list<ParameterW>;

  DarkEnergyFluid() = default;
  DarkEnergyFluid(const DarkEnergyFluid&) = default;
  DarkEnergyFluid& operator=(const DarkEnergyFluid&) = default;
  DarkEnergyFluid(DarkEnergyFluid&&) = default;
  DarkEnergyFluid& operator=(DarkEnergyFluid&&) = default;
  ~DarkEnergyFluid() override = default;

  explicit DarkEnergyFluid(double parameter_w) noexcept;

  EQUATION_OF_STATE_FORWARD_DECLARE_MEMBERS(DarkEnergyFluid, 2)

  WRAPPED_PUPable_decl_base_template(  // NOLINT
      SINGLE_ARG(EquationOfState<IsRelativistic, 2>), DarkEnergyFluid);

 private:
  EQUATION_OF_STATE_FORWARD_DECLARE_MEMBER_IMPLS(2)

  double parameter_w_ = std::numeric_limits<double>::signaling_NaN();
};

/// \cond
template <bool IsRelativistic>
PUP::able::PUP_ID EquationsOfState::DarkEnergyFluid<IsRelativistic>::my_PUP_ID =
    0;
/// \endcond
}  // namespace EquationsOfState


#include "quadrotor_msgs/PositionCommand.h"
#include <ros/ros.h>
#include <Eigen/Eigen>
#include <iostream>

int main(int argc, char** argv)
{
  ros::init(argc, argv, "pub_goal_node");
  ros::NodeHandle nh("~");
  ros::Publisher goal_pub = nh.advertise<quadrotor_msgs::PositionCommand>("/goal", 1);
  std::random_device rd;
  std::mt19937_64 gen_;
  std::uniform_real_distribution<double> pos_mean_rand_;
  gen_ = std::mt19937_64(rd());
  pos_mean_rand_ = std::uniform_real_distribution<double>(-1.0, 1.0);

  quadrotor_msgs::PositionCommand goal;
  int pub_num = 1000; // number of samples
  ros::Rate rate(1.0/30.0);
  Eigen::Vector3d last_pt(0.0, 0.0, 0.0);
  int i(0);
  while (ros::ok() && i < pub_num)
  {
    Eigen::Vector3d pt;
    double pos_mean = pos_mean_rand_(gen_);
    pt[0] = pos_mean * 20.0; // half map size x, should be half as the arg map_size_x in simulator.launch
    pos_mean = pos_mean_rand_(gen_);
    pt[1] = pos_mean * 20.0; // half map size y, should be half as the arg map_size_y in simulator.launch
    pt[2] = 0.0;
    if ((pt - last_pt).norm() >= 5)
    {
      i++;
      std::cout << i << std::endl;
      goal.position.x = pt[0];
      goal.position.y = pt[1];
      goal.position.z = pt[2];
      goal_pub.publish(goal);
      last_pt = pt;
      rate.sleep();
    }
  }

}